util3d_motion_estimation.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 
28 #include "rtabmap/core/util3d_motion_estimation.h"
29 
30 #include "rtabmap/utilite/UStl.h"
31 #include "rtabmap/utilite/UMath.h"
32 #include "rtabmap/utilite/ULogger.h"
33 #include "rtabmap/utilite/UTimer.h"
34 #include "rtabmap/core/util3d_transforms.h"
35 #include "rtabmap/core/util3d_registration.h"
36 #include "rtabmap/core/util3d_correspondences.h"
37 #include "rtabmap/core/util3d.h"
38 
39 #include <pcl/common/common.h>
40 
41 #include "opencv/solvepnp.h"
42 
43 #ifdef RTABMAP_OPENGV
44 #include <opengv/absolute_pose/methods.hpp>
45 #include <opengv/absolute_pose/NoncentralAbsoluteAdapter.hpp>
46 #include <opengv/absolute_pose/NoncentralAbsoluteMultiAdapter.hpp>
47 #include <opengv/sac/Ransac.hpp>
48 #include <opengv/sac/MultiRansac.hpp>
49 #include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>
50 #include <opengv/sac_problems/absolute_pose/MultiNoncentralAbsolutePoseSacProblem.hpp>
51 #endif
52 
53 namespace rtabmap
54 {
55 
56 namespace util3d
57 {
58 
59 Transform estimateMotion3DTo2D(
60  const std::map<int, cv::Point3f> & words3A,
61  const std::map<int, cv::KeyPoint> & words2B,
62  const CameraModel & cameraModel,
63  int minInliers,
64  int iterations,
65  double reprojError,
66  int flagsPnP,
67  int refineIterations,
68  int varianceMedianRatio,
69  float maxVariance,
70  const Transform & guess,
71  const std::map<int, cv::Point3f> & words3B,
72  cv::Mat * covariance,
73  std::vector<int> * matchesOut,
74  std::vector<int> * inliersOut,
75  bool splitLinearCovarianceComponents)
76 {
77  UASSERT(cameraModel.isValidForProjection());
78  UASSERT(!guess.isNull());
79  UASSERT(varianceMedianRatio>1);
80  Transform transform;
81  std::vector<int> matches, inliers;
82 
83  if(covariance)
84  {
85  *covariance = cv::Mat::eye(6,6,CV_64FC1);
86  }
87 
88  // find correspondences
89  std::vector<int> ids = uKeys(words2B);
90  std::vector<cv::Point3f> objectPoints(ids.size());
91  std::vector<cv::Point2f> imagePoints(ids.size());
92  int oi=0;
93  matches.resize(ids.size());
94  for(unsigned int i=0; i<ids.size(); ++i)
95  {
96  std::map<int, cv::Point3f>::const_iterator iter=words3A.find(ids[i]);
97  if(iter != words3A.end() && util3d::isFinite(iter->second))
98  {
99  const cv::Point3f & pt = iter->second;
100  objectPoints[oi].x = pt.x;
101  objectPoints[oi].y = pt.y;
102  objectPoints[oi].z = pt.z;
103  imagePoints[oi] = words2B.find(ids[i])->second.pt;
104  matches[oi++] = ids[i];
105  }
106  }
107 
108  objectPoints.resize(oi);
109  imagePoints.resize(oi);
110  matches.resize(oi);
111 
112  UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s reprojError=%f iterations=%d",
113  (int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(),
114  guess.prettyPrint().c_str(), reprojError, iterations);
115 
116  if((int)matches.size() >= minInliers)
117  {
118  //PnPRansac
119  cv::Mat K = cameraModel.K();
120  cv::Mat D = cameraModel.D();
121  Transform guessCameraFrame = (guess * cameraModel.localTransform()).inverse();
122  cv::Mat R = (cv::Mat_<double>(3,3) <<
123  (double)guessCameraFrame.r11(), (double)guessCameraFrame.r12(), (double)guessCameraFrame.r13(),
124  (double)guessCameraFrame.r21(), (double)guessCameraFrame.r22(), (double)guessCameraFrame.r23(),
125  (double)guessCameraFrame.r31(), (double)guessCameraFrame.r32(), (double)guessCameraFrame.r33());
126 
127  cv::Mat rvec(3,1, CV_64FC1);
128  cv::Rodrigues(R, rvec);
129  cv::Mat tvec = (cv::Mat_<double>(3,1) <<
130  (double)guessCameraFrame.x(), (double)guessCameraFrame.y(), (double)guessCameraFrame.z());
131 
132  util3d::solvePnPRansac(
133  objectPoints,
134  imagePoints,
135  K,
136  D,
137  rvec,
138  tvec,
139  true,
140  iterations,
141  reprojError,
142  minInliers, // min inliers
143  inliers,
144  flagsPnP,
145  refineIterations);
146 
147  if((int)inliers.size() >= minInliers)
148  {
149  cv::Rodrigues(rvec, R);
150  Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
151  R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
152  R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
153 
154  transform = (cameraModel.localTransform() * pnp).inverse();
155 
156  // compute variance (like in PCL computeVariance() method of sac_model.h)
157  if(covariance && (!words3B.empty() || cameraModel.imageSize() != cv::Size()))
158  {
159  std::vector<float> errorSqrdDists(inliers.size());
160  std::vector<float> errorSqrdX;
161  std::vector<float> errorSqrdY;
162  std::vector<float> errorSqrdZ;
163  if(splitLinearCovarianceComponents)
164  {
165  errorSqrdX.resize(inliers.size());
166  errorSqrdY.resize(inliers.size());
167  errorSqrdZ.resize(inliers.size());
168  }
169  std::vector<float> errorSqrdAngles(inliers.size());
170  Transform localTransformInv = cameraModel.localTransform().inverse();
171  Transform transformCameraFrame = transform * cameraModel.localTransform();
172  Transform transformCameraFrameInv = transformCameraFrame.inverse();
173  for(unsigned int i=0; i<inliers.size(); ++i)
174  {
175  cv::Point3f objPt = objectPoints[inliers[i]];
176 
177  // Get 3D point from cameraB base frame in cameraA base frame
178  std::map<int, cv::Point3f>::const_iterator iter = words3B.find(matches[inliers[i]]);
179  cv::Point3f newPt;
180  if(iter!=words3B.end() && util3d::isFinite(iter->second))
181  {
182  newPt = util3d::transformPoint(iter->second, transform);
183  }
184  else
185  {
186 
187  // Project obj point from base frame of cameraA in cameraB frame (z+ in front of the cameraB)
188  cv::Point3f objPtCamBFrame = util3d::transformPoint(objPt, transformCameraFrameInv);
189 
190  //compute from projection
191  Eigen::Vector3f ray = projectDepthTo3DRay(
192  cameraModel.imageSize(),
193  imagePoints.at(inliers[i]).x,
194  imagePoints.at(inliers[i]).y,
195  cameraModel.cx(),
196  cameraModel.cy(),
197  cameraModel.fx(),
198  cameraModel.fy());
199  // transform in camera B frame
200  newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPtCamBFrame.z*1.1; // Add 10 % error
201 
202  //transform back into cameraA base frame
203  newPt = util3d::transformPoint(newPt, transformCameraFrame);
204  }
205 
206  if(splitLinearCovarianceComponents)
207  {
208  double errorX = objPt.x-newPt.x;
209  double errorY = objPt.y-newPt.y;
210  double errorZ = objPt.z-newPt.z;
211  errorSqrdX[i] = errorX * errorX;
212  errorSqrdY[i] = errorY * errorY;
213  errorSqrdZ[i] = errorZ * errorZ;
214  }
215 
216  errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
217 
218  Eigen::Vector4f v1(objPt.x, objPt.y, objPt.z, 0);
219  Eigen::Vector4f v2(newPt.x, newPt.y, newPt.z, 0);
220  errorSqrdAngles[i] = pcl::getAngle3D(v1, v2);
221  }
222 
223  std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
224  //divide by 4 instead of 2 to ignore very very far features (stereo)
225  double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () / varianceMedianRatio];
226  UASSERT(uIsFinite(median_error_sqr_lin));
227  (*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin;
228  std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
229  double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () / varianceMedianRatio];
230  UASSERT(uIsFinite(median_error_sqr_ang));
231  (*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;
232 
233  if(splitLinearCovarianceComponents)
234  {
235  std::sort(errorSqrdX.begin(), errorSqrdX.end());
236  double median_error_sqr_x = 2.1981 * (double)errorSqrdX[errorSqrdX.size () / varianceMedianRatio];
237  std::sort(errorSqrdY.begin(), errorSqrdY.end());
238  double median_error_sqr_y = 2.1981 * (double)errorSqrdY[errorSqrdY.size () / varianceMedianRatio];
239  std::sort(errorSqrdZ.begin(), errorSqrdZ.end());
240  double median_error_sqr_z = 2.1981 * (double)errorSqrdZ[errorSqrdZ.size () / varianceMedianRatio];
241 
242  UASSERT(uIsFinite(median_error_sqr_x));
243  UASSERT(uIsFinite(median_error_sqr_y));
244  UASSERT(uIsFinite(median_error_sqr_z));
245  covariance->at<double>(0,0) = median_error_sqr_x;
246  covariance->at<double>(1,1) = median_error_sqr_y;
247  covariance->at<double>(2,2) = median_error_sqr_z;
248 
249  median_error_sqr_lin = uMax3(median_error_sqr_x, median_error_sqr_y, median_error_sqr_z);
250  }
251 
252  if(maxVariance > 0 && median_error_sqr_lin > maxVariance)
253  {
254  UWARN("Rejected PnP transform, variance is too high! %f > %f!", median_error_sqr_lin, maxVariance);
255  *covariance = cv::Mat::eye(6,6,CV_64FC1);
256  transform.setNull();
257  }
258  }
259  else if(covariance)
260  {
261  // compute variance, which is the rms of reprojection errors
262  std::vector<cv::Point2f> imagePointsReproj;
263  cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), imagePointsReproj);
264  float err = 0.0f;
265  for(unsigned int i=0; i<inliers.size(); ++i)
266  {
267  err += uNormSquared(imagePoints.at(inliers[i]).x - imagePointsReproj.at(inliers[i]).x, imagePoints.at(inliers[i]).y - imagePointsReproj.at(inliers[i]).y);
268  }
269  UASSERT(uIsFinite(err));
270  *covariance *= std::sqrt(err/float(inliers.size()));
271  }
272  }
273  }
274 
275  if(matchesOut)
276  {
277  *matchesOut = matches;
278  }
279  if(inliersOut)
280  {
281  inliersOut->resize(inliers.size());
282  for(unsigned int i=0; i<inliers.size(); ++i)
283  {
284  inliersOut->at(i) = matches[inliers[i]];
285  }
286  }
287 
288  return transform;
289 }
290 
291 Transform estimateMotion3DTo2D(
292  const std::map<int, cv::Point3f> & words3A,
293  const std::map<int, cv::KeyPoint> & words2B,
294  const std::vector<CameraModel> & cameraModels,
295  unsigned int samplingPolicy,
296  int minInliers,
297  int iterations,
298  double reprojError,
299  int flagsPnP,
300  int refineIterations,
301  int varianceMedianRatio,
302  float maxVariance,
303  const Transform & guess,
304  const std::map<int, cv::Point3f> & words3B,
305  cv::Mat * covariance,
306  std::vector<int> * matchesOut,
307  std::vector<int> * inliersOut,
308  bool splitLinearCovarianceComponents)
309 {
310  std::vector<std::vector<int> > matchesPerCamera;
311  std::vector<std::vector<int> > inliersPerCamera;
312  Transform t = estimateMotion3DTo2D(
313  words3A,
314  words2B,
315  cameraModels,
316  samplingPolicy,
317  minInliers,
318  iterations,
319  reprojError,
320  flagsPnP,
321  refineIterations,
322  varianceMedianRatio,
323  maxVariance,
324  guess,
325  words3B,
326  covariance,
327  matchesOut?&matchesPerCamera:0,
328  inliersOut?&inliersPerCamera:0,
329  splitLinearCovarianceComponents);
330  if(matchesOut)
331  {
332  for(size_t i=0; i<matchesPerCamera.size(); ++i)
333  {
334  matchesOut->insert(matchesOut->end(), matchesPerCamera[i].begin(), matchesPerCamera[i].end());
335  }
336  }
337  if(inliersOut)
338  {
339  for(size_t i=0; i<inliersPerCamera.size(); ++i)
340  {
341  inliersOut->insert(inliersOut->end(), inliersPerCamera[i].begin(), inliersPerCamera[i].end());
342  }
343  }
344  return t;
345 }
346 
347 Transform estimateMotion3DTo2D(
348  const std::map<int, cv::Point3f> & words3A,
349  const std::map<int, cv::KeyPoint> & words2B,
350  const std::vector<CameraModel> & cameraModels,
351  unsigned int samplingPolicy,
352  int minInliers,
353  int iterations,
354  double reprojError,
355  int flagsPnP,
356  int refineIterations,
357  int varianceMedianRatio,
358  float maxVariance,
359  const Transform & guess,
360  const std::map<int, cv::Point3f> & words3B,
361  cv::Mat * covariance,
362  std::vector<std::vector<int> > * matchesOut,
363  std::vector<std::vector<int> > * inliersOut,
364  bool splitLinearCovarianceComponents)
365 {
366  Transform transform;
367 #ifndef RTABMAP_OPENGV
368  UERROR("This function is only available if rtabmap is built with OpenGV dependency.");
369 #else
370  UASSERT(!cameraModels.empty() && cameraModels[0].imageWidth() > 0);
371  int subImageWidth = cameraModels[0].imageWidth();
372  for(size_t i=0; i<cameraModels.size(); ++i)
373  {
374  UASSERT(cameraModels[i].isValidForProjection());
375  UASSERT(subImageWidth == cameraModels[i].imageWidth());
376  }
377 
378  UASSERT(!guess.isNull());
379  UASSERT(varianceMedianRatio > 1);
380 
381  std::vector<int> matches, inliers;
382 
383  if(covariance)
384  {
385  *covariance = cv::Mat::eye(6,6,CV_64FC1);
386  }
387 
388  // find correspondences
389  std::vector<int> ids = uKeys(words2B);
390  std::vector<cv::Point3f> objectPoints(ids.size());
391  std::vector<cv::Point2f> imagePoints(ids.size());
392  int oi=0;
393  matches.resize(ids.size());
394  std::vector<int> cameraIndexes(ids.size());
395  for(unsigned int i=0; i<ids.size(); ++i)
396  {
397  std::map<int, cv::Point3f>::const_iterator iter=words3A.find(ids[i]);
398  if(iter != words3A.end() && util3d::isFinite(iter->second))
399  {
400  const cv::Point2f & kpt = words2B.find(ids[i])->second.pt;
401  int cameraIndex = int(kpt.x / subImageWidth);
402  UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)cameraModels.size(),
403  uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
404  cameraIndex, (int)cameraModels.size(), kpt.x, subImageWidth, cameraModels[cameraIndex].imageWidth()).c_str());
405 
406  const cv::Point3f & pt = iter->second;
407  objectPoints[oi] = pt;
408  imagePoints[oi] = kpt;
409  // convert in image space
410  imagePoints[oi].x = imagePoints[oi].x - (cameraIndex*subImageWidth);
411  cameraIndexes[oi] = cameraIndex;
412  matches[oi++] = ids[i];
413  }
414  }
415 
416  objectPoints.resize(oi);
417  imagePoints.resize(oi);
418  cameraIndexes.resize(oi);
419  matches.resize(oi);
420 
421  UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s reprojError=%f iterations=%d samplingPolicy=%ld",
422  (int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(),
423  guess.prettyPrint().c_str(), reprojError, iterations, samplingPolicy);
424 
425  if((int)matches.size() >= minInliers)
426  {
427  if(samplingPolicy == 0 || samplingPolicy == 2)
428  {
429  std::vector<int> cc;
430  cc.resize(cameraModels.size());
431  std::fill(cc.begin(), cc.end(),0);
432  for(size_t i=0; i<cameraIndexes.size(); ++i)
433  {
434  cc[cameraIndexes[i]] = cc[cameraIndexes[i]] + 1;
435  }
436 
437  for (size_t i=0; i<cameraModels.size(); ++i)
438  {
439  UDEBUG("Matches in Camera %d: %d", i, cc[i]);
440  // opengv multi ransac needs at least 2 matches/camera
441  if (cc[i] < 2)
442  {
443  if(samplingPolicy==2) {
444  UERROR("Not enough matches in camera %ld to do "
445  "homogenoeus random sampling, returning null "
446  "transform. Consider using AUTO sampling "
447  "policy to fallback to ANY policy.", i);
448  return Transform();
449  }
450  else { // samplingPolicy==0
451  samplingPolicy = 1;
452  UWARN("Not enough matches in camera %ld to do "
453  "homogenoeus random sampling, falling back to ANY policy.", i);
454  break;
455  }
456  }
457  }
458  }
459 
460  if(samplingPolicy == 0)
461  {
462  samplingPolicy = 2;
463  }
464 
465  // convert cameras
466  opengv::translations_t camOffsets;
467  opengv::rotations_t camRotations;
468  for(size_t i=0; i<cameraModels.size(); ++i)
469  {
470  camOffsets.push_back(opengv::translation_t(
471  cameraModels[i].localTransform().x(),
472  cameraModels[i].localTransform().y(),
473  cameraModels[i].localTransform().z()));
474  camRotations.push_back(cameraModels[i].localTransform().toEigen4d().block<3,3>(0, 0));
475  }
476 
477  Transform pnp;
478  if(samplingPolicy == 2) // Homogenoeus random sampling
479  {
480  // convert 3d points
481  std::vector<std::shared_ptr<opengv::points_t>> multiPoints;
482  multiPoints.resize(cameraModels.size());
483  // convert 2d-3d correspondences into bearing vectors
484  std::vector<std::shared_ptr<opengv::bearingVectors_t>> multiBearingVectors;
485  multiBearingVectors.resize(cameraModels.size());
486  for(size_t i=0; i<cameraModels.size();++i)
487  {
488  multiPoints[i] = std::make_shared<opengv::points_t>();
489  multiBearingVectors[i] = std::make_shared<opengv::bearingVectors_t>();
490  }
491 
492  for(size_t i=0; i<objectPoints.size(); ++i)
493  {
494  int cameraIndex = cameraIndexes[i];
495  multiPoints[cameraIndex]->push_back(opengv::point_t(objectPoints[i].x,objectPoints[i].y,objectPoints[i].z));
496  cv::Vec3f pt;
497  cameraModels[cameraIndex].project(imagePoints[i].x, imagePoints[i].y, 1, pt[0], pt[1], pt[2]);
498  pt = cv::normalize(pt);
499  multiBearingVectors[cameraIndex]->push_back(opengv::bearingVector_t(pt[0], pt[1], pt[2]));
500  }
501 
502  //create a non-central absolute multi adapter
503  opengv::absolute_pose::NoncentralAbsoluteMultiAdapter adapter(
504  multiBearingVectors,
505  multiPoints,
506  camOffsets,
507  camRotations );
508 
509  adapter.setR(guess.toEigen4d().block<3,3>(0, 0));
510  adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z()));
511 
512  //Create a MultiNoncentralAbsolutePoseSacProblem and MultiRansac
513  //The method is set to GP3P
514  opengv::sac::MultiRansac<opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> ransac;
515  std::shared_ptr<opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> absposeproblem_ptr(
516  new opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem(adapter));
517 
518  ransac.sac_model_ = absposeproblem_ptr;
519  ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx()));
520  ransac.max_iterations_ = iterations;
521  UDEBUG("Ransac params: threshold = %f (reprojError=%f fx=%f), max iterations=%d", ransac.threshold_, reprojError, cameraModels[0].fx(), ransac.max_iterations_);
522 
523  //Run the experiment
524  ransac.computeModel();
525 
526  pnp = Transform::fromEigen3d(ransac.model_coefficients_);
527 
528  UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str());
529  UDEBUG("Ransac iterations done: %d", ransac.iterations_);
530  for (size_t i=0; i < cameraModels.size(); ++i)
531  {
532  inliers.insert(inliers.end(), ransac.inliers_[i].begin(), ransac.inliers_[i].end());
533  }
534  }
535  else
536  {
537  // convert 3d points
538  opengv::points_t points;
539 
540  // convert 2d-3d correspondences into bearing vectors
541  opengv::bearingVectors_t bearingVectors;
542  opengv::absolute_pose::NoncentralAbsoluteAdapter::camCorrespondences_t camCorrespondences;
543 
544  for(size_t i=0; i<objectPoints.size(); ++i)
545  {
546  int cameraIndex = cameraIndexes[i];
547  points.push_back(opengv::point_t(objectPoints[i].x,objectPoints[i].y,objectPoints[i].z));
548  cv::Vec3f pt;
549  cameraModels[cameraIndex].project(imagePoints[i].x, imagePoints[i].y, 1, pt[0], pt[1], pt[2]);
550  pt = cv::normalize(pt);
551  bearingVectors.push_back(opengv::bearingVector_t(pt[0], pt[1], pt[2]));
552  camCorrespondences.push_back(cameraIndex);
553  }
554 
555  //create a non-central absolute adapter
556  opengv::absolute_pose::NoncentralAbsoluteAdapter adapter(
557  bearingVectors,
558  camCorrespondences,
559  points,
560  camOffsets,
561  camRotations );
562 
563  adapter.setR(guess.toEigen4d().block<3,3>(0, 0));
564  adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z()));
565 
566  //Create a AbsolutePoseSacProblem and Ransac
567  //The method is set to GP3P
568  opengv::sac::Ransac<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> ransac;
569  std::shared_ptr<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> absposeproblem_ptr(
570  new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem(adapter, opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem::GP3P));
571 
572  ransac.sac_model_ = absposeproblem_ptr;
573  ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx()));
574  ransac.max_iterations_ = iterations;
575  UDEBUG("Ransac params: threshold = %f (reprojError=%f fx=%f), max iterations=%d", ransac.threshold_, reprojError, cameraModels[0].fx(), ransac.max_iterations_);
576 
577  //Run the experiment
578  ransac.computeModel();
579 
580  pnp = Transform::fromEigen3d(ransac.model_coefficients_);
581 
582  UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str());
583  UDEBUG("Ransac iterations done: %d", ransac.iterations_);
584  inliers = ransac.inliers_;
585  }
586 
587  UDEBUG("Ransac inliers: %ld", inliers.size());
588 
589  if((int)inliers.size() >= minInliers && !pnp.isNull())
590  {
591  transform = pnp;
592 
593  // compute variance (like in PCL computeVariance() method of sac_model.h)
594  if(covariance)
595  {
596  std::vector<float> errorSqrdX;
597  std::vector<float> errorSqrdY;
598  std::vector<float> errorSqrdZ;
599  if(splitLinearCovarianceComponents)
600  {
601  errorSqrdX.resize(inliers.size());
602  errorSqrdY.resize(inliers.size());
603  errorSqrdZ.resize(inliers.size());
604  }
605  std::vector<float> errorSqrdDists(inliers.size());
606  std::vector<float> errorSqrdAngles(inliers.size());
607 
608  std::vector<Transform> transformsCameraFrame(cameraModels.size());
609  std::vector<Transform> transformsCameraFrameInv(cameraModels.size());
610  for(size_t i=0; i<cameraModels.size(); ++i)
611  {
612  transformsCameraFrame[i] = transform * cameraModels[i].localTransform();
613  transformsCameraFrameInv[i] = transformsCameraFrame[i].inverse();
614  }
615 
616  for(unsigned int i=0; i<inliers.size(); ++i)
617  {
618  cv::Point3f objPt = objectPoints[inliers[i]];
619 
620  // Get 3D point from cameraB base frame in cameraA base frame
621  std::map<int, cv::Point3f>::const_iterator iter = words3B.find(matches[inliers[i]]);
622  cv::Point3f newPt;
623  if(iter!=words3B.end() && util3d::isFinite(iter->second))
624  {
625  newPt = util3d::transformPoint(iter->second, transform);
626  }
627  else
628  {
629  int cameraIndex = cameraIndexes[inliers[i]];
630 
631  // Project obj point from base frame of cameraA in cameraB frame (z+ in front of the cameraB)
632  cv::Point3f objPtCamBFrame = util3d::transformPoint(objPt, transformsCameraFrameInv[cameraIndex]);
633 
634  //compute from projection
635  Eigen::Vector3f ray = projectDepthTo3DRay(
636  cameraModels[cameraIndex].imageSize(),
637  imagePoints.at(inliers[i]).x,
638  imagePoints.at(inliers[i]).y,
639  cameraModels[cameraIndex].cx(),
640  cameraModels[cameraIndex].cy(),
641  cameraModels[cameraIndex].fx(),
642  cameraModels[cameraIndex].fy());
643  // transform in camera B frame
644  newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPtCamBFrame.z*1.1; // Add 10 % error
645 
646  //transfor back into cameraA base frame
647  newPt = util3d::transformPoint(newPt, transformsCameraFrame[cameraIndex]);
648  }
649 
650  if(splitLinearCovarianceComponents)
651  {
652  double errorX = objPt.x-newPt.x;
653  double errorY = objPt.y-newPt.y;
654  double errorZ = objPt.z-newPt.z;
655  errorSqrdX[i] = errorX * errorX;
656  errorSqrdY[i] = errorY * errorY;
657  errorSqrdZ[i] = errorZ * errorZ;
658  }
659 
660  errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
661 
662  Eigen::Vector4f v1(objPt.x, objPt.y, objPt.z, 0);
663  Eigen::Vector4f v2(newPt.x, newPt.y, newPt.z, 0);
664  errorSqrdAngles[i] = pcl::getAngle3D(v1, v2);
665  }
666 
667  std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
668  //divide by 4 instead of 2 to ignore very very far features (stereo)
669  double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () / varianceMedianRatio];
670  UASSERT(uIsFinite(median_error_sqr_lin));
671  (*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin;
672  std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
673  double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () / varianceMedianRatio];
674  UASSERT(uIsFinite(median_error_sqr_ang));
675  (*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;
676 
677  if(splitLinearCovarianceComponents)
678  {
679  std::sort(errorSqrdX.begin(), errorSqrdX.end());
680  double median_error_sqr_x = 2.1981 * (double)errorSqrdX[errorSqrdX.size () / varianceMedianRatio];
681  std::sort(errorSqrdY.begin(), errorSqrdY.end());
682  double median_error_sqr_y = 2.1981 * (double)errorSqrdY[errorSqrdY.size () / varianceMedianRatio];
683  std::sort(errorSqrdZ.begin(), errorSqrdZ.end());
684  double median_error_sqr_z = 2.1981 * (double)errorSqrdZ[errorSqrdZ.size () / varianceMedianRatio];
685 
686  UASSERT(uIsFinite(median_error_sqr_x));
687  UASSERT(uIsFinite(median_error_sqr_y));
688  UASSERT(uIsFinite(median_error_sqr_z));
689  covariance->at<double>(0,0) = median_error_sqr_x;
690  covariance->at<double>(1,1) = median_error_sqr_y;
691  covariance->at<double>(2,2) = median_error_sqr_z;
692 
693  median_error_sqr_lin = uMax3(median_error_sqr_x, median_error_sqr_y, median_error_sqr_z);
694  }
695 
696  if(maxVariance > 0 && median_error_sqr_lin > maxVariance)
697  {
698  UWARN("Rejected PnP transform, variance is too high! %f > %f!", median_error_sqr_lin, maxVariance);
699  *covariance = cv::Mat::eye(6,6,CV_64FC1);
700  transform.setNull();
701  }
702  }
703  }
704  }
705 
706  if(matchesOut)
707  {
708  matchesOut->resize(cameraModels.size());
709  UASSERT(matches.size() == cameraIndexes.size());
710  for(size_t i=0; i<matches.size(); ++i)
711  {
712  UASSERT(cameraIndexes[i]>=0 && cameraIndexes[i] < (int)cameraModels.size());
713  matchesOut->at(cameraIndexes[i]).push_back(matches[i]);
714  }
715  }
716  if(inliersOut)
717  {
718  inliersOut->resize(cameraModels.size());
719  for(unsigned int i=0; i<inliers.size(); ++i)
720  {
721  UASSERT(inliers[i]>=0 && inliers[i] < (int)cameraIndexes.size());
722  UASSERT(cameraIndexes[inliers[i]]>=0 && cameraIndexes[inliers[i]] < (int)cameraModels.size());
723  inliersOut->at(cameraIndexes[inliers[i]]).push_back(matches[inliers[i]]);
724  }
725  }
726 #endif
727  return transform;
728 }
729 
730 Transform estimateMotion3DTo3D(
731  const std::map<int, cv::Point3f> & words3A,
732  const std::map<int, cv::Point3f> & words3B,
733  int minInliers,
734  double inliersDistance,
735  int iterations,
736  int refineIterations,
737  cv::Mat * covariance,
738  std::vector<int> * matchesOut,
739  std::vector<int> * inliersOut)
740 {
741  Transform transform;
742  std::vector<cv::Point3f> inliers1; // previous
743  std::vector<cv::Point3f> inliers2; // new
744 
745  std::vector<int> matches;
746  util3d::findCorrespondences(
747  words3A,
748  words3B,
749  inliers1,
750  inliers2,
751  0,
752  &matches);
753  UASSERT(inliers1.size() == inliers2.size());
754  UDEBUG("Unique correspondences = %d", (int)inliers1.size());
755 
756  if(covariance)
757  {
758  *covariance = cv::Mat::eye(6,6,CV_64FC1);
759  }
760 
761  std::vector<int> inliers;
762  if((int)inliers1.size() >= minInliers)
763  {
764  pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1cloud(new pcl::PointCloud<pcl::PointXYZ>);
765  pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2cloud(new pcl::PointCloud<pcl::PointXYZ>);
766  inliers1cloud->resize(inliers1.size());
767  inliers2cloud->resize(inliers1.size());
768  for(unsigned int i=0; i<inliers1.size(); ++i)
769  {
770  (*inliers1cloud)[i].x = inliers1[i].x;
771  (*inliers1cloud)[i].y = inliers1[i].y;
772  (*inliers1cloud)[i].z = inliers1[i].z;
773  (*inliers2cloud)[i].x = inliers2[i].x;
774  (*inliers2cloud)[i].y = inliers2[i].y;
775  (*inliers2cloud)[i].z = inliers2[i].z;
776  }
777  Transform t = util3d::transformFromXYZCorrespondences(
778  inliers2cloud,
779  inliers1cloud,
780  inliersDistance,
781  iterations,
782  refineIterations,
783  3.0,
784  &inliers,
785  covariance);
786 
787  if(!t.isNull() && (int)inliers.size() >= minInliers)
788  {
789  transform = t;
790  }
791  }
792 
793  if(matchesOut)
794  {
795  *matchesOut = matches;
796  }
797  if(inliersOut)
798  {
799  inliersOut->resize(inliers.size());
800  for(unsigned int i=0; i<inliers.size(); ++i)
801  {
802  inliersOut->at(i) = matches[inliers[i]];
803  }
804  }
805 
806  return transform;
807 }
808 
809 
810 std::vector<float> computeReprojErrors(
811  std::vector<cv::Point3f> opoints,
812  std::vector<cv::Point2f> ipoints,
813  const cv::Mat & cameraMatrix,
814  const cv::Mat & distCoeffs,
815  const cv::Mat & rvec,
816  const cv::Mat & tvec,
817  float reprojErrorThreshold,
818  std::vector<int> & inliers)
819 {
820  UASSERT(opoints.size() == ipoints.size());
821  int count = (int)opoints.size();
822 
823  std::vector<cv::Point2f> projpoints;
824  projectPoints(opoints, rvec, tvec, cameraMatrix, distCoeffs, projpoints);
825 
826  inliers.resize(count,0);
827  std::vector<float> err(count);
828  int oi=0;
829  for (int i = 0; i < count; ++i)
830  {
831  float e = (float)cv::norm( ipoints[i] - projpoints[i]);
832  if(e <= reprojErrorThreshold)
833  {
834  inliers[oi] = i;
835  err[oi++] = e;
836  }
837  }
838  inliers.resize(oi);
839  err.resize(oi);
840  return err;
841 }
842 
843 void solvePnPRansac(
844  const std::vector<cv::Point3f> & objectPoints,
845  const std::vector<cv::Point2f> & imagePoints,
846  const cv::Mat & cameraMatrix,
847  const cv::Mat & distCoeffs,
848  cv::Mat & rvec,
849  cv::Mat & tvec,
850  bool useExtrinsicGuess,
851  int iterationsCount,
852  float reprojectionError,
853  int minInliersCount,
854  std::vector<int> & inliers,
855  int flags,
856  int refineIterations,
857  float refineSigma)
858 {
859  if(minInliersCount < 4)
860  {
861  minInliersCount = 4;
862  }
863 
864  // Use OpenCV3 version of solvePnPRansac in OpenCV2.
865  // FIXME: we should use this version of solvePnPRansac in newer 3.3.1 too, which seems a lot less stable!?!? Why!?
866  cv3::solvePnPRansac(
867  objectPoints,
868  imagePoints,
869  cameraMatrix,
870  distCoeffs,
871  rvec,
872  tvec,
873  useExtrinsicGuess,
874  iterationsCount,
875  reprojectionError,
876  0.99, // confidence
877  inliers,
878  flags);
879 
880  float inlierThreshold = reprojectionError;
881  if((int)inliers.size() >= minInliersCount && refineIterations>0)
882  {
883  float error_threshold = inlierThreshold;
884  int refine_iterations = 0;
885  bool inlier_changed = false, oscillating = false;
886  std::vector<int> new_inliers, prev_inliers = inliers;
887  std::vector<size_t> inliers_sizes;
888  //Eigen::VectorXf new_model_coefficients = model_coefficients;
889  cv::Mat new_model_rvec = rvec;
890  cv::Mat new_model_tvec = tvec;
891 
892  do
893  {
894  // Get inliers from the current model
895  std::vector<cv::Point3f> opoints_inliers(prev_inliers.size());
896  std::vector<cv::Point2f> ipoints_inliers(prev_inliers.size());
897  for(unsigned int i=0; i<prev_inliers.size(); ++i)
898  {
899  opoints_inliers[i] = objectPoints[prev_inliers[i]];
900  ipoints_inliers[i] = imagePoints[prev_inliers[i]];
901  }
902 
903  UDEBUG("inliers=%d refine_iterations=%d, rvec=%f,%f,%f tvec=%f,%f,%f", (int)prev_inliers.size(), refine_iterations,
904  *new_model_rvec.ptr<double>(0), *new_model_rvec.ptr<double>(1), *new_model_rvec.ptr<double>(2),
905  *new_model_tvec.ptr<double>(0), *new_model_tvec.ptr<double>(1), *new_model_tvec.ptr<double>(2));
906 
907  // Optimize the model coefficients
908  cv::solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix, distCoeffs, new_model_rvec, new_model_tvec, true, flags);
909  inliers_sizes.push_back(prev_inliers.size());
910 
911  UDEBUG("rvec=%f,%f,%f tvec=%f,%f,%f",
912  *new_model_rvec.ptr<double>(0), *new_model_rvec.ptr<double>(1), *new_model_rvec.ptr<double>(2),
913  *new_model_tvec.ptr<double>(0), *new_model_tvec.ptr<double>(1), *new_model_tvec.ptr<double>(2));
914 
915  // Select the new inliers based on the optimized coefficients and new threshold
916  std::vector<float> err = computeReprojErrors(objectPoints, imagePoints, cameraMatrix, distCoeffs, new_model_rvec, new_model_tvec, error_threshold, new_inliers);
917  UDEBUG("RANSAC refineModel: Number of inliers found (before/after): %d/%d, with an error threshold of %f.",
918  (int)prev_inliers.size (), (int)new_inliers.size (), error_threshold);
919 
920  if ((int)new_inliers.size() < minInliersCount)
921  {
922  ++refine_iterations;
923  if (refine_iterations >= refineIterations)
924  {
925  break;
926  }
927  continue;
928  }
929 
930  // Estimate the variance and the new threshold
931  float m = uMean(err.data(), err.size());
932  float variance = uVariance(err.data(), err.size());
933  error_threshold = std::min(inlierThreshold, refineSigma * float(sqrt(variance)));
934 
935  UDEBUG ("RANSAC refineModel: New estimated error threshold: %f (variance=%f mean=%f) on iteration %d out of %d.",
936  error_threshold, variance, m, refine_iterations, refineIterations);
937  inlier_changed = false;
938  std::swap (prev_inliers, new_inliers);
939 
940  // If the number of inliers changed, then we are still optimizing
941  if (new_inliers.size () != prev_inliers.size ())
942  {
943  // Check if the number of inliers is oscillating in between two values
944  if ((int)inliers_sizes.size () >= minInliersCount)
945  {
946  if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
947  inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
948  {
949  oscillating = true;
950  break;
951  }
952  }
953  inlier_changed = true;
954  continue;
955  }
956 
957  // Check the values of the inlier set
958  for (size_t i = 0; i < prev_inliers.size (); ++i)
959  {
960  // If the value of the inliers changed, then we are still optimizing
961  if (prev_inliers[i] != new_inliers[i])
962  {
963  inlier_changed = true;
964  break;
965  }
966  }
967  }
968  while (inlier_changed && ++refine_iterations < refineIterations);
969 
970  // If the new set of inliers is empty, we didn't do a good job refining
971  if ((int)prev_inliers.size() < minInliersCount)
972  {
973  UWARN ("RANSAC refineModel: Refinement failed: got very low inliers (%d)!", (int)prev_inliers.size());
974  }
975 
976  if (oscillating)
977  {
978  UDEBUG("RANSAC refineModel: Detected oscillations in the model refinement.");
979  }
980 
981  std::swap (inliers, new_inliers);
982  rvec = new_model_rvec;
983  tvec = new_model_tvec;
984  }
985 
986 }
987 
988 }
989 
990 }
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
int
int
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
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
uMean
T uMean(const T *v, unsigned int size)
Definition: UMath.h:399
D
MatrixXcd D
v1
v1
K
K
rtabmap::Transform::r33
float r33() const
Definition: Transform.h:70
fx
const double fx
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:105
rtabmap::CameraModel
Definition: CameraModel.h:38
count
Index count
util3d_motion_estimation.h
util3d_correspondences.h
solvepnp.h
y
Matrix3f y
rtabmap::CameraModel::K
cv::Mat K() const
Definition: CameraModel.h:110
uVariance
T uVariance(const T *v, unsigned int size, T meanV)
Definition: UMath.h:489
glm::normalize
GLM_FUNC_DECL genType normalize(genType const &x)
rtabmap::CameraModel::D
cv::Mat D() const
Definition: CameraModel.h:111
util3d.h
rtabmap::util3d::solvePnPRansac
void RTABMAP_CORE_EXPORT solvePnPRansac(const std::vector< cv::Point3f > &objectPoints, const std::vector< cv::Point2f > &imagePoints, const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, cv::Mat &rvec, cv::Mat &tvec, bool useExtrinsicGuess, int iterationsCount, float reprojectionError, int minInliersCount, std::vector< int > &inliers, int flags, int refineIterations=1, float refineSigma=3.0f)
Definition: util3d_motion_estimation.cpp:843
rtabmap::util3d::estimateMotion3DTo3D
Transform RTABMAP_CORE_EXPORT estimateMotion3DTo3D(const std::map< int, cv::Point3f > &words3A, const std::map< int, cv::Point3f > &words3B, int minInliers=10, double inliersDistance=0.1, int iterations=100, int refineIterations=5, cv::Mat *covariance=0, std::vector< int > *matchesOut=0, std::vector< int > *inliersOut=0)
Definition: util3d_motion_estimation.cpp:730
UTimer.h
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::util3d::projectDepthTo3DRay
Eigen::Vector3f RTABMAP_CORE_EXPORT projectDepthTo3DRay(const cv::Size &imageSize, float x, float y, float cx, float cy, float fx, float fy)
Definition: util3d.cpp:246
UMath.h
Basic mathematics functions.
rtabmap::util3d::findCorrespondences
void RTABMAP_CORE_EXPORT findCorrespondences(const std::multimap< int, cv::KeyPoint > &wordsA, const std::multimap< int, cv::KeyPoint > &wordsB, std::list< std::pair< cv::Point2f, cv::Point2f > > &pairs)
Definition: util3d_correspondences.cpp:300
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::Transform::y
float & y()
Definition: Transform.h:93
matches
matches
rtabmap::Transform::z
float & z()
Definition: Transform.h:94
uKeys
std::vector< K > uKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:67
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
util3d_transforms.h
rtabmap::Transform::r13
float r13() const
Definition: Transform.h:64
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:78
rtabmap::Transform::x
float & x()
Definition: Transform.h:92
rtabmap::CameraModel::isValidForProjection
bool isValidForProjection() const
Definition: CameraModel.h:87
rtabmap::util3d::isFinite
bool RTABMAP_CORE_EXPORT isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3456
std::swap
void swap(GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &a, GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &b)
rtabmap::Transform::r21
float r21() const
Definition: Transform.h:65
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:103
UASSERT
#define UASSERT(condition)
z
z
x
x
m
Matrix3f m
cv3::solvePnPRansac
bool solvePnPRansac(InputArray _opoints, InputArray _ipoints, InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int iterationsCount, float reprojectionError, double confidence, OutputArray _inliers, int flags)
Definition: solvepnp.cpp:112
rtabmap::Transform::r31
float r31() const
Definition: Transform.h:68
rtabmap::Transform::toEigen4d
Eigen::Matrix4d toEigen4d() const
Definition: Transform.cpp:381
rtabmap::Transform::fromEigen3d
static Transform fromEigen3d(const Eigen::Affine3d &matrix)
Definition: Transform.cpp:435
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
UWARN
#define UWARN(...)
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform::r22
float r22() const
Definition: Transform.h:66
rtabmap::Transform
Definition: Transform.h:41
iter
iterator iter(handle obj)
rtabmap::util3d::estimateMotion3DTo2D
Transform RTABMAP_CORE_EXPORT estimateMotion3DTo2D(const std::map< int, cv::Point3f > &words3A, const std::map< int, cv::KeyPoint > &words2B, const CameraModel &cameraModel, int minInliers=10, int iterations=100, double reprojError=5., int flagsPnP=0, int pnpRefineIterations=1, int varianceMedianRatio=4, float maxVariance=0, const Transform &guess=Transform::getIdentity(), const std::map< int, cv::Point3f > &words3B=std::map< int, cv::Point3f >(), cv::Mat *covariance=0, std::vector< int > *matchesOut=0, std::vector< int > *inliersOut=0, bool splitLinearCovarianceComponents=false)
Definition: util3d_motion_estimation.cpp:59
util3d_registration.h
glm::cos
GLM_FUNC_DECL genType cos(genType const &angle)
UStl.h
Wrappers of STL for convenient functions.
UDEBUG
#define UDEBUG(...)
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:119
float
float
uNormSquared
T uNormSquared(const std::vector< T > &v)
Definition: UMath.h:558
rtabmap::Transform::r23
float r23() const
Definition: Transform.h:67
transform
EIGEN_DONT_INLINE void transform(const Quaternion< Scalar > &t, Data &data)
glm::atan
GLM_FUNC_DECL genType atan(genType const &y, genType const &x)
rtabmap::Transform::r12
float r12() const
Definition: Transform.h:63
t
Point2 t(10, 10)
rtabmap::util3d::computeReprojErrors
std::vector< float > computeReprojErrors(std::vector< cv::Point3f > opoints, std::vector< cv::Point2f > ipoints, const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, const cv::Mat &rvec, const cv::Mat &tvec, float reprojErrorThreshold, std::vector< int > &inliers)
Definition: util3d_motion_estimation.cpp:810
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116
rtabmap::Transform::r32
float r32() const
Definition: Transform.h:69
UERROR
#define UERROR(...)
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
i
int i
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:53
v2
v2
R
R
rtabmap::Transform::r11
float r11() const
Definition: Transform.h:62

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:46:07