mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 17:40:23 +08:00
Added ORB_SLAM3 support. IMU Filter: added base frame conversion option. (#698)
Referred issues: #655 https://github.com/introlab/rtabmap_ros/issues/492 Note: IMU not supported yet with ORB_SLAM3. Commits: * Added orbslam3 support. UI-Source->IMU filtering: Added base frame conversion option of IMU data to uniformize yaw initialization. Madgwick: fixed yaw initialization accordingly to Z acc. * fixed regression build error with ORB_SLAM2 * Renamed OdometryORBSLAM2 to OdometryORBSLAM (can be 2 or 3 now)
This commit is contained in:
matlabbe
@@ -87,11 +87,12 @@ SET(SRC_FILES
|
||||
odometry/OdometryViso2.cpp
|
||||
odometry/OdometryDVO.cpp
|
||||
odometry/OdometryOkvis.cpp
|
||||
odometry/OdometryORBSLAM2.cpp
|
||||
odometry/OdometryORBSLAM.cpp
|
||||
odometry/OdometryLOAM.cpp
|
||||
odometry/OdometryMSCKF.cpp
|
||||
odometry/OdometryVINS.cpp
|
||||
|
||||
IMU.cpp
|
||||
IMUThread.cpp
|
||||
IMUFilter.cpp
|
||||
imufilter/ComplementaryFilter.cpp
|
||||
@@ -601,16 +602,16 @@ IF(vins_FOUND)
|
||||
)
|
||||
ENDIF(vins_FOUND)
|
||||
|
||||
IF(ORB_SLAM2_FOUND)
|
||||
IF(ORB_SLAM_FOUND)
|
||||
SET(INCLUDE_DIRS
|
||||
${ORB_SLAM2_INCLUDE_DIRS} #before so that g2o includes are taken from ORB_SLAM2 directory before the official g2o one
|
||||
${ORB_SLAM_INCLUDE_DIRS} #before so that g2o includes are taken from ORB_SLAM directory before the official g2o one
|
||||
${INCLUDE_DIRS}
|
||||
)
|
||||
SET(LIBRARIES
|
||||
${ORB_SLAM2_LIBRARIES}
|
||||
${ORB_SLAM_LIBRARIES}
|
||||
${LIBRARIES}
|
||||
)
|
||||
ENDIF(ORB_SLAM2_FOUND)
|
||||
ENDIF(ORB_SLAM_FOUND)
|
||||
|
||||
IF(GTSAM_FOUND)
|
||||
# Make sure GTSAM is built with system Eigen, not the included one in its package
|
||||
|
||||
@@ -67,7 +67,8 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
|
||||
_bilateralFiltering(false),
|
||||
_bilateralSigmaS(10),
|
||||
_bilateralSigmaR(0.1),
|
||||
_imuFilter(0)
|
||||
_imuFilter(0),
|
||||
_imuBaseFrameConversion(false)
|
||||
{
|
||||
UASSERT(_camera != 0);
|
||||
}
|
||||
@@ -117,10 +118,11 @@ void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
|
||||
_bilateralSigmaR = sigmaR;
|
||||
}
|
||||
|
||||
void CameraThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters)
|
||||
void CameraThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion)
|
||||
{
|
||||
delete _imuFilter;
|
||||
_imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters);
|
||||
_imuBaseFrameConversion = baseFrameConversion;
|
||||
}
|
||||
|
||||
void CameraThread::disableIMUFiltering()
|
||||
@@ -470,21 +472,31 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
|
||||
}
|
||||
else
|
||||
{
|
||||
// Transform IMU data in base_link to correctly initialize yaw
|
||||
IMU imu = data.imu();
|
||||
if(_imuBaseFrameConversion)
|
||||
{
|
||||
UASSERT(!data.imu().localTransform().isNull());
|
||||
imu.convertToBaseFrame();
|
||||
|
||||
}
|
||||
_imuFilter->update(
|
||||
data.imu().angularVelocity()[0],
|
||||
data.imu().angularVelocity()[1],
|
||||
data.imu().angularVelocity()[2],
|
||||
data.imu().linearAcceleration()[0],
|
||||
data.imu().linearAcceleration()[1],
|
||||
data.imu().linearAcceleration()[2],
|
||||
imu.angularVelocity()[0],
|
||||
imu.angularVelocity()[1],
|
||||
imu.angularVelocity()[2],
|
||||
imu.linearAcceleration()[0],
|
||||
imu.linearAcceleration()[1],
|
||||
imu.linearAcceleration()[2],
|
||||
data.stamp());
|
||||
double qx,qy,qz,qw;
|
||||
_imuFilter->getOrientation(qx,qy,qz,qw);
|
||||
|
||||
data.setIMU(IMU(
|
||||
cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1),
|
||||
data.imu().angularVelocity(), data.imu().angularVelocityCovariance(),
|
||||
data.imu().linearAcceleration(), data.imu().linearAccelerationCovariance(),
|
||||
data.imu().localTransform()));
|
||||
imu.angularVelocity(), imu.angularVelocityCovariance(),
|
||||
imu.linearAcceleration(), imu.linearAccelerationCovariance(),
|
||||
imu.localTransform()));
|
||||
|
||||
UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)",
|
||||
data.imu().orientation()[0],
|
||||
data.imu().orientation()[1],
|
||||
|
||||
74
corelib/src/IMU.cpp
Normal file
74
corelib/src/IMU.cpp
Normal file
@@ -0,0 +1,74 @@
|
||||
/*
|
||||
Copyright (c) 2010-2021, 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/IMU.h>
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
void IMU::convertToBaseFrame()
|
||||
{
|
||||
if(!localTransform_.isNull() && !localTransform_.rotation().isIdentity())
|
||||
{
|
||||
cv::Mat rotationMatrix, rotationMatrixT;
|
||||
localTransform_.rotationMatrix().convertTo(rotationMatrix, CV_64FC1);
|
||||
cv::transpose(rotationMatrix, rotationMatrixT);
|
||||
|
||||
cv::Mat_<double> v = rotationMatrix * cv::Mat(linearAcceleration_);
|
||||
linearAcceleration_ = cv::Vec3d(v(0,0), v(0,1), v(0,2));
|
||||
if(!linearAccelerationCovariance_.empty())
|
||||
{
|
||||
linearAccelerationCovariance_ = rotationMatrix * linearAccelerationCovariance_ * rotationMatrixT;
|
||||
}
|
||||
|
||||
v = rotationMatrix * cv::Mat(angularVelocity_);
|
||||
angularVelocity_ = cv::Vec3d(v(0,0), v(0,1), v(0,2));
|
||||
if(!angularVelocityCovariance_.empty())
|
||||
{
|
||||
angularVelocityCovariance_ = rotationMatrix * angularVelocityCovariance_ * rotationMatrixT;
|
||||
}
|
||||
|
||||
if(!(orientation_[0] == 0.0 && orientation_[1] == 0.0 && orientation_[2] == 0.0))
|
||||
{
|
||||
// orientation includes roll and pitch but not yaw in local transform
|
||||
Eigen::Quaterniond qTheta =
|
||||
Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()) *
|
||||
Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY()) *
|
||||
Eigen::AngleAxisd(localTransform_.theta(), Eigen::Vector3d::UnitZ());
|
||||
Eigen::Quaterniond q = qTheta * Eigen::Quaterniond(orientation_[3], orientation_[0], orientation_[1], orientation_[2]) * localTransform_.getQuaterniond().inverse();
|
||||
orientation_ = cv::Vec4d(q.x(),q.y(),q.z(),q.w());
|
||||
if(!orientationCovariance_.empty())
|
||||
{
|
||||
orientationCovariance_ = rotationMatrix * orientationCovariance_ * rotationMatrixT;
|
||||
}
|
||||
}
|
||||
|
||||
localTransform_ = Transform(localTransform_.x(), localTransform_.y(), localTransform_.z(), 0,0,0);
|
||||
}
|
||||
}
|
||||
|
||||
} //namespace rtabmap
|
||||
@@ -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/OdometryORBSLAM2.h"
|
||||
#include "rtabmap/core/odometry/OdometryORBSLAM.h"
|
||||
#include "rtabmap/core/odometry/OdometryLOAM.h"
|
||||
#include "rtabmap/core/odometry/OdometryMSCKF.h"
|
||||
#include "rtabmap/core/odometry/OdometryVINS.h"
|
||||
@@ -80,8 +80,8 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
|
||||
case Odometry::kTypeDVO:
|
||||
odometry = new OdometryDVO(parameters);
|
||||
break;
|
||||
case Odometry::kTypeORBSLAM2:
|
||||
odometry = new OdometryORBSLAM2(parameters);
|
||||
case Odometry::kTypeORBSLAM:
|
||||
odometry = new OdometryORBSLAM(parameters);
|
||||
break;
|
||||
case Odometry::kTypeOkvis:
|
||||
odometry = new OdometryOkvis(parameters);
|
||||
@@ -291,7 +291,12 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
|
||||
{
|
||||
Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
|
||||
// orientation includes roll and pitch but not yaw in local transform
|
||||
Transform imuT = Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse();
|
||||
Transform imuT = Transform(data.imu().localTransform().x(),data.imu().localTransform().y(),data.imu().localTransform().z(), 0,0,data.imu().localTransform().theta()) *
|
||||
orientation*
|
||||
data.imu().localTransform().rotation().inverse();
|
||||
|
||||
IMU imu2 = data.imu();
|
||||
imu2.convertToBaseFrame();
|
||||
|
||||
if( this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f &&
|
||||
this->framesProcessed() == 0)
|
||||
|
||||
@@ -234,6 +234,14 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
|
||||
{
|
||||
// removed parameters
|
||||
|
||||
// 0.20.9
|
||||
removedParameters_.insert(std::make_pair("OdomORBSLAM2/VocPath", std::make_pair(true, Parameters::kOdomORBSLAMVocPath())));
|
||||
removedParameters_.insert(std::make_pair("OdomORBSLAM2/Bf", std::make_pair(true, Parameters::kOdomORBSLAMBf())));
|
||||
removedParameters_.insert(std::make_pair("OdomORBSLAM2/ThDepth", std::make_pair(true, Parameters::kOdomORBSLAMThDepth())));
|
||||
removedParameters_.insert(std::make_pair("OdomORBSLAM2/Fps", std::make_pair(true, Parameters::kOdomORBSLAMFps())));
|
||||
removedParameters_.insert(std::make_pair("OdomORBSLAM2/MaxFeatures", std::make_pair(true, Parameters::kOdomORBSLAMMaxFeatures())));
|
||||
removedParameters_.insert(std::make_pair("OdomORBSLAM2/MapSize", std::make_pair(true, Parameters::kOdomORBSLAMMapSize())));
|
||||
|
||||
// 0.20.
|
||||
removedParameters_.insert(std::make_pair("SuperGlue/Path", std::make_pair(true, Parameters::kPyMatcherPath())));
|
||||
removedParameters_.insert(std::make_pair("SuperGlue/Iterations", std::make_pair(true, Parameters::kPyMatcherIterations())));
|
||||
@@ -823,8 +831,14 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
|
||||
#else
|
||||
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
|
||||
#endif
|
||||
#if RTABMAP_ORB_SLAM == 3
|
||||
str = "With ORB_SLAM3:";
|
||||
#elif RTABMAP_ORB_SLAM == 2
|
||||
str = "With ORB_SLAM2:";
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#else
|
||||
str = "With ORB_SLAM:";
|
||||
#endif
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
|
||||
#else
|
||||
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
|
||||
|
||||
@@ -127,7 +127,7 @@ void CameraRealSense2::close()
|
||||
{
|
||||
UINFO("%s", error.what());
|
||||
}
|
||||
|
||||
|
||||
closing_ = false;
|
||||
}
|
||||
|
||||
@@ -179,7 +179,7 @@ void CameraRealSense2::pose_callback(rs2::frame frame)
|
||||
pose.rotation.y,
|
||||
pose.rotation.w);
|
||||
|
||||
UDEBUG("POSE callback! %f %s (confidence=%d)", frame.get_timestamp(), poseT.prettyPrint().c_str(), (int)pose.tracker_confidence);
|
||||
//UDEBUG("POSE callback! %f %s (confidence=%d)", frame.get_timestamp(), poseT.prettyPrint().c_str(), (int)pose.tracker_confidence);
|
||||
|
||||
UScopeMutex sm(poseMutex_);
|
||||
poseBuffer_.insert(poseBuffer_.end(), std::make_pair(frame.get_timestamp(), std::make_pair(poseT, pose.tracker_confidence)));
|
||||
@@ -191,7 +191,7 @@ void CameraRealSense2::pose_callback(rs2::frame frame)
|
||||
|
||||
void CameraRealSense2::frame_callback(rs2::frame frame)
|
||||
{
|
||||
UDEBUG("Frame callback! %f", frame.get_timestamp());
|
||||
//UDEBUG("Frame callback! %f", frame.get_timestamp());
|
||||
syncer_(frame);
|
||||
}
|
||||
void CameraRealSense2::multiple_message_callback(rs2::frame frame)
|
||||
@@ -486,7 +486,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
|
||||
UINFO("setupDevice...");
|
||||
|
||||
close();
|
||||
|
||||
|
||||
clockSyncWarningShown_ = false;
|
||||
imuGlobalSyncWarningShown_ = false;
|
||||
|
||||
@@ -498,32 +498,40 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
|
||||
}
|
||||
|
||||
bool found=false;
|
||||
for (rs2::device dev : list)
|
||||
try
|
||||
{
|
||||
auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
|
||||
auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
|
||||
uint16_t pid;
|
||||
std::stringstream ss;
|
||||
ss << std::hex << pid_str;
|
||||
ss >> pid;
|
||||
UINFO("Device with serial number %s was found with product ID=%d.", sn, (int)pid);
|
||||
if(dualMode_ && pid == 0x0B37)
|
||||
for (rs2::device dev : list)
|
||||
{
|
||||
// Dual setup: device[0] = D400, device[1] = T265
|
||||
// T265
|
||||
dev_.resize(2);
|
||||
dev_[1] = dev;
|
||||
}
|
||||
else if (!found && (deviceId_.empty() || deviceId_ == sn))
|
||||
{
|
||||
if(dev_.empty())
|
||||
auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
|
||||
auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
|
||||
|
||||
uint16_t pid;
|
||||
std::stringstream ss;
|
||||
ss << std::hex << pid_str;
|
||||
ss >> pid;
|
||||
UINFO("Device with serial number %s was found with product ID=%d.", sn, (int)pid);
|
||||
if(dualMode_ && pid == 0x0B37)
|
||||
{
|
||||
dev_.resize(1);
|
||||
// Dual setup: device[0] = D400, device[1] = T265
|
||||
// T265
|
||||
dev_.resize(2);
|
||||
dev_[1] = dev;
|
||||
}
|
||||
else if (!found && (deviceId_.empty() || deviceId_ == sn))
|
||||
{
|
||||
if(dev_.empty())
|
||||
{
|
||||
dev_.resize(1);
|
||||
}
|
||||
dev_[0] = dev;
|
||||
found=true;
|
||||
}
|
||||
dev_[0] = dev;
|
||||
found=true;
|
||||
}
|
||||
}
|
||||
catch(const rs2::error & error)
|
||||
{
|
||||
UWARN("%s. Is the camera already used with another app?", error.what());
|
||||
}
|
||||
|
||||
if (!found)
|
||||
{
|
||||
@@ -1405,27 +1413,37 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
|
||||
{
|
||||
++iterB;
|
||||
}
|
||||
if(iterA != iterB)
|
||||
std::vector<double> stamps;
|
||||
for(;iterA != iterB;++iterA)
|
||||
{
|
||||
int pub = 0;
|
||||
for(;iterA != iterB;++iterA)
|
||||
{
|
||||
Transform tmp;
|
||||
IMU imuTmp;
|
||||
getPoseAndIMU(iterA->first, tmp, confidence, imuTmp);
|
||||
if(!imuTmp.empty())
|
||||
{
|
||||
UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0));
|
||||
pub++;
|
||||
}
|
||||
else
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
UDEBUG("inter imu published=%d, %f -> %f", pub, lastImuStamp_, imuStamp);
|
||||
stamps.push_back(iterA->first);
|
||||
}
|
||||
imuMutex_.unlock();
|
||||
|
||||
int pub = 0;
|
||||
for(size_t i=0; i<stamps.size(); ++i)
|
||||
{
|
||||
Transform tmp;
|
||||
IMU imuTmp;
|
||||
getPoseAndIMU(stamps[i], tmp, confidence, imuTmp);
|
||||
if(!imuTmp.empty())
|
||||
{
|
||||
UEventsManager::post(new IMUEvent(imuTmp, iterA->first/1000.0));
|
||||
pub++;
|
||||
}
|
||||
else
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(stamps.size())
|
||||
{
|
||||
UDEBUG("inter imu published=%d (rate=%fHz), %f -> %f", pub, double(pub)/((stamps.back()-stamps.front())/1000.0), stamps.front()/1000.0, stamps.back()/1000.0);
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("No inter imu published!?");
|
||||
}
|
||||
}
|
||||
lastImuStamp_ = imuStamp;
|
||||
}
|
||||
|
||||
@@ -217,14 +217,14 @@ static inline bool computeOrientation(
|
||||
// magnetic Field E must not be parallel to A,
|
||||
// choose an arbitrary orthogonal vector
|
||||
Eigen::Vector3f E;
|
||||
if (fabs(A[0]) > 0.1 || fabs(A[1]) > 0.1) {
|
||||
if (fabs(A[2]) > 0.1) {
|
||||
E[0] = 0.0;
|
||||
E[1] = A[2];
|
||||
E[2] = -A[1];
|
||||
} else if (fabs(A[0]) > 0.1 || fabs(A[1]) > 0.1) {
|
||||
E[0] = A[1];
|
||||
E[1] = A[0];
|
||||
E[2] = 0.0;
|
||||
} else if (fabs(A[2]) > 0.1) {
|
||||
E[0] = 0.0;
|
||||
E[1] = A[2];
|
||||
E[2] = A[1];
|
||||
} else {
|
||||
// free fall
|
||||
return false;
|
||||
|
||||
@@ -25,7 +25,6 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "rtabmap/core/odometry/OdometryORBSLAM2.h"
|
||||
#include "rtabmap/core/OdometryInfo.h"
|
||||
#include "rtabmap/core/util2d.h"
|
||||
#include "rtabmap/core/util3d_transforms.h"
|
||||
@@ -35,31 +34,42 @@ 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>
|
||||
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
#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)
|
||||
{
|
||||
{}
|
||||
|
||||
}
|
||||
private:
|
||||
long unsigned int maxFeatureMapSize_;
|
||||
|
||||
protected:
|
||||
void Track()
|
||||
{
|
||||
#if RTABMAP_ORB_SLAM == 3
|
||||
Map* mpMap = mpAtlas->GetCurrentMap();
|
||||
#endif
|
||||
if(mState==NO_IMAGES_YET)
|
||||
{
|
||||
mState = NOT_INITIALIZED;
|
||||
@@ -80,12 +90,23 @@ protected:
|
||||
//mpFrameDrawer->Update(this);
|
||||
|
||||
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
|
||||
{
|
||||
// System is initialized. Track Frame.
|
||||
bool bOK;
|
||||
bool bOK = true;
|
||||
|
||||
// Initial camera pose estimation using motion model or relocalization (if tracking is lost)
|
||||
if(!mbOnlyTracking)
|
||||
@@ -190,7 +211,8 @@ protected:
|
||||
}
|
||||
}
|
||||
|
||||
mCurrentFrame.mpReferenceKF = mpReferenceKF;
|
||||
if(!mCurrentFrame.mpReferenceKF)
|
||||
mCurrentFrame.mpReferenceKF = mpReferenceKF;
|
||||
|
||||
// If we have an initial estimation of the camera pose and matching. Track the local map.
|
||||
if(!mbOnlyTracking)
|
||||
@@ -362,6 +384,9 @@ 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);
|
||||
|
||||
@@ -459,9 +484,11 @@ 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();
|
||||
@@ -489,8 +516,11 @@ 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();
|
||||
@@ -501,10 +531,13 @@ 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)
|
||||
{
|
||||
}
|
||||
{}
|
||||
|
||||
public:
|
||||
void RunNoLoop()
|
||||
@@ -531,12 +564,18 @@ public:
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace ORB_SLAM2
|
||||
} // namespace ORB_SLAM
|
||||
|
||||
class ORBSLAM2System
|
||||
#if RTABMAP_ORB_SLAM == 3
|
||||
using namespace ORB_SLAM3;
|
||||
#else
|
||||
using namespace ORB_SLAM2;
|
||||
#endif
|
||||
|
||||
class ORBSLAMSystem
|
||||
{
|
||||
public:
|
||||
ORBSLAM2System(const rtabmap::ParametersMap & parameters) :
|
||||
ORBSLAMSystem(const rtabmap::ParametersMap & parameters) :
|
||||
mpVocabulary(0),
|
||||
mpKeyFrameDatabase(0),
|
||||
mpMap(0),
|
||||
@@ -548,14 +587,14 @@ public:
|
||||
parameters_(parameters)
|
||||
{
|
||||
std::string vocabularyPath;
|
||||
rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kOdomORBSLAM2VocPath(), vocabularyPath);
|
||||
rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kOdomORBSLAMVocPath(), vocabularyPath);
|
||||
|
||||
if(!vocabularyPath.empty())
|
||||
{
|
||||
//Load ORB Vocabulary
|
||||
vocabularyPath = uReplaceChar(vocabularyPath, '~', UDirectory::homeDir());
|
||||
UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str());
|
||||
mpVocabulary = new ORB_SLAM2::ORBVocabulary();
|
||||
mpVocabulary = new ORBVocabulary();
|
||||
bool bVocLoad = mpVocabulary->loadFromTextFile(vocabularyPath);
|
||||
if(!bVocLoad)
|
||||
{
|
||||
@@ -570,11 +609,11 @@ public:
|
||||
}
|
||||
else
|
||||
{
|
||||
UERROR("ORBSLAM2 vocabulary path should be set! (Parameter name=\"%s\")", rtabmap::Parameters::kOdomORBSLAM2VocPath().c_str());
|
||||
UERROR("ORBSLAM2 vocabulary path should be set! (Parameter name=\"%s\")", rtabmap::Parameters::kOdomORBSLAMVocPath().c_str());
|
||||
}
|
||||
}
|
||||
|
||||
bool init(const rtabmap::CameraModel & model, bool stereo, double baseline)
|
||||
bool init(const rtabmap::CameraModel & model, bool stereo, double baseline, const rtabmap::Transform & localIMUTransform)
|
||||
{
|
||||
if(!mpVocabulary)
|
||||
{
|
||||
@@ -591,11 +630,17 @@ public:
|
||||
{
|
||||
workingDir = ".";
|
||||
}
|
||||
std::string configPath = workingDir+"/rtabmap_orbslam2.yaml";
|
||||
std::string configPath = workingDir+"/rtabmap_orbslam.yaml";
|
||||
std::ofstream ofs (configPath, std::ofstream::out);
|
||||
ofs << "%YAML: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 << "Camera.fx: " << model.fx() << std::endl;
|
||||
ofs << "Camera.fy: " << model.fy() << std::endl;
|
||||
@@ -605,13 +650,13 @@ public:
|
||||
|
||||
if(model.D().cols < 4)
|
||||
{
|
||||
ofs << "Camera.k1: " << 0 << std::endl;
|
||||
ofs << "Camera.k2: " << 0 << std::endl;
|
||||
ofs << "Camera.p1: " << 0 << std::endl;
|
||||
ofs << "Camera.p2: " << 0 << std::endl;
|
||||
ofs << "Camera.k1: " << 0.0 << std::endl;
|
||||
ofs << "Camera.k2: " << 0.0 << std::endl;
|
||||
ofs << "Camera.p1: " << 0.0 << std::endl;
|
||||
ofs << "Camera.p2: " << 0.0 << std::endl;
|
||||
if(!stereo)
|
||||
{
|
||||
ofs << "Camera.k3: " << 0 << std::endl;
|
||||
ofs << "Camera.k3: " << 0.0 << std::endl;
|
||||
}
|
||||
}
|
||||
if(model.D().cols >= 4)
|
||||
@@ -638,8 +683,8 @@ public:
|
||||
//# IR projector baseline times fx (aprox.)
|
||||
if(baseline <= 0.0)
|
||||
{
|
||||
baseline = rtabmap::Parameters::defaultOdomORBSLAM2Bf();
|
||||
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2Bf(), baseline);
|
||||
baseline = rtabmap::Parameters::defaultOdomORBSLAMBf();
|
||||
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMBf(), baseline);
|
||||
}
|
||||
ofs << "Camera.bf: " << model.fx()*baseline << std::endl;
|
||||
ofs << std::endl;
|
||||
@@ -649,14 +694,14 @@ public:
|
||||
ofs << "Camera.RGB: 1" << std::endl;
|
||||
ofs << std::endl;
|
||||
|
||||
float fps = rtabmap::Parameters::defaultOdomORBSLAM2Fps();
|
||||
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2Fps(), fps);
|
||||
float fps = rtabmap::Parameters::defaultOdomORBSLAMFps();
|
||||
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMFps(), fps);
|
||||
ofs << "Camera.fps: " << fps << std::endl;
|
||||
ofs << std::endl;
|
||||
|
||||
//# Close/Far threshold. Baseline times.
|
||||
double thDepth = rtabmap::Parameters::defaultOdomORBSLAM2ThDepth();
|
||||
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2ThDepth(), thDepth);
|
||||
double thDepth = rtabmap::Parameters::defaultOdomORBSLAMThDepth();
|
||||
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMThDepth(), thDepth);
|
||||
ofs << "ThDepth: " << thDepth << std::endl;
|
||||
ofs << std::endl;
|
||||
|
||||
@@ -664,12 +709,37 @@ 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
|
||||
//#--------------------------------------------------------------------------------------------
|
||||
//# ORB Extractor: Number of features per image
|
||||
int features = rtabmap::Parameters::defaultOdomORBSLAM2MaxFeatures();
|
||||
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2MaxFeatures(), features);
|
||||
int features = rtabmap::Parameters::defaultOdomORBSLAMMaxFeatures();
|
||||
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMMaxFeatures(), features);
|
||||
ofs << "ORBextractor.nFeatures: " << features << std::endl;
|
||||
ofs << std::endl;
|
||||
|
||||
@@ -697,29 +767,36 @@ public:
|
||||
ofs << "ORBextractor.minThFAST: " << minThFAST << std::endl;
|
||||
ofs << std::endl;
|
||||
|
||||
int maxFeatureMapSize = rtabmap::Parameters::defaultOdomORBSLAM2MapSize();
|
||||
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAM2MapSize(), maxFeatureMapSize);
|
||||
int maxFeatureMapSize = rtabmap::Parameters::defaultOdomORBSLAMMapSize();
|
||||
rtabmap::Parameters::parse(parameters_, rtabmap::Parameters::kOdomORBSLAMMapSize(), maxFeatureMapSize);
|
||||
|
||||
ofs.close();
|
||||
|
||||
//Create KeyFrame Database
|
||||
mpKeyFrameDatabase = new ORB_SLAM2::KeyFrameDatabase(*mpVocabulary);
|
||||
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 ORB_SLAM2::Tracker(0, mpVocabulary, 0, 0, mpMap, mpKeyFrameDatabase, configPath, stereo?ORB_SLAM2::System::STEREO:ORB_SLAM2::System::RGBD, maxFeatureMapSize);
|
||||
mpTracker = new Tracker(0, mpVocabulary, 0, 0, mpMap, mpKeyFrameDatabase, configPath, stereo?System::STEREO:System::RGBD, maxFeatureMapSize);
|
||||
|
||||
//Initialize the Local Mapping thread and launch
|
||||
mpLocalMapper = new ORB_SLAM2::LocalMapping(mpMap, false);
|
||||
|
||||
#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 ORB_SLAM2::LoopCloser(mpMap, mpKeyFrameDatabase, mpVocabulary, true);
|
||||
mpLoopCloser = new LoopCloser(mpMap, mpKeyFrameDatabase, mpVocabulary, true);
|
||||
|
||||
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, mpLocalMapper);
|
||||
mptLoopClosing = new thread(&ORB_SLAM2::LoopCloser::RunNoLoop, mpLoopCloser);
|
||||
mptLocalMapping = new thread(&LocalMapping::Run, mpLocalMapper);
|
||||
mptLoopClosing = new thread(&LoopCloser::RunNoLoop, mpLoopCloser);
|
||||
|
||||
//Set pointers between threads
|
||||
mpTracker->SetLocalMapper(mpLocalMapper);
|
||||
@@ -731,11 +808,15 @@ public:
|
||||
|
||||
mpLoopCloser->SetTracker(mpTracker);
|
||||
mpLoopCloser->SetLocalMapper(mpLocalMapper);
|
||||
#if RTABMAP_ORB_SLAM == 3
|
||||
if(ULogger::level() > ULogger::kInfo)
|
||||
Verbose::SetTh(Verbose::VERBOSITY_QUIET);
|
||||
#endif
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
virtual ~ORBSLAM2System()
|
||||
virtual ~ORBSLAMSystem()
|
||||
{
|
||||
shutdown();
|
||||
delete mpVocabulary;
|
||||
@@ -776,25 +857,29 @@ public:
|
||||
|
||||
public:
|
||||
// ORB vocabulary used for place recognition and feature matching.
|
||||
ORB_SLAM2::ORBVocabulary* mpVocabulary;
|
||||
ORBVocabulary* mpVocabulary;
|
||||
|
||||
// KeyFrame database for place recognition (relocalization and loop detection).
|
||||
ORB_SLAM2::KeyFrameDatabase* mpKeyFrameDatabase;
|
||||
KeyFrameDatabase* mpKeyFrameDatabase;
|
||||
|
||||
// Map structure that stores the pointers to all KeyFrames and MapPoints.
|
||||
ORB_SLAM2::Map* mpMap;
|
||||
#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
|
||||
// performs relocalization if tracking fails.
|
||||
ORB_SLAM2::Tracker* mpTracker;
|
||||
Tracker* mpTracker;
|
||||
|
||||
// Local Mapper. It manages the local map and performs local bundle adjustment.
|
||||
ORB_SLAM2::LocalMapping* mpLocalMapper;
|
||||
LocalMapping* mpLocalMapper;
|
||||
|
||||
// Loop Closer. It searches loops with every new keyframe. If there is a loop it performs
|
||||
// a pose graph optimization and full bundle adjustment (in a new thread) afterwards.
|
||||
ORB_SLAM2::LoopCloser* mpLoopCloser;
|
||||
LoopCloser* mpLoopCloser;
|
||||
|
||||
// System threads: Local Mapping, Loop Closing, Viewer.
|
||||
// The Tracking thread "lives" in the main execution thread that creates the System object.
|
||||
@@ -807,55 +892,96 @@ public:
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) :
|
||||
OdometryORBSLAM::OdometryORBSLAM(const ParametersMap & parameters) :
|
||||
Odometry(parameters)
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
,
|
||||
orbslam2_(0),
|
||||
orbslam_(0),
|
||||
firstFrame_(true),
|
||||
previousPose_(Transform::getIdentity())
|
||||
previousPose_(Transform::getIdentity()),
|
||||
useIMU_(false) // TODO: Not yet supported with ORB_SLAM3
|
||||
#endif
|
||||
{
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
orbslam2_ = new ORBSLAM2System(parameters);
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
orbslam_ = new ORBSLAMSystem(parameters);
|
||||
#endif
|
||||
}
|
||||
|
||||
OdometryORBSLAM2::~OdometryORBSLAM2()
|
||||
OdometryORBSLAM::~OdometryORBSLAM()
|
||||
{
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
if(orbslam2_)
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
if(orbslam_)
|
||||
{
|
||||
delete orbslam2_;
|
||||
delete orbslam_;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
void OdometryORBSLAM2::reset(const Transform & initialPose)
|
||||
void OdometryORBSLAM::reset(const Transform & initialPose)
|
||||
{
|
||||
Odometry::reset(initialPose);
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
if(orbslam2_)
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
if(orbslam_)
|
||||
{
|
||||
orbslam2_->shutdown();
|
||||
orbslam_->shutdown();
|
||||
}
|
||||
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 OdometryORBSLAM2::computeTransform(
|
||||
Transform OdometryORBSLAM::computeTransform(
|
||||
SensorData & data,
|
||||
const Transform & guess,
|
||||
OdometryInfo * info)
|
||||
{
|
||||
Transform t;
|
||||
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
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)
|
||||
@@ -876,12 +1002,18 @@ Transform OdometryORBSLAM2::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(orbslam2_->mpTracker == 0)
|
||||
if(orbslam_->mpTracker == 0)
|
||||
{
|
||||
CameraModel model = data.cameraModels().size()==1?data.cameraModels()[0]:data.stereoCameraModel().left();
|
||||
if(!orbslam2_->init(model, stereo, data.stereoCameraModel().baseline()))
|
||||
if(!orbslam_->init(model, stereo, data.stereoCameraModel().baseline(), imuLocalTransform_))
|
||||
{
|
||||
return t;
|
||||
}
|
||||
@@ -892,7 +1024,7 @@ Transform OdometryORBSLAM2::computeTransform(
|
||||
if(stereo)
|
||||
{
|
||||
localTransform = data.stereoCameraModel().localTransform();
|
||||
Tcw = ((ORB_SLAM2::Tracker*)orbslam2_->mpTracker)->GrabImageStereo(data.imageRaw(), data.rightRaw(), data.stamp());
|
||||
Tcw = ((Tracker*)orbslam_->mpTracker)->GrabImageStereo(data.imageRaw(), data.rightRaw(), data.stamp());
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -906,11 +1038,11 @@ Transform OdometryORBSLAM2::computeTransform(
|
||||
{
|
||||
depth = util2d::cvtDepthToFloat(data.depthRaw());
|
||||
}
|
||||
Tcw = ((ORB_SLAM2::Tracker*)orbslam2_->mpTracker)->GrabImageRGBD(data.imageRaw(), depth, data.stamp());
|
||||
Tcw = ((Tracker*)orbslam_->mpTracker)->GrabImageRGBD(data.imageRaw(), depth, data.stamp());
|
||||
}
|
||||
|
||||
Transform previousPoseInv = previousPose_.inverse();
|
||||
if(orbslam2_->mpTracker->mState == ORB_SLAM2::Tracking::LOST)
|
||||
if(orbslam_->mpTracker->mState == Tracking::LOST)
|
||||
{
|
||||
covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
|
||||
}
|
||||
@@ -944,8 +1076,8 @@ Transform OdometryORBSLAM2::computeTransform(
|
||||
float baseline = data.stereoCameraModel().baseline();
|
||||
if(baseline <= 0.0f)
|
||||
{
|
||||
baseline = rtabmap::Parameters::defaultOdomORBSLAM2Bf();
|
||||
rtabmap::Parameters::parse(orbslam2_->parameters_, rtabmap::Parameters::kOdomORBSLAM2Bf(), baseline);
|
||||
baseline = rtabmap::Parameters::defaultOdomORBSLAMBf();
|
||||
rtabmap::Parameters::parse(orbslam_->parameters_, rtabmap::Parameters::kOdomORBSLAMBf(), baseline);
|
||||
}
|
||||
double linearVar = 0.0001;
|
||||
if(baseline > 0.0f)
|
||||
@@ -966,39 +1098,39 @@ Transform OdometryORBSLAM2::computeTransform(
|
||||
|
||||
int totalMapPoints= 0;
|
||||
int totalKfs= 0;
|
||||
if(orbslam2_->mpMap)
|
||||
if(orbslam_->mpMap)
|
||||
{
|
||||
totalMapPoints = orbslam2_->mpMap->MapPointsInMap();
|
||||
totalKfs = orbslam2_->mpMap->KeyFramesInMap();
|
||||
totalMapPoints = orbslam_->mpMap->MapPointsInMap();
|
||||
totalKfs = orbslam_->mpMap->KeyFramesInMap();
|
||||
}
|
||||
|
||||
if(info)
|
||||
{
|
||||
info->lost = t.isNull();
|
||||
info->type = (int)kTypeORBSLAM2;
|
||||
info->type = (int)kTypeORBSLAM;
|
||||
info->reg.covariance = covariance;
|
||||
info->localMapSize = totalMapPoints;
|
||||
info->localKeyFrames = totalKfs;
|
||||
|
||||
if(this->isInfoDataFilled() && orbslam2_->mpTracker && orbslam2_->mpMap)
|
||||
if(this->isInfoDataFilled() && orbslam_->mpTracker && orbslam_->mpMap)
|
||||
{
|
||||
const std::vector<cv::KeyPoint> & kpts = orbslam2_->mpTracker->mCurrentFrame.mvKeys;
|
||||
const std::vector<cv::KeyPoint> & kpts = orbslam_->mpTracker->mCurrentFrame.mvKeys;
|
||||
info->reg.matchesIDs.resize(kpts.size());
|
||||
info->reg.inliersIDs.resize(kpts.size());
|
||||
int oi = 0;
|
||||
for (unsigned int i = 0; i < kpts.size(); ++i)
|
||||
{
|
||||
int wordId;
|
||||
if(orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0)
|
||||
if(orbslam_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0)
|
||||
{
|
||||
wordId = orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i]->mnId;
|
||||
wordId = orbslam_->mpTracker->mCurrentFrame.mvpMapPoints[i]->mnId;
|
||||
}
|
||||
else
|
||||
{
|
||||
wordId = -(i+1);
|
||||
}
|
||||
info->words.insert(std::make_pair(wordId, kpts[i]));
|
||||
if(orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0)
|
||||
if(orbslam_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0)
|
||||
{
|
||||
info->reg.matchesIDs[oi] = wordId;
|
||||
info->reg.inliersIDs[oi] = wordId;
|
||||
@@ -1010,7 +1142,7 @@ Transform OdometryORBSLAM2::computeTransform(
|
||||
info->reg.inliers = oi;
|
||||
info->reg.matches = oi;
|
||||
|
||||
std::vector<ORB_SLAM2::MapPoint*> mapPoints = orbslam2_->mpMap->GetAllMapPoints();
|
||||
std::vector<MapPoint*> mapPoints = orbslam_->mpMap->GetAllMapPoints();
|
||||
Eigen::Affine3f fixRot = (this->getPose()*previousPoseInv*originLocalTransform_).toEigen3f();
|
||||
for (unsigned int i = 0; i < mapPoints.size(); ++i)
|
||||
{
|
||||
@@ -1024,7 +1156,7 @@ Transform OdometryORBSLAM2::computeTransform(
|
||||
UINFO("Odom update time = %fs, map points=%d, keyframes=%d, lost=%s", timer.elapsed(), totalMapPoints, totalKfs, t.isNull()?"true":"false");
|
||||
|
||||
#else
|
||||
UERROR("RTAB-Map is not built with ORB_SLAM2 support! Select another visual odometry approach.");
|
||||
UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
|
||||
#endif
|
||||
return t;
|
||||
}
|
||||
@@ -40,7 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include <rtabmap/core/util3d.h>
|
||||
|
||||
|
||||
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
|
||||
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
|
||||
#include "g2o/core/sparse_optimizer.h"
|
||||
#include "g2o/core/block_solver.h"
|
||||
#include "g2o/core/factory.h"
|
||||
@@ -71,7 +71,7 @@ typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> Matr
|
||||
#endif
|
||||
#endif // RTABMAP_G2O
|
||||
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
#include "g2o/types/types_sba.h"
|
||||
#include "g2o/types/types_six_dof_expmap.h"
|
||||
#include "g2o/solvers/linear_solver_eigen.h"
|
||||
@@ -102,7 +102,7 @@ typedef VertexPointXYZ VertexSBAPointXYZ;
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#endif // end defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
|
||||
#endif // end defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
|
||||
|
||||
enum {
|
||||
PARAM_OFFSET=0,
|
||||
@@ -112,7 +112,7 @@ namespace rtabmap {
|
||||
|
||||
bool OptimizerG2O::available()
|
||||
{
|
||||
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
|
||||
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
|
||||
return true;
|
||||
#else
|
||||
return false;
|
||||
@@ -149,10 +149,10 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters)
|
||||
UASSERT(pixelVariance_ > 0.0);
|
||||
UASSERT(baseline_ >= 0.0);
|
||||
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
if(solver_ != 3)
|
||||
{
|
||||
UWARN("g2o built with ORB_SLAM2 has only Eigen solver available, using Eigen=3 instead of %d.", solver_);
|
||||
UWARN("g2o built with ORB_SLAM has only Eigen solver available, using Eigen=3 instead of %d.", solver_);
|
||||
solver_ = 3;
|
||||
}
|
||||
#else
|
||||
@@ -1165,8 +1165,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
|
||||
}
|
||||
UDEBUG("Optimizing graph...end!");
|
||||
#else
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
UERROR("G2O graph optimization cannot be used with g2o built from ORB_SLAM2, only SBA is available.");
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
UERROR("G2O graph optimization cannot be used with g2o built from ORB_SLAM, only SBA is available.");
|
||||
#else
|
||||
UERROR("Not built with G2O support!");
|
||||
#endif
|
||||
@@ -1174,7 +1174,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
|
||||
return optimizedPoses;
|
||||
}
|
||||
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
/**
|
||||
* \brief 3D edge between two SBAcam
|
||||
*/
|
||||
@@ -1259,7 +1259,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
std::set<int> * outliers)
|
||||
{
|
||||
std::map<int, Transform> optimizedPoses;
|
||||
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
|
||||
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
|
||||
UDEBUG("Optimizing graph...");
|
||||
|
||||
optimizedPoses.clear();
|
||||
@@ -1267,13 +1267,13 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
{
|
||||
g2o::SparseOptimizer optimizer;
|
||||
//optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
|
||||
#if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM2)
|
||||
#if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM)
|
||||
std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
|
||||
#else
|
||||
g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
|
||||
#endif
|
||||
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
|
||||
#else
|
||||
if(solver_ == 3)
|
||||
@@ -1316,9 +1316,9 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
|
||||
#endif
|
||||
}
|
||||
#endif // RTABMAP_ORB_SLAM2
|
||||
#endif // RTABMAP_ORB_SLAM
|
||||
|
||||
#ifndef RTABMAP_ORB_SLAM2
|
||||
#ifndef RTABMAP_ORB_SLAM
|
||||
if(optimizer_ == 1)
|
||||
{
|
||||
#ifdef RTABMAP_G2O_CPP11
|
||||
@@ -1331,7 +1331,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
else
|
||||
#endif
|
||||
{
|
||||
#if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM2)
|
||||
#if defined(RTABMAP_G2O_CPP11) and not defined(RTABMAP_ORB_SLAM)
|
||||
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(
|
||||
g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver))));
|
||||
#else
|
||||
@@ -1353,14 +1353,14 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
|
||||
// Add node's pose
|
||||
UASSERT(!camPose.isNull());
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
|
||||
#else
|
||||
g2o::VertexCam * vCam = new g2o::VertexCam();
|
||||
#endif
|
||||
|
||||
Eigen::Affine3d a = camPose.toEigen3d();
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
a = a.inverse();
|
||||
vCam->setEstimate(g2o::SE3Quat(a.linear(), a.translation()));
|
||||
#else
|
||||
@@ -1407,7 +1407,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
|
||||
if(id1 == id2)
|
||||
{
|
||||
#ifndef RTABMAP_ORB_SLAM2
|
||||
#ifndef RTABMAP_ORB_SLAM
|
||||
g2o::HyperGraph::Edge * edge = 0;
|
||||
if(gravitySigma() > 0 && iter->second.type() == Link::kGravity && poses.find(iter->first) != poses.end())
|
||||
{
|
||||
@@ -1455,7 +1455,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
// id1,
|
||||
// id2,
|
||||
// camLink.prettyPrint().c_str());
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
EdgeSE3Expmap * e = new EdgeSE3Expmap();
|
||||
g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id1);
|
||||
g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id2);
|
||||
@@ -1537,7 +1537,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
|
||||
g2o::OptimizableGraph::Edge * e;
|
||||
double baseline = 0.0;
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
g2o::VertexSE3Expmap* vcam = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
|
||||
std::map<int, CameraModel>::const_iterator iterModel = models.find(camId);
|
||||
|
||||
@@ -1551,7 +1551,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
if(uIsFinite(depth) && depth > 0.0 && baseline > 0.0)
|
||||
{
|
||||
// stereo edge
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ();
|
||||
float disparity = baseline * iterModel->second.fx() / depth;
|
||||
Eigen::Vector3d obs( pt.kpt.pt.x, pt.kpt.pt.y, pt.kpt.pt.x-disparity);
|
||||
@@ -1584,7 +1584,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
vpt3d->id()-stepVertexId, camId, (int)pt.kpt.pt.x, (int)pt.kpt.pt.y, depth);
|
||||
}
|
||||
// mono edge
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
g2o::EdgeSE3ProjectXYZ* em = new g2o::EdgeSE3ProjectXYZ();
|
||||
Eigen::Vector2d obs( pt.kpt.pt.x, pt.kpt.pt.y);
|
||||
em->setMeasurement(obs);
|
||||
@@ -1662,7 +1662,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
(*iter)->setLevel(1);
|
||||
++outliersCount;
|
||||
double d = 0.0;
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
if(dynamic_cast<g2o::EdgeStereoSE3ProjectXYZ*>(*iter) != 0)
|
||||
{
|
||||
d = ((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[0]-((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[2];
|
||||
@@ -1716,7 +1716,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
{
|
||||
if(iter->first > 0)
|
||||
{
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(iter->first);
|
||||
#else
|
||||
const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
|
||||
@@ -1725,7 +1725,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
|
||||
{
|
||||
Transform t = Transform::fromEigen3d(v->estimate());
|
||||
|
||||
#ifdef RTABMAP_ORB_SLAM2
|
||||
#ifdef RTABMAP_ORB_SLAM
|
||||
t=t.inverse();
|
||||
#endif
|
||||
|
||||
|
||||
Reference in New Issue
Block a user