Skip to content

Commit

Permalink
Add curly braces to make code more clear
Browse files Browse the repository at this point in the history
  • Loading branch information
mvieth committed Feb 16, 2020
1 parent 0e452dd commit 8d68a6d
Show file tree
Hide file tree
Showing 10 changed files with 82 additions and 0 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,9 @@ pcl::SampleConsensusModelLine<PointT>::getDistancesToModel (
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
{
return;
}

distances.resize (indices_->size ());

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -210,12 +210,18 @@ template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
{
if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
{
return (false);
}

if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[3] < radius_min_)
{
return (false);
}
if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[3] > radius_max_)
{
return (false);
}

return (true);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,9 @@ pcl::SampleConsensusModelParallelLine<PointT>::countWithinDistance (
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
return (0);
}

return (SampleConsensusModelLine<PointT>::countWithinDistance (model_coefficients, threshold));
}
Expand Down Expand Up @@ -104,7 +106,9 @@ pcl::SampleConsensusModelParallelLine<PointT>::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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,9 @@ pcl::SampleConsensusModelParallelPlane<PointT>::countWithinDistance (
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
return (0);
}

return (SampleConsensusModelPlane<PointT>::countWithinDistance (model_coefficients, threshold));
}
Expand All @@ -90,7 +92,9 @@ template <typename PointT> bool
pcl::SampleConsensusModelParallelPlane<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)
Expand All @@ -102,7 +106,9 @@ pcl::SampleConsensusModelParallelPlane<PointT>::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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,9 @@ pcl::SampleConsensusModelPerpendicularPlane<PointT>::countWithinDistance (
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
return (0);
}

return (SampleConsensusModelPlane<PointT>::countWithinDistance (model_coefficients, threshold));
}
Expand All @@ -90,7 +92,9 @@ 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)
Expand All @@ -104,7 +108,9 @@ pcl::SampleConsensusModelPerpendicularPlane<PointT>::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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,9 @@ pcl::SampleConsensusModelPlane<PointT>::isSampleGood (const std::vector<int> &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 ();
Expand Down Expand Up @@ -88,7 +90,9 @@ pcl::SampleConsensusModelPlane<PointT>::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)
Expand Down Expand Up @@ -202,7 +206,9 @@ pcl::SampleConsensusModelPlane<PointT>::countWithinDistance (
input_->points[(*indices_)[i]].z,
1);
if (std::abs (model_coefficients.dot (pt)) < threshold)
{
nr_p++;
}
}
return (nr_p);
}
Expand Down Expand Up @@ -320,8 +326,10 @@ pcl::SampleConsensusModelPlane<PointT>::projectPoints (
using FieldList = typename pcl::traits::fieldList<PointT>::type;
// Iterate over each point
for (std::size_t i = 0; i < inliers.size (); ++i)
{
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
}

// Iterate through the 3d points and calculate the distances from them to the plane
for (std::size_t i = 0; i < inliers.size (); ++i)
Expand Down Expand Up @@ -359,7 +367,9 @@ pcl::SampleConsensusModelPlane<PointT>::doSamplesVerifyModel (
input_->points[index].z,
1);
if (std::abs (model_coefficients.dot (pt)) > threshold)
{
return (false);
}
}

return (true);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -73,11 +73,15 @@ pcl::SampleConsensusModelRegistration<PointT>::computeModelCoefficients (const s
}
// Need 3 samples
if (samples.size () != 3)
{
return (false);
}

std::vector<int> 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);
Expand Down Expand Up @@ -208,7 +212,9 @@ pcl::SampleConsensusModelRegistration<PointT>::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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,9 @@ pcl::SampleConsensusModelRegistration2D<PointT>::getDistancesToModel (const Eige
Eigen::Vector3f uv (projection_matrix_ * p_tr3);

if (uv[2] < 0)
{
continue;
}

uv /= uv[2];

Expand Down Expand Up @@ -212,7 +214,9 @@ pcl::SampleConsensusModelRegistration2D<PointT>::countWithinDistance (
Eigen::Vector3f uv (projection_matrix_ * p_tr3);

if (uv[2] < 0)
{
continue;
}

uv /= uv[2];

Expand All @@ -221,7 +225,9 @@ pcl::SampleConsensusModelRegistration2D<PointT>::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);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -73,12 +73,16 @@ pcl::SampleConsensusModelSphere<PointT>::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)
Expand Down Expand Up @@ -132,6 +136,7 @@ pcl::SampleConsensusModelSphere<PointT>::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 (
Expand All @@ -144,6 +149,7 @@ pcl::SampleConsensusModelSphere<PointT>::getDistancesToModel (
( input_->points[(*indices_)[i]].z - model_coefficients[2] ) *
( input_->points[(*indices_)[i]].z - model_coefficients[2] )
) - model_coefficients[3]);
}
}

//////////////////////////////////////////////////////////////////////////
Expand Down Expand Up @@ -287,6 +293,7 @@ pcl::SampleConsensusModelSphere<PointT>::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 (
Expand All @@ -297,7 +304,10 @@ pcl::SampleConsensusModelSphere<PointT>::doSamplesVerifyModel (
( input_->points[index].z - model_coefficients[2] ) *
( input_->points[index].z - model_coefficients[2] )
) - model_coefficients[3]) > threshold)
{
return (false);
}
}

return (true);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,9 @@ pcl::SampleConsensusModelStick<PointT>::isSampleGood (const std::vector<int> &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);
}
Expand Down Expand Up @@ -98,7 +100,9 @@ pcl::SampleConsensusModelStick<PointT>::getDistancesToModel (
{
// 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 ());
Expand All @@ -116,11 +120,15 @@ pcl::SampleConsensusModelStick<PointT>::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);
}
}
}

Expand All @@ -131,7 +139,9 @@ pcl::SampleConsensusModelStick<PointT>::selectWithinDistance (
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
{
return;
}

float sqr_threshold = static_cast<float> (threshold * threshold);

Expand Down Expand Up @@ -180,7 +190,9 @@ pcl::SampleConsensusModelStick<PointT>::countWithinDistance (
{
// Needs a valid set of model coefficients
if (!isModelValid (model_coefficients))
{
return (0);
}

float sqr_threshold = static_cast<float> (threshold * threshold);

Expand Down Expand Up @@ -210,9 +222,13 @@ pcl::SampleConsensusModelStick<PointT>::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);
Expand Down Expand Up @@ -266,7 +282,9 @@ pcl::SampleConsensusModelStick<PointT>::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);
Expand All @@ -286,8 +304,10 @@ pcl::SampleConsensusModelStick<PointT>::projectPoints (
using FieldList = typename pcl::traits::fieldList<PointT>::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 <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 (const int &inlier : inliers)
Expand All @@ -313,8 +333,10 @@ pcl::SampleConsensusModelStick<PointT>::projectPoints (
using FieldList = typename pcl::traits::fieldList<PointT>::type;
// Iterate over each point
for (std::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 (std::size_t i = 0; i < inliers.size (); ++i)
Expand All @@ -339,7 +361,9 @@ pcl::SampleConsensusModelStick<PointT>::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);
Expand All @@ -354,7 +378,9 @@ pcl::SampleConsensusModelStick<PointT>::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);
Expand Down

0 comments on commit 8d68a6d

Please sign in to comment.