OdometryFovis.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/odometry/OdometryFovis.h"
29 #include "rtabmap/core/OdometryInfo.h"
30 #include "rtabmap/core/util2d.h"
31 #include "rtabmap/utilite/ULogger.h"
32 #include "rtabmap/utilite/UTimer.h"
33 #include "rtabmap/utilite/UStl.h"
34 #include <opencv2/imgproc/types_c.h>
35 
36 #ifdef RTABMAP_FOVIS
37 #include <libfovis/fovis.hpp>
38 #endif
39 
40 namespace rtabmap {
41 
42 OdometryFovis::OdometryFovis(const ParametersMap & parameters) :
43  Odometry(parameters)
44 #ifdef RTABMAP_FOVIS
45  ,
46  fovis_(0),
47  rect_(0),
48  stereoCalib_(0),
49  depthImage_(0),
50  stereoDepth_(0),
51  lost_(false)
52 #endif
53 {
54  fovisParameters_ = Parameters::filterParameters(parameters, "OdomFovis");
55  if(parameters.find(Parameters::kOdomVisKeyFrameThr()) != parameters.end())
56  {
57  fovisParameters_.insert(*parameters.find(Parameters::kOdomVisKeyFrameThr()));
58  }
59 }
60 
61 OdometryFovis::~OdometryFovis()
62 {
63 #ifdef RTABMAP_FOVIS
64  delete fovis_;
65  delete rect_;
66  delete stereoCalib_;
67  delete depthImage_;
68  delete stereoDepth_;
69 #endif
70 }
71 
72 void OdometryFovis::reset(const Transform & initialPose)
73 {
74  Odometry::reset(initialPose);
75 #ifdef RTABMAP_FOVIS
76  if(fovis_)
77  {
78  delete fovis_;
79  fovis_ = 0;
80  }
81  if(rect_)
82  {
83  delete rect_;
84  rect_ = 0;
85  }
86  if(stereoCalib_)
87  {
88  delete stereoCalib_;
89  stereoCalib_ = 0;
90  }
91  if(depthImage_)
92  {
93  delete depthImage_;
94  depthImage_ = 0;
95  }
96  if(stereoDepth_)
97  {
98  delete stereoDepth_;
99  stereoDepth_ = 0;
100  }
101  lost_ = false;
102  previousLocalTransform_.setNull();
103 #endif
104 }
105 
106 // return not null transform if odometry is correctly computed
107 Transform OdometryFovis::computeTransform(
108  SensorData & data,
109  const Transform & guess,
110  OdometryInfo * info)
111 {
112  UDEBUG("");
113  Transform t;
114 
115 #ifdef RTABMAP_FOVIS
116  UTimer timer;
117 
118  if(data.imageRaw().empty() ||
119  data.imageRaw().rows != data.depthOrRightRaw().rows ||
120  data.imageRaw().cols != data.depthOrRightRaw().cols)
121  {
122  UERROR("Not supported input!");
123  return t;
124  }
125 
126  if(!((data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForReprojection()) ||
127  (data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection())))
128  {
129  UERROR("Invalid camera model! Mono cameras=%d (reproj=%d), Stereo cameras=%d (reproj=%d)",
130  (int)data.cameraModels().size(),
131  data.cameraModels().size() && data.cameraModels()[0].isValidForReprojection()?1:0,
132  (int)data.stereoCameraModels().size(),
133  data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection()?1:0);
134  return t;
135  }
136 
137  cv::Mat gray;
138  if(data.imageRaw().type() == CV_8UC3)
139  {
140  cv::cvtColor(data.imageRaw(), gray, CV_BGR2GRAY);
141  }
142  else if(data.imageRaw().type() == CV_8UC1)
143  {
144  gray = data.imageRaw();
145  }
146  else
147  {
148  UFATAL("Not supported color type!");
149  }
150 
151  fovis::VisualOdometryOptions options;
152  if(fovis_ == 0 || (data.cameraModels().size() != 1 && stereoDepth_ == 0))
153  {
154  options = fovis::VisualOdometry::getDefaultOptions();
155 
156  ParametersMap defaults = Parameters::getDefaultParameters("OdomFovis");
157  options["feature-window-size"] = uValue(fovisParameters_, Parameters::kOdomFovisFeatureWindowSize(), defaults.at(Parameters::kOdomFovisFeatureWindowSize()));
158  options["max-pyramid-level"] = uValue(fovisParameters_, Parameters::kOdomFovisMaxPyramidLevel(), defaults.at(Parameters::kOdomFovisMaxPyramidLevel()));
159  options["min-pyramid-level"] = uValue(fovisParameters_, Parameters::kOdomFovisMinPyramidLevel(), defaults.at(Parameters::kOdomFovisMinPyramidLevel()));
160  options["target-pixels-per-feature"] = uValue(fovisParameters_, Parameters::kOdomFovisTargetPixelsPerFeature(), defaults.at(Parameters::kOdomFovisTargetPixelsPerFeature()));
161  options["fast-threshold"] = uValue(fovisParameters_, Parameters::kOdomFovisFastThreshold(), defaults.at(Parameters::kOdomFovisFastThreshold()));
162  options["use-adaptive-threshold"] = uValue(fovisParameters_, Parameters::kOdomFovisUseAdaptiveThreshold(), defaults.at(Parameters::kOdomFovisUseAdaptiveThreshold()));
163  options["fast-threshold-adaptive-gain"] = uValue(fovisParameters_, Parameters::kOdomFovisFastThresholdAdaptiveGain(), defaults.at(Parameters::kOdomFovisFastThresholdAdaptiveGain()));
164  options["use-homography-initialization"] = uValue(fovisParameters_, Parameters::kOdomFovisUseHomographyInitialization(), defaults.at(Parameters::kOdomFovisUseHomographyInitialization()));
165  options["ref-frame-change-threshold"] = uValue(fovisParameters_, Parameters::kOdomVisKeyFrameThr(), uNumber2Str(Parameters::defaultOdomVisKeyFrameThr()));
166 
167  // OdometryFrame
168  options["use-bucketing"] = uValue(fovisParameters_, Parameters::kOdomFovisUseBucketing(), defaults.at(Parameters::kOdomFovisUseBucketing()));
169  options["bucket-width"] = uValue(fovisParameters_, Parameters::kOdomFovisBucketWidth(), defaults.at(Parameters::kOdomFovisBucketWidth()));
170  options["bucket-height"] = uValue(fovisParameters_, Parameters::kOdomFovisBucketHeight(), defaults.at(Parameters::kOdomFovisBucketHeight()));
171  options["max-keypoints-per-bucket"] = uValue(fovisParameters_, Parameters::kOdomFovisMaxKeypointsPerBucket(), defaults.at(Parameters::kOdomFovisMaxKeypointsPerBucket()));
172  options["use-image-normalization"] = uValue(fovisParameters_, Parameters::kOdomFovisUseImageNormalization(), defaults.at(Parameters::kOdomFovisUseImageNormalization()));
173 
174  // MotionEstimator
175  options["inlier-max-reprojection-error"] = uValue(fovisParameters_, Parameters::kOdomFovisInlierMaxReprojectionError(), defaults.at(Parameters::kOdomFovisInlierMaxReprojectionError()));
176  options["clique-inlier-threshold"] = uValue(fovisParameters_, Parameters::kOdomFovisCliqueInlierThreshold(), defaults.at(Parameters::kOdomFovisCliqueInlierThreshold()));
177  options["min-features-for-estimate"] = uValue(fovisParameters_, Parameters::kOdomFovisMinFeaturesForEstimate(), defaults.at(Parameters::kOdomFovisMinFeaturesForEstimate()));
178  options["max-mean-reprojection-error"] = uValue(fovisParameters_, Parameters::kOdomFovisMaxMeanReprojectionError(), defaults.at(Parameters::kOdomFovisMaxMeanReprojectionError()));
179  options["use-subpixel-refinement"] = uValue(fovisParameters_, Parameters::kOdomFovisUseSubpixelRefinement(), defaults.at(Parameters::kOdomFovisUseSubpixelRefinement()));
180  options["feature-search-window"] = uValue(fovisParameters_, Parameters::kOdomFovisFeatureSearchWindow(), defaults.at(Parameters::kOdomFovisFeatureSearchWindow()));
181  options["update-target-features-with-refined"] = uValue(fovisParameters_, Parameters::kOdomFovisUpdateTargetFeaturesWithRefined(), defaults.at(Parameters::kOdomFovisUpdateTargetFeaturesWithRefined()));
182 
183  // StereoDepth
184  options["stereo-require-mutual-match"] = uValue(fovisParameters_, Parameters::kOdomFovisStereoRequireMutualMatch(), defaults.at(Parameters::kOdomFovisStereoRequireMutualMatch()));
185  options["stereo-max-dist-epipolar-line"] = uValue(fovisParameters_, Parameters::kOdomFovisStereoMaxDistEpipolarLine(), defaults.at(Parameters::kOdomFovisStereoMaxDistEpipolarLine()));
186  options["stereo-max-refinement-displacement"] = uValue(fovisParameters_, Parameters::kOdomFovisStereoMaxRefinementDisplacement(), defaults.at(Parameters::kOdomFovisStereoMaxRefinementDisplacement()));
187  options["stereo-max-disparity"] = uValue(fovisParameters_, Parameters::kOdomFovisStereoMaxDisparity(), defaults.at(Parameters::kOdomFovisStereoMaxDisparity()));
188  }
189 
190  fovis::DepthSource * depthSource = 0;
191  cv::Mat depth;
192  cv::Mat right;
193  Transform localTransform = Transform::getIdentity();
194  if(data.cameraModels().size() == 1) //depth
195  {
196  UDEBUG("");
197  fovis::CameraIntrinsicsParameters rgb_params;
198  memset(&rgb_params, 0, sizeof(fovis::CameraIntrinsicsParameters));
199  rgb_params.width = data.cameraModels()[0].imageWidth();
200  rgb_params.height = data.cameraModels()[0].imageHeight();
201 
202  rgb_params.fx = data.cameraModels()[0].fx();
203  rgb_params.fy = data.cameraModels()[0].fy();
204  rgb_params.cx = data.cameraModels()[0].cx()==0.0?double(rgb_params.width) / 2.0:data.cameraModels()[0].cx();
205  rgb_params.cy = data.cameraModels()[0].cy()==0.0?double(rgb_params.height) / 2.0:data.cameraModels()[0].cy();
206  localTransform = data.cameraModels()[0].localTransform();
207 
208  if(rect_ == 0)
209  {
210  UINFO("Init rgbd fovis: %dx%d fx=%f fy=%f cx=%f cy=%f", rgb_params.width, rgb_params.height, rgb_params.fx, rgb_params.fy, rgb_params.cx, rgb_params.cy);
211  rect_ = new fovis::Rectification(rgb_params);
212  }
213 
214  if(depthImage_ == 0)
215  {
216  depthImage_ = new fovis::DepthImage(rgb_params, rgb_params.width, rgb_params.height);
217  }
218  // make sure all zeros are NAN
219  if(data.depthRaw().type() == CV_32FC1)
220  {
221  depth = data.depthRaw();
222  for(int i=0; i<depth.rows; ++i)
223  {
224  for(int j=0; j<depth.cols; ++j)
225  {
226  float & d = depth.at<float>(i,j);
227  if(d == 0.0f)
228  {
229  d = NAN;
230  }
231  }
232  }
233  }
234  else if(data.depthRaw().type() == CV_16UC1)
235  {
236  depth = cv::Mat(data.depthRaw().size(), CV_32FC1);
237  for(int i=0; i<data.depthRaw().rows; ++i)
238  {
239  for(int j=0; j<data.depthRaw().cols; ++j)
240  {
241  float d = float(data.depthRaw().at<unsigned short>(i,j))/1000.0f;
242  depth.at<float>(i, j) = d==0.0f?NAN:d;
243  }
244  }
245  }
246  else
247  {
248  UFATAL("Unknown depth format!");
249  }
250  depthImage_->setDepthImage((float*)depth.data);
251  depthSource = depthImage_;
252  }
253  else if(data.stereoCameraModels().size() == 1) // stereo
254  {
255  UDEBUG("");
256  // initialize left camera parameters
257  fovis::CameraIntrinsicsParameters left_parameters;
258  left_parameters.width = data.stereoCameraModels()[0].left().imageWidth();
259  left_parameters.height = data.stereoCameraModels()[0].left().imageHeight();
260  left_parameters.fx = data.stereoCameraModels()[0].left().fx();
261  left_parameters.fy = data.stereoCameraModels()[0].left().fy();
262  left_parameters.cx = data.stereoCameraModels()[0].left().cx()==0.0?double(left_parameters.width) / 2.0:data.stereoCameraModels()[0].left().cx();
263  left_parameters.cy = data.stereoCameraModels()[0].left().cy()==0.0?double(left_parameters.height) / 2.0:data.stereoCameraModels()[0].left().cy();
264  localTransform = data.stereoCameraModels()[0].localTransform();
265 
266  if(rect_ == 0)
267  {
268  UINFO("Init stereo fovis: %dx%d fx=%f fy=%f cx=%f cy=%f", left_parameters.width, left_parameters.height, left_parameters.fx, left_parameters.fy, left_parameters.cx, left_parameters.cy);
269  rect_ = new fovis::Rectification(left_parameters);
270  }
271 
272  if(stereoCalib_ == 0)
273  {
274  // initialize right camera parameters
275  fovis::CameraIntrinsicsParameters right_parameters;
276  right_parameters.width = data.stereoCameraModels()[0].right().imageWidth();
277  right_parameters.height = data.stereoCameraModels()[0].right().imageHeight();
278  right_parameters.fx = data.stereoCameraModels()[0].right().fx();
279  right_parameters.fy = data.stereoCameraModels()[0].right().fy();
280  right_parameters.cx = data.stereoCameraModels()[0].right().cx()==0.0?double(right_parameters.width) / 2.0:data.stereoCameraModels()[0].right().cx();
281  right_parameters.cy = data.stereoCameraModels()[0].right().cy()==0.0?double(right_parameters.height) / 2.0:data.stereoCameraModels()[0].right().cy();
282 
283  // as we use rectified images, rotation is identity
284  // and translation is baseline only
285  fovis::StereoCalibrationParameters stereo_parameters;
286  stereo_parameters.left_parameters = left_parameters;
287  stereo_parameters.right_parameters = right_parameters;
288  stereo_parameters.right_to_left_rotation[0] = 1.0;
289  stereo_parameters.right_to_left_rotation[1] = 0.0;
290  stereo_parameters.right_to_left_rotation[2] = 0.0;
291  stereo_parameters.right_to_left_rotation[3] = 0.0;
292  stereo_parameters.right_to_left_translation[0] = -data.stereoCameraModels()[0].baseline();
293  stereo_parameters.right_to_left_translation[1] = 0.0;
294  stereo_parameters.right_to_left_translation[2] = 0.0;
295 
296  stereoCalib_ = new fovis::StereoCalibration(stereo_parameters);
297  }
298 
299  if(stereoDepth_ == 0)
300  {
301  stereoDepth_ = new fovis::StereoDepth(stereoCalib_, options);
302  }
303  if(data.rightRaw().type() == CV_8UC3)
304  {
305  cv::cvtColor(data.rightRaw(), right, CV_BGR2GRAY);
306  }
307  else if(data.rightRaw().type() == CV_8UC1)
308  {
309  right = data.rightRaw();
310  }
311  else
312  {
313  UFATAL("Not supported color type!");
314  }
315  stereoDepth_->setRightImage(right.data);
316  depthSource = stereoDepth_;
317  }
318 
319  if(fovis_ == 0)
320  {
321  fovis_ = new fovis::VisualOdometry(rect_, options);
322  }
323 
324  UDEBUG("");
325  fovis_->processFrame(gray.data, depthSource);
326 
327  // get the motion estimate for this frame to the previous frame.
328  t = Transform::fromEigen3d(fovis_->getMotionEstimate());
329 
330  cv::Mat covariance;
331  fovis::MotionEstimateStatusCode statusCode = fovis_->getMotionEstimator()->getMotionEstimateStatus();
332  if(statusCode > fovis::SUCCESS)
333  {
334  UWARN("Fovis error status: %s", fovis::MotionEstimateStatusCodeStrings[statusCode]);
335  t.setNull();
336  lost_ = true;
337  covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
338  previousLocalTransform_.setNull();
339  }
340  else if(lost_)
341  {
342  lost_ = false;
343  // we are not lost anymore but we don't know where we are now according to last valid pose
344  covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
345  previousLocalTransform_.setNull();
346  }
347  else
348  {
349  const Eigen::MatrixXd& cov = fovis_->getMotionEstimator()->getMotionEstimateCov();
350  if(cov.cols() == 6 && cov.rows() == 6 && cov(0,0) > 0.0)
351  {
352  covariance = cv::Mat::eye(6,6, CV_64FC1);
353  memcpy(covariance.data, cov.data(), 36*sizeof(double));
354  covariance *= 100.0; // to be in the same scale than loop closure detection
355  }
356  }
357 
358  if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
359  {
360  // from camera frame to base frame
361  if(!previousLocalTransform_.isNull())
362  {
363  t = previousLocalTransform_ * t * localTransform.inverse();
364  }
365  else
366  {
367  t = localTransform * t * localTransform.inverse();
368  }
369  previousLocalTransform_ = localTransform;
370  }
371 
372  if(info)
373  {
374  info->type = (int)kTypeFovis;
375  info->keyFrameAdded = fovis_->getChangeReferenceFrames();
376  info->features = fovis_->getTargetFrame()->getNumDetectedKeypoints();
377  info->reg.matches = fovis_->getMotionEstimator()->getNumMatches();
378  info->reg.inliers = fovis_->getMotionEstimator()->getNumInliers();
379  info->reg.covariance = covariance;
380 
381  if(this->isInfoDataFilled())
382  {
383  const fovis::FeatureMatch * matches = fovis_->getMotionEstimator()->getMatches();
384  int numMatches = fovis_->getMotionEstimator()->getNumMatches();
385  if(matches && numMatches>0)
386  {
387  info->refCorners.resize(numMatches);
388  info->newCorners.resize(numMatches);
389  info->cornerInliers.resize(numMatches);
390  int oi=0;
391  for (int i = 0; i < numMatches; ++i)
392  {
393  info->refCorners[i].x = matches[i].ref_keypoint->base_uv[0];
394  info->refCorners[i].y = matches[i].ref_keypoint->base_uv[1];
395  info->newCorners[i].x = matches[i].target_keypoint->base_uv[0];
396  info->newCorners[i].y = matches[i].target_keypoint->base_uv[1];
397  info->cornerInliers[oi++] = i;
398  }
399  info->cornerInliers.resize(oi);
400  }
401  }
402  }
403 
404  UINFO("Odom update time = %fs status=%s", timer.elapsed(), fovis::MotionEstimateStatusCodeStrings[statusCode]);
405 
406 #else
407  UERROR("RTAB-Map is not built with FOVIS support! Select another visual odometry approach.");
408 #endif
409  return t;
410 }
411 
412 } // namespace rtabmap
d
d
rtabmap::OdometryInfo::refCorners
std::vector< cv::Point2f > refCorners
Definition: OdometryInfo.h:130
UTimer
Definition: UTimer.h:46
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
f
f
rtabmap::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:203
rtabmap::OdometryFovis::OdometryFovis
OdometryFovis(const rtabmap::ParametersMap &parameters=rtabmap::ParametersMap())
Definition: OdometryFovis.cpp:42
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:401
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:184
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:237
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UFATAL
#define UFATAL(...)
rtabmap::SensorData::rightRaw
cv::Mat rightRaw() const
Definition: SensorData.h:211
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Odometry::isInfoDataFilled
bool isInfoDataFilled() const
Definition: Odometry.h:77
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:236
rtabmap::Parameters::filterParameters
static ParametersMap filterParameters(const ParametersMap &parameters, const std::string &group, bool remove=false)
Definition: Parameters.cpp:217
rtabmap::OdometryInfo::cornerInliers
std::vector< int > cornerInliers
Definition: OdometryInfo.h:132
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:210
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
rtabmap::OdometryFovis::fovisParameters_
ParametersMap fovisParameters_
Definition: OdometryFovis.h:64
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::Transform::fromEigen3d
static Transform fromEigen3d(const Eigen::Affine3d &matrix)
Definition: Transform.cpp:425
false
#define false
Definition: ConvertUTF.c:56
matches
matches
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
Definition: Parameters.h:807
UDEBUG
#define UDEBUG(...)
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::OdometryInfo::newCorners
std::vector< cv::Point2f > newCorners
Definition: OdometryInfo.h:131
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:97
rtabmap::OdometryFovis::previousLocalTransform_
Transform previousLocalTransform_
Definition: OdometryFovis.h:65
rtabmap::OdometryFovis::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: OdometryFovis.cpp:72
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::OdometryFovis::computeTransform
virtual Transform computeTransform(SensorData &image, const Transform &guess=Transform(), OdometryInfo *info=0)
Definition: OdometryFovis.cpp:107
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:240
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29