template <typename PointT> bool
pcl::SampleConsensusModelParallelLine<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
// Needs a valid model coefficients
if (model_coefficients.size () != 6)
{
PCL_ERROR ("[pcl::SampleConsensusParallelLine::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
return (false);
}
// Check against template, if given
if (eps_angle_ > 0.0)
{
// Obtain the line direction
Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
double angle_diff = fabs (getAngle3D (axis, line_dir));
//angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
angle_diff = fabs (angle_diff - (M_PI/2.0));
// Check whether the current plane model satisfies our angle threshold criterion with respect to the given axis
if (angle_diff > eps_angle_)
return (false);
}
return (true);
}
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelNormalParallelPlane<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
// Needs a valid model coefficients
if (model_coefficients.size () != 4)
{
PCL_ERROR ("[pcl::SampleConsensusModelNormalParallelPlane::isModelValid] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
return (false);
}
// Check against template, if given
if (eps_angle_ > 0.0)
{
// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;
coeff[3] = 0;
Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
double angle_deviation_from_90 = fabs (getAngle3D (axis, coeff));
angle_deviation_from_90 = fabs (angle_deviation_from_90 - (M_PI/2.0));
// Check whether the current plane model satisfies our angle threshold criterion with respect to the given axis
if (angle_deviation_from_90 > eps_angle_)
return (false);
}
if (eps_dist_ > 0.0)
{
if (fabs (-model_coefficients[3] - distance_from_origin_) > eps_dist_)
return (false);
}
return (true);
}
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCylinder<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
{
if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
// Check against template, if given
if (eps_angle_ > 0.0)
{
// Obtain the cylinder direction
Eigen::Vector4f coeff;
coeff[0] = model_coefficients[3];
coeff[1] = model_coefficients[4];
coeff[2] = model_coefficients[5];
coeff[3] = 0;
Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
double angle_diff = fabs (getAngle3D (axis, coeff));
angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
// Check whether the current cylinder model satisfies our angle threshold criterion with respect to the given axis
if (angle_diff > eps_angle_)
return (false);
}
if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[6] < radius_min_)
return (false);
if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[6] > radius_max_)
return (false);
return (true);
}
template <typename PointT> bool
pcl::SampleConsensusModelPerpendicularPlane<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
// Needs a valid model coefficients
if (model_coefficients.size () != 4)
{
PCL_ERROR ("[pcl::SampleConsensusModelPerpendicularPlane::isModelValid] Invalid number of model coefficients given (%lu)!\n", (unsigned long)model_coefficients.size ());
return (false);
}
// Check against template, if given
if (eps_angle_ > 0.0)
{
// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;
coeff[3] = 0;
Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
double angle_diff = fabs (getAngle3D (axis, coeff));
angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
// Check whether the current plane model satisfies our angle threshold criterion with respect to the given axis
if (angle_diff > eps_angle_)
return (false);
}
return (true);
}
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
// Needs a valid model coefficients
if (model_coefficients.size () != 7)
{
PCL_ERROR ("[pcl::SampleConsensusModelCone::isModelValid] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
return (false);
}
// Check against template, if given
if (eps_angle_ > 0.0)
{
// Obtain the cone direction
Eigen::Vector4f coeff;
coeff[0] = model_coefficients[3];
coeff[1] = model_coefficients[4];
coeff[2] = model_coefficients[5];
coeff[3] = 0;
Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
double angle_diff = fabs (getAngle3D (axis, coeff));
angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
// Check whether the current cone model satisfies our angle threshold criterion with respect to the given axis
if (angle_diff > eps_angle_)
return (false);
}
if (model_coefficients[6] != -std::numeric_limits<double>::max() && model_coefficients[6] < min_angle_)
return (false);
if (model_coefficients[6] != std::numeric_limits<double>::max() && model_coefficients[6] > max_angle_)
return (false);
return (true);
}
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, 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, 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);
}
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, 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);
}
void ObjectModelCylinder::selectWithinDistance (const Eigen::VectorXd &model_coefficients, double threshold,
std::vector<int> &inliers){
assert (model_coefficients.size () == 7);
int nr_p = 0;
inliers.resize (this->inputPointCloud->getSize());
Eigen::Vector4d line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4d line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
double ptdotdir = line_pt.dot (line_dir);
double dirdotdir = 1.0 / 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 < this->inputPointCloud->getSize(); ++i)
{
// Aproximate the distance from the point to the cylinder as the difference between
// dist(point,cylinder_axis) and cylinder radius
Eigen::Vector4d pt = Eigen::Vector4d ((*inputPointCloud->getPointCloud())[i].getX(),
(*inputPointCloud->getPointCloud())[i].getY(),
(*inputPointCloud->getPointCloud())[i].getZ(), 0);
Eigen::Vector4d n = Eigen::Vector4d (this->normals->getNormals()->data()[i].getX(),
this->normals->getNormals()->data()[i].getY(),
this->normals->getNormals()->data()[i].getZ(), 0);
double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
// Calculate the point's projection on the cylinder axis
double k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
Eigen::Vector4d pt_proj = line_pt + k * line_dir;
Eigen::Vector4d 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 = fmin (d_normal, M_PI - d_normal);
if (fabs (this->normalDistanceWeight * d_normal + (1 - this->normalDistanceWeight) * d_euclid) < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
inliers[nr_p] = i;
nr_p++;
}
}
inliers.resize (nr_p);
}
void ObjectModelCylinder::getDistancesToModel (const Eigen::VectorXd &model_coefficients, std::vector<double> &distances){
assert (model_coefficients.size () == 7);
distances.resize (this->inputPointCloud->getSize());
Eigen::Vector4d line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4d line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
double ptdotdir = line_pt.dot (line_dir);
double dirdotdir = 1.0 / 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 < this->inputPointCloud->getSize(); ++i)
{
// Aproximate the distance from the point to the cylinder as the difference between
// dist(point,cylinder_axis) and cylinder radius
// Todo to be revised
Eigen::Vector4d pt = Eigen::Vector4d ((*inputPointCloud->getPointCloud())[i].getX(),
(*inputPointCloud->getPointCloud())[i].getY(), (*inputPointCloud->getPointCloud())[i].getZ(), 0);
Eigen::Vector4d n = Eigen::Vector4d (this->normals->getNormals()->data()[i].getX(),
this->normals->getNormals()->data()[i].getY(),
this->normals->getNormals()->data()[i].getZ(), 0);
double d_euclid = fabs (pointToLineDistance (pt, model_coefficients) - model_coefficients[6]);
// Calculate the point's projection on the cylinder axis
double k = (pt.dot (line_dir) - ptdotdir) * dirdotdir;
Eigen::Vector4d pt_proj = line_pt + k * line_dir;
Eigen::Vector4d 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 = fmin (d_normal, M_PI - d_normal);
distances[i] = fabs (this->normalDistanceWeight * d_normal + (1 - this->normalDistanceWeight)
* d_euclid);
}
}
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> bool
pcl::SampleConsensusModelParallelLine<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
{
if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
// Check against template, if given
if (eps_angle_ > 0.0)
{
// Obtain the line direction
Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
double angle_diff = fabs (getAngle3D (axis, line_dir));
angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
// Check whether the current line model satisfies our angle threshold criterion with respect to the given axis
if (angle_diff > eps_angle_)
return (false);
}
return (true);
}
template <typename PointT> bool
pcl::SampleConsensusModelPerpendicularPlane<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
{
if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
// Check against template, if given
if (eps_angle_ > 0.0)
{
// Obtain the plane normal
Eigen::Vector4f coeff = model_coefficients;
coeff[3] = 0;
Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0);
double angle_diff = fabs (getAngle3D (axis, coeff));
angle_diff = (std::min) (angle_diff, M_PI - angle_diff);
// Check whether the current plane model satisfies our angle threshold criterion with respect to the given axis
if (angle_diff > eps_angle_)
return (false);
}
return (true);
}
template <typename PointT> bool
pcl::LCCPSegmentation<PointT>::connIsConvex (const uint32_t source_label_arg,
const uint32_t target_label_arg,
float &normal_angle)
{
typename pcl::Supervoxel<PointT>::Ptr& sv_source = sv_label_to_supervoxel_map_[source_label_arg];
typename pcl::Supervoxel<PointT>::Ptr& sv_target = sv_label_to_supervoxel_map_[target_label_arg];
const Eigen::Vector3f& source_centroid = sv_source->centroid_.getVector3fMap ();
const Eigen::Vector3f& target_centroid = sv_target->centroid_.getVector3fMap ();
const Eigen::Vector3f& source_normal = sv_source->normal_.getNormalVector3fMap (). normalized ();
const Eigen::Vector3f& target_normal = sv_target->normal_.getNormalVector3fMap (). normalized ();
//NOTE For angles below 0 nothing will be merged
if (concavity_tolerance_threshold_ < 0)
{
return (false);
}
bool is_convex = true;
bool is_smooth = true;
normal_angle = getAngle3D (source_normal, target_normal, true);
// Geometric comparisons
Eigen::Vector3f vec_t_to_s, vec_s_to_t;
vec_t_to_s = source_centroid - target_centroid;
vec_s_to_t = -vec_t_to_s;
Eigen::Vector3f ncross;
ncross = source_normal.cross (target_normal);
// Smoothness Check: Check if there is a step between adjacent patches
if (use_smoothness_check_)
{
float expected_distance = ncross.norm () * seed_resolution_;
float dot_p_1 = vec_t_to_s.dot (source_normal);
float dot_p_2 = vec_s_to_t.dot (target_normal);
float point_dist = (std::fabs (dot_p_1) < std::fabs (dot_p_2)) ? std::fabs (dot_p_1) : std::fabs (dot_p_2);
const float dist_smoothing = smoothness_threshold_ * voxel_resolution_; // This is a slacking variable especially important for patches with very similar normals
if (point_dist > (expected_distance + dist_smoothing))
{
is_smooth &= false;
}
}
// ----------------
// Sanity Criterion: Check if definition convexity/concavity makes sense for connection of given patches
float intersection_angle = getAngle3D (ncross, vec_t_to_s, true);
float min_intersect_angle = (intersection_angle < 90.) ? intersection_angle : 180. - intersection_angle;
float intersect_thresh = 60. * 1. / (1. + exp (-0.25 * (normal_angle - 25.)));
if (min_intersect_angle < intersect_thresh && use_sanity_check_)
{
// std::cout << "Concave/Convex not defined for given case!" << std::endl;
is_convex &= false;
}
// vec_t_to_s is the reference direction for angle measurements
// Convexity Criterion: Check if connection of patches is convex. If this is the case the two supervoxels should be merged.
if ((getAngle3D (vec_t_to_s, source_normal) - getAngle3D (vec_t_to_s, target_normal)) <= 0)
{
is_convex &= true; // connection convex
}
else
{
is_convex &= (normal_angle < concavity_tolerance_threshold_); // concave connections will be accepted if difference of normals is small
}
return (is_convex && is_smooth);
}
