Skip to content

Commit 3a4500a

Browse files
alexvorndranalexvorndran
committed
Unify sample validation of sample consensus models
The changes address the following models: - SampleConsensusModelLine - SampleConsensusModelCone - SampleConsensusModelCylinder - SampleConsensusModelPlane - SampleConsensusModelCircle2D - SampleConsensusModelCircle3D SampleConsensusModelStick is not adressed, since it's currently broken beyond the scope of this PR (see #2452).
1 parent 28e6312 commit 3a4500a

7 files changed

Lines changed: 99 additions & 70 deletions

File tree

‎sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp‎

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,9 @@ pcl::SampleConsensusModelCircle2D<PointT>::isSampleGood(const Indices &samples)
5454
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
5555
return (false);
5656
}
57-
// Get the values at the two points
57+
58+
// Double precision here follows computeModelCoefficients, which means we
59+
// can't use getVector3fMap-accessor to make our lives easier.
5860
Eigen::Array2d p0 ((*input_)[samples[0]].x, (*input_)[samples[0]].y);
5961
Eigen::Array2d p1 ((*input_)[samples[1]].x, (*input_)[samples[1]].y);
6062
Eigen::Array2d p2 ((*input_)[samples[2]].x, (*input_)[samples[2]].y);
@@ -64,19 +66,23 @@ pcl::SampleConsensusModelCircle2D<PointT>::isSampleGood(const Indices &samples)
6466
// Compute the segment values (in 2d) between p2 and p0
6567
p2 -= p0;
6668

67-
Eigen::Array2d dy1dy2 = p1 / p2;
69+
// Check if the triangle area spanned by the three sample points is greater than 0
70+
// https://en.wikipedia.org/wiki/Triangle#Using_vectors
71+
if (std::abs (p1.x()*p2.y() - p2.x()*p1.y()) < Eigen::NumTraits<double>::dummy_precision ()) {
72+
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::isSampleGood] Sample points too similar or collinear!\n");
73+
return (false);
74+
}
6875

69-
return (dy1dy2[0] != dy1dy2[1]);
76+
return (true);
7077
}
7178

7279
//////////////////////////////////////////////////////////////////////////
7380
template <typename PointT> bool
7481
pcl::SampleConsensusModelCircle2D<PointT>::computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const
7582
{
76-
// Need 3 samples
77-
if (samples.size () != sample_size_)
83+
if (!isSampleGood (samples))
7884
{
79-
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
85+
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given!\n");
8086
return (false);
8187
}
8288

‎sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp‎

Lines changed: 20 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -55,23 +55,29 @@ pcl::SampleConsensusModelCircle3D<PointT>::isSampleGood (
5555
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
5656
return (false);
5757
}
58-
// Get the values at the three points
58+
59+
// Double precision here follows computeModelCoefficients, which means we
60+
// can't use getVector3fMap-accessor to make our lives easier.
5961
Eigen::Vector3d p0 ((*input_)[samples[0]].x, (*input_)[samples[0]].y, (*input_)[samples[0]].z);
6062
Eigen::Vector3d p1 ((*input_)[samples[1]].x, (*input_)[samples[1]].y, (*input_)[samples[1]].z);
6163
Eigen::Vector3d p2 ((*input_)[samples[2]].x, (*input_)[samples[2]].y, (*input_)[samples[2]].z);
6264

63-
// calculate vectors between points
64-
p1 -= p0;
65-
p2 -= p0;
65+
// Check if the squared norm of the cross-product is non-zero, otherwise
66+
// common_helper_vec, which plays an important role in computeModelCoefficients,
67+
// would likely be ill-formed.
68+
if ((p1 - p0).cross(p1 - p2).squaredNorm() < Eigen::NumTraits<float>::dummy_precision ())
69+
{
70+
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::isSampleGood] Sample points too similar or collinear!\n");
71+
return (false);
72+
}
6673

67-
return (p1.dot (p2) < 0.000001);
74+
return (true);
6875
}
6976

7077
//////////////////////////////////////////////////////////////////////////
7178
template <typename PointT> bool
7279
pcl::SampleConsensusModelCircle3D<PointT>::computeModelCoefficients (const Indices &samples, Eigen::VectorXf &model_coefficients) const
7380
{
74-
// Need 3 samples
7581
if (samples.size () != sample_size_)
7682
{
7783
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
@@ -94,6 +100,14 @@ pcl::SampleConsensusModelCircle3D<PointT>::computeModelCoefficients (const Indic
94100

95101
Eigen::Vector3d common_helper_vec = helper_vec01.cross (helper_vec12);
96102

103+
// The same check is implemented in isSampleGood, so be sure to look there too
104+
// if you find the need to change something here.
105+
if (common_helper_vec.squaredNorm() < Eigen::NumTraits<float>::dummy_precision ())
106+
{
107+
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Sample points too similar or collinear!\n");
108+
return (false);
109+
}
110+
97111
double commonDividend = 2.0 * common_helper_vec.squaredNorm ();
98112

99113
double alpha = (helper_vec12.squaredNorm () * helper_vec01.dot (helper_vec02)) / commonDividend;

‎sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -61,10 +61,10 @@ template <typename PointT, typename PointNT> bool
6161
pcl::SampleConsensusModelCone<PointT, PointNT>::computeModelCoefficients (
6262
const Indices &samples, Eigen::VectorXf &model_coefficients) const
6363
{
64-
// Need 3 samples
65-
if (samples.size () != sample_size_)
64+
// Make sure that the samples are valid
65+
if (!isSampleGood (samples))
6666
{
67-
PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
67+
PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Invalid set of samples given\n");
6868
return (false);
6969
}
7070

‎sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp‎

Lines changed: 16 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,19 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::isSampleGood (const Indices
5555
PCL_ERROR ("[pcl::SampleConsensusModelCylinder::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
5656
return (false);
5757
}
58+
59+
// Make sure that the two sample points are not identical
60+
if (
61+
std::abs ((*input_)[samples[0]].x - (*input_)[samples[1]].x) <= std::numeric_limits<float>::epsilon ()
62+
&&
63+
std::abs ((*input_)[samples[0]].y - (*input_)[samples[1]].y) <= std::numeric_limits<float>::epsilon ()
64+
&&
65+
std::abs ((*input_)[samples[0]].z - (*input_)[samples[1]].z) <= std::numeric_limits<float>::epsilon ())
66+
{
67+
PCL_ERROR ("[pcl::SampleConsensusModelCylinder::isSampleGood] The two sample points are (almost) identical!\n");
68+
return (false);
69+
}
70+
5871
return (true);
5972
}
6073

@@ -63,10 +76,10 @@ template <typename PointT, typename PointNT> bool
6376
pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients (
6477
const Indices &samples, Eigen::VectorXf &model_coefficients) const
6578
{
66-
// Need 2 samples
67-
if (samples.size () != sample_size_)
79+
// Make sure that the samples are valid
80+
if (!isSampleGood (samples))
6881
{
69-
PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
82+
PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given!\n");
7083
return (false);
7184
}
7285

@@ -76,13 +89,6 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients (
7689
return (false);
7790
}
7891

79-
if (std::abs ((*input_)[samples[0]].x - (*input_)[samples[1]].x) <= std::numeric_limits<float>::epsilon () &&
80-
std::abs ((*input_)[samples[0]].y - (*input_)[samples[1]].y) <= std::numeric_limits<float>::epsilon () &&
81-
std::abs ((*input_)[samples[0]].z - (*input_)[samples[1]].z) <= std::numeric_limits<float>::epsilon ())
82-
{
83-
return (false);
84-
}
85-
8692
Eigen::Vector4f p1 ((*input_)[samples[0]].x, (*input_)[samples[0]].y, (*input_)[samples[0]].z, 0.0f);
8793
Eigen::Vector4f p2 ((*input_)[samples[1]].x, (*input_)[samples[1]].y, (*input_)[samples[1]].z, 0.0f);
8894

‎sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp‎

Lines changed: 15 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -55,36 +55,31 @@ pcl::SampleConsensusModelLine<PointT>::isSampleGood (const Indices &samples) con
5555
PCL_ERROR ("[pcl::SampleConsensusModelLine::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
5656
return (false);
5757
}
58+
5859
// Make sure that the two sample points are not identical
5960
if (
60-
((*input_)[samples[0]].x != (*input_)[samples[1]].x)
61-
||
62-
((*input_)[samples[0]].y != (*input_)[samples[1]].y)
63-
||
64-
((*input_)[samples[0]].z != (*input_)[samples[1]].z))
61+
std::abs ((*input_)[samples[0]].x - (*input_)[samples[1]].x) <= std::numeric_limits<float>::epsilon ()
62+
&&
63+
std::abs ((*input_)[samples[0]].y - (*input_)[samples[1]].y) <= std::numeric_limits<float>::epsilon ()
64+
&&
65+
std::abs ((*input_)[samples[0]].z - (*input_)[samples[1]].z) <= std::numeric_limits<float>::epsilon ())
6566
{
66-
return (true);
67+
PCL_ERROR ("[pcl::SampleConsensusModelLine::isSampleGood] The two sample points are (almost) identical!\n");
68+
return (false);
6769
}
6870

69-
return (false);
71+
return (true);
7072
}
7173

7274
//////////////////////////////////////////////////////////////////////////
7375
template <typename PointT> bool
7476
pcl::SampleConsensusModelLine<PointT>::computeModelCoefficients (
7577
const Indices &samples, Eigen::VectorXf &model_coefficients) const
7678
{
77-
// Need 2 samples
78-
if (samples.size () != sample_size_)
79-
{
80-
PCL_ERROR ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
81-
return (false);
82-
}
83-
84-
if (std::abs ((*input_)[samples[0]].x - (*input_)[samples[1]].x) <= std::numeric_limits<float>::epsilon () &&
85-
std::abs ((*input_)[samples[0]].y - (*input_)[samples[1]].y) <= std::numeric_limits<float>::epsilon () &&
86-
std::abs ((*input_)[samples[0]].z - (*input_)[samples[1]].z) <= std::numeric_limits<float>::epsilon ())
79+
// Make sure that the samples are valid
80+
if (!isSampleGood (samples))
8781
{
82+
PCL_ERROR ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given!\n");
8883
return (false);
8984
}
9085

@@ -97,6 +92,9 @@ pcl::SampleConsensusModelLine<PointT>::computeModelCoefficients (
9792
model_coefficients[4] = (*input_)[samples[1]].y - model_coefficients[1];
9893
model_coefficients[5] = (*input_)[samples[1]].z - model_coefficients[2];
9994

95+
// This precondition should hold if the samples have been found to be good
96+
assert (model_coefficients.template tail<3> ().squaredNorm () > 0.0f);
97+
10098
model_coefficients.template tail<3> ().normalize ();
10199
PCL_DEBUG ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g).\n",
102100
model_coefficients[0], model_coefficients[1], model_coefficients[2],

‎sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp‎

Lines changed: 30 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -55,58 +55,62 @@ pcl::SampleConsensusModelPlane<PointT>::isSampleGood (const Indices &samples) co
5555
PCL_ERROR ("[pcl::SampleConsensusModelPlane::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
5656
return (false);
5757
}
58-
// Get the values at the two points
59-
pcl::Array4fMapConst p0 = (*input_)[samples[0]].getArray4fMap ();
60-
pcl::Array4fMapConst p1 = (*input_)[samples[1]].getArray4fMap ();
61-
pcl::Array4fMapConst p2 = (*input_)[samples[2]].getArray4fMap ();
6258

63-
Eigen::Array4f p2p0 = p2 - p0;
64-
Eigen::Array4f dy1dy2 = (p1-p0) / p2p0;
59+
// Check if the sample points are collinear
60+
// Similar checks are implemented as precaution in computeModelCoefficients,
61+
// so if you find the need to fix something in here, look there, too.
62+
pcl::Vector3fMapConst p0 = (*input_)[samples[0]].getVector3fMap ();
63+
pcl::Vector3fMapConst p1 = (*input_)[samples[1]].getVector3fMap ();
64+
pcl::Vector3fMapConst p2 = (*input_)[samples[2]].getVector3fMap ();
65+
66+
// Check if the norm of the cross-product would be non-zero, otherwise
67+
// normalization will fail. One could also interpret this as kind of check
68+
// if the triangle spanned by those three points would have an area greater
69+
// than zero.
70+
if ((p1 - p0).cross(p2 - p0).stableNorm() < Eigen::NumTraits<float>::dummy_precision ())
71+
{
72+
PCL_ERROR ("[pcl::SampleConsensusModelPlane::isSampleGood] Sample points too similar or collinear!\n");
73+
return (false);
74+
}
6575

66-
return ( ((dy1dy2[0] != dy1dy2[1]) || (dy1dy2[2] != dy1dy2[1])) && (!p2p0.isZero()) );
76+
return (true);
6777
}
6878

6979
//////////////////////////////////////////////////////////////////////////
7080
template <typename PointT> bool
7181
pcl::SampleConsensusModelPlane<PointT>::computeModelCoefficients (
7282
const Indices &samples, Eigen::VectorXf &model_coefficients) const
7383
{
74-
// Need 3 samples
84+
// The checks are redundant with isSampleGood above, but since most of the
85+
// computed values are also used to compute the model coefficients, this might
86+
// be a situation where this duplication is acceptable.
7587
if (samples.size () != sample_size_)
7688
{
7789
PCL_ERROR ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
7890
return (false);
7991
}
8092

81-
pcl::Array4fMapConst p0 = (*input_)[samples[0]].getArray4fMap ();
82-
pcl::Array4fMapConst p1 = (*input_)[samples[1]].getArray4fMap ();
83-
pcl::Array4fMapConst p2 = (*input_)[samples[2]].getArray4fMap ();
93+
pcl::Vector3fMapConst p0 = (*input_)[samples[0]].getVector3fMap ();
94+
pcl::Vector3fMapConst p1 = (*input_)[samples[1]].getVector3fMap ();
95+
pcl::Vector3fMapConst p2 = (*input_)[samples[2]].getVector3fMap ();
8496

85-
// Compute the segment values (in 3d) between p1 and p0
86-
Eigen::Array4f p1p0 = p1 - p0;
87-
// Compute the segment values (in 3d) between p2 and p0
88-
Eigen::Array4f p2p0 = p2 - p0;
97+
const Eigen::Vector3f cross = (p1 - p0).cross(p2 - p0);
98+
const float crossNorm = cross.stableNorm();
8999

90-
// Avoid some crashes by checking for collinearity here
91-
Eigen::Array4f dy1dy2 = p1p0 / p2p0;
92-
if ( p2p0.isZero() || ((dy1dy2[0] == dy1dy2[1]) && (dy1dy2[2] == dy1dy2[1])) ) // Check for collinearity
100+
// Checking for collinearity here
101+
if (crossNorm < Eigen::NumTraits<float>::dummy_precision ())
93102
{
103+
PCL_ERROR ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Chosen samples are collinear!\n");
94104
return (false);
95105
}
96106

97107
// Compute the plane coefficients from the 3 given points in a straightforward manner
98108
// calculate the plane normal n = (p2-p1) x (p3-p1) = cross (p2-p1, p3-p1)
99109
model_coefficients.resize (model_size_);
100-
model_coefficients[0] = p1p0[1] * p2p0[2] - p1p0[2] * p2p0[1];
101-
model_coefficients[1] = p1p0[2] * p2p0[0] - p1p0[0] * p2p0[2];
102-
model_coefficients[2] = p1p0[0] * p2p0[1] - p1p0[1] * p2p0[0];
103-
model_coefficients[3] = 0.0f;
104-
105-
// Normalize
106-
model_coefficients.normalize ();
110+
model_coefficients.template head<3>() = cross / crossNorm;
107111

108112
// ... + d = 0
109-
model_coefficients[3] = -1.0f * (model_coefficients.template head<4>().dot (p0.matrix ()));
113+
model_coefficients[3] = -1.0f * (model_coefficients.template head<3>().dot (p0));
110114

111115
PCL_DEBUG ("[pcl::SampleConsensusModelPlane::computeModelCoefficients] Model is (%g,%g,%g,%g).\n",
112116
model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3]);

‎test/sample_consensus/test_sample_consensus_quadric_models.cpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -636,7 +636,8 @@ TEST (SampleConsensusModelCircle3D, RANSAC)
636636
EXPECT_NEAR (-3.0, coeff[2], 1e-3);
637637
EXPECT_NEAR ( 0.1, coeff[3], 1e-3);
638638
EXPECT_NEAR ( 0.0, coeff[4], 1e-3);
639-
EXPECT_NEAR (-1.0, coeff[5], 1e-3);
639+
// Use abs in y component because both variants are valid normal vectors
640+
EXPECT_NEAR ( 1.0, std::abs (coeff[5]), 1e-3);
640641
EXPECT_NEAR ( 0.0, coeff[6], 1e-3);
641642

642643
Eigen::VectorXf coeff_refined;
@@ -647,7 +648,7 @@ TEST (SampleConsensusModelCircle3D, RANSAC)
647648
EXPECT_NEAR (-3.0, coeff_refined[2], 1e-3);
648649
EXPECT_NEAR ( 0.1, coeff_refined[3], 1e-3);
649650
EXPECT_NEAR ( 0.0, coeff_refined[4], 1e-3);
650-
EXPECT_NEAR (-1.0, coeff_refined[5], 1e-3);
651+
EXPECT_NEAR ( 1.0, std::abs (coeff_refined[5]), 1e-3);
651652
EXPECT_NEAR ( 0.0, coeff_refined[6], 1e-3);
652653
}
653654

0 commit comments

Comments
 (0)