35 #include <pcl/common/transforms.h>
36 #include <opencv2/imgproc/types_c.h>
39 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
41 #include <Converter.h>
51 #
if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
56 useIMU_(
Parameters::defaultOdomORBSLAMInertial()),
57 parameters_(parameters),
62 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
69 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
81 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
89 originLocalTransform_.setNull();
90 previousPose_.setIdentity();
91 imuLocalTransform_.setNull();
93 lastImageStamp_ = 0.0;
99 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
108 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
109 std::string vocabularyPath;
112 if(vocabularyPath.empty())
114 UERROR(
"ORB_SLAM vocabulary path should be set! (Parameter name=\"%s\")", rtabmap::Parameters::kOdomORBSLAMVocPath().
c_str());
119 UWARN(
"Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str());
122 std::string workingDir;
124 if(workingDir.empty())
128 std::string configPath = workingDir+
"/rtabmap_orbslam.yaml";
129 std::ofstream ofs (configPath, std::ofstream::out);
130 ofs <<
"%YAML:1.0" << std::endl;
133 ofs <<
"File.version: \"1.0\"" << std::endl;
136 ofs <<
"Camera.type: \"PinHole\"" << std::endl;
139 ofs << fixed << setprecision(13);
141 for(
int i=1;
i<(stereo?3:2); ++
i)
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;
152 if(
model.D().cols < 4)
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;
160 ofs <<
"Camera" <<
i <<
".k3: " << 0.0 << std::endl;
163 if(
model.D().cols >= 4)
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;
170 if(
model.D().cols >= 5)
172 ofs <<
"Camera" <<
i <<
".k3: " <<
model.D().at<
double>(0,4) << std::endl;
174 if(
model.D().cols > 5)
176 UWARN(
"Unhandled camera distortion size %d, only 5 first coefficients used",
model.D().cols);
184 baseline = rtabmap::Parameters::defaultOdomORBSLAMBf();
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;
202 ofs <<
"Camera.width: " <<
model1.imageWidth() << std::endl;
203 ofs <<
"Camera.height: " <<
model1.imageHeight() << std::endl;
208 ofs <<
"Camera.RGB: 0" << std::endl;
211 float fps = rtabmap::Parameters::defaultOdomORBSLAMFps();
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());
221 ofs <<
"Camera.fps: " << (
int)fps << std::endl;
225 double thDepth = rtabmap::Parameters::defaultOdomORBSLAMThDepth();
227 ofs <<
"Stereo.ThDepth: " << thDepth << std::endl;
228 ofs <<
"Stereo.b: " <<
baseline << std::endl;
232 ofs <<
"RGBD.DepthMapFactor: " << 1.0 << std::endl;
235 bool withIMU =
false;
236 if(!imuLocalTransform_.isNull())
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;
254 ofs <<
"IMU.InsertKFsWhenLost: " << 0 << std::endl;
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();
268 ofs <<
"IMU.NoiseGyro: " << gyroNoise << std::endl;
269 ofs <<
"IMU.NoiseAcc: " << accNoise << std::endl;
270 ofs <<
"IMU.GyroWalk: " << gyroWalk << std::endl;
271 ofs <<
"IMU.AccWalk: " << accWalk << std::endl;
272 if(samplingRate == 0)
275 UASSERT(orbslamImus_.size() > 1 && orbslamImus_[0].t < orbslamImus_[1].t);
276 samplingRate = 1./(orbslamImus_[1].t - orbslamImus_[0].t);
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());
282 ofs <<
"IMU.Frequency: " << samplingRate << std::endl;
292 int features = rtabmap::Parameters::defaultOdomORBSLAMMaxFeatures();
294 ofs <<
"ORBextractor.nFeatures: " << features << std::endl;
298 double scaleFactor = rtabmap::Parameters::defaultORBScaleFactor();
300 ofs <<
"ORBextractor.scaleFactor: " << scaleFactor << std::endl;
304 int levels = rtabmap::Parameters::defaultORBNLevels();
306 ofs <<
"ORBextractor.nLevels: " << levels << std::endl;
313 int iniThFAST = rtabmap::Parameters::defaultFASTThreshold();
314 int minThFAST = rtabmap::Parameters::defaultFASTMinThreshold();
317 ofs <<
"ORBextractor.iniThFAST: " << iniThFAST << std::endl;
318 ofs <<
"ORBextractor.minThFAST: " << minThFAST << std::endl;
321 int maxFeatureMapSize = rtabmap::Parameters::defaultOdomORBSLAMMapSize();
325 ofs <<
"loopClosing: " << 0 << std::endl;
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;
343 orbslam_ =
new ORB_SLAM3::System(
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,
353 UERROR(
"RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
366 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
372 if(!
data.imu().empty())
374 if(lastImuStamp_ == 0.0 || lastImuStamp_ <
data.stamp())
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],
384 lastImuStamp_ =
data.stamp();
389 UERROR(
"Received IMU with stamp (%f) <= than the previous IMU (%f), ignoring it!",
data.stamp(), lastImuStamp_);
396 if(orbslamImus_.size()>1 && added)
398 imuLocalTransform_ =
data.imu().localTransform();
402 if(
data.imageRaw().empty() || imuLocalTransform_.isNull())
408 if(
data.imageRaw().empty() ||
409 data.imageRaw().rows !=
data.depthOrRightRaw().rows ||
410 data.imageRaw().cols !=
data.depthOrRightRaw().cols)
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);
417 if(!((
data.cameraModels().size() == 1 &&
418 data.cameraModels()[0].isValidForReprojection()) ||
419 (
data.stereoCameraModels().size() == 1 &&
420 data.stereoCameraModels()[0].isValidForProjection())))
422 UERROR(
"Invalid camera model!");
426 bool stereo =
data.cameraModels().size() == 0;
432 if(lastImageStamp_ == 0.0)
434 lastImageStamp_ =
data.stamp();
439 if(!
init(
model, !stereo?
model:
data.stereoCameraModels()[0].right(),
data.stamp(), stereo,
data.cameraModels().size()==1?0.0:
data.stereoCameraModels()[0].baseline()))
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);
455 Tcw = orbslam_->TrackStereo(leftMono,
data.rightRaw(),
data.stamp(), orbslamImus_);
456 orbslamImus_.clear();
460 localTransform =
data.cameraModels()[0].localTransform();
462 if(
data.depthRaw().type() == CV_32FC1)
464 depth =
data.depthRaw();
466 else if(
data.depthRaw().type() == CV_16UC1)
470 Tcw = orbslam_->TrackRGBD(
data.imageRaw(), depth,
data.stamp(), orbslamImus_);
471 orbslamImus_.clear();
475 std::vector<ORB_SLAM3::MapPoint*> mapPoints = orbslam_->GetTrackedMapPoints();
476 if(orbslam_->isLost() || mapPoints.empty())
478 covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
482 cv::Mat TcwMat = ORB_SLAM3::Converter::toCvMat(ORB_SLAM3::Converter::toSE3Quat(Tcw)).
clone();
483 UASSERT(TcwMat.cols == 4 && TcwMat.rows == 4);
488 if(!localTransform.
isNull())
490 if(originLocalTransform_.isNull())
492 originLocalTransform_ = localTransform;
495 p = originLocalTransform_ *
p.inverse() * localTransform.
inverse();
497 t = previousPoseInv*
p;
504 covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
509 float baseline =
data.cameraModels().size()==1?0.0f:
data.stereoCameraModels()[0].baseline();
512 baseline = rtabmap::Parameters::defaultOdomORBSLAMBf();
515 double linearVar = 0.0001;
519 linearVar *= linearVar;
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;
534 info->lost =
t.isNull();
536 info->reg.covariance = covariance;
537 info->localMapSize = mapPoints.size();
538 info->localKeyFrames = 0;
542 std::vector<cv::KeyPoint> kpts = orbslam_->GetTrackedKeyPointsUn();
543 info->reg.matchesIDs.resize(kpts.size());
544 info->reg.inliersIDs.resize(kpts.size());
547 UASSERT(mapPoints.size() == kpts.size());
548 for (
unsigned int i = 0;
i < kpts.size(); ++
i)
551 if(mapPoints[
i] != 0)
553 wordId = mapPoints[
i]->mnId;
559 info->words.insert(std::make_pair(wordId, kpts[
i]));
560 if(mapPoints[
i] != 0)
562 info->reg.matchesIDs[oi] = wordId;
563 info->reg.inliersIDs[oi] = wordId;
567 info->reg.matchesIDs.resize(oi);
568 info->reg.inliersIDs.resize(oi);
569 info->reg.inliers = oi;
570 info->reg.matches = oi;
573 for (
unsigned int i = 0;
i < mapPoints.size(); ++
i)
577 Eigen::Vector3f pt = mapPoints[
i]->GetWorldPos();
579 info->localMap.insert(std::make_pair(mapPoints[
i]->mnId, cv::Point3f(ptt.x, ptt.y, ptt.z)));
585 UINFO(
"Odom update time = %fs, map points=%ld, lost=%s",
timer.
elapsed(), mapPoints.size(),
t.isNull()?
"true":
"false");
588 UERROR(
"RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");