41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_
44 #include <pcl/sample_consensus/sac_model_stick.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)
69 if (samples.size () != 2)
71 PCL_ERROR (
"[pcl::SampleConsensusModelStick::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
75 model_coefficients.resize (7);
76 model_coefficients[0] = input_->points[samples[0]].x;
77 model_coefficients[1] = input_->points[samples[0]].y;
78 model_coefficients[2] = input_->points[samples[0]].z;
80 model_coefficients[3] = input_->points[samples[1]].x;
81 model_coefficients[4] = input_->points[samples[1]].y;
82 model_coefficients[5] = input_->points[samples[1]].z;
95 template <
typename Po
intT>
void
97 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
100 if (!isModelValid (model_coefficients))
103 float sqr_threshold =
static_cast<float> (radius_max_ * radius_max_);
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)
116 float sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
118 if (sqr_distance < sqr_threshold)
120 distances[i] = sqrt (sqr_distance);
123 distances[i] = 2 * sqrt (sqr_distance);
128 template <
typename Po
intT>
void
130 const Eigen::VectorXf &model_coefficients,
const double threshold, std::vector<int> &inliers)
133 if (!isModelValid (model_coefficients))
136 float sqr_threshold =
static_cast<float> (threshold * threshold);
139 inliers.resize (indices_->size ());
140 error_sqr_dists_.resize (indices_->size ());
143 Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
144 Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
145 Eigen::Vector4f line_dir = line_pt2 - line_pt1;
148 line_dir.normalize ();
152 for (
size_t i = 0; i < indices_->size (); ++i)
156 Eigen::Vector4f dir = input_->points[(*indices_)[i]].getVector4fMap () - line_pt1;
163 float sqr_distance = dir.cross3 (line_dir).squaredNorm ();
164 if (sqr_distance < sqr_threshold)
167 inliers[nr_p] = (*indices_)[i];
168 error_sqr_dists_[nr_p] =
static_cast<double> (sqr_distance);
172 inliers.resize (nr_p);
173 error_sqr_dists_.resize (nr_p);
177 template <
typename Po
intT>
int
179 const Eigen::VectorXf &model_coefficients,
const double threshold)
182 if (!isModelValid (model_coefficients))
185 float sqr_threshold =
static_cast<float> (threshold * threshold);
187 int nr_i = 0, nr_o = 0;
190 Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
191 Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
192 Eigen::Vector4f line_dir = line_pt2 - line_pt1;
193 line_dir.normalize ();
199 for (
size_t i = 0; i < indices_->size (); ++i)
203 Eigen::Vector4f dir = input_->points[(*indices_)[i]].getVector4fMap () - line_pt1;
210 float sqr_distance = dir.cross3 (line_dir).squaredNorm ();
212 if (sqr_distance < sqr_threshold)
214 else if (sqr_distance < 4 * sqr_threshold)
218 return (nr_i - nr_o < 0 ? 0 : nr_i - nr_o);
222 template <
typename Po
intT>
void
224 const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
227 if (!isModelValid (model_coefficients))
229 optimized_coefficients = model_coefficients;
234 if (inliers.size () <= 2)
236 PCL_ERROR (
"[pcl::SampleConsensusModelStick::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
237 optimized_coefficients = model_coefficients;
241 optimized_coefficients.resize (7);
244 Eigen::Vector4f centroid;
245 Eigen::Matrix3f covariance_matrix;
249 optimized_coefficients[0] = centroid[0];
250 optimized_coefficients[1] = centroid[1];
251 optimized_coefficients[2] = centroid[2];
254 Eigen::Vector3f eigen_values;
255 Eigen::Vector3f eigen_vector;
259 optimized_coefficients.template segment<3> (3).matrix () = eigen_vector;
263 template <
typename Po
intT>
void
265 const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points,
bool copy_data_fields)
268 if (!isModelValid (model_coefficients))
272 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
273 Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
275 projected_points.
header = input_->header;
276 projected_points.
is_dense = input_->is_dense;
279 if (copy_data_fields)
282 projected_points.
points.resize (input_->points.size ());
283 projected_points.
width = input_->width;
284 projected_points.
height = input_->height;
288 for (
size_t i = 0; i < projected_points.
points.size (); ++i)
293 for (
size_t i = 0; i < inliers.size (); ++i)
295 Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
297 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
299 Eigen::Vector4f pp = line_pt + k * line_dir;
301 projected_points.
points[inliers[i]].x = pp[0];
302 projected_points.
points[inliers[i]].y = pp[1];
303 projected_points.
points[inliers[i]].z = pp[2];
309 projected_points.
points.resize (inliers.size ());
310 projected_points.
width =
static_cast<uint32_t
> (inliers.size ());
311 projected_points.
height = 1;
315 for (
size_t i = 0; i < inliers.size (); ++i)
320 for (
size_t i = 0; i < inliers.size (); ++i)
322 Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
324 float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
326 Eigen::Vector4f pp = line_pt + k * line_dir;
328 projected_points.
points[i].x = pp[0];
329 projected_points.
points[i].y = pp[1];
330 projected_points.
points[i].z = pp[2];
336 template <
typename Po
intT>
bool
338 const std::set<int> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
341 if (!isModelValid (model_coefficients))
345 Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
346 Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0);
348 line_dir.normalize ();
350 float sqr_threshold =
static_cast<float> (threshold * threshold);
352 for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
356 if ((line_pt - input_->points[*it].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold)
363 #define PCL_INSTANTIATE_SampleConsensusModelStick(T) template class PCL_EXPORTS pcl::SampleConsensusModelStick<T>;
365 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_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).
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
Recompute the stick coefficients using the given inlier set and return them to the user...
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients)
Check whether the given index samples can form a valid stick model, compute the model coefficients fr...
bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
Verify whether a subset of indices verifies the given stick model coefficients.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)
Compute all squared distances from the cloud data to a given stick model.
bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
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.
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...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)
Count all the points which respect the given model coefficients as inliers.
void projectPoints(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true)
Create a new point cloud with inliers projected onto the stick model.
Helper functor structure for concatenate.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
pcl::PCLHeader header
The point cloud header.