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
53 (input_->points[samples[0]].x != input_->points[samples[1]].x)
55 (input_->points[samples[0]].y != input_->points[samples[1]].y)
57 (input_->points[samples[0]].z != input_->points[samples[1]].z))
64 template <
typename Po
intT>
bool
66 const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
const
69 if (samples.size () != 2)
71 PCL_ERROR (
"[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
75 if (fabs (input_->points[samples[0]].x - input_->points[samples[1]].x) <= std::numeric_limits<float>::epsilon () &&
76 fabs (input_->points[samples[0]].y - input_->points[samples[1]].y) <= std::numeric_limits<float>::epsilon () &&
77 fabs (input_->points[samples[0]].z - input_->points[samples[1]].z) <= std::numeric_limits<float>::epsilon ())
82 model_coefficients.resize (6);
83 model_coefficients[0] = input_->points[samples[0]].x;
84 model_coefficients[1] = input_->points[samples[0]].y;
85 model_coefficients[2] = input_->points[samples[0]].z;
87 model_coefficients[3] = input_->points[samples[1]].x - model_coefficients[0];
88 model_coefficients[4] = input_->points[samples[1]].y - model_coefficients[1];
89 model_coefficients[5] = input_->points[samples[1]].z - model_coefficients[2];
91 model_coefficients.template tail<3> ().normalize ();
96 template <
typename Po
intT>
void
98 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
101 if (!isModelValid (model_coefficients))
104 distances.resize (indices_->size ());
107 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
108 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
109 line_dir.normalize ();
112 for (
size_t i = 0; i < indices_->size (); ++i)
117 distances[i] = sqrt ((line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ());
122 template <
typename Po
intT>
void
124 const Eigen::VectorXf &model_coefficients,
const double threshold, std::vector<int> &inliers)
127 if (!isModelValid (model_coefficients))
130 double sqr_threshold = threshold * threshold;
133 inliers.resize (indices_->size ());
134 error_sqr_dists_.resize (indices_->size ());
137 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
138 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
139 line_dir.normalize ();
142 for (
size_t i = 0; i < indices_->size (); ++i)
146 double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
148 if (sqr_distance < sqr_threshold)
151 inliers[nr_p] = (*indices_)[i];
152 error_sqr_dists_[nr_p] = sqr_distance;
156 inliers.resize (nr_p);
157 error_sqr_dists_.resize (nr_p);
161 template <
typename Po
intT>
int
163 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
166 if (!isModelValid (model_coefficients))
169 double sqr_threshold = threshold * threshold;
174 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
175 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
176 line_dir.normalize ();
179 for (
size_t i = 0; i < indices_->size (); ++i)
183 double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
185 if (sqr_distance < sqr_threshold)
192 template <
typename Po
intT>
void
194 const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const
197 if (!isModelValid (model_coefficients))
199 optimized_coefficients = model_coefficients;
204 if (inliers.size () <= 2)
206 PCL_ERROR (
"[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
207 optimized_coefficients = model_coefficients;
211 optimized_coefficients.resize (6);
214 Eigen::Vector4f centroid;
216 Eigen::Matrix3f covariance_matrix;
218 optimized_coefficients[0] = centroid[0];
219 optimized_coefficients[1] = centroid[1];
220 optimized_coefficients[2] = centroid[2];
229 optimized_coefficients.template tail<3> ().matrix () = eigen_vector;
233 template <
typename Po
intT>
void
235 const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
const
238 if (!isModelValid (model_coefficients))
242 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
243 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
245 projected_points.
header = input_->header;
246 projected_points.
is_dense = input_->is_dense;
249 if (copy_data_fields)
252 projected_points.
points.resize (input_->points.size ());
253 projected_points.
width = input_->width;
254 projected_points.
height = input_->height;
258 for (
size_t i = 0; i < projected_points.
points.size (); ++i)
263 for (
size_t i = 0; i < inliers.size (); ++i)
265 Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
267 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
269 Eigen::Vector4f pp = line_pt + k * line_dir;
271 projected_points.
points[inliers[i]].x = pp[0];
272 projected_points.
points[inliers[i]].y = pp[1];
273 projected_points.
points[inliers[i]].z = pp[2];
279 projected_points.
points.resize (inliers.size ());
280 projected_points.
width =
static_cast<uint32_t
> (inliers.size ());
281 projected_points.
height = 1;
285 for (
size_t i = 0; i < inliers.size (); ++i)
290 for (
size_t i = 0; i < inliers.size (); ++i)
292 Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
294 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
296 Eigen::Vector4f pp = line_pt + k * line_dir;
298 projected_points.
points[i].x = pp[0];
299 projected_points.
points[i].y = pp[1];
300 projected_points.
points[i].z = pp[2];
306 template <
typename Po
intT>
bool
308 const std::set<int> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
311 if (!isModelValid (model_coefficients))
315 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
316 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
317 line_dir.normalize ();
319 double sqr_threshold = threshold * threshold;
321 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
325 if ((line_pt - input_->points[*it].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold)
332 #define PCL_INSTANTIATE_SampleConsensusModelLine(T) template class PCL_EXPORTS pcl::SampleConsensusModelLine<T>;
334 #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...
uint32_t width
The point cloud width (if organized as an image-structure).
struct pcl::PointXYZIEdge EIGEN_ALIGN16
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.
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.
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients) const
Check whether the given index samples can form a valid line model, compute the model coefficients fro...
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 line model coefficients.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all squared distances from the cloud data to a given line model.
uint32_t height
The point cloud height (if organized as an image-structure).
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...
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
Recompute the line 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. ...
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 line model.
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.
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.
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)...
pcl::PCLHeader header
The point cloud header.