Skip to content

Commit d381bb8

Browse files
mviethmvieth
authored
Merge pull request #5270 from alexvorndran/sample_consensus-sample-validation
[sample_consensus] Fix inconsistent sample validation
2 parents 4b44139 + 8185eb0 commit d381bb8

9 files changed

Lines changed: 356 additions & 70 deletions

‎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_line_models.cpp‎

Lines changed: 83 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -149,6 +149,89 @@ TEST (SampleConsensusModelLine, OnGroundPlane)
149149
EXPECT_NEAR (0, coeff[5], 1e-4);
150150
}
151151

152+
TEST (SampleConsensusModelLine, SampleValidationPointsEqual)
153+
{
154+
PointCloud<PointXYZ> cloud;
155+
cloud.resize (3);
156+
157+
// The "cheat point" makes it possible to find a set of valid samples and
158+
// therefore avoids the log message of an unsuccessful sample validation
159+
// being printed a 1000 times without any chance of success.
160+
// The order is chosen such that with a known, fixed rng-state/-seed all
161+
// validation steps are actually exercised.
162+
const pcl::index_t firstKnownEqualPoint = 0;
163+
const pcl::index_t secondKnownEqualPoint = 1;
164+
const pcl::index_t cheatPointIndex = 2;
165+
166+
cloud[firstKnownEqualPoint].getVector3fMap () << 0.1f, 0.0f, 0.0f;
167+
cloud[secondKnownEqualPoint].getVector3fMap () << 0.1f, 0.0f, 0.0f;
168+
cloud[cheatPointIndex].getVector3fMap () << 0.0f, 0.1f, 0.0f; // <-- cheat point
169+
170+
// Create a shared line model pointer directly and explicitly disable the
171+
// random seed for the reasons mentioned above
172+
SampleConsensusModelLinePtr model (
173+
new SampleConsensusModelLine<PointXYZ> (cloud.makeShared (), /* random = */ false));
174+
175+
// Algorithm tests
176+
pcl::Indices samples;
177+
int iterations = 0;
178+
model->getSamples(iterations, samples);
179+
EXPECT_EQ (samples.size(), 2);
180+
// The "cheat point" has to be part of the sample, otherwise something is wrong.
181+
// The best option would be to assert on stderr output here, but that doesn't
182+
// seem to be that simple.
183+
EXPECT_TRUE (std::find(samples.begin (), samples.end (), cheatPointIndex) != samples.end ());
184+
185+
pcl::Indices forcedSamples = {firstKnownEqualPoint, secondKnownEqualPoint};
186+
Eigen::VectorXf modelCoefficients;
187+
EXPECT_FALSE (model->computeModelCoefficients (forcedSamples, modelCoefficients));
188+
}
189+
190+
TEST (SampleConsensusModelLine, SampleValidationPointsValid)
191+
{
192+
PointCloud<PointXYZ> cloud;
193+
cloud.resize (2);
194+
195+
// These two points only differ in one coordinate so this also acts as a
196+
// regression test for 36c2bd6209f87dc7c6f56e2c0314e19f9cab95ec
197+
cloud[0].getVector3fMap () << 0.0f, 0.0f, 0.0f;
198+
cloud[1].getVector3fMap () << 0.1f, 0.0f, 0.0f;
199+
200+
// Create a shared line model pointer directly
201+
SampleConsensusModelLinePtr model (new SampleConsensusModelLine<PointXYZ> (cloud.makeShared ()));
202+
203+
// Algorithm tests
204+
pcl::Indices samples;
205+
int iterations = 0;
206+
model->getSamples(iterations, samples);
207+
EXPECT_EQ (samples.size(), 2);
208+
209+
pcl::Indices forcedSamples = {0, 1};
210+
Eigen::VectorXf modelCoefficients;
211+
EXPECT_TRUE (model->computeModelCoefficients (forcedSamples, modelCoefficients));
212+
}
213+
214+
TEST (SampleConsensusModelLine, SampleValidationNotEnoughSamples)
215+
{
216+
PointCloud<PointXYZ> cloud;
217+
cloud.resize (1);
218+
219+
cloud[0].getVector3fMap () << 0.1f, 0.0f, 0.0f;
220+
221+
// Create a shared line model pointer directly
222+
SampleConsensusModelLinePtr model (new SampleConsensusModelLine<PointXYZ> (cloud.makeShared ()));
223+
224+
// Algorithm tests
225+
pcl::Indices samples;
226+
int iterations = 0;
227+
model->getSamples(iterations, samples);
228+
EXPECT_EQ (samples.size(), 0);
229+
230+
pcl::Indices forcedSamples = {0,};
231+
Eigen::VectorXf modelCoefficients;
232+
EXPECT_FALSE (model->computeModelCoefficients (forcedSamples, modelCoefficients));
233+
}
234+
152235
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
153236
TEST (SampleConsensusModelParallelLine, RANSAC)
154237
{

0 commit comments

Comments
 (0)