41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_
44 #include <pcl/sample_consensus/sac_model_line.h>
45 #include <pcl/common/centroid.h>
46 #include <pcl/common/concatenate.h>
49 template <
typename Po
intT>
bool
52 if (samples.size () != sample_size_)
54 PCL_ERROR (
"[pcl::SampleConsensusModelLine::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
59 (input_->points[samples[0]].x != input_->points[samples[1]].x)
61 (input_->points[samples[0]].y != input_->points[samples[1]].y)
63 (input_->points[samples[0]].z != input_->points[samples[1]].z))
72 template <
typename Po
intT>
bool
74 const Indices &samples, Eigen::VectorXf &model_coefficients)
const
77 if (samples.size () != sample_size_)
79 PCL_ERROR (
"[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
83 if (std::abs (input_->points[samples[0]].x - input_->points[samples[1]].x) <= std::numeric_limits<float>::epsilon () &&
84 std::abs (input_->points[samples[0]].y - input_->points[samples[1]].y) <= std::numeric_limits<float>::epsilon () &&
85 std::abs (input_->points[samples[0]].z - input_->points[samples[1]].z) <= std::numeric_limits<float>::epsilon ())
90 model_coefficients.resize (model_size_);
91 model_coefficients[0] = input_->points[samples[0]].x;
92 model_coefficients[1] = input_->points[samples[0]].y;
93 model_coefficients[2] = input_->points[samples[0]].z;
95 model_coefficients[3] = input_->points[samples[1]].x - model_coefficients[0];
96 model_coefficients[4] = input_->points[samples[1]].y - model_coefficients[1];
97 model_coefficients[5] = input_->points[samples[1]].z - model_coefficients[2];
99 model_coefficients.template tail<3> ().normalize ();
104 template <
typename Po
intT>
void
106 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
109 if (!isModelValid (model_coefficients))
114 distances.resize (indices_->size ());
117 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
118 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
119 line_dir.normalize ();
122 for (std::size_t i = 0; i < indices_->size (); ++i)
127 distances[i] = sqrt ((line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ());
132 template <
typename Po
intT>
void
134 const Eigen::VectorXf &model_coefficients,
const double threshold, Indices &inliers)
137 if (!isModelValid (model_coefficients))
140 double sqr_threshold = threshold * threshold;
143 error_sqr_dists_.clear ();
144 inliers.reserve (indices_->size ());
145 error_sqr_dists_.reserve (indices_->size ());
148 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
149 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
150 line_dir.normalize ();
153 for (std::size_t i = 0; i < indices_->size (); ++i)
157 double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
159 if (sqr_distance < sqr_threshold)
162 inliers.push_back ((*indices_)[i]);
163 error_sqr_dists_.push_back (sqr_distance);
169 template <
typename Po
intT> std::size_t
171 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
174 if (!isModelValid (model_coefficients))
177 double sqr_threshold = threshold * threshold;
179 std::size_t nr_p = 0;
182 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
183 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
184 line_dir.normalize ();
187 for (std::size_t i = 0; i < indices_->size (); ++i)
191 double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
193 if (sqr_distance < sqr_threshold)
200 template <
typename Po
intT>
void
202 const Indices &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const
205 if (!isModelValid (model_coefficients))
207 optimized_coefficients = model_coefficients;
212 if (inliers.size () <= sample_size_)
214 PCL_ERROR (
"[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
215 optimized_coefficients = model_coefficients;
219 optimized_coefficients.resize (model_size_);
222 Eigen::Vector4f centroid;
224 Eigen::Matrix3f covariance_matrix;
226 optimized_coefficients[0] = centroid[0];
227 optimized_coefficients[1] = centroid[1];
228 optimized_coefficients[2] = centroid[2];
231 EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
232 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
237 optimized_coefficients.template tail<3> ().matrix () = eigen_vector;
241 template <
typename Po
intT>
void
243 const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
const
246 if (!isModelValid (model_coefficients))
250 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
251 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
253 projected_points.
header = input_->header;
254 projected_points.
is_dense = input_->is_dense;
257 if (copy_data_fields)
260 projected_points.
points.resize (input_->points.size ());
261 projected_points.
width = input_->width;
262 projected_points.
height = input_->height;
266 for (std::size_t i = 0; i < projected_points.
points.size (); ++i)
271 for (
const auto &inlier : inliers)
273 Eigen::Vector4f pt (input_->points[inlier].x, input_->points[inlier].y, input_->points[inlier].z, 0.0f);
275 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
277 Eigen::Vector4f pp = line_pt + k * line_dir;
279 projected_points.
points[inlier].x = pp[0];
280 projected_points.
points[inlier].y = pp[1];
281 projected_points.
points[inlier].z = pp[2];
287 projected_points.
points.resize (inliers.size ());
288 projected_points.
width =
static_cast<std::uint32_t
> (inliers.size ());
289 projected_points.
height = 1;
293 for (std::size_t i = 0; i < inliers.size (); ++i)
298 for (std::size_t i = 0; i < inliers.size (); ++i)
300 Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0.0f);
302 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
304 Eigen::Vector4f pp = line_pt + k * line_dir;
306 projected_points.
points[i].x = pp[0];
307 projected_points.
points[i].y = pp[1];
308 projected_points.
points[i].z = pp[2];
314 template <
typename Po
intT>
bool
316 const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
319 if (!isModelValid (model_coefficients))
323 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
324 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
325 line_dir.normalize ();
327 double sqr_threshold = threshold * threshold;
329 for (
const auto &index : indices)
333 if ((line_pt - input_->points[index].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold)
340 #define PCL_INSTANTIATE_SampleConsensusModelLine(T) template class PCL_EXPORTS pcl::SampleConsensusModelLine<T>;
342 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_
void computeCorrespondingEigenVector(const Matrix &mat, const typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the corresponding eigenvector to the given eigenvalue of the symmetric positive semi defin...
std::uint32_t width
The point cloud width (if organized as an image-structure).
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
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 line model.
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::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 line model coefficients.
unsigned int computeCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, const Eigen::Matrix< Scalar, 4, 1 > ¢roid, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix)
Compute the 3x3 covariance matrix of a given set of points.
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the line coefficients using the given inlier set and return them to the user...
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all squared distances from the cloud data to a given line model.
std::uint32_t height
The point cloud height (if organized as an image-structure).
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Helper functor structure for concatenate.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
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 computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid line model, compute the model coefficients fro...
pcl::PCLHeader header
The point cloud header.