util3d_registration.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
29 
32 #include "rtabmap/core/util3d.h"
33 
34 #include <pcl/registration/icp.h>
35 #include <pcl/registration/transformation_estimation_2D.h>
36 #include <pcl/registration/transformation_estimation_svd.h>
37 #include <pcl/sample_consensus/sac_model_registration.h>
38 #include <pcl/sample_consensus/ransac.h>
39 #include <pcl/common/common.h>
41 #include <rtabmap/utilite/UMath.h>
42 
43 namespace rtabmap
44 {
45 
46 namespace util3d
47 {
48 
49 // Get transform from cloud2 to cloud1
51  const pcl::PointCloud<pcl::PointXYZ> & cloud1,
52  const pcl::PointCloud<pcl::PointXYZ> & cloud2)
53 {
54  pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> svd;
55 
56  // Perform the alignment
57  Eigen::Matrix4f matrix;
58  svd.estimateRigidTransformation(cloud1, cloud2, matrix);
60 }
61 
62 // Get transform from cloud2 to cloud1
64  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud1,
65  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
66  double inlierThreshold,
67  int iterations,
68  int refineIterations,
69  double refineSigma,
70  std::vector<int> * inliersOut,
71  cv::Mat * covariance)
72 {
73  //NOTE: this method is a mix of two methods:
74  // - getRemainingCorrespondences() in pcl/registration/impl/correspondence_rejection_sample_consensus.hpp
75  // - refineModel() in pcl/sample_consensus/sac.h
76 
77  if(covariance)
78  {
79  *covariance = cv::Mat::eye(6,6,CV_64FC1);
80  }
82  if(cloud1->size() >=3 && cloud1->size() == cloud2->size())
83  {
84  // RANSAC
85  UDEBUG("iterations=%d inlierThreshold=%f", iterations, inlierThreshold);
86  std::vector<int> source_indices (cloud2->size());
87  std::vector<int> target_indices (cloud1->size());
88 
89  // Copy the query-match indices
90  for (int i = 0; i < (int)cloud1->size(); ++i)
91  {
92  source_indices[i] = i;
93  target_indices[i] = i;
94  }
95 
96  // From the set of correspondences found, attempt to remove outliers
97  // Create the registration model
98  pcl::SampleConsensusModelRegistration<pcl::PointXYZ>::Ptr model;
99  model.reset(new pcl::SampleConsensusModelRegistration<pcl::PointXYZ>(cloud2, source_indices));
100  // Pass the target_indices
101  model->setInputTarget (cloud1, target_indices);
102  // Create a RANSAC model
103  pcl::RandomSampleConsensus<pcl::PointXYZ> sac (model, inlierThreshold);
104  sac.setMaxIterations(iterations);
105 
106  // Compute the set of inliers
107  if(sac.computeModel())
108  {
109  std::vector<int> inliers;
110  Eigen::VectorXf model_coefficients;
111 
112  sac.getInliers(inliers);
113  sac.getModelCoefficients (model_coefficients);
114 
115  if (refineIterations>0)
116  {
117  double error_threshold = inlierThreshold;
118  int refine_iterations = 0;
119  bool inlier_changed = false, oscillating = false;
120  std::vector<int> new_inliers, prev_inliers = inliers;
121  std::vector<size_t> inliers_sizes;
122  Eigen::VectorXf new_model_coefficients = model_coefficients;
123  do
124  {
125  // Optimize the model coefficients
126  model->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients);
127  inliers_sizes.push_back (prev_inliers.size ());
128 
129  // Select the new inliers based on the optimized coefficients and new threshold
130  model->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers);
131  UDEBUG("RANSAC refineModel: Number of inliers found (before/after): %d/%d, with an error threshold of %f.",
132  (int)prev_inliers.size (), (int)new_inliers.size (), error_threshold);
133 
134  if (new_inliers.empty ())
135  {
136  ++refine_iterations;
137  if (refine_iterations >= refineIterations)
138  {
139  break;
140  }
141  continue;
142  }
143 
144  // Estimate the variance and the new threshold
145  double variance = model->computeVariance ();
146  error_threshold = std::min (inlierThreshold, refineSigma * sqrt(variance));
147 
148  UDEBUG ("RANSAC refineModel: New estimated error threshold: %f (variance=%f) on iteration %d out of %d.",
149  error_threshold, variance, refine_iterations, refineIterations);
150  inlier_changed = false;
151  std::swap (prev_inliers, new_inliers);
152 
153  // If the number of inliers changed, then we are still optimizing
154  if (new_inliers.size () != prev_inliers.size ())
155  {
156  // Check if the number of inliers is oscillating in between two values
157  if (inliers_sizes.size () >= 4)
158  {
159  if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
160  inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
161  {
162  oscillating = true;
163  break;
164  }
165  }
166  inlier_changed = true;
167  continue;
168  }
169 
170  // Check the values of the inlier set
171  for (size_t i = 0; i < prev_inliers.size (); ++i)
172  {
173  // If the value of the inliers changed, then we are still optimizing
174  if (prev_inliers[i] != new_inliers[i])
175  {
176  inlier_changed = true;
177  break;
178  }
179  }
180  }
181  while (inlier_changed && ++refine_iterations < refineIterations);
182 
183  // If the new set of inliers is empty, we didn't do a good job refining
184  if (new_inliers.empty ())
185  {
186  UWARN ("RANSAC refineModel: Refinement failed: got an empty set of inliers!");
187  }
188 
189  if (oscillating)
190  {
191  UDEBUG("RANSAC refineModel: Detected oscillations in the model refinement.");
192  }
193 
194  std::swap (inliers, new_inliers);
195  model_coefficients = new_model_coefficients;
196  }
197 
198  if (inliers.size() >= 3)
199  {
200  if(inliersOut)
201  {
202  *inliersOut = inliers;
203  }
204  if(covariance)
205  {
206  double variance = model->computeVariance();
207  UASSERT(uIsFinite(variance));
208  *covariance *= variance;
209  }
210 
211  // get best transformation
212  Eigen::Matrix4f bestTransformation;
213  bestTransformation.row (0) = model_coefficients.segment<4>(0);
214  bestTransformation.row (1) = model_coefficients.segment<4>(4);
215  bestTransformation.row (2) = model_coefficients.segment<4>(8);
216  bestTransformation.row (3) = model_coefficients.segment<4>(12);
217 
218  transform = Transform::fromEigen4f(bestTransformation);
219  UDEBUG("RANSAC inliers=%d/%d tf=%s", (int)inliers.size(), (int)cloud1->size(), transform.prettyPrint().c_str());
220 
221  return transform.inverse(); // inverse to get actual pose transform (not correspondences transform)
222  }
223  else
224  {
225  UDEBUG("RANSAC: Model with inliers < 3");
226  }
227  }
228  else
229  {
230  UDEBUG("RANSAC: Failed to find model");
231  }
232  }
233  else
234  {
235  UDEBUG("Not enough points to compute the transform");
236  }
237  return Transform();
238 }
239 
240 template<typename PointNormalT>
242  const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloudA,
243  const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloudB,
244  double maxCorrespondenceDistance,
245  double maxCorrespondenceAngle,
246  double & variance,
247  int & correspondencesOut,
248  bool reciprocal)
249 {
250  variance = 1;
251  correspondencesOut = 0;
252  typename pcl::registration::CorrespondenceEstimation<PointNormalT, PointNormalT>::Ptr est;
253  est.reset(new pcl::registration::CorrespondenceEstimation<PointNormalT, PointNormalT>);
254  const typename pcl::PointCloud<PointNormalT>::ConstPtr & target = cloudA->size()>cloudB->size()?cloudA:cloudB;
255  const typename pcl::PointCloud<PointNormalT>::ConstPtr & source = cloudA->size()>cloudB->size()?cloudB:cloudA;
256  est->setInputTarget(target);
257  est->setInputSource(source);
258  pcl::Correspondences correspondences;
259  est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
260 
261  if(correspondences.size())
262  {
263  std::vector<double> distances(correspondences.size());
264  correspondencesOut = 0;
265  for(unsigned int i=0; i<correspondences.size(); ++i)
266  {
267  distances[i] = correspondences[i].distance;
268  if(maxCorrespondenceAngle <= 0.0)
269  {
270  ++correspondencesOut;
271  }
272  else
273  {
274  Eigen::Vector4f v1(
275  target->at(correspondences[i].index_match).normal_x,
276  target->at(correspondences[i].index_match).normal_y,
277  target->at(correspondences[i].index_match).normal_z,
278  0);
279  Eigen::Vector4f v2(
280  source->at(correspondences[i].index_query).normal_x,
281  source->at(correspondences[i].index_query).normal_y,
282  source->at(correspondences[i].index_query).normal_z,
283  0);
284  float angle = pcl::getAngle3D(v1, v2);
285  if(angle < maxCorrespondenceAngle)
286  {
287  ++correspondencesOut;
288  }
289  }
290  }
291  if(correspondencesOut)
292  {
293  distances.resize(correspondencesOut);
294 
295  //variance
296  std::sort(distances.begin (), distances.end ());
297  double median_error_sqr = distances[distances.size () >> 1];
298  variance = (2.1981 * median_error_sqr);
299  }
300  }
301 }
302 
304  const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
305  const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
306  double maxCorrespondenceDistance,
307  double maxCorrespondenceAngle,
308  double & variance,
309  int & correspondencesOut,
310  bool reciprocal)
311 {
312  computeVarianceAndCorrespondencesImpl<pcl::PointNormal>(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut, reciprocal);
313 }
314 
316  const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudA,
317  const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudB,
318  double maxCorrespondenceDistance,
319  double maxCorrespondenceAngle,
320  double & variance,
321  int & correspondencesOut,
322  bool reciprocal)
323 {
324  computeVarianceAndCorrespondencesImpl<pcl::PointXYZINormal>(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut, reciprocal);
325 }
326 
327 template<typename PointT>
329  const typename pcl::PointCloud<PointT>::ConstPtr & cloudA,
330  const typename pcl::PointCloud<PointT>::ConstPtr & cloudB,
331  double maxCorrespondenceDistance,
332  double & variance,
333  int & correspondencesOut,
334  bool reciprocal)
335 {
336  variance = 1;
337  correspondencesOut = 0;
338  typename pcl::registration::CorrespondenceEstimation<PointT, PointT>::Ptr est;
339  est.reset(new pcl::registration::CorrespondenceEstimation<PointT, PointT>);
340  est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
341  est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
342  pcl::Correspondences correspondences;
343  est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
344 
345  if(correspondences.size()>=3)
346  {
347  std::vector<double> distances(correspondences.size());
348  for(unsigned int i=0; i<correspondences.size(); ++i)
349  {
350  distances[i] = correspondences[i].distance;
351  }
352 
353  //variance
354  std::sort(distances.begin (), distances.end ());
355  double median_error_sqr = distances[distances.size () >> 1];
356  variance = (2.1981 * median_error_sqr);
357  }
358 
359  correspondencesOut = (int)correspondences.size();
360 }
361 
363  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
364  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
365  double maxCorrespondenceDistance,
366  double & variance,
367  int & correspondencesOut,
368  bool reciprocal)
369 {
370  computeVarianceAndCorrespondencesImpl<pcl::PointXYZ>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut, reciprocal);
371 }
372 
374  const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudA,
375  const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudB,
376  double maxCorrespondenceDistance,
377  double & variance,
378  int & correspondencesOut,
379  bool reciprocal)
380 {
381  computeVarianceAndCorrespondencesImpl<pcl::PointXYZI>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut, reciprocal);
382 }
383 
384 // return transform from source to target (All points must be finite!!!)
385 template<typename PointT>
386 Transform icpImpl(const typename pcl::PointCloud<PointT>::ConstPtr & cloud_source,
387  const typename pcl::PointCloud<PointT>::ConstPtr & cloud_target,
388  double maxCorrespondenceDistance,
389  int maximumIterations,
390  bool & hasConverged,
391  pcl::PointCloud<PointT> & cloud_source_registered,
392  float epsilon,
393  bool icp2D)
394 {
395  pcl::IterativeClosestPoint<PointT, PointT> icp;
396  // Set the input source and target
397  icp.setInputTarget (cloud_target);
398  icp.setInputSource (cloud_source);
399 
400  if(icp2D)
401  {
402  typename pcl::registration::TransformationEstimation2D<PointT, PointT>::Ptr est;
403  est.reset(new pcl::registration::TransformationEstimation2D<PointT, PointT>);
404  icp.setTransformationEstimation(est);
405  }
406 
407  // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
408  icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
409  // Set the maximum number of iterations (criterion 1)
410  icp.setMaximumIterations (maximumIterations);
411  // Set the transformation epsilon (criterion 2)
412  icp.setTransformationEpsilon (epsilon*epsilon);
413  // Set the euclidean distance difference epsilon (criterion 3)
414  //icp.setEuclideanFitnessEpsilon (1);
415  //icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
416 
417  // Perform the alignment
418  icp.align (cloud_source_registered);
419  hasConverged = icp.hasConverged();
420  return Transform::fromEigen4f(icp.getFinalTransformation());
421 }
422 
423 // return transform from source to target (All points must be finite!!!)
424 Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
425  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
426  double maxCorrespondenceDistance,
427  int maximumIterations,
428  bool & hasConverged,
429  pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
430  float epsilon,
431  bool icp2D)
432 {
433  return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
434 }
435 
436 // return transform from source to target (All points must be finite!!!)
437 Transform icp(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
438  const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_target,
439  double maxCorrespondenceDistance,
440  int maximumIterations,
441  bool & hasConverged,
442  pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
443  float epsilon,
444  bool icp2D)
445 {
446  return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
447 }
448 
449 // return transform from source to target (All points/normals must be finite!!!)
450 template<typename PointNormalT>
452  const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloud_source,
453  const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloud_target,
454  double maxCorrespondenceDistance,
455  int maximumIterations,
456  bool & hasConverged,
457  pcl::PointCloud<PointNormalT> & cloud_source_registered,
458  float epsilon,
459  bool icp2D)
460 {
461  pcl::IterativeClosestPoint<PointNormalT, PointNormalT> icp;
462  // Set the input source and target
463  icp.setInputTarget (cloud_target);
464  icp.setInputSource (cloud_source);
465 
466  typename pcl::registration::TransformationEstimationPointToPlaneLLS<PointNormalT, PointNormalT>::Ptr est;
467  est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<PointNormalT, PointNormalT>);
468  icp.setTransformationEstimation(est);
469 
470  // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
471  icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
472  // Set the maximum number of iterations (criterion 1)
473  icp.setMaximumIterations (maximumIterations);
474  // Set the transformation epsilon (criterion 2)
475  icp.setTransformationEpsilon (epsilon*epsilon);
476  // Set the euclidean distance difference epsilon (criterion 3)
477  //icp.setEuclideanFitnessEpsilon (1);
478  //icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
479 
480  // Perform the alignment
481  icp.align (cloud_source_registered);
482  hasConverged = icp.hasConverged();
483  Transform t = Transform::fromEigen4f(icp.getFinalTransformation());
484 
485  if(icp2D)
486  {
487  // FIXME probably an estimation approach already 2D like in icp() version above exists.
488  t = t.to3DoF();
489  }
490 
491  return t;
492 }
493 
494 // return transform from source to target (All points/normals must be finite!!!)
496  const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
497  const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
498  double maxCorrespondenceDistance,
499  int maximumIterations,
500  bool & hasConverged,
501  pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
502  float epsilon,
503  bool icp2D)
504 {
505  return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
506 }
507 // return transform from source to target (All points/normals must be finite!!!)
509  const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_source,
510  const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_target,
511  double maxCorrespondenceDistance,
512  int maximumIterations,
513  bool & hasConverged,
514  pcl::PointCloud<pcl::PointXYZINormal> & cloud_source_registered,
515  float epsilon,
516  bool icp2D)
517 {
518  return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
519 }
520 
521 }
522 
523 }
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
int
int
rtabmap::util3d::transformFromXYZCorrespondences
Transform RTABMAP_CORE_EXPORT transformFromXYZCorrespondences(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud1, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud2, double inlierThreshold=0.02, int iterations=100, int refineModelIterations=10, double refineModelSigma=3.0, std::vector< int > *inliers=0, cv::Mat *variance=0)
Definition: util3d_registration.cpp:63
v1
v1
rtabmap::Transform::fromEigen4f
static Transform fromEigen4f(const Eigen::Matrix4f &matrix)
Definition: Transform.cpp:416
util3d.h
svd
cout<< "Here is the matrix m:"<< endl<< m<< endl;JacobiSVD< MatrixXf > svd(m, ComputeThinU|ComputeThinV)
rtabmap::util3d::computeVarianceAndCorrespondences
void RTABMAP_CORE_EXPORT computeVarianceAndCorrespondences(const pcl::PointCloud< pcl::PointNormal >::ConstPtr &cloudA, const pcl::PointCloud< pcl::PointNormal >::ConstPtr &cloudB, double maxCorrespondenceDistance, double maxCorrespondenceAngle, double &variance, int &correspondencesOut, bool reciprocal)
Definition: util3d_registration.cpp:303
UMath.h
Basic mathematics functions.
rtabmap::util3d::icp
Transform RTABMAP_CORE_EXPORT icp(const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_source, const pcl::PointCloud< pcl::PointXYZ >::ConstPtr &cloud_target, double maxCorrespondenceDistance, int maximumIterations, bool &hasConverged, pcl::PointCloud< pcl::PointXYZ > &cloud_source_registered, float epsilon=0.0f, bool icp2D=false)
Definition: util3d_registration.cpp:424
rtabmap::util3d::icpPointToPlane
Transform RTABMAP_CORE_EXPORT icpPointToPlane(const pcl::PointCloud< pcl::PointNormal >::ConstPtr &cloud_source, const pcl::PointCloud< pcl::PointNormal >::ConstPtr &cloud_target, double maxCorrespondenceDistance, int maximumIterations, bool &hasConverged, pcl::PointCloud< pcl::PointNormal > &cloud_source_registered, float epsilon=0.0f, bool icp2D=false)
Definition: util3d_registration.cpp:495
util3d_transforms.h
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
util3d_filtering.h
source
const char * source
Definition: lz4hc.h:181
std::swap
void swap(GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &a, GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &b)
rtabmap::util3d::computeVarianceAndCorrespondencesImpl
void computeVarianceAndCorrespondencesImpl(const typename pcl::PointCloud< PointNormalT >::ConstPtr &cloudA, const typename pcl::PointCloud< PointNormalT >::ConstPtr &cloudB, double maxCorrespondenceDistance, double maxCorrespondenceAngle, double &variance, int &correspondencesOut, bool reciprocal)
Definition: util3d_registration.cpp:241
target
target
UASSERT
#define UASSERT(condition)
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
UWARN
#define UWARN(...)
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
util3d_registration.h
UDEBUG
#define UDEBUG(...)
icp
epsilon
double epsilon
rtabmap::util3d::icpImpl
Transform icpImpl(const typename pcl::PointCloud< PointT >::ConstPtr &cloud_source, const typename pcl::PointCloud< PointT >::ConstPtr &cloud_target, double maxCorrespondenceDistance, int maximumIterations, bool &hasConverged, pcl::PointCloud< PointT > &cloud_source_registered, float epsilon, bool icp2D)
Definition: util3d_registration.cpp:386
rtabmap::util3d::icpPointToPlaneImpl
Transform icpPointToPlaneImpl(const typename pcl::PointCloud< PointNormalT >::ConstPtr &cloud_source, const typename pcl::PointCloud< PointNormalT >::ConstPtr &cloud_target, double maxCorrespondenceDistance, int maximumIterations, bool &hasConverged, pcl::PointCloud< PointNormalT > &cloud_source_registered, float epsilon, bool icp2D)
Definition: util3d_registration.cpp:451
transform
EIGEN_DONT_INLINE void transform(const Quaternion< Scalar > &t, Data &data)
matrix
Map< const Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(const T *data, int rows, int cols, int stride)
t
Point2 t(10, 10)
rtabmap::util3d::transformFromXYZCorrespondencesSVD
Transform RTABMAP_CORE_EXPORT transformFromXYZCorrespondencesSVD(const pcl::PointCloud< pcl::PointXYZ > &cloud1, const pcl::PointCloud< pcl::PointXYZ > &cloud2)
Definition: util3d_registration.cpp:50
rtabmap
Definition: CameraARCore.cpp:35
trace.model
model
Definition: trace.py:4
i
int i
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:53
v2
v2


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:23