template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCone<PointT, PointNT>::optimizeModelCoefficients (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
{
optimized_coefficients = model_coefficients;
// Needs a set of valid model coefficients
if (model_coefficients.size () != 7)
{
PCL_ERROR ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
return;
}
if (inliers.empty ())
{
PCL_DEBUG ("[pcl::SampleConsensusModelCone:optimizeModelCoefficients] Inliers vector empty! Returning the same coefficients.\n");
return;
}
tmp_inliers_ = &inliers;
OptimizationFunctor functor (static_cast<int> (inliers.size ()), this);
Eigen::NumericalDiff<OptimizationFunctor > num_diff (functor);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff);
int info = lm.minimize (optimized_coefficients);
// Compute the L2 norm of the residuals
PCL_DEBUG ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n",
info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]);
Eigen::Vector3f line_dir (optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5]);
line_dir.normalize ();
optimized_coefficients[3] = line_dir[0];
optimized_coefficients[4] = line_dir[1];
optimized_coefficients[5] = line_dir[2];
}
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
return;
double sqr_threshold = threshold * threshold;
int nr_p = 0;
inliers.resize (indices_->size ());
error_sqr_dists_.resize (indices_->size ());
// Obtain the line point and direction
Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
line_dir.normalize ();
// Iterate through the 3d points and calculate the distances from them to the line
for (size_t i = 0; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the line
// D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
if (sqr_distance < sqr_threshold)
{
// Returns the indices of the points whose squared distances are smaller than the threshold
inliers[nr_p] = (*indices_)[i];
error_sqr_dists_[nr_p] = sqr_distance;
++nr_p;
}
}
inliers.resize (nr_p);
error_sqr_dists_.resize (nr_p);
}
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::getDistancesToModel (
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
return;
distances.resize (indices_->size ());
// Obtain the line point and direction
Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
line_dir.normalize ();
// Iterate through the 3d points and calculate the distances from them to the line
for (size_t i = 0; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the line
// D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
// Need to estimate sqrt here to keep MSAC and friends general
distances[i] = sqrt ((line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ());
}
}
template <typename PointT> void
pcl::SampleConsensusModelStick<PointT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
{
// Needs a valid model coefficients
if (!isModelValid (model_coefficients))
return;
// Obtain the line point and direction
Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
// Copy all the data fields from the input cloud to the projected one?
if (copy_data_fields)
{
// Allocate enough space and copy the basics
projected_points.points.resize (input_->points.size ());
projected_points.width = input_->width;
projected_points.height = input_->height;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < projected_points.points.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the line
for (size_t i = 0; i < inliers.size (); ++i)
{
Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
// double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
Eigen::Vector4f pp = line_pt + k * line_dir;
// Calculate the projection of the point on the line (pointProj = A + k * B)
projected_points.points[inliers[i]].x = pp[0];
projected_points.points[inliers[i]].y = pp[1];
projected_points.points[inliers[i]].z = pp[2];
}
}
else
{
// Allocate enough space and copy the basics
projected_points.points.resize (inliers.size ());
projected_points.width = static_cast<uint32_t> (inliers.size ());
projected_points.height = 1;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < inliers.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the line
for (size_t i = 0; i < inliers.size (); ++i)
{
Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
// double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);
Eigen::Vector4f pp = line_pt + k * line_dir;
// Calculate the projection of the point on the line (pointProj = A + k * B)
projected_points.points[i].x = pp[0];
projected_points.points[i].y = pp[1];
projected_points.points[i].z = pp[2];
}
}
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 7)
{
PCL_ERROR ("[pcl::SampleConsensusModelCylinder::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
return;
}
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float ptdotdir = line_pt.dot (line_dir);
float dirdotdir = 1.0f / line_dir.dot (line_dir);
// Copy all the data fields from the input cloud to the projected one?
if (copy_data_fields)
{
// Allocate enough space and copy the basics
projected_points.points.resize (input_->points.size ());
projected_points.width = input_->width;
projected_points.height = input_->height;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < projected_points.points.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the cylinder
for (size_t i = 0; i < inliers.size (); ++i)
{
Eigen::Vector4f p (input_->points[inliers[i]].x,
input_->points[inliers[i]].y,
input_->points[inliers[i]].z,
1);
float k = (p.dot (line_dir) - ptdotdir) * dirdotdir;
pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
pp = line_pt + k * line_dir;
Eigen::Vector4f dir = p - pp;
dir.normalize ();
// Calculate the projection of the point onto the cylinder
pp += dir * model_coefficients[6];
}
}
else
{
// Allocate enough space and copy the basics
projected_points.points.resize (inliers.size ());
projected_points.width = static_cast<uint32_t> (inliers.size ());
projected_points.height = 1;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < inliers.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
pcl::Vector4fMapConst p = input_->points[inliers[i]].getVector4fMap ();
float k = (p.dot (line_dir) - ptdotdir) * dirdotdir;
// Calculate the projection of the point on the line
pp = line_pt + k * line_dir;
Eigen::Vector4f dir = p - pp;
dir.normalize ();
// Calculate the projection of the point onto the cylinder
pp += dir * model_coefficients[6];
}
}
}
