Point Cloud Library (PCL) 1.12.1
ppf_registration.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2011, Alexandru-Eugen Ichim
6 * Willow Garage, Inc
7 * Copyright (c) 2012-, Open Perception, Inc.
8 *
9 * All rights reserved.
10 *
11 * Redistribution and use in source and binary forms, with or without
12 * modification, are permitted provided that the following conditions
13 * are met:
14 *
15 * * Redistributions of source code must retain the above copyright
16 * notice, this list of conditions and the following disclaimer.
17 * * Redistributions in binary form must reproduce the above
18 * copyright notice, this list of conditions and the following
19 * disclaimer in the documentation and/or other materials provided
20 * with the distribution.
21 * * Neither the name of the copyright holder(s) nor the names of its
22 * contributors may be used to endorse or promote products derived
23 * from this software without specific prior written permission.
24 *
25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36 * POSSIBILITY OF SUCH DAMAGE.
37 *
38 * $Id$
39 *
40 */
41
42#ifndef PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
43#define PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
44
45#include <pcl/common/transforms.h>
46#include <pcl/features/pfh.h>
47#include <pcl/features/pfh_tools.h> // for computePairFeatures
48#include <pcl/features/ppf.h>
49#include <pcl/registration/ppf_registration.h>
50//////////////////////////////////////////////////////////////////////////////////////////////
51template <typename PointSource, typename PointTarget>
52void
54 const PointCloudTargetConstPtr& cloud)
55{
57
58 scene_search_tree_ =
60 scene_search_tree_->setInputCloud(target_);
61}
62
63//////////////////////////////////////////////////////////////////////////////////////////////
64template <typename PointSource, typename PointTarget>
65void
67 PointCloudSource& output, const Eigen::Matrix4f& guess)
68{
69 if (!search_method_) {
70 PCL_ERROR("[pcl::PPFRegistration::computeTransformation] Search method not set - "
71 "skipping computeTransformation!\n");
72 return;
73 }
74
75 if (guess != Eigen::Matrix4f::Identity()) {
76 PCL_ERROR("[pcl::PPFRegistration::computeTransformation] setting initial transform "
77 "(guess) not implemented!\n");
78 }
79
80 const auto aux_size = static_cast<std::size_t>(
81 std::floor(2 * M_PI / search_method_->getAngleDiscretizationStep()));
82
83 const std::vector<unsigned int> tmp_vec(aux_size, 0);
84 std::vector<std::vector<unsigned int>> accumulator_array(input_->size(), tmp_vec);
85
86 PCL_INFO("Accumulator array size: %u x %u.\n",
87 accumulator_array.size(),
88 accumulator_array.back().size());
89
90 PoseWithVotesList voted_poses;
91 // Consider every <scene_reference_point_sampling_rate>-th point as the reference
92 // point => fix s_r
93 float f1, f2, f3, f4;
94 for (index_t scene_reference_index = 0;
95 scene_reference_index < static_cast<index_t>(target_->size());
96 scene_reference_index += scene_reference_point_sampling_rate_) {
97 Eigen::Vector3f scene_reference_point =
98 (*target_)[scene_reference_index].getVector3fMap(),
99 scene_reference_normal =
100 (*target_)[scene_reference_index].getNormalVector3fMap();
101
102 float rotation_angle_sg =
103 std::acos(scene_reference_normal.dot(Eigen::Vector3f::UnitX()));
104 bool parallel_to_x_sg =
105 (scene_reference_normal.y() == 0.0f && scene_reference_normal.z() == 0.0f);
106 Eigen::Vector3f rotation_axis_sg =
107 (parallel_to_x_sg)
108 ? (Eigen::Vector3f::UnitY())
109 : (scene_reference_normal.cross(Eigen::Vector3f::UnitX()).normalized());
110 Eigen::AngleAxisf rotation_sg(rotation_angle_sg, rotation_axis_sg);
111 Eigen::Affine3f transform_sg(
112 Eigen::Translation3f(rotation_sg * ((-1) * scene_reference_point)) *
113 rotation_sg);
114
115 // For every other point in the scene => now have pair (s_r, s_i) fixed
116 pcl::Indices indices;
117 std::vector<float> distances;
118 scene_search_tree_->radiusSearch((*target_)[scene_reference_index],
119 search_method_->getModelDiameter() / 2,
120 indices,
121 distances);
122 for (const auto& scene_point_index : indices)
123 // for(std::size_t i = 0; i < target_->size (); ++i)
124 {
125 // size_t scene_point_index = i;
126 if (scene_reference_index != scene_point_index) {
127 if (/*pcl::computePPFPairFeature*/ pcl::computePairFeatures(
128 (*target_)[scene_reference_index].getVector4fMap(),
129 (*target_)[scene_reference_index].getNormalVector4fMap(),
130 (*target_)[scene_point_index].getVector4fMap(),
131 (*target_)[scene_point_index].getNormalVector4fMap(),
132 f1,
133 f2,
134 f3,
135 f4)) {
136 std::vector<std::pair<std::size_t, std::size_t>> nearest_indices;
137 search_method_->nearestNeighborSearch(f1, f2, f3, f4, nearest_indices);
138
139 // Compute alpha_s angle
140 Eigen::Vector3f scene_point = (*target_)[scene_point_index].getVector3fMap();
141
142 Eigen::Vector3f scene_point_transformed = transform_sg * scene_point;
143 float alpha_s =
144 std::atan2(-scene_point_transformed(2), scene_point_transformed(1));
145 if (std::sin(alpha_s) * scene_point_transformed(2) < 0.0f)
146 alpha_s *= (-1);
147 alpha_s *= (-1);
148
149 // Go through point pairs in the model with the same discretized feature
150 for (const auto& nearest_index : nearest_indices) {
151 std::size_t model_reference_index = nearest_index.first;
152 std::size_t model_point_index = nearest_index.second;
153 // Calculate angle alpha = alpha_m - alpha_s
154 float alpha =
155 search_method_->alpha_m_[model_reference_index][model_point_index] -
156 alpha_s;
157 if (alpha < -M_PI) {
158 alpha += (2 * M_PI);
159 }
160 else if (alpha > M_PI) {
161 alpha -= (2 * M_PI);
162 }
163 unsigned int alpha_discretized = static_cast<unsigned int>(std::floor(
164 (alpha + M_PI) / search_method_->getAngleDiscretizationStep()));
165 accumulator_array[model_reference_index][alpha_discretized]++;
166 }
167 }
168 else
169 PCL_ERROR("[pcl::PPFRegistration::computeTransformation] Computing pair "
170 "feature vector between points %u and %u went wrong.\n",
171 scene_reference_index,
172 scene_point_index);
173 }
174 }
175
176 std::size_t max_votes_i = 0, max_votes_j = 0;
177 unsigned int max_votes = 0;
178
179 for (std::size_t i = 0; i < accumulator_array.size(); ++i)
180 for (std::size_t j = 0; j < accumulator_array.back().size(); ++j) {
181 if (accumulator_array[i][j] > max_votes) {
182 max_votes = accumulator_array[i][j];
183 max_votes_i = i;
184 max_votes_j = j;
185 }
186 // Reset accumulator_array for the next set of iterations with a new scene
187 // reference point
188 accumulator_array[i][j] = 0;
189 }
190
191 Eigen::Vector3f model_reference_point = (*input_)[max_votes_i].getVector3fMap(),
192 model_reference_normal =
193 (*input_)[max_votes_i].getNormalVector3fMap();
194 float rotation_angle_mg =
195 std::acos(model_reference_normal.dot(Eigen::Vector3f::UnitX()));
196 bool parallel_to_x_mg =
197 (model_reference_normal.y() == 0.0f && model_reference_normal.z() == 0.0f);
198 Eigen::Vector3f rotation_axis_mg =
199 (parallel_to_x_mg)
200 ? (Eigen::Vector3f::UnitY())
201 : (model_reference_normal.cross(Eigen::Vector3f::UnitX()).normalized());
202 Eigen::AngleAxisf rotation_mg(rotation_angle_mg, rotation_axis_mg);
203 Eigen::Affine3f transform_mg(
204 Eigen::Translation3f(rotation_mg * ((-1) * model_reference_point)) *
205 rotation_mg);
206 Eigen::Affine3f max_transform =
207 transform_sg.inverse() *
208 Eigen::AngleAxisf((static_cast<float>(max_votes_j + 0.5) *
209 search_method_->getAngleDiscretizationStep() -
210 M_PI),
211 Eigen::Vector3f::UnitX()) *
212 transform_mg;
213
214 voted_poses.push_back(PoseWithVotes(max_transform, max_votes));
215 }
216 PCL_DEBUG("Done with the Hough Transform ...\n");
217
218 // Cluster poses for filtering out outliers and obtaining more precise results
219 PoseWithVotesList results;
220 clusterPoses(voted_poses, results);
221
222 pcl::transformPointCloud(*input_, output, results.front().pose);
223
224 transformation_ = final_transformation_ = results.front().pose.matrix();
225 converged_ = true;
226}
227
228//////////////////////////////////////////////////////////////////////////////////////////////
229template <typename PointSource, typename PointTarget>
230void
234{
235 PCL_INFO("Clustering poses ...\n");
236 // Start off by sorting the poses by the number of votes
237 sort(poses.begin(), poses.end(), poseWithVotesCompareFunction);
238
239 std::vector<PoseWithVotesList> clusters;
240 std::vector<std::pair<std::size_t, unsigned int>> cluster_votes;
241 for (std::size_t poses_i = 0; poses_i < poses.size(); ++poses_i) {
242 bool found_cluster = false;
243 for (std::size_t clusters_i = 0; clusters_i < clusters.size(); ++clusters_i) {
244 if (posesWithinErrorBounds(poses[poses_i].pose,
245 clusters[clusters_i].front().pose)) {
246 found_cluster = true;
247 clusters[clusters_i].push_back(poses[poses_i]);
248 cluster_votes[clusters_i].second += poses[poses_i].votes;
249 break;
250 }
251 }
252
253 if (!found_cluster) {
254 // Create a new cluster with the current pose
255 PoseWithVotesList new_cluster;
256 new_cluster.push_back(poses[poses_i]);
257 clusters.push_back(new_cluster);
258 cluster_votes.push_back(std::pair<std::size_t, unsigned int>(
259 clusters.size() - 1, poses[poses_i].votes));
260 }
261 }
262
263 // Sort clusters by total number of votes
264 std::sort(cluster_votes.begin(), cluster_votes.end(), clusterVotesCompareFunction);
265 // Compute pose average and put them in result vector
266 /// @todo some kind of threshold for determining whether a cluster has enough votes or
267 /// not... now just taking the first three clusters
268 result.clear();
269 std::size_t max_clusters = (clusters.size() < 3) ? clusters.size() : 3;
270 for (std::size_t cluster_i = 0; cluster_i < max_clusters; ++cluster_i) {
271 PCL_INFO("Winning cluster has #votes: %d and #poses voted: %d.\n",
272 cluster_votes[cluster_i].second,
273 clusters[cluster_votes[cluster_i].first].size());
274 Eigen::Vector3f translation_average(0.0, 0.0, 0.0);
275 Eigen::Vector4f rotation_average(0.0, 0.0, 0.0, 0.0);
276 for (typename PoseWithVotesList::iterator v_it =
277 clusters[cluster_votes[cluster_i].first].begin();
278 v_it != clusters[cluster_votes[cluster_i].first].end();
279 ++v_it) {
280 translation_average += v_it->pose.translation();
281 /// averaging rotations by just averaging the quaternions in 4D space - reference
282 /// "On Averaging Rotations" by CLAUS GRAMKOW
283 rotation_average += Eigen::Quaternionf(v_it->pose.rotation()).coeffs();
284 }
285
286 translation_average /=
287 static_cast<float>(clusters[cluster_votes[cluster_i].first].size());
288 rotation_average /=
289 static_cast<float>(clusters[cluster_votes[cluster_i].first].size());
290
291 Eigen::Affine3f transform_average;
292 transform_average.translation().matrix() = translation_average;
293 transform_average.linear().matrix() =
294 Eigen::Quaternionf(rotation_average).normalized().toRotationMatrix();
295
296 result.push_back(PoseWithVotes(transform_average, cluster_votes[cluster_i].second));
297 }
298}
299
300//////////////////////////////////////////////////////////////////////////////////////////////
301template <typename PointSource, typename PointTarget>
302bool
304 Eigen::Affine3f& pose1, Eigen::Affine3f& pose2)
305{
306 float position_diff = (pose1.translation() - pose2.translation()).norm();
307 Eigen::AngleAxisf rotation_diff_mat(
308 (pose1.rotation().inverse().lazyProduct(pose2.rotation()).eval()));
309
310 float rotation_diff_angle = std::abs(rotation_diff_mat.angle());
311
312 return (position_diff < clustering_position_diff_threshold_ &&
313 rotation_diff_angle < clustering_rotation_diff_threshold_);
314}
315
316//////////////////////////////////////////////////////////////////////////////////////////////
317template <typename PointSource, typename PointTarget>
318bool
322{
323 return (a.votes > b.votes);
324}
325
326//////////////////////////////////////////////////////////////////////////////////////////////
327template <typename PointSource, typename PointTarget>
328bool
330 const std::pair<std::size_t, unsigned int>& a,
331 const std::pair<std::size_t, unsigned int>& b)
332{
333 return (a.second > b.second);
334}
335
336//#define PCL_INSTANTIATE_PPFRegistration(PointSource,PointTarget) template class
337// PCL_EXPORTS pcl::PPFRegistration<PointSource, PointTarget>;
338
339#endif // PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
Definition: kdtree_flann.h:151
Class that registers two point clouds based on their sets of PPFSignatures.
std::vector< PoseWithVotes, Eigen::aligned_allocator< PoseWithVotes > > PoseWithVotesList
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
void setInputTarget(const PointCloudTargetConstPtr &cloud) override
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
Registration represents the base registration class for general purpose, ICP-like methods.
Definition: registration.h:57
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
Definition: transforms.hpp:221
PCL_EXPORTS bool computePairFeatures(const Eigen::Vector4f &p1, const Eigen::Vector4f &n1, const Eigen::Vector4f &p2, const Eigen::Vector4f &n2, float &f1, float &f2, float &f3, float &f4)
Compute the 4-tuple representation containing the three angles and one distance between two points re...
__device__ __forceinline__ float3 normalized(const float3 &v)
Definition: utils.hpp:101
__device__ __host__ __forceinline__ float norm(const float3 &v1, const float3 &v2)
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
#define M_PI
Definition: pcl_macros.h:201
Structure for storing a pose (represented as an Eigen::Affine3f) and an integer for counting votes.