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>
46 #include <pcl/common/concatenate.h>
47 #include <pcl/common/eigen.h>
50 template <
typename Po
intT>
bool
53 if (samples.size () != sample_size_)
55 PCL_ERROR (
"[pcl::SampleConsensusModelLine::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
60 ((*input_)[samples[0]].x != (*input_)[samples[1]].x)
62 ((*input_)[samples[0]].y != (*input_)[samples[1]].y)
64 ((*input_)[samples[0]].z != (*input_)[samples[1]].z))
73 template <
typename Po
intT>
bool
75 const Indices &samples, Eigen::VectorXf &model_coefficients)
const
78 if (samples.size () != sample_size_)
80 PCL_ERROR (
"[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
84 if (std::abs ((*input_)[samples[0]].x - (*input_)[samples[1]].x) <= std::numeric_limits<float>::epsilon () &&
85 std::abs ((*input_)[samples[0]].y - (*input_)[samples[1]].y) <= std::numeric_limits<float>::epsilon () &&
86 std::abs ((*input_)[samples[0]].z - (*input_)[samples[1]].z) <= std::numeric_limits<float>::epsilon ())
91 model_coefficients.resize (model_size_);
92 model_coefficients[0] = (*input_)[samples[0]].x;
93 model_coefficients[1] = (*input_)[samples[0]].y;
94 model_coefficients[2] = (*input_)[samples[0]].z;
96 model_coefficients[3] = (*input_)[samples[1]].x - model_coefficients[0];
97 model_coefficients[4] = (*input_)[samples[1]].y - model_coefficients[1];
98 model_coefficients[5] = (*input_)[samples[1]].z - model_coefficients[2];
100 model_coefficients.template tail<3> ().normalize ();
105 template <
typename Po
intT>
void
107 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
110 if (!isModelValid (model_coefficients))
115 distances.resize (indices_->size ());
118 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
119 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
120 line_dir.normalize ();
123 for (std::size_t i = 0; i < indices_->size (); ++i)
128 distances[i] = sqrt ((line_pt - (*input_)[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ());
133 template <
typename Po
intT>
void
135 const Eigen::VectorXf &model_coefficients,
const double threshold,
Indices &inliers)
138 if (!isModelValid (model_coefficients))
141 double sqr_threshold = threshold * threshold;
144 error_sqr_dists_.clear ();
145 inliers.reserve (indices_->size ());
146 error_sqr_dists_.reserve (indices_->size ());
149 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
150 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
151 line_dir.normalize ();
154 for (std::size_t i = 0; i < indices_->size (); ++i)
158 double sqr_distance = (line_pt - (*input_)[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
160 if (sqr_distance < sqr_threshold)
163 inliers.push_back ((*indices_)[i]);
164 error_sqr_dists_.push_back (sqr_distance);
170 template <
typename Po
intT> std::size_t
172 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
175 if (!isModelValid (model_coefficients))
178 double sqr_threshold = threshold * threshold;
180 std::size_t nr_p = 0;
183 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
184 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
185 line_dir.normalize ();
188 for (std::size_t i = 0; i < indices_->size (); ++i)
192 double sqr_distance = (line_pt - (*input_)[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
194 if (sqr_distance < sqr_threshold)
201 template <
typename Po
intT>
void
203 const Indices &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const
206 if (!isModelValid (model_coefficients))
208 optimized_coefficients = model_coefficients;
213 if (inliers.size () <= sample_size_)
215 PCL_ERROR (
"[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
216 optimized_coefficients = model_coefficients;
220 optimized_coefficients.resize (model_size_);
223 Eigen::Vector4f centroid;
225 Eigen::Matrix3f covariance_matrix;
227 optimized_coefficients[0] = centroid[0];
228 optimized_coefficients[1] = centroid[1];
229 optimized_coefficients[2] = centroid[2];
232 EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
233 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
238 optimized_coefficients.template tail<3> ().matrix () = eigen_vector;
242 template <
typename Po
intT>
void
244 const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
const
247 if (!isModelValid (model_coefficients))
251 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
252 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
254 projected_points.
header = input_->header;
255 projected_points.
is_dense = input_->is_dense;
258 if (copy_data_fields)
261 projected_points.
resize (input_->size ());
262 projected_points.
width = input_->width;
263 projected_points.
height = input_->height;
265 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
267 for (std::size_t i = 0; i < projected_points.
size (); ++i)
272 for (
const auto &inlier : inliers)
274 Eigen::Vector4f pt ((*input_)[inlier].x, (*input_)[inlier].y, (*input_)[inlier].z, 0.0f);
276 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
278 Eigen::Vector4f pp = line_pt + k * line_dir;
280 projected_points[inlier].x = pp[0];
281 projected_points[inlier].y = pp[1];
282 projected_points[inlier].z = pp[2];
288 projected_points.
resize (inliers.size ());
289 projected_points.
width = inliers.size ();
290 projected_points.
height = 1;
292 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
294 for (std::size_t i = 0; i < inliers.size (); ++i)
299 for (std::size_t i = 0; i < inliers.size (); ++i)
301 Eigen::Vector4f pt ((*input_)[inliers[i]].x, (*input_)[inliers[i]].y, (*input_)[inliers[i]].z, 0.0f);
303 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
305 Eigen::Vector4f pp = line_pt + k * line_dir;
307 projected_points[i].x = pp[0];
308 projected_points[i].y = pp[1];
309 projected_points[i].z = pp[2];
315 template <
typename Po
intT>
bool
317 const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
320 if (!isModelValid (model_coefficients))
324 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
325 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
326 line_dir.normalize ();
328 double sqr_threshold = threshold * threshold;
330 for (
const auto &index : indices)
334 if ((line_pt - (*input_)[index].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold)
341 #define PCL_INSTANTIATE_SampleConsensusModelLine(T) template class PCL_EXPORTS pcl::SampleConsensusModelLine<T>;
343 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_