Loading...
Searching...
No Matches
hough_3d.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_HOUGH_3D_IMPL_H_
#define PCL_RECOGNITION_HOUGH_3D_IMPL_H_
#include <pcl/common/io.h> // for copyPointCloud
#include <pcl/recognition/cg/hough_3d.h>
#include <pcl/registration/correspondence_types.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
//#include <pcl/sample_consensus/ransac.h>
//#include <pcl/sample_consensus/sac_model_registration.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/board.h>
template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT>
template<typename PointType, typename PointRfType> void
pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::computeRf (const typename pcl::PointCloud<PointType>::ConstPtr &input, pcl::PointCloud<PointRfType> &rf)
{
if (local_rf_search_radius_ == 0)
{
PCL_WARN ("[pcl::Hough3DGrouping::computeRf()] Warning! Reference frame search radius not set. Computing with default value. Results might be incorrect, algorithm might be slow.\n");
local_rf_search_radius_ = static_cast<float> (hough_bin_size_);
}
pcl::PointCloud<Normal>::Ptr normal_cloud (new pcl::PointCloud<Normal> ());
NormalEstimation<PointType, Normal> norm_est;
norm_est.setInputCloud (input);
if (local_rf_normals_search_radius_ <= 0.0f)
{
norm_est.setKSearch (15);
}
else
{
norm_est.setRadiusSearch (local_rf_normals_search_radius_);
}
norm_est.compute (*normal_cloud);
BOARDLocalReferenceFrameEstimation<PointType, Normal, PointRfType> rf_est;
rf_est.setInputCloud (input);
rf_est.setInputNormals (normal_cloud);
rf_est.setFindHoles (true);
rf_est.setRadiusSearch (local_rf_search_radius_);
rf_est.compute (rf);
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool
pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::train ()
{
if (!input_)
{
PCL_ERROR ("[pcl::Hough3DGrouping::train()] Error! Input cloud not set.\n");
return (false);
}
if (!input_rf_)
{
ModelRfCloudPtr new_input_rf (new ModelRfCloud ());
computeRf<PointModelT, PointModelRfT> (input_, *new_input_rf);
input_rf_ = new_input_rf;
//PCL_ERROR(
// "[pcl::Hough3DGrouping::train()] Error! Input reference frame not set.\n");
//return (false);
}
if (input_->size () != input_rf_->size ())
{
PCL_ERROR ("[pcl::Hough3DGrouping::train()] Error! Input cloud size != Input RF cloud size.\n");
return (false);
}
model_votes_.clear ();
model_votes_.resize (input_->size ());
// compute model centroid
Eigen::Vector3f centroid (0, 0, 0);
for (std::size_t i = 0; i < input_->size (); ++i)
{
centroid += input_->at (i).getVector3fMap ();
}
centroid /= static_cast<float> (input_->size ());
// compute model votes
for (std::size_t i = 0; i < input_->size (); ++i)
{
Eigen::Vector3f x_ax ((*input_rf_)[i].x_axis[0], (*input_rf_)[i].x_axis[1], (*input_rf_)[i].x_axis[2]);
Eigen::Vector3f y_ax ((*input_rf_)[i].y_axis[0], (*input_rf_)[i].y_axis[1], (*input_rf_)[i].y_axis[2]);
Eigen::Vector3f z_ax ((*input_rf_)[i].z_axis[0], (*input_rf_)[i].z_axis[1], (*input_rf_)[i].z_axis[2]);
model_votes_[i].x () = x_ax.dot (centroid - input_->at (i).getVector3fMap ());
model_votes_[i].y () = y_ax.dot (centroid - input_->at (i).getVector3fMap ());
model_votes_[i].z () = z_ax.dot (centroid - input_->at (i).getVector3fMap ());
}
needs_training_ = false;
return (true);
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool
pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::houghVoting ()
{
if (needs_training_)
{
if (!train ())//checks input and input_rf
return (false);
}
//if (!scene_)
//{
// PCL_ERROR(
// "[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene cloud not set.\n");
// return (false);
//}
if (!scene_rf_)
{
ModelRfCloudPtr new_scene_rf (new ModelRfCloud ());
computeRf<PointSceneT, PointSceneRfT> (scene_, *new_scene_rf);
scene_rf_ = new_scene_rf;
//PCL_ERROR(
// "[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene reference frame not set.\n");
//return (false);
}
if (scene_->size () != scene_rf_->size ())
{
PCL_ERROR ("[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene cloud size != Scene RF cloud size.\n");
return (false);
}
if (!model_scene_corrs_)
{
PCL_ERROR ("[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Correspondences not set, please set them before calling again this function.\n");
return (false);
}
int n_matches = static_cast<int> (model_scene_corrs_->size ());
if (n_matches == 0)
{
return (false);
}
std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > scene_votes (n_matches);
Eigen::Vector3d d_min, d_max, bin_size;
d_min.setConstant (std::numeric_limits<double>::max ());
d_max.setConstant (-std::numeric_limits<double>::max ());
bin_size.setConstant (hough_bin_size_);
float max_distance = -std::numeric_limits<float>::max ();
// Calculating 3D Hough space dimensions and vote position for each match
for (int i=0; i< n_matches; ++i)
{
int scene_index = model_scene_corrs_->at (i).index_match;
int model_index = model_scene_corrs_->at (i).index_query;
const Eigen::Vector3f& scene_point = scene_->at (scene_index).getVector3fMap ();
const PointSceneRfT& scene_point_rf = scene_rf_->at (scene_index);
Eigen::Vector3f scene_point_rf_x (scene_point_rf.x_axis[0], scene_point_rf.x_axis[1], scene_point_rf.x_axis[2]);
Eigen::Vector3f scene_point_rf_y (scene_point_rf.y_axis[0], scene_point_rf.y_axis[1], scene_point_rf.y_axis[2]);
Eigen::Vector3f scene_point_rf_z (scene_point_rf.z_axis[0], scene_point_rf.z_axis[1], scene_point_rf.z_axis[2]);
//const Eigen::Vector3f& model_point = input_->at (model_index).getVector3fMap ();
const Eigen::Vector3f& model_point_vote = model_votes_[model_index];
scene_votes[i].x () = scene_point_rf_x[0] * model_point_vote.x () + scene_point_rf_y[0] * model_point_vote.y () + scene_point_rf_z[0] * model_point_vote.z () + scene_point.x ();
scene_votes[i].y () = scene_point_rf_x[1] * model_point_vote.x () + scene_point_rf_y[1] * model_point_vote.y () + scene_point_rf_z[1] * model_point_vote.z () + scene_point.y ();
scene_votes[i].z () = scene_point_rf_x[2] * model_point_vote.x () + scene_point_rf_y[2] * model_point_vote.y () + scene_point_rf_z[2] * model_point_vote.z () + scene_point.z ();
if (scene_votes[i].x () < d_min.x ())
d_min.x () = scene_votes[i].x ();
if (scene_votes[i].x () > d_max.x ())
d_max.x () = scene_votes[i].x ();
if (scene_votes[i].y () < d_min.y ())
d_min.y () = scene_votes[i].y ();
if (scene_votes[i].y () > d_max.y ())
d_max.y () = scene_votes[i].y ();
if (scene_votes[i].z () < d_min.z ())
d_min.z () = scene_votes[i].z ();
if (scene_votes[i].z () > d_max.z ())
d_max.z () = scene_votes[i].z ();
// Calculate max distance for interpolated votes
if (use_interpolation_ && max_distance < model_scene_corrs_->at (i).distance)
{
max_distance = model_scene_corrs_->at (i).distance;
}
}
// Hough Voting
hough_space_.reset (new pcl::recognition::HoughSpace3D (d_min, bin_size, d_max));
for (int i = 0; i < n_matches; ++i)
{
double weight = 1.0;
if (use_distance_weight_ && max_distance != 0)
{
weight = 1.0 - (model_scene_corrs_->at (i).distance / max_distance);
}
if (use_interpolation_)
{
hough_space_->voteInt (scene_votes[i], weight, i);
}
else
{
hough_space_->vote (scene_votes[i], weight, i);
}
}
hough_space_initialized_ = true;
return (true);
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> void
pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::clusterCorrespondences (std::vector<Correspondences> &model_instances)
{
model_instances.clear ();
found_transformations_.clear ();
if (!hough_space_initialized_ && !houghVoting ())
{
return;
}
// Finding max bins and voters
std::vector<double> max_values;
std::vector<std::vector<int> > max_ids;
hough_space_->findMaxima (hough_threshold_, max_values, max_ids);
// Insert maximas into result vector, after Ransac correspondence rejection
// 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 (hough_bin_size_);
corr_rejector.setInputSource (input_);
corr_rejector.setInputTarget (temp_scene_cloud_ptr);
for (std::size_t j = 0; j < max_values.size (); ++j)
{
Correspondences temp_corrs, filtered_corrs;
for (const int &i : max_ids[j])
{
temp_corrs.push_back (model_scene_corrs_->at (i));
}
// 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, typename PointModelRfT, typename PointSceneRfT> bool
//pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::getTransformMatrix (const PointCloudConstPtr &scene_cloud, const Correspondences &corrs, Eigen::Matrix4f &transform)
//{
// std::vector<int> model_indices;
// std::vector<int> scene_indices;
// pcl::registration::getQueryIndices (corrs, model_indices);
// pcl::registration::getMatchIndices (corrs, scene_indices);
//
// typename pcl::SampleConsensusModelRegistration<PointModelT>::Ptr model (new pcl::SampleConsensusModelRegistration<PointModelT> (input_, model_indices));
// model->setInputTarget (scene_cloud, scene_indices);
//
// pcl::RandomSampleConsensus<PointModelT> ransac (model);
// ransac.setDistanceThreshold (hough_bin_size_);
// ransac.setMaxIterations (10000);
// if (!ransac.computeModel ())
// return (false);
//
// // Transform model coefficients from vectorXf to matrix4f
// Eigen::VectorXf coeffs;
// ransac.getModelCoefficients (coeffs);
//
// transform.row (0) = coeffs.segment<4> (0);
// transform.row (1) = coeffs.segment<4> (4);
// transform.row (2) = coeffs.segment<4> (8);
// transform.row (3) = coeffs.segment<4> (12);
//
// return (true);
//}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointModelT, typename PointSceneT, typename PointModelRfT, typename PointSceneRfT> bool
pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::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, typename PointModelRfT, typename PointSceneRfT> bool
pcl::Hough3DGrouping<PointModelT, PointSceneT, PointModelRfT, PointSceneRfT>::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::Hough3DGrouping::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_;
//// 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);
//for (std::size_t i = 0; i < model_instances.size (); ++i)
//{
// Eigen::Matrix4f curr_transf;
// if (getTransformMatrix (temp_scene_cloud_ptr, model_instances[i], curr_transf))
// transformations.push_back (curr_transf);
//}
this->deinitCompute ();
return (!transformations.empty());
}
#define PCL_INSTANTIATE_Hough3DGrouping(T,ST,RFT,SRFT) template class PCL_EXPORTS pcl::Hough3DGrouping<T,ST,RFT,SRFT>;
#endif // PCL_RECOGNITION_HOUGH_3D_IMPL_H_
pcl::BOARDLocalReferenceFrameEstimation
BOARDLocalReferenceFrameEstimation implements the BOrder Aware Repeatable Directions algorithm for lo...
Definition board.h:59
pcl::BOARDLocalReferenceFrameEstimation::setFindHoles
void setFindHoles(bool find_holes)
Sets whether holes in the margin of the support, for each point, are searched and accounted for in th...
Definition board.h:102
pcl::FeatureFromNormals::setInputNormals
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition feature.h:339
pcl::Feature::setRadiusSearch
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
Definition feature.h:198
pcl::Feature::setKSearch
void setKSearch(int k)
Set the number of k nearest neighbors to use for the feature estimation.
Definition feature.h:184
pcl::Feature::compute
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition feature.hpp:194
pcl::Hough3DGrouping::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 hough_3d.hpp:334
pcl::Hough3DGrouping::ModelRfCloudPtr
typename ModelRfCloud::Ptr ModelRfCloudPtr
Definition hough_3d.h:152
pcl::Hough3DGrouping::houghVoting
bool houghVoting()
The Hough space voting procedure.
Definition hough_3d.hpp:138
pcl::Hough3DGrouping::train
bool train()
Call this function after setting the input, the input_rf and the hough_bin_size parameters to perform...
Definition hough_3d.hpp:85
pcl::Hough3DGrouping::ModelRfCloud
pcl::PointCloud< PointModelRfT > ModelRfCloud
Definition hough_3d.h:151
pcl::Hough3DGrouping::computeRf
void computeRf(const typename pcl::PointCloud< PointType >::ConstPtr &input, pcl::PointCloud< PointRfType > &rf)
Computes the reference frame for an input cloud.
Definition hough_3d.hpp:55
pcl::Hough3DGrouping::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition hough_3d.h:160
pcl::Hough3DGrouping::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 hough_3d.hpp:259
pcl::NormalEstimation
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
Definition normal_3d.h:244
pcl::NormalEstimation::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition normal_3d.h:328
pcl::PCLBase::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:173
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition point_cloud.h:413
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition point_cloud.h:414
pcl::recognition::HoughSpace3D
HoughSpace3D is a 3D voting space.
Definition hough_3d.h:58
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