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>
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 ());
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_