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;
226 PCL_WARN (
"[pcl::SampleConsensusModelLine::optimizeModelCoefficients] compute3DCentroid failed (returned 0) because there are no valid inliers.\n");
227 optimized_coefficients = model_coefficients;
230 Eigen::Matrix3f covariance_matrix;
232 optimized_coefficients[0] = centroid[0];
233 optimized_coefficients[1] = centroid[1];
234 optimized_coefficients[2] = centroid[2];
237 EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
238 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
243 optimized_coefficients.template tail<3> ().matrix () = eigen_vector;
247 template <
typename Po
intT>
void
249 const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
const
252 if (!isModelValid (model_coefficients))
256 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
257 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
259 projected_points.
header = input_->header;
260 projected_points.
is_dense = input_->is_dense;
263 if (copy_data_fields)
266 projected_points.
resize (input_->size ());
267 projected_points.
width = input_->width;
268 projected_points.
height = input_->height;
270 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
272 for (std::size_t i = 0; i < projected_points.
size (); ++i)
277 for (
const auto &inlier : inliers)
279 Eigen::Vector4f pt ((*input_)[inlier].x, (*input_)[inlier].y, (*input_)[inlier].z, 0.0f);
281 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
283 Eigen::Vector4f pp = line_pt + k * line_dir;
285 projected_points[inlier].x = pp[0];
286 projected_points[inlier].y = pp[1];
287 projected_points[inlier].z = pp[2];
293 projected_points.
resize (inliers.size ());
294 projected_points.
width = inliers.size ();
295 projected_points.
height = 1;
297 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
299 for (std::size_t i = 0; i < inliers.size (); ++i)
304 for (std::size_t i = 0; i < inliers.size (); ++i)
306 Eigen::Vector4f pt ((*input_)[inliers[i]].x, (*input_)[inliers[i]].y, (*input_)[inliers[i]].z, 0.0f);
308 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
310 Eigen::Vector4f pp = line_pt + k * line_dir;
312 projected_points[i].x = pp[0];
313 projected_points[i].y = pp[1];
314 projected_points[i].z = pp[2];
320 template <
typename Po
intT>
bool
322 const std::set<index_t> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const
325 if (!isModelValid (model_coefficients))
329 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0.0f);
330 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0.0f);
331 line_dir.normalize ();
333 double sqr_threshold = threshold * threshold;
335 for (
const auto &index : indices)
339 if ((line_pt - (*input_)[index].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold)
346 #define PCL_INSTANTIATE_SampleConsensusModelLine(T) template class PCL_EXPORTS pcl::SampleConsensusModelLine<T>;
348 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_