OdometryORBSLAM3.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/OdometryInfo.h"
29 #include "rtabmap/core/util2d.h"
30 #include "rtabmap/core/util3d_transforms.h"
31 #include "rtabmap/utilite/ULogger.h"
32 #include "rtabmap/utilite/UTimer.h"
33 #include "rtabmap/utilite/UStl.h"
34 #include "rtabmap/utilite/UDirectory.h"
35 #include <pcl/common/transforms.h>
36 #include <opencv2/imgproc/types_c.h>
37 #include <rtabmap/core/odometry/OdometryORBSLAM3.h>
38 
39 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
40 #include <thread>
41 #include <Converter.h>
42 
43 using namespace std;
44 
45 #endif
46 
47 namespace rtabmap {
48 
49 OdometryORBSLAM3::OdometryORBSLAM3(const ParametersMap & parameters) :
50  Odometry(parameters)
51 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
52  ,
53  orbslam_(0),
54  firstFrame_(true),
55  previousPose_(Transform::getIdentity()),
56  useIMU_(Parameters::defaultOdomORBSLAMInertial()),
57  parameters_(parameters),
58  lastImuStamp_(0.0),
59  lastImageStamp_(0.0)
60 #endif
61 {
62 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
63  Parameters::parse(parameters, Parameters::kOdomORBSLAMInertial(), useIMU_);
64 #endif
65 }
66 
67 OdometryORBSLAM3::~OdometryORBSLAM3()
68 {
69 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
70  if(orbslam_)
71  {
72  orbslam_->Shutdown();
73  delete orbslam_;
74  }
75 #endif
76 }
77 
78 void OdometryORBSLAM3::reset(const Transform & initialPose)
79 {
80  Odometry::reset(initialPose);
81 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
82  if(orbslam_)
83  {
84  orbslam_->Shutdown();
85  delete orbslam_;
86  orbslam_=0;
87  }
88  firstFrame_ = true;
89  originLocalTransform_.setNull();
90  previousPose_.setIdentity();
91  imuLocalTransform_.setNull();
92  lastImuStamp_ = 0.0;
93  lastImageStamp_ = 0.0;
94 #endif
95 }
96 
97 bool OdometryORBSLAM3::canProcessAsyncIMU() const
98 {
99 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
100  return useIMU_;
101 #else
102  return false;
103 #endif
104 }
105 
106 bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap::CameraModel & model2, double stamp, bool stereo, double baseline)
107 {
108 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
109  std::string vocabularyPath;
110  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMVocPath(), vocabularyPath);
111 
112  if(vocabularyPath.empty())
113  {
114  UERROR("ORB_SLAM vocabulary path should be set! (Parameter name=\"%s\")", rtabmap::Parameters::kOdomORBSLAMVocPath().c_str());
115  return false;
116  }
117  //Load ORB Vocabulary
118  vocabularyPath = uReplaceChar(vocabularyPath, '~', UDirectory::homeDir());
119  UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str());
120 
121  // Create configuration file
122  std::string workingDir;
123  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kRtabmapWorkingDirectory(), workingDir);
124  if(workingDir.empty())
125  {
126  workingDir = ".";
127  }
128  std::string configPath = workingDir+"/rtabmap_orbslam.yaml";
129  std::ofstream ofs (configPath, std::ofstream::out);
130  ofs << "%YAML:1.0" << std::endl;
131  ofs << std::endl;
132 
133  ofs << "File.version: \"1.0\"" << std::endl;
134  ofs << std::endl;
135 
136  ofs << "Camera.type: \"PinHole\"" << std::endl;
137  ofs << std::endl;
138 
139  ofs << fixed << setprecision(13);
140 
141  for(int i=1; i<(stereo?3:2); ++i)
142  {
143  const CameraModel & model = i==1?model1:model2;
144 
145  //# Camera calibration and distortion parameters (OpenCV)
146  ofs << "Camera" << i << ".fx: " << model.fx() << std::endl;
147  ofs << "Camera" << i << ".fy: " << model.fy() << std::endl;
148  ofs << "Camera" << i << ".cx: " << model.cx() << std::endl;
149  ofs << "Camera" << i << ".cy: " << model.cy() << std::endl;
150  ofs << std::endl;
151 
152  if(model.D().cols < 4)
153  {
154  ofs << "Camera" << i << ".k1: " << 0.0 << std::endl;
155  ofs << "Camera" << i << ".k2: " << 0.0 << std::endl;
156  ofs << "Camera" << i << ".p1: " << 0.0 << std::endl;
157  ofs << "Camera" << i << ".p2: " << 0.0 << std::endl;
158  if(!stereo)
159  {
160  ofs << "Camera" << i << ".k3: " << 0.0 << std::endl;
161  }
162  }
163  if(model.D().cols >= 4)
164  {
165  ofs << "Camera" << i << ".k1: " << model.D().at<double>(0,0) << std::endl;
166  ofs << "Camera" << i << ".k2: " << model.D().at<double>(0,1) << std::endl;
167  ofs << "Camera" << i << ".p1: " << model.D().at<double>(0,2) << std::endl;
168  ofs << "Camera" << i << ".p2: " << model.D().at<double>(0,3) << std::endl;
169  }
170  if(model.D().cols >= 5)
171  {
172  ofs << "Camera" << i << ".k3: " << model.D().at<double>(0,4) << std::endl;
173  }
174  if(model.D().cols > 5)
175  {
176  UWARN("Unhandled camera distortion size %d, only 5 first coefficients used", model.D().cols);
177  }
178  ofs << std::endl;
179  }
180 
181  //# IR projector baseline times fx (aprox.)
182  if(baseline <= 0.0)
183  {
184  baseline = rtabmap::Parameters::defaultOdomORBSLAMBf();
185  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMBf(), baseline);
186  }
187  else
188  {
189  // # Transformation matrix from right camera to left camera
190  ofs << "Stereo.T_c1_c2: !!opencv-matrix" << std::endl;
191  ofs << " rows: 4" << std::endl;
192  ofs << " cols: 4" << std::endl;
193  ofs << " dt: f" << std::endl;
194  ofs << " data: [" << 1 << ", " << 0 << ", " << 0 << ", " << baseline << ", " << std::endl;
195  ofs << " " << 0 << ", " << 1 << ", " << 0 << ", " << 0 << ", " << std::endl;
196  ofs << " " << 0 << ", " << 0 << ", " << 1 << ", " << 0 << ", " << std::endl;
197  ofs << " 0.0, 0.0, 0.0, 1.0]" << std::endl;
198  ofs << std::endl;
199  }
200  ofs << "Camera.bf: " << model1.fx()*baseline << std::endl;
201 
202  ofs << "Camera.width: " << model1.imageWidth() << std::endl;
203  ofs << "Camera.height: " << model1.imageHeight() << std::endl;
204  ofs << std::endl;
205 
206  //# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
207  //Camera.RGB: 1
208  ofs << "Camera.RGB: 0" << std::endl;
209  ofs << std::endl;
210 
211  float fps = rtabmap::Parameters::defaultOdomORBSLAMFps();
212  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMFps(), fps);
213  if(fps == 0)
214  {
215  UASSERT(stamp > lastImageStamp_);
216  fps = std::round(1./(stamp - lastImageStamp_));
217  UWARN("Camera FPS estimated at %d Hz. If this doesn't look good, "
218  "set explicitly parameter %s to expected frequency.",
219  int(fps), Parameters::kOdomORBSLAMFps().c_str());
220  }
221  ofs << "Camera.fps: " << (int)fps << std::endl;
222  ofs << std::endl;
223 
224  //# Close/Far threshold. Baseline times.
225  double thDepth = rtabmap::Parameters::defaultOdomORBSLAMThDepth();
226  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMThDepth(), thDepth);
227  ofs << "Stereo.ThDepth: " << thDepth << std::endl;
228  ofs << "Stereo.b: " << baseline << std::endl;
229  ofs << std::endl;
230 
231  //# Deptmap values factor
232  ofs << "RGBD.DepthMapFactor: " << 1.0 << std::endl;
233  ofs << std::endl;
234 
235  bool withIMU = false;
236  if(!imuLocalTransform_.isNull())
237  {
238  withIMU = true;
239  //#--------------------------------------------------------------------------------------------
240  //# IMU Parameters TODO: hard-coded, not used
241  //#--------------------------------------------------------------------------------------------
242  // Transformation from camera 0 to body-frame (imu)
243  rtabmap::Transform camImuT = model1.localTransform()*imuLocalTransform_;
244  ofs << "IMU.T_b_c1: !!opencv-matrix" << std::endl;
245  ofs << " rows: 4" << std::endl;
246  ofs << " cols: 4" << std::endl;
247  ofs << " dt: f" << std::endl;
248  ofs << " data: [" << camImuT.data()[0] << ", " << camImuT.data()[1] << ", " << camImuT.data()[2] << ", " << camImuT.data()[3] << ", " << std::endl;
249  ofs << " " << camImuT.data()[4] << ", " << camImuT.data()[5] << ", " << camImuT.data()[6] << ", " << camImuT.data()[7] << ", " << std::endl;
250  ofs << " " << camImuT.data()[8] << ", " << camImuT.data()[9] << ", " << camImuT.data()[10] << ", " << camImuT.data()[11] << ", " << std::endl;
251  ofs << " 0.0, 0.0, 0.0, 1.0]" << std::endl;
252  ofs << std::endl;
253 
254  ofs << "IMU.InsertKFsWhenLost: " << 0 << std::endl;
255  ofs << std::endl;
256 
257  double gyroNoise = rtabmap::Parameters::defaultOdomORBSLAMGyroNoise();
258  double accNoise = rtabmap::Parameters::defaultOdomORBSLAMAccNoise();
259  double gyroWalk = rtabmap::Parameters::defaultOdomORBSLAMGyroWalk();
260  double accWalk = rtabmap::Parameters::defaultOdomORBSLAMAccWalk();
261  double samplingRate = rtabmap::Parameters::defaultOdomORBSLAMSamplingRate();
262  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMGyroNoise(), gyroNoise);
263  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMAccNoise(), accNoise);
264  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMGyroWalk(), gyroWalk);
265  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMAccWalk(), accWalk);
266  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMSamplingRate(), samplingRate);
267 
268  ofs << "IMU.NoiseGyro: " << gyroNoise << std::endl; // 1e-2
269  ofs << "IMU.NoiseAcc: " << accNoise << std::endl; // 1e-1
270  ofs << "IMU.GyroWalk: " << gyroWalk << std::endl; // 1e-6
271  ofs << "IMU.AccWalk: " << accWalk << std::endl; // 1e-4
272  if(samplingRate == 0)
273  {
274  // estimate rate from imu received.
275  UASSERT(orbslamImus_.size() > 1 && orbslamImus_[0].t < orbslamImus_[1].t);
276  samplingRate = 1./(orbslamImus_[1].t - orbslamImus_[0].t);
277  samplingRate = std::round(samplingRate);
278  UWARN("IMU sampling rate estimated at %.0f Hz. If this doesn't look good, "
279  "set explicitly parameter %s to expected frequency.",
280  samplingRate, Parameters::kOdomORBSLAMSamplingRate().c_str());
281  }
282  ofs << "IMU.Frequency: " << samplingRate << std::endl; // 200
283  ofs << std::endl;
284  }
285 
286 
287 
288  //#--------------------------------------------------------------------------------------------
289  //# ORB Parameters
290  //#--------------------------------------------------------------------------------------------
291  //# ORB Extractor: Number of features per image
292  int features = rtabmap::Parameters::defaultOdomORBSLAMMaxFeatures();
293  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMMaxFeatures(), features);
294  ofs << "ORBextractor.nFeatures: " << features << std::endl;
295  ofs << std::endl;
296 
297  //# ORB Extractor: Scale factor between levels in the scale pyramid
298  double scaleFactor = rtabmap::Parameters::defaultORBScaleFactor();
299  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kORBScaleFactor(), scaleFactor);
300  ofs << "ORBextractor.scaleFactor: " << scaleFactor << std::endl;
301  ofs << std::endl;
302 
303  //# ORB Extractor: Number of levels in the scale pyramid
304  int levels = rtabmap::Parameters::defaultORBNLevels();
305  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kORBNLevels(), levels);
306  ofs << "ORBextractor.nLevels: " << levels << std::endl;
307  ofs << std::endl;
308 
309  //# ORB Extractor: Fast threshold
310  //# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
311  //# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
312  //# You can lower these values if your images have low contrast
313  int iniThFAST = rtabmap::Parameters::defaultFASTThreshold();
314  int minThFAST = rtabmap::Parameters::defaultFASTMinThreshold();
315  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kFASTThreshold(), iniThFAST);
316  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kFASTMinThreshold(), minThFAST);
317  ofs << "ORBextractor.iniThFAST: " << iniThFAST << std::endl;
318  ofs << "ORBextractor.minThFAST: " << minThFAST << std::endl;
319  ofs << std::endl;
320 
321  int maxFeatureMapSize = rtabmap::Parameters::defaultOdomORBSLAMMapSize();
322  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMMapSize(), maxFeatureMapSize);
323 
324  //# Disable loop closure detection
325  ofs << "loopClosing: " << 0 << std::endl;
326  ofs << std::endl;
327 
328  //# Set dummy Viewer parameters
329  ofs << "Viewer.KeyFrameSize: " << 0.05 << std::endl;
330  ofs << "Viewer.KeyFrameLineWidth: " << 1.0 << std::endl;
331  ofs << "Viewer.GraphLineWidth: " << 0.9 << std::endl;
332  ofs << "Viewer.PointSize: " << 2.0 << std::endl;
333  ofs << "Viewer.CameraSize: " << 0.08 << std::endl;
334  ofs << "Viewer.CameraLineWidth: " << 3.0 << std::endl;
335  ofs << "Viewer.ViewpointX: " << 0.0 << std::endl;
336  ofs << "Viewer.ViewpointY: " << -0.7 << std::endl;
337  ofs << "Viewer.ViewpointZ: " << -3.5 << std::endl;
338  ofs << "Viewer.ViewpointF: " << 500.0 << std::endl;
339  ofs << std::endl;
340 
341  ofs.close();
342 
343  orbslam_ = new ORB_SLAM3::System(
344  vocabularyPath,
345  configPath,
346  stereo && withIMU?ORB_SLAM3::System::IMU_STEREO:
347  stereo?ORB_SLAM3::System::STEREO:
348  withIMU?ORB_SLAM3::System::IMU_RGBD:
349  ORB_SLAM3::System::RGBD,
350  false);
351  return true;
352 #else
353  UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
354 #endif
355  return false;
356 }
357 
358 // return not null transform if odometry is correctly computed
359 Transform OdometryORBSLAM3::computeTransform(
360  SensorData & data,
361  const Transform & guess,
362  OdometryInfo * info)
363 {
364  Transform t;
365 
366 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
367  UTimer timer;
368 
369  if(useIMU_)
370  {
371  bool added = false;
372  if(!data.imu().empty())
373  {
374  if(lastImuStamp_ == 0.0 || lastImuStamp_ < data.stamp())
375  {
376  orbslamImus_.push_back(ORB_SLAM3::IMU::Point(
377  data.imu().linearAcceleration().val[0],
378  data.imu().linearAcceleration().val[1],
379  data.imu().linearAcceleration().val[2],
380  data.imu().angularVelocity().val[0],
381  data.imu().angularVelocity().val[1],
382  data.imu().angularVelocity().val[2],
383  data.stamp()));
384  lastImuStamp_ = data.stamp();
385  added = true;
386  }
387  else
388  {
389  UERROR("Received IMU with stamp (%f) <= than the previous IMU (%f), ignoring it!", data.stamp(), lastImuStamp_);
390  }
391  }
392 
393  if(orbslam_ == 0)
394  {
395  // We need two samples to estimate imu frame rate
396  if(orbslamImus_.size()>1 && added)
397  {
398  imuLocalTransform_ = data.imu().localTransform();
399  }
400  }
401 
402  if(data.imageRaw().empty() || imuLocalTransform_.isNull())
403  {
404  return Transform();
405  }
406  }
407 
408  if(data.imageRaw().empty() ||
409  data.imageRaw().rows != data.depthOrRightRaw().rows ||
410  data.imageRaw().cols != data.depthOrRightRaw().cols)
411  {
412  UERROR("Not supported input! RGB (%dx%d) and depth (%dx%d) should have the same size.",
413  data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows);
414  return t;
415  }
416 
417  if(!((data.cameraModels().size() == 1 &&
418  data.cameraModels()[0].isValidForReprojection()) ||
419  (data.stereoCameraModels().size() == 1 &&
420  data.stereoCameraModels()[0].isValidForProjection())))
421  {
422  UERROR("Invalid camera model!");
423  return t;
424  }
425 
426  bool stereo = data.cameraModels().size() == 0;
427 
428  cv::Mat covariance;
429  if(orbslam_ == 0)
430  {
431  // We need two frames to estimate camera frame rate
432  if(lastImageStamp_ == 0.0)
433  {
434  lastImageStamp_ = data.stamp();
435  return t;
436  }
437 
438  CameraModel model = data.cameraModels().size()==1?data.cameraModels()[0]:data.stereoCameraModels()[0].left();
439  if(!init(model, !stereo?model:data.stereoCameraModels()[0].right(), data.stamp(), stereo, data.cameraModels().size()==1?0.0:data.stereoCameraModels()[0].baseline()))
440  {
441  return t;
442  }
443  }
444 
445  Sophus::SE3f Tcw;
446  Transform localTransform;
447  if(stereo)
448  {
449  localTransform = data.stereoCameraModels()[0].localTransform();
450  cv::Mat leftMono = data.imageRaw();
451  if(data.imageRaw().channels() == 3) {
452  leftMono = cv::Mat();
453  cv::cvtColor(data.imageRaw(), leftMono, CV_BGR2GRAY);
454  }
455  Tcw = orbslam_->TrackStereo(leftMono, data.rightRaw(), data.stamp(), orbslamImus_);
456  orbslamImus_.clear();
457  }
458  else
459  {
460  localTransform = data.cameraModels()[0].localTransform();
461  cv::Mat depth;
462  if(data.depthRaw().type() == CV_32FC1)
463  {
464  depth = data.depthRaw();
465  }
466  else if(data.depthRaw().type() == CV_16UC1)
467  {
468  depth = util2d::cvtDepthToFloat(data.depthRaw());
469  }
470  Tcw = orbslam_->TrackRGBD(data.imageRaw(), depth, data.stamp(), orbslamImus_);
471  orbslamImus_.clear();
472  }
473 
474  Transform previousPoseInv = previousPose_.inverse();
475  std::vector<ORB_SLAM3::MapPoint*> mapPoints = orbslam_->GetTrackedMapPoints();
476  if(orbslam_->isLost() || mapPoints.empty())
477  {
478  covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
479  }
480  else
481  {
482  cv::Mat TcwMat = ORB_SLAM3::Converter::toCvMat(ORB_SLAM3::Converter::toSE3Quat(Tcw)).clone();
483  UASSERT(TcwMat.cols == 4 && TcwMat.rows == 4);
484  Transform p = Transform(cv::Mat(TcwMat, cv::Range(0,3), cv::Range(0,4)));
485 
486  if(!p.isNull())
487  {
488  if(!localTransform.isNull())
489  {
490  if(originLocalTransform_.isNull())
491  {
492  originLocalTransform_ = localTransform;
493  }
494  // transform in base frame
495  p = originLocalTransform_ * p.inverse() * localTransform.inverse();
496  }
497  t = previousPoseInv*p;
498  }
499  previousPose_ = p;
500 
501  if(firstFrame_)
502  {
503  // just recovered of being lost, set high covariance
504  covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
505  firstFrame_ = false;
506  }
507  else
508  {
509  float baseline = data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline();
510  if(baseline <= 0.0f)
511  {
512  baseline = rtabmap::Parameters::defaultOdomORBSLAMBf();
513  rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMBf(), baseline);
514  }
515  double linearVar = 0.0001;
516  if(baseline > 0.0f)
517  {
518  linearVar = baseline/8.0;
519  linearVar *= linearVar;
520  }
521 
522  covariance = cv::Mat::eye(6,6, CV_64FC1);
523  covariance.at<double>(0,0) = linearVar;
524  covariance.at<double>(1,1) = linearVar;
525  covariance.at<double>(2,2) = linearVar;
526  covariance.at<double>(3,3) = 0.0001;
527  covariance.at<double>(4,4) = 0.0001;
528  covariance.at<double>(5,5) = 0.0001;
529  }
530  }
531 
532  if(info)
533  {
534  info->lost = t.isNull();
535  info->type = (int)kTypeORBSLAM;
536  info->reg.covariance = covariance;
537  info->localMapSize = mapPoints.size();
538  info->localKeyFrames = 0;
539 
540  if(this->isInfoDataFilled())
541  {
542  std::vector<cv::KeyPoint> kpts = orbslam_->GetTrackedKeyPointsUn();
543  info->reg.matchesIDs.resize(kpts.size());
544  info->reg.inliersIDs.resize(kpts.size());
545  int oi = 0;
546 
547  UASSERT(mapPoints.size() == kpts.size());
548  for (unsigned int i = 0; i < kpts.size(); ++i)
549  {
550  int wordId;
551  if(mapPoints[i] != 0)
552  {
553  wordId = mapPoints[i]->mnId;
554  }
555  else
556  {
557  wordId = -(i+1);
558  }
559  info->words.insert(std::make_pair(wordId, kpts[i]));
560  if(mapPoints[i] != 0)
561  {
562  info->reg.matchesIDs[oi] = wordId;
563  info->reg.inliersIDs[oi] = wordId;
564  ++oi;
565  }
566  }
567  info->reg.matchesIDs.resize(oi);
568  info->reg.inliersIDs.resize(oi);
569  info->reg.inliers = oi;
570  info->reg.matches = oi;
571 
572  Eigen::Affine3f fixRot = (this->getPose()*previousPoseInv*originLocalTransform_).toEigen3f();
573  for (unsigned int i = 0; i < mapPoints.size(); ++i)
574  {
575  if(mapPoints[i])
576  {
577  Eigen::Vector3f pt = mapPoints[i]->GetWorldPos();
578  pcl::PointXYZ ptt = pcl::transformPoint(pcl::PointXYZ(pt[0], pt[1], pt[2]), fixRot);
579  info->localMap.insert(std::make_pair(mapPoints[i]->mnId, cv::Point3f(ptt.x, ptt.y, ptt.z)));
580  }
581  }
582  }
583  }
584 
585  UINFO("Odom update time = %fs, map points=%ld, lost=%s", timer.elapsed(), mapPoints.size(), t.isNull()?"true":"false");
586 
587 #else
588  UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
589 #endif
590  return t;
591 }
592 
593 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::OdometryORBSLAM3::init
bool init(const rtabmap::CameraModel &model1, const rtabmap::CameraModel &model2, double stamp, bool stereo, double baseline)
Definition: OdometryORBSLAM3.cpp:106
int
int
UINFO
#define UINFO(...)
OdometryORBSLAM3.h
OdometryInfo.h
Eigen::Transform
timer
rtabmap::OdometryORBSLAM3::~OdometryORBSLAM3
virtual ~OdometryORBSLAM3()
Definition: OdometryORBSLAM3.cpp:67
rtabmap::OdometryORBSLAM3::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: OdometryORBSLAM3.cpp:78
UDirectory.h
model2
noiseModel::Unit::shared_ptr model2
rtabmap::CameraModel
Definition: CameraModel.h:38
if
if(ret >=0) *info
true
#define true
Definition: ConvertUTF.c:57
rtabmap::util2d::cvtDepthToFloat
cv::Mat RTABMAP_CORE_EXPORT cvtDepthToFloat(const cv::Mat &depth16U)
Definition: util2d.cpp:928
rtabmap::OdometryORBSLAM3::canProcessAsyncIMU
virtual bool canProcessAsyncIMU() const
Definition: OdometryORBSLAM3.cpp:97
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UTimer.h
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
glm::round
GLM_FUNC_DECL genType round(genType const &x)
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
rtabmap::OdometryORBSLAM3::computeTransform
virtual Transform computeTransform(SensorData &image, const Transform &guess=Transform(), OdometryInfo *info=0)
Definition: OdometryORBSLAM3.cpp:359
UDirectory::homeDir
static std::string homeDir()
Definition: UDirectory.cpp:355
rtabmap::Odometry::getPose
const Transform & getPose() const
Definition: Odometry.h:76
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
data
int data[]
util3d_transforms.h
rtabmap::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:210
info
else if n * info
UASSERT
#define UASSERT(condition)
p
Point3_ p(2)
rtabmap::Parameters
Definition: Parameters.h:170
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
timer::elapsed
double elapsed() const
uReplaceChar
std::string UTILITE_EXPORT uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:33
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
util2d.h
std
rtabmap::Odometry::kTypeORBSLAM
@ kTypeORBSLAM
Definition: Odometry.h:52
c_str
const char * c_str(Args &&...args)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Odometry
Definition: Odometry.h:42
UTimer
Definition: UTimer.h:46
rtabmap::OdometryInfo
Definition: OdometryInfo.h:40
rtabmap::Transform::clone
Transform clone() const
Definition: Transform.cpp:102
t
Point2 t(10, 10)
model1
noiseModel::Isotropic::shared_ptr model1
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
rtabmap::Transform::data
const float * data() const
Definition: Transform.h:88
i
int i
baseline
double baseline
rtabmap::Odometry::isInfoDataFilled
bool isInfoDataFilled() const
Definition: Odometry.h:77

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