i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updating orbslam3 v1 support (#1152)

Browse Source 
* Refactoring ORB_SLAM3 integration. Fixed realsense2 inter IMU stamps.

* Renamed OdometryORBSLAM -> OdometryORBSLAM2

* revert a change

* Fixed build without orb_slam

* Source camera: added feature detection option

* Fixed jfr2018 docker files
This commit is contained in:
matlabbe
2023-11-19 01:05:52 -08:00
committed by GitHub
parent 9c56a429cc
commit aaff1abc4f
22 changed files with 1420 additions and 575 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/src/CMakeLists.txt
View File

@@ -87,7 +87,8 @@ SET(SRC_FILES
odometry/OdometryViso2.cpp
odometry/OdometryDVO.cpp
odometry/OdometryOkvis.cpp
odometry/OdometryORBSLAM.cpp
odometry/OdometryORBSLAM2.cpp
odometry/OdometryORBSLAM3.cpp
odometry/OdometryLOAM.cpp
odometry/OdometryFLOAM.cpp
odometry/OdometryMSCKF.cpp

83
corelib/src/CameraThread.cpp
View File

@@ -36,10 +36,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/StereoDense.h"
#include "rtabmap/core/DBReader.h"
#include "rtabmap/core/IMUFilter.h"
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include <opencv2/stitching/detail/exposure_compensate.hpp>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <pcl/io/io.h>
@@ -73,7 +75,9 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
_bilateralSigmaS(10),
_bilateralSigmaR(0.1),
_imuFilter(0),
_imuBaseFrameConversion(false)
_imuBaseFrameConversion(false),
_featureDetector(0),
_depthAsMask(Parameters::defaultVisDepthAsMask())
{
UASSERT(_camera != 0);
}
@@ -113,7 +117,9 @@ CameraThread::CameraThread(
_bilateralSigmaS(10),
_bilateralSigmaR(0.1),
_imuFilter(0),
_imuBaseFrameConversion(false)
_imuBaseFrameConversion(false),
_featureDetector(0),
_depthAsMask(Parameters::defaultVisDepthAsMask())
{
UASSERT(_camera != 0 && _odomSensor != 0 && !_extrinsicsOdomToCamera.isNull());
UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str());
@@ -152,7 +158,9 @@ CameraThread::CameraThread(
_bilateralSigmaS(10),
_bilateralSigmaR(0.1),
_imuFilter(0),
_imuBaseFrameConversion(false)
_imuBaseFrameConversion(false),
_featureDetector(0),
_depthAsMask(Parameters::defaultVisDepthAsMask())
{
UASSERT(_camera != 0);
UDEBUG("_odomAsGt =%s", _odomAsGt?"true":"false");
@@ -166,6 +174,7 @@ CameraThread::~CameraThread()
delete _distortionModel;
delete _stereoDense;
delete _imuFilter;
delete _featureDetector;
}
void CameraThread::setImageRate(float imageRate)
@@ -217,6 +226,30 @@ void CameraThread::disableIMUFiltering()
_imuFilter = 0;
}
void CameraThread::enableFeatureDetection(const ParametersMap & parameters)
{
delete _featureDetector;
ParametersMap params = parameters;
ParametersMap defaultParams = Parameters::getDefaultParameters("Vis");
uInsert(params, ParametersPair(Parameters::kKpDetectorStrategy(), uValue(params, Parameters::kVisFeatureType(), defaultParams.at(Parameters::kVisFeatureType()))));
uInsert(params, ParametersPair(Parameters::kKpMaxFeatures(), uValue(params, Parameters::kVisMaxFeatures(), defaultParams.at(Parameters::kVisMaxFeatures()))));
uInsert(params, ParametersPair(Parameters::kKpMaxDepth(), uValue(params, Parameters::kVisMaxDepth(), defaultParams.at(Parameters::kVisMaxDepth()))));
uInsert(params, ParametersPair(Parameters::kKpMinDepth(), uValue(params, Parameters::kVisMinDepth(), defaultParams.at(Parameters::kVisMinDepth()))));
uInsert(params, ParametersPair(Parameters::kKpRoiRatios(), uValue(params, Parameters::kVisRoiRatios(), defaultParams.at(Parameters::kVisRoiRatios()))));
uInsert(params, ParametersPair(Parameters::kKpSubPixEps(), uValue(params, Parameters::kVisSubPixEps(), defaultParams.at(Parameters::kVisSubPixEps()))));
uInsert(params, ParametersPair(Parameters::kKpSubPixIterations(), uValue(params, Parameters::kVisSubPixIterations(), defaultParams.at(Parameters::kVisSubPixIterations()))));
uInsert(params, ParametersPair(Parameters::kKpSubPixWinSize(), uValue(params, Parameters::kVisSubPixWinSize(), defaultParams.at(Parameters::kVisSubPixWinSize()))));
uInsert(params, ParametersPair(Parameters::kKpGridRows(), uValue(params, Parameters::kVisGridRows(), defaultParams.at(Parameters::kVisGridRows()))));
uInsert(params, ParametersPair(Parameters::kKpGridCols(), uValue(params, Parameters::kVisGridCols(), defaultParams.at(Parameters::kVisGridCols()))));
_featureDetector = Feature2D::create(params);
_depthAsMask = Parameters::parse(params, Parameters::kVisDepthAsMask(), _depthAsMask);
}
void CameraThread::disableFeatureDetection()
{
delete _featureDetector;
_featureDetector = 0;
}
void CameraThread::setScanParameters(
bool fromDepth,
int downsampleStep,
@@ -732,6 +765,50 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
data.stamp());
}
}
if(_featureDetector && !data.imageRaw().empty())
{
UDEBUG("Detecting features");
cv::Mat grayScaleImg = data.imageRaw();
if(data.imageRaw().channels() > 1)
{
cv::Mat tmp;
cv::cvtColor(grayScaleImg, tmp, cv::COLOR_BGR2GRAY);
grayScaleImg = tmp;
}
cv::Mat depthMask;
if(!data.depthRaw().empty() && _depthAsMask)
{
if( data.imageRaw().rows % data.depthRaw().rows == 0 &&
data.imageRaw().cols % data.depthRaw().cols == 0 &&
data.imageRaw().rows/data.depthRaw().rows == data.imageRaw().cols/data.depthRaw().cols)
{
depthMask = util2d::interpolate(data.depthRaw(), data.imageRaw().rows/data.depthRaw().rows, 0.1f);
}
else
{
UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
Parameters::kVisDepthAsMask().c_str(),
data.imageRaw().rows, data.imageRaw().cols,
data.depthRaw().rows, data.depthRaw().cols);
}
}
std::vector<cv::KeyPoint> keypoints = _featureDetector->generateKeypoints(grayScaleImg, depthMask);
cv::Mat descriptors;
std::vector<cv::Point3f> keypoints3D;
if(!keypoints.empty())
{
descriptors = _featureDetector->generateDescriptors(grayScaleImg, keypoints);
if(!keypoints.empty())
{
keypoints3D = _featureDetector->generateKeypoints3D(data, keypoints);
}
}
data.setFeatures(keypoints, keypoints3D, descriptors);
}
}
} // namespace rtabmap

7
corelib/src/Odometry.cpp
View File

@@ -32,7 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/odometry/OdometryViso2.h"
#include "rtabmap/core/odometry/OdometryDVO.h"
#include "rtabmap/core/odometry/OdometryOkvis.h"
#include "rtabmap/core/odometry/OdometryORBSLAM.h"
#include "rtabmap/core/odometry/OdometryORBSLAM3.h"
#include "rtabmap/core/odometry/OdometryLOAM.h"
#include "rtabmap/core/odometry/OdometryFLOAM.h"
#include "rtabmap/core/odometry/OdometryMSCKF.h"
@@ -51,6 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util2d.h"
#include <pcl/pcl_base.h>
#include <rtabmap/core/odometry/OdometryORBSLAM2.h>
namespace rtabmap {
@@ -84,7 +85,11 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
odometry = new OdometryDVO(parameters);
break;
case Odometry::kTypeORBSLAM:
#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
odometry = new OdometryORBSLAM(parameters);
#else
odometry = new OdometryORBSLAM3(parameters);
#endif
break;
case Odometry::kTypeOkvis:
odometry = new OdometryOkvis(parameters);

2
corelib/src/RegistrationVis.cpp
View File

@@ -1095,7 +1095,7 @@ Transform RegistrationVis::computeTransformationImpl(
std::vector<std::vector<float> > dists;
float radius = (float)_guessWinSize; // pixels
rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams());
index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams(32, 0, false));
UASSERT(indices.size() == cornersProjectedMat.rows);
UASSERT(descriptorsFrom.cols == descriptorsTo.cols);

2
corelib/src/camera/CameraRealSense2.cpp
View File

@@ -1501,7 +1501,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
getPoseAndIMU(stamps[i], tmp, confidence, imuTmp);
if(!imuTmp.empty())
{
UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0));
UEventsManager::post(new IMUEvent(imuTmp, stamps[i]/1000.0));
pub++;
}
else

171
corelib/src/odometry/OdometryORBSLAM.cpp → corelib/src/odometry/OdometryORBSLAM2.cpp
View File

@@ -34,28 +34,21 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UDirectory.h"
#include <pcl/common/transforms.h>
#include <opencv2/imgproc/types_c.h>
#include <rtabmap/core/odometry/OdometryORBSLAM.h>
#include <rtabmap/core/odometry/OdometryORBSLAM2.h>
#ifdef RTABMAP_ORB_SLAM
#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
#include <System.h>
#include <thread>
using namespace std;
#if RTABMAP_ORB_SLAM == 3
namespace ORB_SLAM3 {
#else
namespace ORB_SLAM2 {
#endif
// Override original Tracking object to comment all rendering stuff
class Tracker: public Tracking
{
public:
#if RTABMAP_ORB_SLAM == 3
Tracker(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Atlas* pMap,
#else
Tracker(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,
#endif
KeyFrameDatabase* pKFDB, const std::string &strSettingPath, const int sensor, long unsigned int maxFeatureMapSize) :
Tracking(pSys, pVoc, pFrameDrawer, pMapDrawer, pMap, pKFDB, strSettingPath, sensor),
maxFeatureMapSize_(maxFeatureMapSize)
@@ -67,9 +60,6 @@ private:
protected:
void Track()
{
#if RTABMAP_ORB_SLAM == 3
Map* mpMap = mpAtlas->GetCurrentMap();
#endif
if(mState==NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
@@ -91,17 +81,8 @@ protected:
if(mState!=OK)
{
#if RTABMAP_ORB_SLAM == 3
mLastFrame = Frame(mCurrentFrame);
#endif
return;
}
#if RTABMAP_ORB_SLAM == 3
if(mpAtlas->GetAllMaps().size() == 1)
{
mnFirstFrameId = mCurrentFrame.mnId;
}
#endif
}
else
{
@@ -384,9 +365,6 @@ protected:
// Set Frame pose to the origin
mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
#if RTABMAP_ORB_SLAM == 3
Map* mpMap = mpAtlas->GetCurrentMap();
#endif
// Create KeyFrame
KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
@@ -484,11 +462,9 @@ public:
cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY);
}
}
#if RTABMAP_ORB_SLAM == 3
mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth, mpCamera);
#else
mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
#endif
Track();
return mCurrentFrame.mTcw.clone();
@@ -516,11 +492,8 @@ public:
UASSERT(imDepth.type()==CV_32F);
#if RTABMAP_ORB_SLAM == 3
mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth, mpCamera);
#else
mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
#endif
Track();
return mCurrentFrame.mTcw.clone();
@@ -531,11 +504,7 @@ public:
class LoopCloser: public LoopClosing
{
public:
#if RTABMAP_ORB_SLAM == 3
LoopCloser(Atlas* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale) :
#else
LoopCloser(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale) :
#endif
LoopClosing(pMap, pDB, pVoc, bFixScale)
{}
@@ -566,16 +535,12 @@ public:
} // namespace ORB_SLAM
#if RTABMAP_ORB_SLAM == 3
using namespace ORB_SLAM3;
#else
using namespace ORB_SLAM2;
#endif
class ORBSLAMSystem
class ORBSLAM2System
{
public:
ORBSLAMSystem(const rtabmap::ParametersMap & parameters) :
ORBSLAM2System(const rtabmap::ParametersMap & parameters) :
mpVocabulary(0),
mpKeyFrameDatabase(0),
mpMap(0),
@@ -613,7 +578,7 @@ public:
}
}
bool init(const rtabmap::CameraModel & model, bool stereo, double baseline, const rtabmap::Transform & localIMUTransform)
bool init(const rtabmap::CameraModel & model, bool stereo, double baseline)
{
if(!mpVocabulary)
{
@@ -709,31 +674,6 @@ public:
ofs << "DepthMapFactor: " << 1000.0 << std::endl;
ofs << std::endl;
if(!localIMUTransform.isNull())
{
//#--------------------------------------------------------------------------------------------
//# IMU Parameters TODO: hard-coded, not used
//#--------------------------------------------------------------------------------------------
// Transformation from camera 0 to body-frame (imu)
rtabmap::Transform camImuT = model.localTransform()*localIMUTransform;
ofs << "Tbc: !!opencv-matrix" << std::endl;
ofs << " rows: 4" << std::endl;
ofs << " cols: 4" << std::endl;
ofs << " dt: f" << std::endl;
ofs << " data: [" << camImuT.data()[0] << ", " << camImuT.data()[1] << ", " << camImuT.data()[2] << ", " << camImuT.data()[3] << ", " << std::endl;
ofs << " " << camImuT.data()[4] << ", " << camImuT.data()[5] << ", " << camImuT.data()[6] << ", " << camImuT.data()[7] << ", " << std::endl;
ofs << " " << camImuT.data()[8] << ", " << camImuT.data()[9] << ", " << camImuT.data()[10] << ", " << camImuT.data()[11] << ", " << std::endl;
ofs << " 0.0, 0.0, 0.0, 1.0]" << std::endl;
ofs << std::endl;
ofs << "IMU.NoiseGyro: " << 1.7e-4 << std::endl;
ofs << "IMU.NoiseAcc: " << 2.0e-3 << std::endl;
ofs << "IMU.GyroWalk: " << 1.9393e-5 << std::endl;
ofs << "IMU.AccWalk: " << 3.e-3 << std::endl;
ofs << "IMU.Frequency: " << 200 << std::endl;
ofs << std::endl;
}
//#--------------------------------------------------------------------------------------------
//# ORB Parameters
//#--------------------------------------------------------------------------------------------
@@ -776,22 +716,15 @@ public:
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
//Create the Map
#if RTABMAP_ORB_SLAM == 3
mpMap = new Atlas(0);
#else
mpMap = new ORB_SLAM2::Map();
#endif
//Initialize the Tracking thread
//(it will live in the main thread of execution, the one that called this constructor)
mpTracker = new Tracker(0, mpVocabulary, 0, 0, mpMap, mpKeyFrameDatabase, configPath, stereo?System::STEREO:System::RGBD, maxFeatureMapSize);
//Initialize the Local Mapping thread and launch
#if RTABMAP_ORB_SLAM == 3
mpLocalMapper = new LocalMapping(0, mpMap, false, stereo && !localIMUTransform.isNull());
#else
mpLocalMapper = new LocalMapping(mpMap, false);
#endif
//Initialize the Loop Closing thread and launch
mpLoopCloser = new LoopCloser(mpMap, mpKeyFrameDatabase, mpVocabulary, true);
@@ -812,17 +745,10 @@ public:
// Reset all static variables
Frame::mbInitialComputations = true;
#if RTABMAP_ORB_SLAM == 3
if(ULogger::level() > ULogger::kInfo)
Verbose::SetTh(Verbose::VERBOSITY_QUIET);
mpTracker->Reset(true);
#endif
return true;
}
virtual ~ORBSLAMSystem()
virtual ~ORBSLAM2System()
{
shutdown();
delete mpVocabulary;
@@ -869,11 +795,7 @@ public:
KeyFrameDatabase* mpKeyFrameDatabase;
// Map structure that stores the pointers to all KeyFrames and MapPoints.
#if RTABMAP_ORB_SLAM == 3
Atlas* mpMap;
#else
Map* mpMap;
#endif
// Tracker. It receives a frame and computes the associated camera pose.
// It also decides when to insert a new keyframe, create some new MapPoints and
@@ -898,24 +820,23 @@ public:
namespace rtabmap {
OdometryORBSLAM::OdometryORBSLAM(const ParametersMap & parameters) :
OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) :
Odometry(parameters)
#ifdef RTABMAP_ORB_SLAM
#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
,
orbslam_(0),
firstFrame_(true),
previousPose_(Transform::getIdentity()),
useIMU_(false) // TODO: Not yet supported with ORB_SLAM3
previousPose_(Transform::getIdentity())
#endif
{
#ifdef RTABMAP_ORB_SLAM
orbslam_ = new ORBSLAMSystem(parameters);
#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
orbslam_ = new ORBSLAM2System(parameters);
#endif
}
OdometryORBSLAM::~OdometryORBSLAM()
OdometryORBSLAM2::~OdometryORBSLAM2()
{
#ifdef RTABMAP_ORB_SLAM
#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
if(orbslam_)
{
delete orbslam_;
@@ -923,10 +844,10 @@ OdometryORBSLAM::~OdometryORBSLAM()
#endif
}
void OdometryORBSLAM::reset(const Transform & initialPose)
void OdometryORBSLAM2::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
#ifdef RTABMAP_ORB_SLAM
#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
if(orbslam_)
{
orbslam_->shutdown();
@@ -934,60 +855,20 @@ void OdometryORBSLAM::reset(const Transform & initialPose)
firstFrame_ = true;
originLocalTransform_.setNull();
previousPose_.setIdentity();
imuLocalTransform_.setNull();
#endif
}
bool OdometryORBSLAM::canProcessAsyncIMU() const
{
#ifdef RTABMAP_ORB_SLAM
return useIMU_;
#else
return false;
#endif
}
// return not null transform if odometry is correctly computed
Transform OdometryORBSLAM::computeTransform(
Transform OdometryORBSLAM2::computeTransform(
SensorData & data,
const Transform & guess,
OdometryInfo * info)
{
Transform t;
#ifdef RTABMAP_ORB_SLAM
#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
UTimer timer;
#if RTABMAP_ORB_SLAM == 3
if(useIMU_)
{
if(orbslam_->mpTracker == 0)
{
if(!data.imu().empty())
{
imuLocalTransform_ = data.imu().localTransform();
}
}
else if(!data.imu().empty())
{
ORB_SLAM3::IMU::Point pt(
data.imu().linearAcceleration().val[0],
data.imu().linearAcceleration().val[1],
data.imu().linearAcceleration().val[2],
data.imu().angularVelocity().val[0],
data.imu().angularVelocity().val[1],
data.imu().angularVelocity().val[2],
data.stamp());
orbslam_->mpTracker->GrabImuData(pt);
}
if(data.imageRaw().empty() || imuLocalTransform_.isNull())
{
return Transform();
}
}
#endif
if(data.imageRaw().empty() ||
data.imageRaw().rows != data.depthOrRightRaw().rows ||
data.imageRaw().cols != data.depthOrRightRaw().cols)
@@ -1007,18 +888,12 @@ Transform OdometryORBSLAM::computeTransform(
}
bool stereo = data.cameraModels().size() == 0;
if(!stereo && useIMU_)
{
UWARN("Disabling IMU support (ORB_SLAM3 doesn't support IMU with RGB-D mode).");
useIMU_ = false;
imuLocalTransform_.setNull();
}
cv::Mat covariance;
if(orbslam_->mpTracker == 0)
{
CameraModel model = data.cameraModels().size()==1?data.cameraModels()[0]:data.stereoCameraModels()[0].left();
if(!orbslam_->init(model, stereo, data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline(), imuLocalTransform_))
if(!orbslam_->init(model, stereo, data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline()))
{
return t;
}

571
corelib/src/odometry/OdometryORBSLAM3.cpp Normal file
View File

@@ -0,0 +1,571 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UDirectory.h"
#include <pcl/common/transforms.h>
#include <opencv2/imgproc/types_c.h>
#include <rtabmap/core/odometry/OdometryORBSLAM3.h>
#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
#include <thread>
#include <Converter.h>
using namespace std;
#endif
namespace rtabmap {
OdometryORBSLAM3::OdometryORBSLAM3(const ParametersMap & parameters) :
Odometry(parameters)
#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
,
orbslam_(0),
firstFrame_(true),
previousPose_(Transform::getIdentity()),
useIMU_(Parameters::defaultOdomORBSLAMInertial()),
parameters_(parameters),
lastImuStamp_(0.0),
lastImageStamp_(0.0)
#endif
{
#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
Parameters::parse(parameters, Parameters::kOdomORBSLAMInertial(), useIMU_);
#endif
}
OdometryORBSLAM3::~OdometryORBSLAM3()
{
#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
if(orbslam_)
{
orbslam_->Shutdown();
delete orbslam_;
}
#endif
}
void OdometryORBSLAM3::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
if(orbslam_)
{
orbslam_->Shutdown();
delete orbslam_;
orbslam_=0;
}
firstFrame_ = true;
originLocalTransform_.setNull();
previousPose_.setIdentity();
imuLocalTransform_.setNull();
lastImuStamp_ = 0.0;
lastImageStamp_ = 0.0;
#endif
}
bool OdometryORBSLAM3::canProcessAsyncIMU() const
{
#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
return useIMU_;
#else
return false;
#endif
}
bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model, double stamp, bool stereo, double baseline)
{
#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
std::string vocabularyPath;
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMVocPath(), vocabularyPath);
if(vocabularyPath.empty())
{
UERROR("ORB_SLAM vocabulary path should be set! (Parameter name=\"%s\")", rtabmap::Parameters::kOdomORBSLAMVocPath().c_str());
return false;
}
//Load ORB Vocabulary
vocabularyPath = uReplaceChar(vocabularyPath, '~', UDirectory::homeDir());
UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str());
// Create configuration file
std::string workingDir;
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kRtabmapWorkingDirectory(), workingDir);
if(workingDir.empty())
{
workingDir = ".";
}
std::string configPath = workingDir+"/rtabmap_orbslam.yaml";
std::ofstream ofs (configPath, std::ofstream::out);
ofs << "%YAML:1.0" << std::endl;
ofs << std::endl;
ofs << "File.version: \"1.0\"" << std::endl;
ofs << std::endl;
ofs << "Camera.type: \"PinHole\"" << std::endl;
ofs << std::endl;
ofs << fixed << setprecision(13);
//# Camera calibration and distortion parameters (OpenCV)
ofs << "Camera1.fx: " << model.fx() << std::endl;
ofs << "Camera1.fy: " << model.fy() << std::endl;
ofs << "Camera1.cx: " << model.cx() << std::endl;
ofs << "Camera1.cy: " << model.cy() << std::endl;
ofs << std::endl;
if(model.D().cols < 4)
{
ofs << "Camera1.k1: " << 0.0 << std::endl;
ofs << "Camera1.k2: " << 0.0 << std::endl;
ofs << "Camera1.p1: " << 0.0 << std::endl;
ofs << "Camera1.p2: " << 0.0 << std::endl;
if(!stereo)
{
ofs << "Camera1.k3: " << 0.0 << std::endl;
}
}
if(model.D().cols >= 4)
{
ofs << "Camera1.k1: " << model.D().at<double>(0,0) << std::endl;
ofs << "Camera1.k2: " << model.D().at<double>(0,1) << std::endl;
ofs << "Camera1.p1: " << model.D().at<double>(0,2) << std::endl;
ofs << "Camera1.p2: " << model.D().at<double>(0,3) << std::endl;
}
if(model.D().cols >= 5)
{
ofs << "Camera1.k3: " << model.D().at<double>(0,4) << std::endl;
}
if(model.D().cols > 5)
{
UWARN("Unhandled camera distortion size %d, only 5 first coefficients used", model.D().cols);
}
ofs << std::endl;
//# IR projector baseline times fx (aprox.)
if(baseline <= 0.0)
{
baseline = rtabmap::Parameters::defaultOdomORBSLAMBf();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMBf(), baseline);
}
ofs << "Camera.bf: " << model.fx()*baseline << std::endl;
ofs << "Camera.width: " << model.imageWidth() << std::endl;
ofs << "Camera.height: " << model.imageHeight() << std::endl;
ofs << std::endl;
//# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
//Camera.RGB: 1
ofs << "Camera.RGB: 1" << std::endl;
ofs << std::endl;
float fps = rtabmap::Parameters::defaultOdomORBSLAMFps();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMFps(), fps);
if(fps == 0)
{
UASSERT(stamp > lastImageStamp_);
fps = std::round(1./(stamp - lastImageStamp_));
UWARN("Camera FPS estimated at %d Hz. If this doesn't look good, "
"set explicitly parameter %s to expected frequency.",
int(fps), Parameters::kOdomORBSLAMFps().c_str());
}
ofs << "Camera.fps: " << (int)fps << std::endl;
ofs << std::endl;
//# Close/Far threshold. Baseline times.
double thDepth = rtabmap::Parameters::defaultOdomORBSLAMThDepth();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMThDepth(), thDepth);
ofs << "Stereo.ThDepth: " << thDepth << std::endl;
ofs << "Stereo.b: " << baseline << std::endl;
ofs << std::endl;
//# Deptmap values factor
ofs << "RGBD.DepthMapFactor: " << 1.0 << std::endl;
ofs << std::endl;
bool withIMU = false;
if(!imuLocalTransform_.isNull())
{
withIMU = true;
//#--------------------------------------------------------------------------------------------
//# IMU Parameters TODO: hard-coded, not used
//#--------------------------------------------------------------------------------------------
// Transformation from camera 0 to body-frame (imu)
rtabmap::Transform camImuT = model.localTransform()*imuLocalTransform_;
ofs << "IMU.T_b_c1: !!opencv-matrix" << std::endl;
ofs << " rows: 4" << std::endl;
ofs << " cols: 4" << std::endl;
ofs << " dt: f" << std::endl;
ofs << " data: [" << camImuT.data()[0] << ", " << camImuT.data()[1] << ", " << camImuT.data()[2] << ", " << camImuT.data()[3] << ", " << std::endl;
ofs << " " << camImuT.data()[4] << ", " << camImuT.data()[5] << ", " << camImuT.data()[6] << ", " << camImuT.data()[7] << ", " << std::endl;
ofs << " " << camImuT.data()[8] << ", " << camImuT.data()[9] << ", " << camImuT.data()[10] << ", " << camImuT.data()[11] << ", " << std::endl;
ofs << " 0.0, 0.0, 0.0, 1.0]" << std::endl;
ofs << std::endl;
ofs << "IMU.InsertKFsWhenLost: " << 0 << std::endl;
ofs << std::endl;
double gyroNoise = rtabmap::Parameters::defaultOdomORBSLAMGyroNoise();
double accNoise = rtabmap::Parameters::defaultOdomORBSLAMAccNoise();
double gyroWalk = rtabmap::Parameters::defaultOdomORBSLAMGyroWalk();
double accWalk = rtabmap::Parameters::defaultOdomORBSLAMAccWalk();
double samplingRate = rtabmap::Parameters::defaultOdomORBSLAMSamplingRate();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMGyroNoise(), gyroNoise);
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMAccNoise(), accNoise);
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMGyroWalk(), gyroWalk);
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMAccWalk(), accWalk);
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMSamplingRate(), samplingRate);
ofs << "IMU.NoiseGyro: " << gyroNoise << std::endl; // 1e-2
ofs << "IMU.NoiseAcc: " << accNoise << std::endl; // 1e-1
ofs << "IMU.GyroWalk: " << gyroWalk << std::endl; // 1e-6
ofs << "IMU.AccWalk: " << accWalk << std::endl; // 1e-4
if(samplingRate == 0)
{
// estimate rate from imu received.
UASSERT(orbslamImus_.size() > 1 && orbslamImus_[0].t < orbslamImus_[1].t);
samplingRate = 1./(orbslamImus_[1].t - orbslamImus_[0].t);
samplingRate = std::round(samplingRate);
UWARN("IMU sampling rate estimated at %.0f Hz. If this doesn't look good, "
"set explicitly parameter %s to expected frequency.",
samplingRate, Parameters::kOdomORBSLAMSamplingRate().c_str());
}
ofs << "IMU.Frequency: " << samplingRate << std::endl; // 200
ofs << std::endl;
}
//#--------------------------------------------------------------------------------------------
//# ORB Parameters
//#--------------------------------------------------------------------------------------------
//# ORB Extractor: Number of features per image
int features = rtabmap::Parameters::defaultOdomORBSLAMMaxFeatures();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMMaxFeatures(), features);
ofs << "ORBextractor.nFeatures: " << features << std::endl;
ofs << std::endl;
//# ORB Extractor: Scale factor between levels in the scale pyramid
double scaleFactor = rtabmap::Parameters::defaultORBScaleFactor();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kORBScaleFactor(), scaleFactor);
ofs << "ORBextractor.scaleFactor: " << scaleFactor << std::endl;
ofs << std::endl;
//# ORB Extractor: Number of levels in the scale pyramid
int levels = rtabmap::Parameters::defaultORBNLevels();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kORBNLevels(), levels);
ofs << "ORBextractor.nLevels: " << levels << std::endl;
ofs << std::endl;
//# ORB Extractor: Fast threshold
//# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
//# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
//# You can lower these values if your images have low contrast
int iniThFAST = rtabmap::Parameters::defaultFASTThreshold();
int minThFAST = rtabmap::Parameters::defaultFASTMinThreshold();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kFASTThreshold(), iniThFAST);
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kFASTMinThreshold(), minThFAST);
ofs << "ORBextractor.iniThFAST: " << iniThFAST << std::endl;
ofs << "ORBextractor.minThFAST: " << minThFAST << std::endl;
ofs << std::endl;
int maxFeatureMapSize = rtabmap::Parameters::defaultOdomORBSLAMMapSize();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMMapSize(), maxFeatureMapSize);
//# Disable loop closure detection
ofs << "loopClosing: " << 0 << std::endl;
ofs << std::endl;
//# Set dummy Viewer parameters
ofs << "Viewer.KeyFrameSize: " << 0.05 << std::endl;
ofs << "Viewer.KeyFrameLineWidth: " << 1.0 << std::endl;
ofs << "Viewer.GraphLineWidth: " << 0.9 << std::endl;
ofs << "Viewer.PointSize: " << 2.0 << std::endl;
ofs << "Viewer.CameraSize: " << 0.08 << std::endl;
ofs << "Viewer.CameraLineWidth: " << 3.0 << std::endl;
ofs << "Viewer.ViewpointX: " << 0.0 << std::endl;
ofs << "Viewer.ViewpointY: " << -0.7 << std::endl;
ofs << "Viewer.ViewpointZ: " << -3.5 << std::endl;
ofs << "Viewer.ViewpointF: " << 500.0 << std::endl;
ofs << std::endl;
ofs.close();
orbslam_ = new ORB_SLAM3::System(
vocabularyPath,
configPath,
stereo && withIMU?ORB_SLAM3::System::IMU_STEREO:
stereo?ORB_SLAM3::System::STEREO:
withIMU?ORB_SLAM3::System::IMU_RGBD:
ORB_SLAM3::System::RGBD,
false);
return true;
#else
UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
#endif
return false;
}
// return not null transform if odometry is correctly computed
Transform OdometryORBSLAM3::computeTransform(
SensorData & data,
const Transform & guess,
OdometryInfo * info)
{
Transform t;
#if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 3
UTimer timer;
if(useIMU_)
{
bool added = false;
if(!data.imu().empty())
{
if(lastImuStamp_ == 0.0 || lastImuStamp_ < data.stamp())
{
orbslamImus_.push_back(ORB_SLAM3::IMU::Point(
data.imu().linearAcceleration().val[0],
data.imu().linearAcceleration().val[1],
data.imu().linearAcceleration().val[2],
data.imu().angularVelocity().val[0],
data.imu().angularVelocity().val[1],
data.imu().angularVelocity().val[2],
data.stamp()));
lastImuStamp_ = data.stamp();
added = true;
}
else
{
UERROR("Received IMU with stamp (%f) <= than the previous IMU (%f), ignoring it!", data.stamp(), lastImuStamp_);
}
}
if(orbslam_ == 0)
{
// We need two samples to estimate imu frame rate
if(orbslamImus_.size()>1 && added)
{
imuLocalTransform_ = data.imu().localTransform();
}
}
if(data.imageRaw().empty() || imuLocalTransform_.isNull())
{
return Transform();
}
}
if(data.imageRaw().empty() ||
data.imageRaw().rows != data.depthOrRightRaw().rows ||
data.imageRaw().cols != data.depthOrRightRaw().cols)
{
UERROR("Not supported input! RGB (%dx%d) and depth (%dx%d) should have the same size.",
data.imageRaw().cols, data.imageRaw().rows, data.depthOrRightRaw().cols, data.depthOrRightRaw().rows);
return t;
}
if(!((data.cameraModels().size() == 1 &&
data.cameraModels()[0].isValidForReprojection()) ||
(data.stereoCameraModels().size() == 1 &&
data.stereoCameraModels()[0].isValidForProjection())))
{
UERROR("Invalid camera model!");
return t;
}
bool stereo = data.cameraModels().size() == 0;
cv::Mat covariance;
if(orbslam_ == 0)
{
// We need two frames to estimate camera frame rate
if(lastImageStamp_ == 0.0)
{
lastImageStamp_ = data.stamp();
return t;
}
CameraModel model = data.cameraModels().size()==1?data.cameraModels()[0]:data.stereoCameraModels()[0].left();
if(!init(model, data.stamp(), stereo, data.cameraModels().size()==1?0.0:data.stereoCameraModels()[0].baseline()))
{
return t;
}
}
Sophus::SE3f Tcw;
Transform localTransform;
if(stereo)
{
localTransform = data.stereoCameraModels()[0].localTransform();
Tcw = orbslam_->TrackStereo(data.imageRaw(), data.rightRaw(), data.stamp(), orbslamImus_);
orbslamImus_.clear();
}
else
{
localTransform = data.cameraModels()[0].localTransform();
cv::Mat depth;
if(data.depthRaw().type() == CV_32FC1)
{
depth = data.depthRaw();
}
else if(data.depthRaw().type() == CV_16UC1)
{
depth = util2d::cvtDepthToFloat(data.depthRaw());
}
Tcw = orbslam_->TrackRGBD(data.imageRaw(), depth, data.stamp(), orbslamImus_);
orbslamImus_.clear();
}
Transform previousPoseInv = previousPose_.inverse();
std::vector<ORB_SLAM3::MapPoint*> mapPoints = orbslam_->GetTrackedMapPoints();
if(orbslam_->isLost() || mapPoints.empty())
{
covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
}
else
{
cv::Mat TcwMat = ORB_SLAM3::Converter::toCvMat(ORB_SLAM3::Converter::toSE3Quat(Tcw)).clone();
UASSERT(TcwMat.cols == 4 && TcwMat.rows == 4);
Transform p = Transform(cv::Mat(TcwMat, cv::Range(0,3), cv::Range(0,4)));
if(!p.isNull())
{
if(!localTransform.isNull())
{
if(originLocalTransform_.isNull())
{
originLocalTransform_ = localTransform;
}
// transform in base frame
p = originLocalTransform_ * p.inverse() * localTransform.inverse();
}
t = previousPoseInv*p;
}
previousPose_ = p;
if(firstFrame_)
{
// just recovered of being lost, set high covariance
covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
firstFrame_ = false;
}
else
{
float baseline = data.cameraModels().size()==1?0.0f:data.stereoCameraModels()[0].baseline();
if(baseline <= 0.0f)
{
baseline = rtabmap::Parameters::defaultOdomORBSLAMBf();
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMBf(), baseline);
}
double linearVar = 0.0001;
if(baseline > 0.0f)
{
linearVar = baseline/8.0;
linearVar *= linearVar;
}
covariance = cv::Mat::eye(6,6, CV_64FC1);
covariance.at<double>(0,0) = linearVar;
covariance.at<double>(1,1) = linearVar;
covariance.at<double>(2,2) = linearVar;
covariance.at<double>(3,3) = 0.0001;
covariance.at<double>(4,4) = 0.0001;
covariance.at<double>(5,5) = 0.0001;
}
}
if(info)
{
info->lost = t.isNull();
info->type = (int)kTypeORBSLAM;
info->reg.covariance = covariance;
info->localMapSize = mapPoints.size();
info->localKeyFrames = 0;
if(this->isInfoDataFilled())
{
std::vector<cv::KeyPoint> kpts = orbslam_->GetTrackedKeyPointsUn();
info->reg.matchesIDs.resize(kpts.size());
info->reg.inliersIDs.resize(kpts.size());
int oi = 0;
UASSERT(mapPoints.size() == kpts.size());
for (unsigned int i = 0; i < kpts.size(); ++i)
{
int wordId;
if(mapPoints[i] != 0)
{
wordId = mapPoints[i]->mnId;
}
else
{
wordId = -(i+1);
}
info->words.insert(std::make_pair(wordId, kpts[i]));
if(mapPoints[i] != 0)
{
info->reg.matchesIDs[oi] = wordId;
info->reg.inliersIDs[oi] = wordId;
++oi;
}
}
info->reg.matchesIDs.resize(oi);
info->reg.inliersIDs.resize(oi);
info->reg.inliers = oi;
info->reg.matches = oi;
Eigen::Affine3f fixRot = (this->getPose()*previousPoseInv*originLocalTransform_).toEigen3f();
for (unsigned int i = 0; i < mapPoints.size(); ++i)
{
if(mapPoints[i])
{
Eigen::Vector3f pt = mapPoints[i]->GetWorldPos();
pcl::PointXYZ ptt = pcl::transformPoint(pcl::PointXYZ(pt[0], pt[1], pt[2]), fixRot);
info->localMap.insert(std::make_pair(mapPoints[i]->mnId, cv::Point3f(ptt.x, ptt.y, ptt.z)));
}
}
}
}
UINFO("Odom update time = %fs, map points=%ld, lost=%s", timer.elapsed(), mapPoints.size(), t.isNull()?"true":"false");
#else
UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
#endif
return t;
}
} // namespace rtabmap