381 lines
17 KiB
C++
381 lines
17 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2011, Alexandru-Eugen Ichim
|
|
* Willow Garage, Inc
|
|
* Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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_REGISTRATION_IMPL_PPF_REGISTRATION_H_
|
|
#define PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
|
|
|
|
#include <pcl/common/transforms.h>
|
|
#include <pcl/features/pfh.h>
|
|
#include <pcl/features/pfh_tools.h> // for computePairFeatures
|
|
#include <pcl/features/ppf.h>
|
|
#include <pcl/registration/ppf_registration.h>
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointSource, typename PointTarget>
|
|
void
|
|
pcl::PPFRegistration<PointSource, PointTarget>::setInputTarget(
|
|
const PointCloudTargetConstPtr& cloud)
|
|
{
|
|
Registration<PointSource, PointTarget>::setInputTarget(cloud);
|
|
|
|
scene_search_tree_ =
|
|
typename pcl::KdTreeFLANN<PointTarget>::Ptr(new pcl::KdTreeFLANN<PointTarget>);
|
|
scene_search_tree_->setInputCloud(target_);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointSource, typename PointTarget>
|
|
void
|
|
pcl::PPFRegistration<PointSource, PointTarget>::computeTransformation(
|
|
PointCloudSource& output, const Eigen::Matrix4f& guess)
|
|
{
|
|
if (!search_method_) {
|
|
PCL_ERROR("[pcl::PPFRegistration::computeTransformation] Search method not set - "
|
|
"skipping computeTransformation!\n");
|
|
return;
|
|
}
|
|
|
|
if (guess != Eigen::Matrix4f::Identity()) {
|
|
PCL_ERROR("[pcl::PPFRegistration::computeTransformation] setting initial transform "
|
|
"(guess) not implemented!\n");
|
|
}
|
|
|
|
const auto aux_size = static_cast<std::size_t>(
|
|
std::ceil(2 * M_PI / search_method_->getAngleDiscretizationStep()));
|
|
if (std::abs(std::round(2 * M_PI / search_method_->getAngleDiscretizationStep()) -
|
|
2 * M_PI / search_method_->getAngleDiscretizationStep()) > 0.1) {
|
|
PCL_WARN("[pcl::PPFRegistration::computeTransformation] The chosen angle "
|
|
"discretization step (%g) does not result in a uniform discretization. "
|
|
"Consider using e.g. 2pi/%zu or 2pi/%zu\n",
|
|
search_method_->getAngleDiscretizationStep(),
|
|
aux_size - 1,
|
|
aux_size);
|
|
}
|
|
|
|
const std::vector<unsigned int> tmp_vec(aux_size, 0);
|
|
std::vector<std::vector<unsigned int>> accumulator_array(input_->size(), tmp_vec);
|
|
|
|
PCL_DEBUG("[PPFRegistration] Accumulator array size: %u x %u.\n",
|
|
accumulator_array.size(),
|
|
accumulator_array.back().size());
|
|
|
|
PoseWithVotesList voted_poses;
|
|
// Consider every <scene_reference_point_sampling_rate>-th point as the reference
|
|
// point => fix s_r
|
|
float f1, f2, f3, f4;
|
|
for (index_t scene_reference_index = 0;
|
|
scene_reference_index < static_cast<index_t>(target_->size());
|
|
scene_reference_index += scene_reference_point_sampling_rate_) {
|
|
Eigen::Vector3f scene_reference_point =
|
|
(*target_)[scene_reference_index].getVector3fMap(),
|
|
scene_reference_normal =
|
|
(*target_)[scene_reference_index].getNormalVector3fMap();
|
|
|
|
float rotation_angle_sg =
|
|
std::acos(scene_reference_normal.dot(Eigen::Vector3f::UnitX()));
|
|
bool parallel_to_x_sg =
|
|
(scene_reference_normal.y() == 0.0f && scene_reference_normal.z() == 0.0f);
|
|
Eigen::Vector3f rotation_axis_sg =
|
|
(parallel_to_x_sg)
|
|
? (Eigen::Vector3f::UnitY())
|
|
: (scene_reference_normal.cross(Eigen::Vector3f::UnitX()).normalized());
|
|
Eigen::AngleAxisf rotation_sg(rotation_angle_sg, rotation_axis_sg);
|
|
Eigen::Affine3f transform_sg(
|
|
Eigen::Translation3f(rotation_sg * ((-1) * scene_reference_point)) *
|
|
rotation_sg);
|
|
|
|
// For every other point in the scene => now have pair (s_r, s_i) fixed
|
|
pcl::Indices indices;
|
|
std::vector<float> distances;
|
|
scene_search_tree_->radiusSearch((*target_)[scene_reference_index],
|
|
search_method_->getModelDiameter() / 2,
|
|
indices,
|
|
distances);
|
|
for (const auto& scene_point_index : indices)
|
|
// for(std::size_t i = 0; i < target_->size (); ++i)
|
|
{
|
|
// size_t scene_point_index = i;
|
|
if (scene_reference_index != scene_point_index) {
|
|
if (/*pcl::computePPFPairFeature*/ pcl::computePairFeatures(
|
|
(*target_)[scene_reference_index].getVector4fMap(),
|
|
(*target_)[scene_reference_index].getNormalVector4fMap(),
|
|
(*target_)[scene_point_index].getVector4fMap(),
|
|
(*target_)[scene_point_index].getNormalVector4fMap(),
|
|
f1,
|
|
f2,
|
|
f3,
|
|
f4)) {
|
|
std::vector<std::pair<std::size_t, std::size_t>> nearest_indices;
|
|
search_method_->nearestNeighborSearch(f1, f2, f3, f4, nearest_indices);
|
|
|
|
// Compute alpha_s angle
|
|
const Eigen::Vector3f scene_point =
|
|
(*target_)[scene_point_index].getVector3fMap();
|
|
|
|
const Eigen::Vector3f scene_point_transformed = transform_sg * scene_point;
|
|
float alpha_s =
|
|
std::atan2(-scene_point_transformed(2), scene_point_transformed(1));
|
|
if (std::sin(alpha_s) * scene_point_transformed(2) < 0.0f)
|
|
alpha_s *= (-1);
|
|
alpha_s *= (-1);
|
|
|
|
// Go through point pairs in the model with the same discretized feature
|
|
for (const auto& nearest_index : nearest_indices) {
|
|
std::size_t model_reference_index = nearest_index.first;
|
|
std::size_t model_point_index = nearest_index.second;
|
|
// Calculate angle alpha = alpha_m - alpha_s
|
|
float alpha =
|
|
search_method_->alpha_m_[model_reference_index][model_point_index] -
|
|
alpha_s;
|
|
if (alpha < -M_PI) {
|
|
alpha += (2 * M_PI);
|
|
}
|
|
else if (alpha > M_PI) {
|
|
alpha -= (2 * M_PI);
|
|
}
|
|
auto alpha_discretized = static_cast<unsigned int>(std::floor(
|
|
(alpha + M_PI) / search_method_->getAngleDiscretizationStep()));
|
|
accumulator_array[model_reference_index][alpha_discretized]++;
|
|
}
|
|
}
|
|
else
|
|
PCL_ERROR("[pcl::PPFRegistration::computeTransformation] Computing pair "
|
|
"feature vector between points %u and %u went wrong.\n",
|
|
scene_reference_index,
|
|
scene_point_index);
|
|
}
|
|
}
|
|
|
|
// the paper says: "For stability reasons, all peaks that received a certain amount
|
|
// of votes relative to the maximum peak are used." No specific value is mentioned,
|
|
// but 90% seems good
|
|
unsigned int max_votes = 0;
|
|
const std::size_t size_i = accumulator_array.size(),
|
|
size_j = accumulator_array.back().size();
|
|
for (std::size_t i = 0; i < size_i; ++i)
|
|
for (std::size_t j = 0; j < size_j; ++j) {
|
|
if (accumulator_array[i][j] > max_votes)
|
|
max_votes = accumulator_array[i][j];
|
|
}
|
|
max_votes *= 0.9;
|
|
for (std::size_t i = 0; i < size_i; ++i)
|
|
for (std::size_t j = 0; j < size_j; ++j) {
|
|
if (accumulator_array[i][j] >= max_votes) {
|
|
const Eigen::Vector3f model_reference_point = (*input_)[i].getVector3fMap(),
|
|
model_reference_normal =
|
|
(*input_)[i].getNormalVector3fMap();
|
|
const float rotation_angle_mg =
|
|
std::acos(model_reference_normal.dot(Eigen::Vector3f::UnitX()));
|
|
const bool parallel_to_x_mg = (model_reference_normal.y() == 0.0f &&
|
|
model_reference_normal.z() == 0.0f);
|
|
const Eigen::Vector3f rotation_axis_mg =
|
|
(parallel_to_x_mg)
|
|
? (Eigen::Vector3f::UnitY())
|
|
: (model_reference_normal.cross(Eigen::Vector3f::UnitX())
|
|
.normalized());
|
|
const Eigen::AngleAxisf rotation_mg(rotation_angle_mg, rotation_axis_mg);
|
|
const Eigen::Affine3f transform_mg(
|
|
Eigen::Translation3f(rotation_mg * ((-1) * model_reference_point)) *
|
|
rotation_mg);
|
|
const Eigen::Affine3f max_transform =
|
|
transform_sg.inverse() *
|
|
Eigen::AngleAxisf((static_cast<float>(j + 0.5) *
|
|
search_method_->getAngleDiscretizationStep() -
|
|
M_PI),
|
|
Eigen::Vector3f::UnitX()) *
|
|
transform_mg;
|
|
|
|
voted_poses.push_back(PoseWithVotes(max_transform, accumulator_array[i][j]));
|
|
}
|
|
// Reset accumulator_array for the next set of iterations with a new scene
|
|
// reference point
|
|
accumulator_array[i][j] = 0;
|
|
}
|
|
}
|
|
PCL_DEBUG("[PPFRegistration] Done with the Hough Transform ...\n");
|
|
|
|
// Cluster poses for filtering out outliers and obtaining more precise results
|
|
clusterPoses(voted_poses, best_pose_candidates);
|
|
|
|
pcl::transformPointCloud(*input_, output, best_pose_candidates.front().pose);
|
|
|
|
transformation_ = final_transformation_ = best_pose_candidates.front().pose.matrix();
|
|
converged_ = true;
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointSource, typename PointTarget>
|
|
void
|
|
pcl::PPFRegistration<PointSource, PointTarget>::clusterPoses(
|
|
typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotesList& poses,
|
|
typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotesList& result)
|
|
{
|
|
PCL_DEBUG("[PPFRegistration] Clustering poses (initially got %zu poses)\n",
|
|
poses.size());
|
|
// Start off by sorting the poses by the number of votes
|
|
sort(poses.begin(), poses.end(), poseWithVotesCompareFunction);
|
|
|
|
std::vector<PoseWithVotesList> clusters;
|
|
std::vector<std::pair<std::size_t, unsigned int>> cluster_votes;
|
|
for (std::size_t poses_i = 0; poses_i < poses.size(); ++poses_i) {
|
|
bool found_cluster = false;
|
|
float lowest_position_diff = std::numeric_limits<float>::max(),
|
|
lowest_rotation_diff_angle = std::numeric_limits<float>::max();
|
|
std::size_t best_cluster = 0;
|
|
for (std::size_t clusters_i = 0; clusters_i < clusters.size(); ++clusters_i) {
|
|
// if a pose can be added to more than one cluster (posesWithinErrorBounds returns
|
|
// true), then add it to the one where position and rotation difference are
|
|
// smallest
|
|
float position_diff, rotation_diff_angle;
|
|
if (posesWithinErrorBounds(poses[poses_i].pose,
|
|
clusters[clusters_i].front().pose,
|
|
position_diff,
|
|
rotation_diff_angle)) {
|
|
if (!found_cluster) {
|
|
found_cluster = true;
|
|
best_cluster = clusters_i;
|
|
lowest_position_diff = position_diff;
|
|
lowest_rotation_diff_angle = rotation_diff_angle;
|
|
}
|
|
else if (position_diff < lowest_position_diff &&
|
|
rotation_diff_angle < lowest_rotation_diff_angle) {
|
|
best_cluster = clusters_i;
|
|
lowest_position_diff = position_diff;
|
|
lowest_rotation_diff_angle = rotation_diff_angle;
|
|
}
|
|
}
|
|
}
|
|
if (found_cluster) {
|
|
clusters[best_cluster].push_back(poses[poses_i]);
|
|
cluster_votes[best_cluster].second += poses[poses_i].votes;
|
|
}
|
|
else {
|
|
// Create a new cluster with the current pose
|
|
PoseWithVotesList new_cluster;
|
|
new_cluster.push_back(poses[poses_i]);
|
|
clusters.push_back(new_cluster);
|
|
cluster_votes.push_back(std::pair<std::size_t, unsigned int>(
|
|
clusters.size() - 1, poses[poses_i].votes));
|
|
}
|
|
}
|
|
PCL_DEBUG("[PPFRegistration] %zu poses remaining after clustering. Now averaging "
|
|
"each cluster and removing clusters with too few votes.\n",
|
|
clusters.size());
|
|
|
|
// Sort clusters by total number of votes
|
|
std::sort(cluster_votes.begin(), cluster_votes.end(), clusterVotesCompareFunction);
|
|
// Compute pose average and put them in result vector
|
|
result.clear();
|
|
for (std::size_t cluster_i = 0; cluster_i < clusters.size(); ++cluster_i) {
|
|
// Remove all clusters that have less than 10% of the votes of the peak cluster.
|
|
// This way, if there is e.g. one cluster with far more votes than all other
|
|
// clusters, only that one is kept.
|
|
if (cluster_votes[cluster_i].second < 0.1 * cluster_votes[0].second)
|
|
continue;
|
|
PCL_DEBUG("Winning cluster has #votes: %d and #poses voted: %d.\n",
|
|
cluster_votes[cluster_i].second,
|
|
clusters[cluster_votes[cluster_i].first].size());
|
|
Eigen::Vector3f translation_average(0.0, 0.0, 0.0);
|
|
Eigen::Vector4f rotation_average(0.0, 0.0, 0.0, 0.0);
|
|
for (const auto& vote : clusters[cluster_votes[cluster_i].first]) {
|
|
translation_average += vote.pose.translation();
|
|
/// averaging rotations by just averaging the quaternions in 4D space - reference
|
|
/// "On Averaging Rotations" by CLAUS GRAMKOW
|
|
rotation_average += Eigen::Quaternionf(vote.pose.rotation()).coeffs();
|
|
}
|
|
|
|
translation_average /=
|
|
static_cast<float>(clusters[cluster_votes[cluster_i].first].size());
|
|
rotation_average /=
|
|
static_cast<float>(clusters[cluster_votes[cluster_i].first].size());
|
|
|
|
Eigen::Affine3f transform_average;
|
|
transform_average.translation().matrix() = translation_average;
|
|
transform_average.linear().matrix() =
|
|
Eigen::Quaternionf(rotation_average).normalized().toRotationMatrix();
|
|
|
|
result.push_back(PoseWithVotes(transform_average, cluster_votes[cluster_i].second));
|
|
}
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointSource, typename PointTarget>
|
|
bool
|
|
pcl::PPFRegistration<PointSource, PointTarget>::posesWithinErrorBounds(
|
|
Eigen::Affine3f& pose1,
|
|
Eigen::Affine3f& pose2,
|
|
float& position_diff,
|
|
float& rotation_diff_angle)
|
|
{
|
|
position_diff = (pose1.translation() - pose2.translation()).norm();
|
|
Eigen::AngleAxisf rotation_diff_mat(
|
|
(pose1.rotation().inverse().lazyProduct(pose2.rotation()).eval()));
|
|
|
|
rotation_diff_angle = std::abs(rotation_diff_mat.angle());
|
|
|
|
return (position_diff < clustering_position_diff_threshold_ &&
|
|
rotation_diff_angle < clustering_rotation_diff_threshold_);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointSource, typename PointTarget>
|
|
bool
|
|
pcl::PPFRegistration<PointSource, PointTarget>::poseWithVotesCompareFunction(
|
|
const typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotes& a,
|
|
const typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotes& b)
|
|
{
|
|
return (a.votes > b.votes);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////
|
|
template <typename PointSource, typename PointTarget>
|
|
bool
|
|
pcl::PPFRegistration<PointSource, PointTarget>::clusterVotesCompareFunction(
|
|
const std::pair<std::size_t, unsigned int>& a,
|
|
const std::pair<std::size_t, unsigned int>& b)
|
|
{
|
|
return (a.second > b.second);
|
|
}
|
|
|
|
//#define PCL_INSTANTIATE_PPFRegistration(PointSource,PointTarget) template class
|
|
// PCL_EXPORTS pcl::PPFRegistration<PointSource, PointTarget>;
|
|
|
|
#endif // PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
|