41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_SPHERE_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_SPHERE_H_
44 #include <pcl/sample_consensus/eigen.h>
45 #include <pcl/sample_consensus/sac_model_sphere.h>
48 template <
typename Po
intT>
bool
55 template <
typename Po
intT>
bool
57 const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
const
60 if (samples.size () != 4)
62 PCL_ERROR (
"[pcl::SampleConsensusModelSphere::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
67 for (
int i = 0; i < 4; i++)
69 temp (i, 0) = input_->points[samples[i]].x;
70 temp (i, 1) = input_->points[samples[i]].y;
71 temp (i, 2) = input_->points[samples[i]].z;
74 float m11 = temp.determinant ();
78 for (
int i = 0; i < 4; ++i)
79 temp (i, 0) = (input_->points[samples[i]].x) * (input_->points[samples[i]].x) +
80 (input_->points[samples[i]].y) * (input_->points[samples[i]].y) +
81 (input_->points[samples[i]].z) * (input_->points[samples[i]].z);
82 float m12 = temp.determinant ();
84 for (
int i = 0; i < 4; ++i)
86 temp (i, 1) = temp (i, 0);
87 temp (i, 0) = input_->points[samples[i]].x;
89 float m13 = temp.determinant ();
91 for (
int i = 0; i < 4; ++i)
93 temp (i, 2) = temp (i, 1);
94 temp (i, 1) = input_->points[samples[i]].y;
96 float m14 = temp.determinant ();
98 for (
int i = 0; i < 4; ++i)
100 temp (i, 0) = temp (i, 2);
101 temp (i, 1) = input_->points[samples[i]].x;
102 temp (i, 2) = input_->points[samples[i]].y;
103 temp (i, 3) = input_->points[samples[i]].z;
105 float m15 = temp.determinant ();
108 model_coefficients.resize (4);
109 model_coefficients[0] = 0.5f * m12 / m11;
110 model_coefficients[1] = 0.5f * m13 / m11;
111 model_coefficients[2] = 0.5f * m14 / m11;
113 model_coefficients[3] = std::sqrt (model_coefficients[0] * model_coefficients[0] +
114 model_coefficients[1] * model_coefficients[1] +
115 model_coefficients[2] * model_coefficients[2] - m15 / m11);
121 template <
typename Po
intT>
void
123 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
126 if (!isModelValid (model_coefficients))
131 distances.resize (indices_->size ());
134 for (
size_t i = 0; i < indices_->size (); ++i)
137 distances[i] = fabs (std::sqrt (
138 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
139 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
141 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
142 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) +
144 ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) *
145 ( input_->points[(*indices_)[i]].z - model_coefficients[2] )
146 ) - model_coefficients[3]);
150 template <
typename Po
intT>
void
152 const Eigen::VectorXf &model_coefficients,
const double threshold, std::vector<int> &inliers)
155 if (!isModelValid (model_coefficients))
162 inliers.resize (indices_->size ());
163 error_sqr_dists_.resize (indices_->size ());
166 for (
size_t i = 0; i < indices_->size (); ++i)
168 double distance = fabs (std::sqrt (
169 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
170 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
172 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
173 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) +
175 ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) *
176 ( input_->points[(*indices_)[i]].z - model_coefficients[2] )
177 ) - model_coefficients[3]);
180 if (distance < threshold)
183 inliers[nr_p] = (*indices_)[i];
184 error_sqr_dists_[nr_p] =
static_cast<double> (distance);
188 inliers.resize (nr_p);
189 error_sqr_dists_.resize (nr_p);
193 template <
typename Po
intT>
int
195 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
198 if (!isModelValid (model_coefficients))
204 for (
size_t i = 0; i < indices_->size (); ++i)
208 if (fabs (std::sqrt (
209 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
210 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
212 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
213 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) +
215 ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) *
216 ( input_->points[(*indices_)[i]].z - model_coefficients[2] )
217 ) - model_coefficients[3]) < threshold)
224 template <
typename Po
intT>
void
226 const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const
228 optimized_coefficients = model_coefficients;
231 if (model_coefficients.size () != 4)
233 PCL_ERROR (
"[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
238 if (inliers.size () <= 4)
240 PCL_ERROR (
"[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
244 OptimizationFunctor functor (
this, inliers);
245 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
246 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>,
float> lm (num_diff);
247 int info = lm.minimize (optimized_coefficients);
250 PCL_DEBUG (
"[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g \nFinal solution: %g %g %g %g\n",
251 info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3]);
255 template <
typename Po
intT>
void
257 const std::vector<int> &,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool)
const
260 if (model_coefficients.size () != 4)
262 PCL_ERROR (
"[pcl::SampleConsensusModelSphere::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
267 projected_points.
points.resize (input_->points.size ());
268 projected_points.
header = input_->header;
269 projected_points.
width = input_->width;
270 projected_points.
height = input_->height;
271 projected_points.
is_dense = input_->is_dense;
273 PCL_WARN (
"[pcl::SampleConsensusModelSphere::projectPoints] Not implemented yet.\n");
274 projected_points.
points = input_->points;
278 template <
typename Po
intT>
bool
280 const std::set<int> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
283 if (model_coefficients.size () != 4)
285 PCL_ERROR (
"[pcl::SampleConsensusModelSphere::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
289 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
293 ( input_->points[*it].x - model_coefficients[0] ) *
294 ( input_->points[*it].x - model_coefficients[0] ) +
295 ( input_->points[*it].y - model_coefficients[1] ) *
296 ( input_->points[*it].y - model_coefficients[1] ) +
297 ( input_->points[*it].z - model_coefficients[2] ) *
298 ( input_->points[*it].z - model_coefficients[2] )
299 ) - model_coefficients[3]) > threshold)
305 #define PCL_INSTANTIATE_SampleConsensusModelSphere(T) template class PCL_EXPORTS pcl::SampleConsensusModelSphere<T>;
307 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_SPHERE_H_
bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
Verify whether a subset of indices verifies the given sphere model coefficients.
uint32_t width
The point cloud width (if organized as an image-structure).
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
Recompute the sphere coefficients using the given inlier set and return them to the user...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
void projectPoints(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const
Create a new point cloud with inliers projected onto the sphere model.
uint32_t height
The point cloud height (if organized as an image-structure).
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, std::vector< int > &inliers)
Select all the points which respect the given model coefficients as inliers.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients) const
Check whether the given index samples can form a valid sphere model, compute the model coefficients f...
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all distances from the cloud data to a given sphere model.
pcl::PCLHeader header
The point cloud header.