TEST (RANSAC, Base)
{
// Create a shared plane model pointer directly
SampleConsensusModelPlanePtr model (new SampleConsensusModelPlane<PointXYZ> (cloud_));
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
// Basic tests
ASSERT_EQ (sac.getDistanceThreshold (), 0.03);
sac.setDistanceThreshold (0.03);
ASSERT_EQ (sac.getDistanceThreshold (), 0.03);
sac.setProbability (0.99);
ASSERT_EQ (sac.getProbability (), 0.99);
sac.setMaxIterations (10000);
ASSERT_EQ (sac.getMaxIterations (), 10000);
}
template <typename PointSource, typename PointTarget> void
pcl::IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
{
// Allocate enough space to hold the results
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
// Point cloud containing the correspondences of each point in <input, indices>
PointCloudTarget input_corresp;
input_corresp.points.resize (indices_->size ());
nr_iterations_ = 0;
converged_ = false;
double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
// If the guessed transformation is non identity
if (guess != Eigen::Matrix4f::Identity ())
{
// Initialise final transformation to the guessed one
final_transformation_ = guess;
// Apply guessed transformation prior to search for neighbours
transformPointCloud (output, output, guess);
}
// Resize the vector of distances between correspondences
std::vector<float> previous_correspondence_distances (indices_->size ());
correspondence_distances_.resize (indices_->size ());
while (!converged_) // repeat until convergence
{
// Save the previously estimated transformation
previous_transformation_ = transformation_;
// And the previous set of distances
previous_correspondence_distances = correspondence_distances_;
int cnt = 0;
std::vector<int> source_indices (indices_->size ());
std::vector<int> target_indices (indices_->size ());
// Iterating over the entire index vector and find all correspondences
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!this->searchForNeighbors (output, (*indices_)[idx], nn_indices, nn_dists))
{
PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[idx]);
return;
}
// Check if the distance to the nearest neighbor is smaller than the user imposed threshold
if (nn_dists[0] < dist_threshold)
{
source_indices[cnt] = (*indices_)[idx];
target_indices[cnt] = nn_indices[0];
cnt++;
}
// Save the nn_dists[0] to a global vector of distances
correspondence_distances_[(*indices_)[idx]] = std::min (nn_dists[0], static_cast<float> (dist_threshold));
}
if (cnt < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
converged_ = false;
return;
}
// Resize to the actual number of valid correspondences
source_indices.resize (cnt); target_indices.resize (cnt);
std::vector<int> source_indices_good;
std::vector<int> target_indices_good;
{
// From the set of correspondences found, attempt to remove outliers
// Create the registration model
typedef typename SampleConsensusModelRegistration<PointSource>::Ptr SampleConsensusModelRegistrationPtr;
SampleConsensusModelRegistrationPtr model;
model.reset (new SampleConsensusModelRegistration<PointSource> (output.makeShared (), source_indices));
// Pass the target_indices
model->setInputTarget (target_, target_indices);
// Create a RANSAC model
RandomSampleConsensus<PointSource> sac (model, inlier_threshold_);
sac.setMaxIterations (ransac_iterations_);
// Compute the set of inliers
if (!sac.computeModel ())
{
source_indices_good = source_indices;
target_indices_good = target_indices;
}
else
{
std::vector<int> inliers;
// Get the inliers
sac.getInliers (inliers);
source_indices_good.resize (inliers.size ());
target_indices_good.resize (inliers.size ());
boost::unordered_map<int, int> source_to_target;
for (unsigned int i = 0; i < source_indices.size(); ++i)
source_to_target[source_indices[i]] = target_indices[i];
// Copy just the inliers
std::copy(inliers.begin(), inliers.end(), source_indices_good.begin());
for (size_t i = 0; i < inliers.size (); ++i)
target_indices_good[i] = source_to_target[inliers[i]];
}
}
// Check whether we have enough correspondences
cnt = static_cast<int> (source_indices_good.size ());
if (cnt < min_number_correspondences_)
{
PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ());
converged_ = false;
return;
}
PCL_DEBUG ("[pcl::%s::computeTransformation] Number of correspondences %d [%f%%] out of %zu points [100.0%%], RANSAC rejected: %zu [%f%%].\n",
getClassName ().c_str (),
cnt,
(static_cast<float> (cnt) * 100.0f) / static_cast<float> (indices_->size ()),
indices_->size (),
source_indices.size () - cnt,
static_cast<float> (source_indices.size () - cnt) * 100.0f / static_cast<float> (source_indices.size ()));
// Estimate the transform
//rigid_transformation_estimation_(output, source_indices_good, *target_, target_indices_good, transformation_);
transformation_estimation_->estimateRigidTransformation (output, source_indices_good, *target_, target_indices_good, transformation_);
// Tranform the data
transformPointCloud (output, output, transformation_);
// Obtain the final transformation
final_transformation_ = transformation_ * final_transformation_;
nr_iterations_++;
// Update the vizualization of icp convergence
if (update_visualizer_ != 0)
update_visualizer_(output, source_indices_good, *target_, target_indices_good );
// Various/Different convergence termination criteria
// 1. Number of iterations has reached the maximum user imposed number of iterations (via
// setMaximumIterations)
// 2. The epsilon (difference) between the previous transformation and the current estimated transformation
// is smaller than an user imposed value (via setTransformationEpsilon)
// 3. The sum of Euclidean squared errors is smaller than a user defined threshold (via
// setEuclideanFitnessEpsilon)
if (nr_iterations_ >= max_iterations_ ||
(transformation_ - previous_transformation_).array ().abs ().sum () < transformation_epsilon_ ||
fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)) <= euclidean_fitness_epsilon_
)
{
converged_ = true;
PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ());
PCL_DEBUG ("nr_iterations_ (%d) >= max_iterations_ (%d)\n", nr_iterations_, max_iterations_);
PCL_DEBUG ("(transformation_ - previous_transformation_).array ().abs ().sum () (%f) < transformation_epsilon_ (%f)\n",
(transformation_ - previous_transformation_).array ().abs ().sum (), transformation_epsilon_);
PCL_DEBUG ("fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)) (%f) <= euclidean_fitness_epsilon_ (%f)\n",
fabs (this->getFitnessScore (correspondence_distances_, previous_correspondence_distances)),
euclidean_fitness_epsilon_);
}
}
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
int max_iterations = 1000, double threshold = 0.05, bool negative = false)
{
// Convert data to PointCloud<T>
PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>);
fromROSMsg (*input, *xyz);
// Estimate
TicToc tt;
print_highlight (stderr, "Computing ");
tt.tic ();
// Refine the plane indices
typedef SampleConsensusModelPlane<PointXYZ>::Ptr SampleConsensusModelPlanePtr;
SampleConsensusModelPlanePtr model (new SampleConsensusModelPlane<PointXYZ> (xyz));
RandomSampleConsensus<PointXYZ> sac (model, threshold);
sac.setMaxIterations (max_iterations);
bool res = sac.computeModel ();
vector<int> inliers;
sac.getInliers (inliers);
Eigen::VectorXf coefficients;
sac.getModelCoefficients (coefficients);
if (!res || inliers.empty ())
{
PCL_ERROR ("No planar model found. Relax thresholds and continue.\n");
return;
}
sac.refineModel (2, 50);
sac.getInliers (inliers);
sac.getModelCoefficients (coefficients);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms, plane has : "); print_value ("%zu", inliers.size ()); print_info (" points]\n");
print_info ("Model coefficients: [");
print_value ("%g %g %g %g", coefficients[0], coefficients[1], coefficients[2], coefficients[3]); print_info ("]\n");
// Instead of returning the planar model as a set of inliers, return the outliers, but perform a cluster segmentation first
if (negative)
{
// Remove the plane indices from the data
PointIndices::Ptr everything_but_the_plane (new PointIndices);
std::vector<int> indices_fullset (xyz->size ());
for (int p_it = 0; p_it < static_cast<int> (indices_fullset.size ()); ++p_it)
indices_fullset[p_it] = p_it;
std::sort (inliers.begin (), inliers.end ());
set_difference (indices_fullset.begin (), indices_fullset.end (),
inliers.begin (), inliers.end (),
inserter (everything_but_the_plane->indices, everything_but_the_plane->indices.begin ()));
// Extract largest cluster minus the plane
vector<PointIndices> cluster_indices;
EuclideanClusterExtraction<PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setInputCloud (xyz);
ec.setIndices (everything_but_the_plane);
ec.extract (cluster_indices);
// Convert data back
copyPointCloud (*input, cluster_indices[0].indices, output);
}
else
{
// Convert data back
PointCloud<PointXYZ> output_inliers;
copyPointCloud (*input, inliers, output);
}
}