From 8d68a6d5e35e4817bbbfd3cb44a05342902de899 Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Sat, 15 Feb 2020 12:58:44 +0100 Subject: [PATCH] Add curly braces to make code more clear --- .../sample_consensus/impl/sac_model_line.hpp | 2 ++ .../impl/sac_model_normal_sphere.hpp | 6 +++++ .../impl/sac_model_parallel_line.hpp | 4 +++ .../impl/sac_model_parallel_plane.hpp | 6 +++++ .../impl/sac_model_perpendicular_plane.hpp | 6 +++++ .../sample_consensus/impl/sac_model_plane.hpp | 10 +++++++ .../impl/sac_model_registration.hpp | 6 +++++ .../impl/sac_model_registration_2d.hpp | 6 +++++ .../impl/sac_model_sphere.hpp | 10 +++++++ .../sample_consensus/impl/sac_model_stick.hpp | 26 +++++++++++++++++++ 10 files changed, 82 insertions(+) diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp index 30a15884373..95b1d845667 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp @@ -102,7 +102,9 @@ pcl::SampleConsensusModelLine::getDistancesToModel ( { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) + { return; + } distances.resize (indices_->size ()); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp index c33f06494d9..efad1932775 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp @@ -210,12 +210,18 @@ template bool pcl::SampleConsensusModelNormalSphere::isModelValid (const Eigen::VectorXf &model_coefficients) const { if (!SampleConsensusModel::isModelValid (model_coefficients)) + { return (false); + } if (radius_min_ != -std::numeric_limits::max() && model_coefficients[3] < radius_min_) + { return (false); + } if (radius_max_ != std::numeric_limits::max() && model_coefficients[3] > radius_max_) + { return (false); + } return (true); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp index 1ab1a430192..b810b3266a1 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp @@ -66,7 +66,9 @@ pcl::SampleConsensusModelParallelLine::countWithinDistance ( { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) + { return (0); + } return (SampleConsensusModelLine::countWithinDistance (model_coefficients, threshold)); } @@ -104,7 +106,9 @@ pcl::SampleConsensusModelParallelLine::isModelValid (const Eigen::Vector 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); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp index 3de6be6def6..388746a861c 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp @@ -65,7 +65,9 @@ pcl::SampleConsensusModelParallelPlane::countWithinDistance ( { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) + { return (0); + } return (SampleConsensusModelPlane::countWithinDistance (model_coefficients, threshold)); } @@ -90,7 +92,9 @@ template bool pcl::SampleConsensusModelParallelPlane::isModelValid (const Eigen::VectorXf &model_coefficients) const { if (!SampleConsensusModel::isModelValid (model_coefficients)) + { return (false); + } // Check against template, if given if (eps_angle_ > 0.0) @@ -102,7 +106,9 @@ pcl::SampleConsensusModelParallelPlane::isModelValid (const Eigen::Vecto Eigen::Vector4f axis (axis_[0], axis_[1], axis_[2], 0); if (std::abs (axis.dot (coeff)) > sin_angle_) + { return (false); + } } return (true); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp index e7bc7f0e98b..43e563e904b 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp @@ -65,7 +65,9 @@ pcl::SampleConsensusModelPerpendicularPlane::countWithinDistance ( { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) + { return (0); + } return (SampleConsensusModelPlane::countWithinDistance (model_coefficients, threshold)); } @@ -90,7 +92,9 @@ template bool pcl::SampleConsensusModelPerpendicularPlane::isModelValid (const Eigen::VectorXf &model_coefficients) const { if (!SampleConsensusModel::isModelValid (model_coefficients)) + { return (false); + } // Check against template, if given if (eps_angle_ > 0.0) @@ -104,7 +108,9 @@ pcl::SampleConsensusModelPerpendicularPlane::isModelValid (const Eigen:: 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); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp index 5b99792fba8..19f3be827f3 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp @@ -53,7 +53,9 @@ pcl::SampleConsensusModelPlane::isSampleGood (const std::vector &sa { // Need an extra check in case the sample selection is empty if (samples.empty ()) + { return (false); + } // Get the values at the two points pcl::Array4fMapConst p0 = input_->points[samples[0]].getArray4fMap (); pcl::Array4fMapConst p1 = input_->points[samples[1]].getArray4fMap (); @@ -88,7 +90,9 @@ pcl::SampleConsensusModelPlane::computeModelCoefficients ( // Avoid some crashes by checking for collinearity here Eigen::Array4f dy1dy2 = p1p0 / p2p0; if ( (dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1]) ) // Check for collinearity + { return (false); + } // Compute the plane coefficients from the 3 given points in a straightforward manner // calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1) @@ -202,7 +206,9 @@ pcl::SampleConsensusModelPlane::countWithinDistance ( input_->points[(*indices_)[i]].z, 1); if (std::abs (model_coefficients.dot (pt)) < threshold) + { nr_p++; + } } return (nr_p); } @@ -320,8 +326,10 @@ pcl::SampleConsensusModelPlane::projectPoints ( using FieldList = typename pcl::traits::fieldList::type; // Iterate over each point for (std::size_t i = 0; i < inliers.size (); ++i) + { // Iterate over each dimension pcl::for_each_type (NdConcatenateFunctor (input_->points[inliers[i]], projected_points.points[i])); + } // Iterate through the 3d points and calculate the distances from them to the plane for (std::size_t i = 0; i < inliers.size (); ++i) @@ -359,7 +367,9 @@ pcl::SampleConsensusModelPlane::doSamplesVerifyModel ( input_->points[index].z, 1); if (std::abs (model_coefficients.dot (pt)) > threshold) + { return (false); + } } return (true); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp index 5b9d1f7ddd2..401ea38c9ac 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp @@ -73,11 +73,15 @@ pcl::SampleConsensusModelRegistration::computeModelCoefficients (const s } // Need 3 samples if (samples.size () != 3) + { return (false); + } std::vector indices_tgt (3); for (int i = 0; i < 3; ++i) + { indices_tgt[i] = correspondences_.at (samples[i]); + } estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients); return (true); @@ -208,7 +212,9 @@ pcl::SampleConsensusModelRegistration::countWithinDistance ( // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) + { return (0); + } Eigen::Matrix4f transform; transform.row (0).matrix () = model_coefficients.segment<4>(0); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp index 0a12544a239..29d688c383e 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp @@ -98,7 +98,9 @@ pcl::SampleConsensusModelRegistration2D::getDistancesToModel (const Eige Eigen::Vector3f uv (projection_matrix_ * p_tr3); if (uv[2] < 0) + { continue; + } uv /= uv[2]; @@ -212,7 +214,9 @@ pcl::SampleConsensusModelRegistration2D::countWithinDistance ( Eigen::Vector3f uv (projection_matrix_ * p_tr3); if (uv[2] < 0) + { continue; + } uv /= uv[2]; @@ -221,7 +225,9 @@ pcl::SampleConsensusModelRegistration2D::countWithinDistance ( (uv[0] - target_->points[(*indices_tgt_)[i]].u) + (uv[1] - target_->points[(*indices_tgt_)[i]].v) * (uv[1] - target_->points[(*indices_tgt_)[i]].v)) < thresh) + { ++nr_p; + } } return (nr_p); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp index fb392efc92f..71a957ab1d0 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp @@ -73,12 +73,16 @@ pcl::SampleConsensusModelSphere::computeModelCoefficients ( } float m11 = temp.determinant (); if (m11 == 0) + { return (false); // the points don't define a sphere! + } for (int i = 0; i < 4; ++i) + { temp (i, 0) = (input_->points[samples[i]].x) * (input_->points[samples[i]].x) + (input_->points[samples[i]].y) * (input_->points[samples[i]].y) + (input_->points[samples[i]].z) * (input_->points[samples[i]].z); + } float m12 = temp.determinant (); for (int i = 0; i < 4; ++i) @@ -132,6 +136,7 @@ pcl::SampleConsensusModelSphere::getDistancesToModel ( // Iterate through the 3d points and calculate the distances from them to the sphere for (std::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] = std::abs (std::sqrt ( @@ -144,6 +149,7 @@ pcl::SampleConsensusModelSphere::getDistancesToModel ( ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) * ( input_->points[(*indices_)[i]].z - model_coefficients[2] ) ) - model_coefficients[3]); + } } ////////////////////////////////////////////////////////////////////////// @@ -287,6 +293,7 @@ pcl::SampleConsensusModelSphere::doSamplesVerifyModel ( } for (const int &index : indices) + { // Calculate the distance from the point to the sphere as the difference between //dist(point,sphere_origin) and sphere_radius if (std::abs (sqrt ( @@ -297,7 +304,10 @@ pcl::SampleConsensusModelSphere::doSamplesVerifyModel ( ( input_->points[index].z - model_coefficients[2] ) * ( input_->points[index].z - model_coefficients[2] ) ) - model_coefficients[3]) > threshold) + { return (false); + } + } return (true); } diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp index 8d030eafa85..f4a994d930e 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp @@ -55,7 +55,9 @@ pcl::SampleConsensusModelStick::isSampleGood (const std::vector &sa (input_->points[samples[0]].y != input_->points[samples[1]].y) && (input_->points[samples[0]].z != input_->points[samples[1]].z)) + { return (true); + } return (false); } @@ -98,7 +100,9 @@ pcl::SampleConsensusModelStick::getDistancesToModel ( { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) + { return; + } float sqr_threshold = static_cast (radius_max_ * radius_max_); distances.resize (indices_->size ()); @@ -116,11 +120,15 @@ pcl::SampleConsensusModelStick::getDistancesToModel ( 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); + } } } @@ -131,7 +139,9 @@ pcl::SampleConsensusModelStick::selectWithinDistance ( { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) + { return; + } float sqr_threshold = static_cast (threshold * threshold); @@ -180,7 +190,9 @@ pcl::SampleConsensusModelStick::countWithinDistance ( { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) + { return (0); + } float sqr_threshold = static_cast (threshold * threshold); @@ -210,9 +222,13 @@ pcl::SampleConsensusModelStick::countWithinDistance ( 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 : nr_i - nr_o); @@ -266,7 +282,9 @@ pcl::SampleConsensusModelStick::projectPoints ( { // 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); @@ -286,8 +304,10 @@ pcl::SampleConsensusModelStick::projectPoints ( using FieldList = typename pcl::traits::fieldList::type; // Iterate over each point for (std::size_t i = 0; i < projected_points.points.size (); ++i) + { // Iterate over each dimension pcl::for_each_type (NdConcatenateFunctor (input_->points[i], projected_points.points[i])); + } // Iterate through the 3d points and calculate the distances from them to the line for (const int &inlier : inliers) @@ -313,8 +333,10 @@ pcl::SampleConsensusModelStick::projectPoints ( using FieldList = typename pcl::traits::fieldList::type; // Iterate over each point for (std::size_t i = 0; i < inliers.size (); ++i) + { // Iterate over each dimension pcl::for_each_type (NdConcatenateFunctor (input_->points[inliers[i]], projected_points.points[i])); + } // Iterate through the 3d points and calculate the distances from them to the line for (std::size_t i = 0; i < inliers.size (); ++i) @@ -339,7 +361,9 @@ pcl::SampleConsensusModelStick::doSamplesVerifyModel ( { // 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); @@ -354,7 +378,9 @@ pcl::SampleConsensusModelStick::doSamplesVerifyModel ( // 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[index].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold) + { return (false); + } } return (true);