Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
trimmed_icp.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2012, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 * $Id$
37 *
38 */
39
40/*
41 * trimmed_icp.h
42 *
43 * Created on: Mar 10, 2013
44 * Author: papazov
45 */
46
47#pragma once
48
49#include <pcl/registration/transformation_estimation_svd.h>
50#include <pcl/kdtree/kdtree_flann.h>
51#include <pcl/correspondence.h>
52#include <pcl/point_cloud.h>
53#include <pcl/pcl_exports.h>
54#include <limits>
55#include <pcl/recognition/ransac_based/auxiliary.h>
56
57namespace pcl
58{
59 namespace recognition
60 {
61 template<typename PointT, typename Scalar>
62 class PCL_EXPORTS TrimmedICP: public pcl::registration::TransformationEstimationSVD<PointT, PointT, Scalar>
63 {
64 public:
67
68 using Matrix4 = typename Eigen::Matrix<Scalar, 4, 4>;
69
70 public:
72 : new_to_old_energy_ratio_ (0.99f)
73 {}
74
76 {}
77
78 /** \brief Call this method before calling align().
79 *
80 * \param[in] target is target point cloud. The method builds a kd-tree based on 'target' for performing fast closest point search.
81 * The source point cloud will be registered to 'target' (see align() method).
82 * */
83 inline void
85 {
86 target_points_ = target;
87 kdtree_.setInputCloud (target);
88 }
89
90 /** \brief The method performs trimmed ICP, i.e., it rigidly registers the source to the target (passed to the init() method).
91 *
92 * \param[in] source_points is the point cloud to be registered to the target.
93 * \param[in] num_source_points_to_use gives the number of closest source points taken into account for registration. By closest
94 * source points we mean the source points closest to the target. These points are computed anew at each iteration.
95 * \param[in,out] guess_and_result is the estimated rigid transform. IMPORTANT: this matrix is also taken as the initial guess
96 * for the alignment. If there is no guess, set the matrix to identity!
97 * */
98 inline void
100 {
102
104 {
105 printf ("WARNING in 'TrimmedICP::%s()': the user-defined number of source points of interest is greater or equal to "
106 "the total number of source points. Trimmed ICP will work correctly but won't be very efficient. Either set "
107 "the number of source points to use to a lower value or use standard ICP.\n", __func__);
109 }
110
111 // These are vectors containing source to target correspondences
113
114 // Some variables for the closest point search
117 std::vector<float> sqr_dist_to_target (1);
118 float old_energy, energy = std::numeric_limits<float>::max ();
119
120// printf ("\nalign\n");
121
122 do
123 {
124 // Update the correspondences
125 for ( int i = 0 ; i < num_source_points ; ++i )
126 {
127 // Transform the i-th source point based on the current transform matrix
129
130 // Perform the closest point search
131 kdtree_.nearestKSearch (transformed_source_point, 1, target_index, sqr_dist_to_target);
132
133 // Update the i-th correspondence
134 full_src_to_tgt[i].index_query = i;
135 full_src_to_tgt[i].index_match = target_index[0];
136 full_src_to_tgt[i].distance = sqr_dist_to_target[0];
137 }
138
139 // Sort in ascending order according to the squared distance
140 std::sort (full_src_to_tgt.begin (), full_src_to_tgt.end (), TrimmedICP::compareCorrespondences);
141
143 energy = 0.0f;
144
145 // Now, setup the trimmed correspondences used for the transform estimation
146 for ( int i = 0 ; i < num_trimmed_source_points ; ++i )
147 {
148 trimmed_src_to_tgt[i].index_query = full_src_to_tgt[i].index_query;
149 trimmed_src_to_tgt[i].index_match = full_src_to_tgt[i].index_match;
150 energy += full_src_to_tgt[i].distance;
151 }
152
153 this->estimateRigidTransformation (source_points, *target_points_, trimmed_src_to_tgt, guess_and_result);
154
155// printf ("energy = %f, energy diff. = %f, ratio = %f\n", energy, old_energy - energy, energy/old_energy);
156 }
157 while ( energy/old_energy < new_to_old_energy_ratio_ ); // iterate if enough progress
158
159// printf ("\n");
160 }
161
162 inline void
164 {
165 if ( ratio >= 1 )
166 new_to_old_energy_ratio_ = 0.99f;
167 else
168 new_to_old_energy_ratio_ = ratio;
169 }
170
171 protected:
172 static inline bool
174 {
175 return a.distance < b.distance;
176 }
177
178 protected:
182 };
183 } // namespace recognition
184} // namespace pcl
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
shared_ptr< const PointCloud< PointT > > ConstPtr
pcl::KdTreeFLANN< PointT > kdtree_
PointCloudConstPtr target_points_
typename PointCloud::ConstPtr PointCloudConstPtr
Definition trimmed_icp.h:66
void init(const PointCloudConstPtr &target)
Call this method before calling align().
Definition trimmed_icp.h:84
static bool compareCorrespondences(const pcl::Correspondence &a, const pcl::Correspondence &b)
typename Eigen::Matrix< Scalar, 4, 4 > Matrix4
Definition trimmed_icp.h:68
void setNewToOldEnergyRatio(float ratio)
void align(const PointCloud &source_points, int num_source_points_to_use, Matrix4 &guess_and_result) const
The method performs trimmed ICP, i.e., it rigidly registers the source to the target (passed to the i...
Definition trimmed_icp.h:99
TransformationEstimationSVD implements SVD-based estimation of the transformation aligning the given ...
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Correspondence represents a match between two entities (e.g., points, descriptors,...
A point structure representing Euclidean xyz coordinates.