Skip to content

Commit

Permalink
Merge pull request #3642 from mvieth/sample_consensus
Browse files Browse the repository at this point in the history 
[sample consensus] Better performance, error handling and other improvements
  • Loading branch information
@SergioRAgostinho
SergioRAgostinho authored Mar 19, 2020
2 parents 749ca70 + eb3a8de commit 55fc3fa
Show file tree
Hide file tree
Showing 19 changed files with 446 additions and 349 deletions.
50 changes: 37 additions & 13 deletions sample_consensus/include/pcl/sample_consensus/impl/lmeds.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,19 +61,20 @@ pcl::LeastMedianSquares<PointT>::computeModel (int debug_verbosity_level)
Eigen::VectorXf model_coefficients;
std::vector<double> distances;

int n_inliers_count = 0;

unsigned skipped_count = 0;
// suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
const unsigned max_skip = max_iterations_ * 10;

// Iterate
while (iterations_ < max_iterations_ && skipped_count < max_skip)
while ((iterations_ < max_iterations_) && (skipped_count < max_skip))
{
// Get X samples which satisfy the model criteria
sac_model_->getSamples (iterations_, selection);

if (selection.empty ()) break;
if (selection.empty ())
{
break;
}

// Search for inliers in the point cloud for the current plane model M
if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
Expand All @@ -83,7 +84,7 @@ pcl::LeastMedianSquares<PointT>::computeModel (int debug_verbosity_level)
continue;
}

double d_cur_penalty = 0;
double d_cur_penalty;
// d_cur_penalty = sum (min (dist, threshold))

// Iterate through the 3d points and calculate the distances from them to the model
Expand All @@ -96,22 +97,36 @@ pcl::LeastMedianSquares<PointT>::computeModel (int debug_verbosity_level)
++skipped_count;
continue;
}
// Move all NaNs in distances to the end
const auto new_end = (sac_model_->getInputCloud()->is_dense ? distances.end() : std::partition (distances.begin(), distances.end(), [](double d){return !std::isnan (d);}));
const auto nr_valid_dists = std::distance (distances.begin (), new_end);

std::sort (distances.begin (), distances.end ());
// d_cur_penalty = median (distances)
std::size_t mid = sac_model_->getIndices ()->size () / 2;
if (mid >= distances.size ())
const std::size_t mid = nr_valid_dists / 2;
PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] There are %lu valid distances remaining after removing NaN values.\n", nr_valid_dists);
if (nr_valid_dists == 0)
{
//iterations_++;
++skipped_count;
continue;
}

// Do we have a "middle" point or should we "estimate" one ?
if (sac_model_->getIndices ()->size () % 2 == 0)
d_cur_penalty = (sqrt (distances[mid-1]) + sqrt (distances[mid])) / 2;
if ((nr_valid_dists % 2) == 0)
{
// Looking at two values instead of one probably doesn't matter because they are mostly barely different, but let's do it for accuracy's sake
std::nth_element (distances.begin (), distances.begin () + (mid - 1), new_end);
const double tmp = distances[mid-1];
const double tmp2 = *(std::min_element (distances.begin () + mid, new_end));
d_cur_penalty = (sqrt (tmp) + sqrt (tmp2)) / 2.0;
PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Computing median with two values (%g and %g) because number of distances is even.\n", tmp, distances[mid]);
}
else
{
std::nth_element (distances.begin (), distances.begin () + mid, new_end);
d_cur_penalty = sqrt (distances[mid]);
PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Computing median with one value (%g) because number of distances is odd.\n", distances[mid]);
}

// Better match ?
if (d_cur_penalty < d_best_penalty)
Expand All @@ -125,13 +140,17 @@ pcl::LeastMedianSquares<PointT>::computeModel (int debug_verbosity_level)

++iterations_;
if (debug_verbosity_level > 1)
{
PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Trial %d out of %d. Best penalty is %f.\n", iterations_, max_iterations_, d_best_penalty);
}
}

if (model_.empty ())
{
if (debug_verbosity_level > 0)
{
PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Unable to find a solution!\n");
}
return (false);
}

Expand Down Expand Up @@ -160,21 +179,26 @@ pcl::LeastMedianSquares<PointT>::computeModel (int debug_verbosity_level)

inliers_.resize (distances.size ());
// Get the inliers for the best model found
n_inliers_count = 0;
std::size_t n_inliers_count = 0;
for (std::size_t i = 0; i < distances.size (); ++i)
{
if (distances[i] <= threshold_)
{
inliers_[n_inliers_count++] = indices[i];
}
}

// Resize the inliers vector
inliers_.resize (n_inliers_count);

if (debug_verbosity_level > 0)
PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Model: %lu size, %d inliers.\n", model_.size (), n_inliers_count);
{
PCL_DEBUG ("[pcl::LeastMedianSquares::computeModel] Model: %lu size, %lu inliers.\n", model_.size (), n_inliers_count);
}

return (true);
}

#define PCL_INSTANTIATE_LeastMedianSquares(T) template class PCL_EXPORTS pcl::LeastMedianSquares<T>;

#endif // PCL_SAMPLE_CONSENSUS_IMPL_LMEDS_H_

41 changes: 22 additions & 19 deletions sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,11 @@
template <typename PointT> bool
pcl::SampleConsensusModelCircle2D<PointT>::isSampleGood(const std::vector<int> &samples) const
{
if (samples.size () != sample_size_)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
return (false);
}
// Get the values at the two points
Eigen::Array2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y);
Eigen::Array2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y);
Expand All @@ -69,13 +74,13 @@ template <typename PointT> bool
pcl::SampleConsensusModelCircle2D<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
{
// Need 3 samples
if (samples.size () != 3)
if (samples.size () != sample_size_)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
return (false);
}

model_coefficients.resize (3);
model_coefficients.resize (model_size_);

Eigen::Vector2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y);
Eigen::Vector2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y);
Expand Down Expand Up @@ -137,9 +142,10 @@ pcl::SampleConsensusModelCircle2D<PointT>::selectWithinDistance (
inliers.clear ();
return;
}
int nr_p = 0;
inliers.resize (indices_->size ());
error_sqr_dists_.resize (indices_->size ());
inliers.clear ();
error_sqr_dists_.clear ();
inliers.reserve (indices_->size ());
error_sqr_dists_.reserve (indices_->size ());

// Iterate through the 3d points and calculate the distances from them to the circle
for (std::size_t i = 0; i < indices_->size (); ++i)
Expand All @@ -156,13 +162,10 @@ pcl::SampleConsensusModelCircle2D<PointT>::selectWithinDistance (
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.push_back ((*indices_)[i]);
error_sqr_dists_.push_back (static_cast<double> (distance));
}
}
inliers.resize (nr_p);
error_sqr_dists_.resize (nr_p);
}

//////////////////////////////////////////////////////////////////////////
Expand Down Expand Up @@ -201,16 +204,16 @@ pcl::SampleConsensusModelCircle2D<PointT>::optimizeModelCoefficients (
optimized_coefficients = model_coefficients;

// Needs a set of valid model coefficients
if (model_coefficients.size () != 3)
if (!isModelValid (model_coefficients))
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Given model is invalid!\n");
return;
}

// Need at least 3 samples
if (inliers.size () <= 3)
// Need more than the minimum sample size to make a difference
if (inliers.size () <= sample_size_)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
return;
}

Expand All @@ -231,9 +234,9 @@ pcl::SampleConsensusModelCircle2D<PointT>::projectPoints (
PointCloud &projected_points, bool copy_data_fields) const
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 3)
if (!isModelValid (model_coefficients))
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Given model is invalid!\n");
return;
}

Expand Down Expand Up @@ -297,9 +300,9 @@ pcl::SampleConsensusModelCircle2D<PointT>::doSamplesVerifyModel (
const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid model coefficients
if (model_coefficients.size () != 3)
if (!isModelValid (model_coefficients))
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel] Given model is invalid!\n");
return (false);
}

Expand Down
35 changes: 19 additions & 16 deletions sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,11 @@ template <typename PointT> bool
pcl::SampleConsensusModelCircle3D<PointT>::isSampleGood (
const std::vector<int> &samples) const
{
if (samples.size () != sample_size_)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
return (false);
}
// Get the values at the three points
Eigen::Vector3d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z);
Eigen::Vector3d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z);
Expand All @@ -65,13 +70,13 @@ template <typename PointT> bool
pcl::SampleConsensusModelCircle3D<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
{
// Need 3 samples
if (samples.size () != 3)
if (samples.size () != sample_size_)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
return (false);
}

model_coefficients.resize (7); //needing 7 coefficients: centerX, centerY, centerZ, radius, normalX, normalY, normalZ
model_coefficients.resize (model_size_); //needing 7 coefficients: centerX, centerY, centerZ, radius, normalX, normalY, normalZ

Eigen::Vector3d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z);
Eigen::Vector3d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z);
Expand Down Expand Up @@ -169,8 +174,8 @@ pcl::SampleConsensusModelCircle3D<PointT>::selectWithinDistance (
inliers.clear ();
return;
}
int nr_p = 0;
inliers.resize (indices_->size ());
inliers.clear ();
inliers.reserve (indices_->size ());

// Iterate through the 3d points and calculate the distances from them to the sphere
for (std::size_t i = 0; i < indices_->size (); ++i)
Expand Down Expand Up @@ -199,11 +204,9 @@ pcl::SampleConsensusModelCircle3D<PointT>::selectWithinDistance (
if (distanceVector.norm () < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
inliers[nr_p] = (*indices_)[i];
nr_p++;
inliers.push_back ((*indices_)[i]);
}
}
inliers.resize (nr_p);
}

//////////////////////////////////////////////////////////////////////////
Expand Down Expand Up @@ -257,16 +260,16 @@ pcl::SampleConsensusModelCircle3D<PointT>::optimizeModelCoefficients (
optimized_coefficients = model_coefficients;

// Needs a set of valid model coefficients
if (model_coefficients.size () != 7)
if (!isModelValid (model_coefficients))
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Given model is invalid!\n");
return;
}

// Need at least 3 samples
if (inliers.size () <= 3)
// Need more than the minimum sample size to make a difference
if (inliers.size () <= sample_size_)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
return;
}

Expand All @@ -290,9 +293,9 @@ pcl::SampleConsensusModelCircle3D<PointT>::projectPoints (
PointCloud &projected_points, bool copy_data_fields) const
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 7)
if (!isModelValid (model_coefficients))
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Given model is invalid!\n");
return;
}

Expand Down Expand Up @@ -393,9 +396,9 @@ pcl::SampleConsensusModelCircle3D<PointT>::doSamplesVerifyModel (
const double threshold) const
{
// Needs a valid model coefficients
if (model_coefficients.size () != 7)
if (!isModelValid (model_coefficients))
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel] Given model is invalid!\n");
return (false);
}

Expand Down
Loading

0 comments on commit 55fc3fa

Please sign in to comment.