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); }