Loading...
Searching...
No Matches
geometric_consistency.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2012, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*/
#ifndef PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_
#define PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_
#include <pcl/recognition/cg/geometric_consistency.h>
#include <pcl/registration/correspondence_types.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/common/io.h>
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
inline bool
gcCorrespSorter (pcl::Correspondence i, pcl::Correspondence j)
{
return (i.distance < j.distance);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointModelT, typename PointSceneT> void
pcl::GeometricConsistencyGrouping<PointModelT, PointSceneT>::clusterCorrespondences (std::vector<Correspondences> &model_instances)
{
model_instances.clear ();
found_transformations_.clear ();
if (!model_scene_corrs_)
{
PCL_ERROR(
"[pcl::GeometricConsistencyGrouping::clusterCorrespondences()] Error! Correspondences not set, please set them before calling again this function.\n");
return;
}
CorrespondencesPtr sorted_corrs (new Correspondences (*model_scene_corrs_));
std::sort (sorted_corrs->begin (), sorted_corrs->end (), gcCorrespSorter);
model_scene_corrs_ = sorted_corrs;
PCL_DEBUG_STREAM("[pcl::GeometricConsistencyGrouping::clusterCorrespondences] Five best correspondences: ");
for(std::size_t i=0; i<std::min<std::size_t>(model_scene_corrs_->size(), 5); ++i)
PCL_DEBUG_STREAM("[" << (*input_)[(*model_scene_corrs_)[i].index_query] << " " << (*scene_)[(*model_scene_corrs_)[i].index_match] << " " << (*model_scene_corrs_)[i].distance << "] ");
PCL_DEBUG_STREAM(std::endl);
std::vector<int> consensus_set;
std::vector<bool> taken_corresps (model_scene_corrs_->size (), false);
Eigen::Vector3f dist_ref, dist_trg;
//temp copy of scene cloud with the type cast to ModelT in order to use Ransac
PointCloudPtr temp_scene_cloud_ptr (new PointCloud ());
pcl::copyPointCloud (*scene_, *temp_scene_cloud_ptr);
pcl::registration::CorrespondenceRejectorSampleConsensus<PointModelT> corr_rejector;
corr_rejector.setMaximumIterations (10000);
corr_rejector.setInlierThreshold (gc_size_);
corr_rejector.setInputSource(input_);
corr_rejector.setInputTarget (temp_scene_cloud_ptr);
for (std::size_t i = 0; i < model_scene_corrs_->size (); ++i)
{
if (taken_corresps[i])
continue;
consensus_set.clear ();
consensus_set.push_back (static_cast<int> (i));
for (std::size_t j = 0; j < model_scene_corrs_->size (); ++j)
{
if ( j != i && !taken_corresps[j])
{
//Let's check if j fits into the current consensus set
bool is_a_good_candidate = true;
for (const int &k : consensus_set)
{
int scene_index_k = model_scene_corrs_->at (k).index_match;
int model_index_k = model_scene_corrs_->at (k).index_query;
int scene_index_j = model_scene_corrs_->at (j).index_match;
int model_index_j = model_scene_corrs_->at (j).index_query;
const Eigen::Vector3f& scene_point_k = scene_->at (scene_index_k).getVector3fMap ();
const Eigen::Vector3f& model_point_k = input_->at (model_index_k).getVector3fMap ();
const Eigen::Vector3f& scene_point_j = scene_->at (scene_index_j).getVector3fMap ();
const Eigen::Vector3f& model_point_j = input_->at (model_index_j).getVector3fMap ();
dist_ref = scene_point_k - scene_point_j;
dist_trg = model_point_k - model_point_j;
double distance = std::abs (dist_ref.norm () - dist_trg.norm ());
if (distance > gc_size_)
{
is_a_good_candidate = false;
break;
}
}
if (is_a_good_candidate)
consensus_set.push_back (static_cast<int> (j));
}
}
if (static_cast<int> (consensus_set.size ()) > gc_threshold_)
{
Correspondences temp_corrs, filtered_corrs;
for (const int &j : consensus_set)
{
temp_corrs.push_back (model_scene_corrs_->at (j));
taken_corresps[ j ] = true;
}
//ransac filtering
corr_rejector.getRemainingCorrespondences (temp_corrs, filtered_corrs);
//save transformations for recognize
found_transformations_.push_back (corr_rejector.getBestTransformation ());
model_instances.push_back (filtered_corrs);
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointModelT, typename PointSceneT> bool
pcl::GeometricConsistencyGrouping<PointModelT, PointSceneT>::recognize (
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations)
{
std::vector<pcl::Correspondences> model_instances;
return (this->recognize (transformations, model_instances));
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointModelT, typename PointSceneT> bool
pcl::GeometricConsistencyGrouping<PointModelT, PointSceneT>::recognize (
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs)
{
transformations.clear ();
if (!this->initCompute ())
{
PCL_ERROR(
"[pcl::GeometricConsistencyGrouping::recognize()] Error! Model cloud or Scene cloud not set, please set them before calling again this function.\n");
return (false);
}
clusterCorrespondences (clustered_corrs);
transformations = found_transformations_;
this->deinitCompute ();
return (true);
}
#define PCL_INSTANTIATE_GeometricConsistencyGrouping(T,ST) template class PCL_EXPORTS pcl::GeometricConsistencyGrouping<T,ST>;
#endif // PCL_RECOGNITION_GEOMETRIC_CONSISTENCY_IMPL_H_
pcl::CorrespondenceGrouping::model_scene_corrs_
CorrespondencesConstPtr model_scene_corrs_
The correspondences between points in the input and the scene datasets.
Definition correspondence_grouping.h:138
pcl::CorrespondenceGrouping::initCompute
bool initCompute()
This method should get called before starting the actual computation.
Definition correspondence_grouping.h:159
pcl::CorrespondenceGrouping::deinitCompute
bool deinitCompute()
This method should get called after finishing the actual computation.
Definition correspondence_grouping.h:191
pcl::CorrespondenceGrouping::scene_
SceneCloudConstPtr scene_
The scene cloud.
Definition correspondence_grouping.h:133
pcl::GeometricConsistencyGrouping::gc_threshold_
int gc_threshold_
Minimum cluster size.
Definition geometric_consistency.h:130
pcl::GeometricConsistencyGrouping::gc_size_
double gc_size_
Resolution of the consensus set used to cluster correspondences together.
Definition geometric_consistency.h:133
pcl::GeometricConsistencyGrouping::clusterCorrespondences
void clusterCorrespondences(std::vector< Correspondences > &model_instances) override
Cluster the input correspondences in order to distinguish between different instances of the model in...
Definition geometric_consistency.hpp:57
pcl::GeometricConsistencyGrouping::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition geometric_consistency.h:58
pcl::GeometricConsistencyGrouping::PointCloud
pcl::PointCloud< PointModelT > PointCloud
Definition geometric_consistency.h:57
pcl::GeometricConsistencyGrouping::recognize
bool recognize(std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > &transformations)
The main function, recognizes instances of the model into the scene set by the user.
Definition geometric_consistency.hpp:157
pcl::GeometricConsistencyGrouping::found_transformations_
std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > found_transformations_
Transformations found by clusterCorrespondences method.
Definition geometric_consistency.h:136
pcl::PCLBase< PointModelT >::input_
PointCloudConstPtr input_
Definition pcl_base.h:147
pcl::registration::CorrespondenceRejectorSampleConsensus
CorrespondenceRejectorSampleConsensus implements a correspondence rejection using Random Sample Conse...
Definition correspondence_rejection_sample_consensus.h:54
pcl::registration::CorrespondenceRejectorSampleConsensus::setInputSource
virtual void setInputSource(const PointCloudConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!).
Definition correspondence_rejection_sample_consensus.h:91
pcl::registration::CorrespondenceRejectorSampleConsensus::getBestTransformation
Eigen::Matrix4f getBestTransformation()
Get the best transformation after RANSAC rejection.
Definition correspondence_rejection_sample_consensus.h:193
pcl::registration::CorrespondenceRejectorSampleConsensus::setInputTarget
virtual void setInputTarget(const PointCloudConstPtr &cloud)
Provide a target point cloud dataset (must contain XYZ data!).
Definition correspondence_rejection_sample_consensus.h:107
pcl::registration::CorrespondenceRejectorSampleConsensus::setInlierThreshold
void setInlierThreshold(double threshold)
Set the maximum distance between corresponding points.
Definition correspondence_rejection_sample_consensus.h:157
pcl::registration::CorrespondenceRejectorSampleConsensus::setMaximumIterations
void setMaximumIterations(int max_iterations)
Set the maximum number of iterations.
Definition correspondence_rejection_sample_consensus.h:175
pcl::registration::CorrespondenceRejectorSampleConsensus::getRemainingCorrespondences
void getRemainingCorrespondences(const pcl::Correspondences &original_correspondences, pcl::Correspondences &remaining_correspondences) override
Get a list of valid correspondences after rejection from the original set of correspondences.
Definition correspondence_rejection_sample_consensus.hpp:55
pcl::copyPointCloud
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
Definition io.hpp:142
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition correspondence.h:89
pcl::CorrespondencesPtr
shared_ptr< Correspondences > CorrespondencesPtr
Definition correspondence.h:90
pcl::Correspondence
Correspondence represents a match between two entities (e.g., points, descriptors,...
Definition correspondence.h:61
pcl::Correspondence::distance
float distance
Definition correspondence.h:69