template <typename PointT> bool
pcl::SampleConsensusModelStick<PointT>::doSamplesVerifyModel (
const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
return (false);
// 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[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0);
//Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
line_dir.normalize ();
float sqr_threshold = static_cast<float> (threshold * threshold);
// Iterate through the 3d points and calculate the distances from them to the line
for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
{
// 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)
if ((line_pt - input_->points[*it].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold)
return (false);
}
return (true);
}
template <typename PointT> int
pcl::SampleConsensusModelLine<PointT>::countWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold)
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
return (0);
double sqr_threshold = threshold * threshold;
int nr_p = 0;
// 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)
nr_p++;
}
return (nr_p);
}
template <typename PointT> void
pcl::SampleConsensusModelStick<PointT>::getDistancesToModel (
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
return;
float sqr_threshold = static_cast<float> (radius_max_ * radius_max_);
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)
float sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();
if (sqr_distance < sqr_threshold)
// Need to estimate sqrt here to keep MSAC and friends general
distances[i] = sqrt (sqr_distance);
else
// Penalize outliers by doubling the distance
distances[i] = 2 * sqrt (sqr_distance);
}
}
template <typename PointT> void
pcl::SampleConsensusModelCircle2D<PointT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold,
std::vector<int> &inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
inliers.clear ();
return;
}
int nr_p = 0;
inliers.resize (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the sphere
for (size_t i = 0; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the sphere as the difference between
// dist(point,sphere_origin) and sphere_radius
float distance = fabsf (sqrtf (
( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
( input_->points[(*indices_)[i]].y - model_coefficients[1] )
) - model_coefficients[2]);
if (distance < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
inliers[nr_p] = (*indices_)[i];
nr_p++;
}
}
inliers.resize (nr_p);
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
if (!normals_)
{
PCL_ERROR ("[pcl::SampleConsensusModelNormalSphere::selectWithinDistance] No input dataset containing normals was given!\n");
inliers.clear ();
return;
}
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
inliers.clear ();
return;
}
// Obtain the sphere center
Eigen::Vector4f center = model_coefficients;
center[3] = 0;
int nr_p = 0;
inliers.resize (indices_->size ());
error_sqr_dists_.resize (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the sphere center as the difference between
// dist(point,sphere_origin) and sphere_radius
Eigen::Vector4f p (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z,
0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0],
normals_->points[(*indices_)[i]].normal[1],
normals_->points[(*indices_)[i]].normal[2],
0);
Eigen::Vector4f n_dir = p - center;
double d_euclid = fabs (n_dir.norm () - model_coefficients[3]);
// Calculate the angular distance between the point normal and the plane normal
double d_normal = fabs (getAngle3D (n, n_dir));
d_normal = (std::min) (d_normal, M_PI - d_normal);
double distance = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
if (distance < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
inliers[nr_p] = (*indices_)[i];
error_sqr_dists_[nr_p] = static_cast<double> (distance);
++nr_p;
}
}
inliers.resize (nr_p);
error_sqr_dists_.resize (nr_p);
}
template <typename PointT> void
pcl::SampleConsensusModelSphere<PointT>::getDistancesToModel (
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
distances.clear ();
return;
}
distances.resize (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the sphere
for (size_t i = 0; i < indices_->size (); ++i)
// Calculate the distance from the point to the sphere as the difference between
//dist(point,sphere_origin) and sphere_radius
distances[i] = fabs (sqrtf (
( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
( input_->points[(*indices_)[i]].y - model_coefficients[1] ) +
( input_->points[(*indices_)[i]].z - model_coefficients[2] ) *
( input_->points[(*indices_)[i]].z - model_coefficients[2] )
) - model_coefficients[3]);
}
template <typename PointT> int
pcl::SampleConsensusModelCircle2D<PointT>::countWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
return (0);
int nr_p = 0;
// Iterate through the 3d points and calculate the distances from them to the sphere
for (size_t i = 0; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the sphere as the difference between
// dist(point,sphere_origin) and sphere_radius
float distance = fabsf (sqrtf (
( input_->points[(*indices_)[i]].x - model_coefficients[0] ) *
( input_->points[(*indices_)[i]].x - model_coefficients[0] ) +
( input_->points[(*indices_)[i]].y - model_coefficients[1] ) *
( input_->points[(*indices_)[i]].y - model_coefficients[1] )
) - model_coefficients[2]);
if (distance < threshold)
nr_p++;
}
return (nr_p);
}
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::optimizeModelCoefficients (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
{
if (indices_->size () != indices_tgt_->size ())
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
optimized_coefficients = model_coefficients;
return;
}
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients) || !target_)
{
optimized_coefficients = model_coefficients;
return;
}
std::vector<int> indices_src (inliers.size ());
std::vector<int> indices_tgt (inliers.size ());
for (size_t i = 0; i < inliers.size (); ++i)
{
// NOTE: not tested!
indices_src[i] = (*indices_)[inliers[i]];
indices_tgt[i] = (*indices_tgt_)[inliers[i]];
}
estimateRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients);
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCone<PointT, PointNT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
inliers.clear ();
return;
}
int nr_p = 0;
inliers.resize (indices_->size ());
Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float opening_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
// Iterate through the 3d points and calculate the distances from them to the cone
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
// Calculate the point's projection on the cone axis
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
Eigen::Vector4f pt_proj = apex + k * axis_dir;
// Calculate the direction of the point from center
Eigen::Vector4f pp_pt_dir = pt - pt_proj;
pp_pt_dir.normalize ();
// Calculate the actual radius of the cone at the level of the projected point
Eigen::Vector4f height = apex - pt_proj;
double actual_cone_radius = tan(opening_angle) * height.norm ();
height.normalize ();
// Calculate the cones perfect normals
Eigen::Vector4f cone_normal = sinf (opening_angle) * height + cosf (opening_angle) * pp_pt_dir;
// Aproximate the distance from the point to the cone as the difference between
// dist(point,cone_axis) and actual cone radius
double d_euclid = fabs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
// Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
double d_normal = fabs (getAngle3D (n, cone_normal));
d_normal = (std::min) (d_normal, M_PI - d_normal);
if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
inliers[nr_p] = (*indices_)[i];
nr_p++;
}
}
inliers.resize (nr_p);
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
if (!normals_)
{
PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::selectWithinDistance] No input dataset containing normals was given!\n");
inliers.clear ();
return;
}
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
inliers.clear ();
return;
}
// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;
coeff[3] = 0;
int nr_p = 0;
inliers.resize (indices_->size ());
error_sqr_dists_.resize (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < indices_->size (); ++i)
{
const PointT &pt = input_->points[(*indices_)[i]];
const PointNT &nt = normals_->points[(*indices_)[i]];
// Calculate the distance from the point to the plane normal as the dot product
// D = (P-A).N/|N|
Eigen::Vector4f p (pt.x, pt.y, pt.z, 0);
Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0);
double d_euclid = fabs (coeff.dot (p) + model_coefficients[3]);
// Calculate the angular distance between the point normal and the plane normal
double d_normal = fabs (getAngle3D (n, coeff));
d_normal = (std::min) (d_normal, M_PI - d_normal);
// Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence
double weight = normal_distance_weight_ * (1.0 - nt.curvature);
double distance = fabs (weight * d_normal + (1.0 - weight) * d_euclid);
if (distance < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
inliers[nr_p] = (*indices_)[i];
error_sqr_dists_[nr_p] = distance;
++nr_p;
}
}
inliers.resize (nr_p);
error_sqr_dists_.resize (nr_p);
}
template <typename PointT> int
pcl::SampleConsensusModelParallelPlane<PointT>::countWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
return (0);
return (SampleConsensusModelPlane<PointT>::countWithinDistance (model_coefficients, threshold));
}
void SAC_Parallel_Plane::selectWithinDistance(arma::vec& coeff,double threshold,arma::uvec& inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (coeff))
{
inliers.reset();
return;
}
SAC_Plane::selectWithinDistance(coeff,threshold,inliers);
}
template <typename PointT> void
pcl::SampleConsensusModelParallelPlane<PointT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
inliers.clear ();
return;
}
SampleConsensusModelPlane<PointT>::selectWithinDistance (model_coefficients, threshold, inliers);
}
template <typename PointT> void
pcl::SampleConsensusModelParallelPlane<PointT>::getDistancesToModel (
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
distances.clear ();
return;
}
SampleConsensusModelPlane<PointT>::getDistancesToModel (model_coefficients, distances);
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
inliers.clear ();
return;
}
int nr_p = 0;
inliers.resize (indices_->size ());
error_sqr_dists_.resize (indices_->size ());
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);
// Iterate through the 3d points and calculate the distances from them to the sphere
for (size_t i = 0; i < indices_->size (); ++i)
{
// Approximate the distance from the point to the cylinder as the difference between
// dist(point,cylinder_axis) and cylinder radius
Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
// Calculate the point's projection on the cylinder axis
float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
Eigen::Vector4f pt_proj = line_pt + k * line_dir;
Eigen::Vector4f dir = pt - pt_proj;
dir.normalize ();
// Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
double d_normal = fabs (getAngle3D (n, dir));
d_normal = (std::min) (d_normal, M_PI - d_normal);
double distance = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
if (distance < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
inliers[nr_p] = (*indices_)[i];
error_sqr_dists_[nr_p] = distance;
++nr_p;
}
}
inliers.resize (nr_p);
error_sqr_dists_.resize (nr_p);
}
void TokenNameFinderModel::validateArtifactMap() throw(InvalidFormatException)
{
BaseModel::validateArtifactMap();
if (dynamic_cast<AbstractModel*>(artifactMap->get(MAXENT_MODEL_ENTRY_NAME)) != 0)
{
AbstractModel *model = static_cast<AbstractModel*>(artifactMap->get(MAXENT_MODEL_ENTRY_NAME));
isModelValid(model);
}
else
{
throw InvalidFormatException("Token Name Finder model is incomplete!");
}
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::getDistancesToModel (
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
if (!normals_)
{
PCL_ERROR ("[pcl::SampleConsensusModelNormalSphere::getDistancesToModel] No input dataset containing normals was given!\n");
return;
}
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
distances.clear ();
return;
}
// Obtain the sphere centroid
Eigen::Vector4f center = model_coefficients;
center[3] = 0;
distances.resize (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the sphere as the difference between
// dist(point,sphere_origin) and sphere_radius
Eigen::Vector4f p (input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z,
0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0],
normals_->points[(*indices_)[i]].normal[1],
normals_->points[(*indices_)[i]].normal[2],
0);
Eigen::Vector4f n_dir = (p-center);
double d_euclid = fabs (n_dir.norm () - model_coefficients[3]);
//
// Calculate the angular distance between the point normal and the plane normal
double d_normal = fabs (getAngle3D (n, n_dir));
d_normal = (std::min) (d_normal, M_PI - d_normal);
distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
}
}
template <typename PointT> void
pcl::SampleConsensusModelCircle3D<PointT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold,
std::vector<int> &inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
inliers.clear ();
return;
}
int nr_p = 0;
inliers.resize (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the sphere
for (size_t i = 0; i < indices_->size (); ++i)
{
// what i have:
// P : Sample Point
Eigen::Vector3d P (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z);
// C : Circle Center
Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
// N : Circle (Plane) Normal
Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
// r : Radius
double r = model_coefficients[3];
Eigen::Vector3d helper_vectorPC = P - C;
// 1.1. get line parameter
double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
// Projected Point on plane
Eigen::Vector3d P_proj = P + lambda * N;
Eigen::Vector3d helper_vectorP_projC = P_proj - C;
// K : Point on Circle
Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
Eigen::Vector3d distanceVector = P - K;
if (distanceVector.norm () < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
inliers[nr_p] = (*indices_)[i];
nr_p++;
}
}
inliers.resize (nr_p);
}
template <typename PointT> void
pcl::SampleConsensusModelStick<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;
float sqr_threshold = static_cast<float> (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_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
Eigen::Vector4f line_dir = line_pt2 - line_pt1;
//Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
//Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0);
line_dir.normalize ();
//float norm = line_dir.squaredNorm ();
// 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)
Eigen::Vector4f dir = input_->points[(*indices_)[i]].getVector4fMap () - line_pt1;
//float u = dir.dot (line_dir);
// If the point falls outside of the segment, ignore it
//if (u < 0.0f || u > 1.0f)
// continue;
float sqr_distance = dir.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] = static_cast<double> (sqr_distance);
++nr_p;
}
}
inliers.resize (nr_p);
error_sqr_dists_.resize (nr_p);
}
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
if (indices_->size () != indices_tgt_->size ())
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
inliers.clear ();
return;
}
if (!target_)
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] No target dataset given!\n");
return;
}
double thresh = threshold * threshold;
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
inliers.clear ();
return;
}
inliers.resize (indices_->size ());
Eigen::Matrix4f transform;
transform.row (0) = model_coefficients.segment<4>(0);
transform.row (1) = model_coefficients.segment<4>(4);
transform.row (2) = model_coefficients.segment<4>(8);
transform.row (3) = model_coefficients.segment<4>(12);
int nr_p = 0;
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f pt_src = input_->points[(*indices_)[i]].getVector4fMap ();
pt_src[3] = 1;
Eigen::Vector4f pt_tgt = target_->points[(*indices_tgt_)[i]].getVector4fMap ();
pt_tgt[3] = 1;
Eigen::Vector4f p_tr (transform * pt_src);
// Calculate the distance from the transformed point to its correspondence
if ((p_tr - pt_tgt).squaredNorm () < thresh)
inliers[nr_p++] = (*indices_)[i];
}
inliers.resize (nr_p);
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
if (!normals_)
{
PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::selectWithinDistance] No input dataset containing normals was given!\n");
inliers.clear ();
return;
}
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
inliers.clear ();
return;
}
// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;
coeff[3] = 0;
int nr_p = 0;
inliers.resize (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < indices_->size (); ++i)
{
// Calculate the distance from the point to the plane normal as the dot product
// D = (P-A).N/|N|
Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
double d_euclid = fabs (coeff.dot (p) + model_coefficients[3]);
// Calculate the angular distance between the point normal and the plane normal
double d_normal = fabs (getAngle3D (n, coeff));
d_normal = (std::min) (d_normal, M_PI - d_normal);
if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
inliers[nr_p] = (*indices_)[i];
nr_p++;
}
}
inliers.resize (nr_p);
}
template <typename PointT> void
pcl::SampleConsensusModelCircle3D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
distances.clear ();
return;
}
distances.resize (indices_->size ());
// Iterate through the 3d points and calculate the distances from them to the sphere
for (size_t i = 0; i < indices_->size (); ++i)
// Calculate the distance from the point to the circle:
// 1. calculate intersection point of the plane in which the circle lies and the
// line from the sample point with the direction of the plane normal (projected point)
// 2. calculate the intersection point of the line from the circle center to the projected point
// with the circle
// 3. calculate distance from corresponding point on the circle to the sample point
{
// what i have:
// P : Sample Point
Eigen::Vector3d P (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z);
// C : Circle Center
Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
// N : Circle (Plane) Normal
Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
// r : Radius
double r = model_coefficients[3];
Eigen::Vector3d helper_vectorPC = P - C;
// 1.1. get line parameter
double lambda = (helper_vectorPC.dot (N)) / N.squaredNorm ();
// Projected Point on plane
Eigen::Vector3d P_proj = P + lambda * N;
Eigen::Vector3d helper_vectorP_projC = P_proj - C;
// K : Point on Circle
Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
Eigen::Vector3d distanceVector = P - K;
distances[i] = distanceVector.norm ();
}
}
template <typename PointT> int
pcl::SampleConsensusModelStick<PointT>::countWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
return (0);
float sqr_threshold = static_cast<float> (threshold * threshold);
int nr_i = 0, nr_o = 0;
// Obtain the line point and direction
Eigen::Vector4f line_pt1 (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f line_pt2 (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
Eigen::Vector4f line_dir = line_pt2 - line_pt1;
line_dir.normalize ();
//Eigen::Vector4f line_dir (model_coefficients[3] - model_coefficients[0], model_coefficients[4] - model_coefficients[1], model_coefficients[5] - model_coefficients[2], 0);
//Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
// 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)
Eigen::Vector4f dir = input_->points[(*indices_)[i]].getVector4fMap () - line_pt1;
//float u = dir.dot (line_dir);
// If the point falls outside of the segment, ignore it
//if (u < 0.0f || u > 1.0f)
// continue;
float sqr_distance = dir.cross3 (line_dir).squaredNorm ();
// Use a larger threshold (4 times the radius) to get more points in
if (sqr_distance < sqr_threshold)
nr_i++;
else if (sqr_distance < 4 * sqr_threshold)
nr_o++;
}
return (nr_i - nr_o < 0 ? 0 : nr_i - nr_o);
}
template <typename PointT> void
pcl::SampleConsensusModelStick<PointT>::optimizeModelCoefficients (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
{
optimized_coefficients = model_coefficients;
return;
}
// Need at least 2 points to estimate a line
if (inliers.size () <= 2)
{
PCL_ERROR ("[pcl::SampleConsensusModelStick::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ());
optimized_coefficients = model_coefficients;
return;
}
optimized_coefficients.resize (7);
// Compute the 3x3 covariance matrix
Eigen::Vector4f centroid;
Eigen::Matrix3f covariance_matrix;
computeMeanAndCovarianceMatrix (*input_, inliers, covariance_matrix, centroid);
optimized_coefficients[0] = centroid[0];
optimized_coefficients[1] = centroid[1];
optimized_coefficients[2] = centroid[2];
// Extract the eigenvalues and eigenvectors
Eigen::Vector3f eigen_values;
Eigen::Vector3f eigen_vector;
pcl::eigen33 (covariance_matrix, eigen_values);
pcl::computeCorrespondingEigenVector (covariance_matrix, eigen_values [2], eigen_vector);
//optimized_coefficients.template segment<3> (3) = eigen_vector;
optimized_coefficients[0] = eigen_vector[0];
optimized_coefficients[1] = eigen_vector[1];
optimized_coefficients[2] = eigen_vector[2];
}
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
if (indices_->size () != indices_tgt_->size ())
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistancesToModel] Number of source indices (%zu) differs than number of target indices (%zu)!\n", indices_->size (), indices_tgt_->size ());
distances.clear ();
return;
}
if (!target_)
{
PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistanceToModel] No target dataset given!\n");
return;
}
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
distances.clear ();
return;
}
distances.resize (indices_->size ());
// Get the 4x4 transformation
Eigen::Matrix4f transform;
transform.row (0) = model_coefficients.segment<4>(0);
transform.row (1) = model_coefficients.segment<4>(4);
transform.row (2) = model_coefficients.segment<4>(8);
transform.row (3) = model_coefficients.segment<4>(12);
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f pt_src = input_->points[(*indices_)[i]].getVector4fMap ();
pt_src[3] = 1;
Eigen::Vector4f pt_tgt = target_->points[(*indices_tgt_)[i]].getVector4fMap ();
pt_tgt[3] = 1;
Eigen::Vector4f p_tr (transform * pt_src);
// Calculate the distance from the transformed point to its correspondence
// need to compute the real norm here to keep MSAC and friends general
distances[i] = (p_tr - pt_tgt).norm ();
}
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::getDistancesToModel (
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
distances.clear ();
return;
}
distances.resize (indices_->size ());
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);
// Iterate through the 3d points and calculate the distances from them to the sphere
for (size_t i = 0; i < indices_->size (); ++i)
{
// Aproximate the distance from the point to the cylinder as the difference between
// dist(point,cylinder_axis) and cylinder radius
// @note need to revise this.
Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
// Calculate the point's projection on the cylinder axis
float k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
Eigen::Vector4f pt_proj = line_pt + k * line_dir;
Eigen::Vector4f dir = pt - pt_proj;
dir.normalize ();
// Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
double d_normal = fabs (getAngle3D (n, dir));
d_normal = (std::min) (d_normal, M_PI - d_normal);
distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
}
}
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::optimizeModelCoefficients (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
{
optimized_coefficients = model_coefficients;
return;
}
// Need at least 2 points to estimate a line
if (inliers.size () <= 2)
{
PCL_ERROR ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", (unsigned long)inliers.size ());
optimized_coefficients = model_coefficients;
return;
}
optimized_coefficients.resize (6);
// Compute the 3x3 covariance matrix
Eigen::Vector4f centroid;
compute3DCentroid (*input_, inliers, centroid);
Eigen::Matrix3f covariance_matrix;
computeCovarianceMatrix (*input_, inliers, centroid, covariance_matrix);
optimized_coefficients[0] = centroid[0];
optimized_coefficients[1] = centroid[1];
optimized_coefficients[2] = centroid[2];
// Extract the eigenvalues and eigenvectors
EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
EIGEN_ALIGN16 Eigen::Matrix3f eigen_vectors;
pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);
optimized_coefficients.template tail<3> () = eigen_vectors.col (2).normalized ();
}
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::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];
}
}
}
