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
51 if (samples.size () != sample_size_)
53 PCL_ERROR (
"[pcl::SampleConsensusModelSphere::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
60 template <
typename Po
intT>
bool
62 const Indices &samples, Eigen::VectorXf &model_coefficients)
const
65 if (samples.size () != sample_size_)
67 PCL_ERROR (
"[pcl::SampleConsensusModelSphere::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
72 for (
int i = 0; i < 4; i++)
74 temp (i, 0) = input_->points[samples[i]].x;
75 temp (i, 1) = input_->points[samples[i]].y;
76 temp (i, 2) = input_->points[samples[i]].z;
79 float m11 = temp.determinant ();
85 for (
int i = 0; i < 4; ++i)
87 temp (i, 0) = (input_->points[samples[i]].x) * (input_->points[samples[i]].x) +
88 (input_->points[samples[i]].y) * (input_->points[samples[i]].y) +
89 (input_->points[samples[i]].z) * (input_->points[samples[i]].z);
91 float m12 = temp.determinant ();
93 for (
int i = 0; i < 4; ++i)
95 temp (i, 1) = temp (i, 0);
96 temp (i, 0) = input_->points[samples[i]].x;
98 float m13 = temp.determinant ();
100 for (
int i = 0; i < 4; ++i)
102 temp (i, 2) = temp (i, 1);
103 temp (i, 1) = input_->points[samples[i]].y;
105 float m14 = temp.determinant ();
107 for (
int i = 0; i < 4; ++i)
109 temp (i, 0) = temp (i, 2);
110 temp (i, 1) = input_->points[samples[i]].x;
111 temp (i, 2) = input_->points[samples[i]].y;
112 temp (i, 3) = input_->points[samples[i]].z;
114 float m15 = temp.determinant ();
117 model_coefficients.resize (model_size_);
118 model_coefficients[0] = 0.5f * m12 / m11;
119 model_coefficients[1] = 0.5f * m13 / m11;
120 model_coefficients[2] = 0.5f * m14 / m11;
122 model_coefficients[3] = std::sqrt (model_coefficients[0] * model_coefficients[0] +
123 model_coefficients[1] * model_coefficients[1] +
124 model_coefficients[2] * model_coefficients[2] - m15 / m11);
130 template <
typename Po
intT>
void
132 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
135 if (!isModelValid (model_coefficients))
140 distances.resize (indices_->size ());
143 for (std::size_t i = 0; i < indices_->size (); ++i)
147 distances[i] = std::abs (std::sqrt (
148 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
149 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
151 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
152 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) +
154 ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) *
155 ( input_->points[(*indices_)[i]].z - model_coefficients[2] )
156 ) - model_coefficients[3]);
161 template <
typename Po
intT>
void
163 const Eigen::VectorXf &model_coefficients,
const double threshold, Indices &inliers)
166 if (!isModelValid (model_coefficients))
173 error_sqr_dists_.clear ();
174 inliers.reserve (indices_->size ());
175 error_sqr_dists_.reserve (indices_->size ());
178 for (std::size_t i = 0; i < indices_->size (); ++i)
180 double distance = std::abs (std::sqrt (
181 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
182 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
184 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
185 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) +
187 ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) *
188 ( input_->points[(*indices_)[i]].z - model_coefficients[2] )
189 ) - model_coefficients[3]);
192 if (distance < threshold)
195 inliers.push_back ((*indices_)[i]);
196 error_sqr_dists_.push_back (static_cast<double> (distance));
202 template <
typename Po
intT> std::size_t
204 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
207 if (!isModelValid (model_coefficients))
210 std::size_t nr_p = 0;
213 for (std::size_t i = 0; i < indices_->size (); ++i)
217 if (std::abs (std::sqrt (
218 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
219 ( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
221 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
222 ( input_->points[(*indices_)[i]].y - model_coefficients[1] ) +
224 ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) *
225 ( input_->points[(*indices_)[i]].z - model_coefficients[2] )
226 ) - model_coefficients[3]) < threshold)
233 template <
typename Po
intT>
void
235 const Indices &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const
237 optimized_coefficients = model_coefficients;
240 if (!isModelValid (model_coefficients))
242 PCL_ERROR (
"[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Given model is invalid!\n");
247 if (inliers.size () <= sample_size_)
249 PCL_ERROR (
"[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
253 OptimizationFunctor functor (
this, inliers);
254 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
255 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>,
float> lm (num_diff);
256 int info = lm.minimize (optimized_coefficients);
259 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",
260 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]);
264 template <
typename Po
intT>
void
266 const Indices &,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool)
const
269 if (!isModelValid (model_coefficients))
271 PCL_ERROR (
"[pcl::SampleConsensusModelSphere::projectPoints] Given model is invalid!\n");
276 projected_points.
points.resize (input_->points.size ());
277 projected_points.
header = input_->header;
278 projected_points.
width = input_->width;
279 projected_points.
height = input_->height;
280 projected_points.
is_dense = input_->is_dense;
282 PCL_WARN (
"[pcl::SampleConsensusModelSphere::projectPoints] Not implemented yet.\n");
283 projected_points.
points = input_->points;
287 template <
typename Po
intT>
bool
289 const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
292 if (!isModelValid (model_coefficients))
294 PCL_ERROR (
"[pcl::SampleConsensusModelSphere::doSamplesVerifyModel] Given model is invalid!\n");
298 for (
const auto &index : indices)
303 ( input_->points[index].x - model_coefficients[0] ) *
304 ( input_->points[index].x - model_coefficients[0] ) +
305 ( input_->points[index].y - model_coefficients[1] ) *
306 ( input_->points[index].y - model_coefficients[1] ) +
307 ( input_->points[index].z - model_coefficients[2] ) *
308 ( input_->points[index].z - model_coefficients[2] )
309 ) - model_coefficients[3]) > threshold)
318 #define PCL_INSTANTIATE_SampleConsensusModelSphere(T) template class PCL_EXPORTS pcl::SampleConsensusModelSphere<T>;
320 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_SPHERE_H_
void projectPoints(const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const override
Create a new point cloud with inliers projected onto the sphere model.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Select all the points which respect the given model coefficients as inliers.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
bool doSamplesVerifyModel(const std::set< index_t > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override
Verify whether a subset of indices verifies the given sphere model coefficients.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the sphere coefficients using the given inlier set and return them to the user...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid sphere model, compute the model coefficients f...
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const override
Count 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)...
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given sphere model.
pcl::PCLHeader header
The point cloud header.