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

ORB-SLAM3 IMU support fixes (#1612)

Browse Source 
* Working IMU_RGBD and IMU_STEREO. Changing Inter IMU base frame is now possible.

* Output error msg when vocabulary path is wrong.

* Showing full local feature map
This commit is contained in:
matlabbe
2025-11-11 14:09:13 -08:00
committed by GitHub
parent c97f0c10dd
commit e612d103bf
8 changed files with 120 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
corelib/src/Camera.cpp
View File

@@ -39,7 +39,8 @@ namespace rtabmap
Camera::Camera(float imageRate, const Transform & localTransform) :
SensorCapture(imageRate, localTransform*CameraModel::opticalRotation()),
imuFilter_(0),
publishInterIMU_(false)
publishInterIMU_(false),
imuBaseFrameConversion_(false)
{}
Camera::~Camera()
@@ -52,15 +53,23 @@ bool Camera::initFromFile(const std::string & calibrationPath)
return init(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front());
}
void Camera::setInterIMUPublishing(bool enabled, IMUFilter * filter)
void Camera::setInterIMUPublishing(bool enabled, IMUFilter * filter, bool baseFrameConversion)
{
publishInterIMU_ = enabled;
delete imuFilter_;
imuFilter_ = filter;
imuBaseFrameConversion_ = baseFrameConversion;
}
void Camera::postInterIMU(const IMU & imu, double stamp)
void Camera::postInterIMU(const IMU & imu_in, double stamp)
{
IMU imu = imu_in;
if(imuBaseFrameConversion_)
{
UASSERT(!imu.localTransform().isNull());
imu.convertToBaseFrame();
}
if(imuFilter_)
{
imuFilter_->update(

4
corelib/src/Parameters.cpp
View File

@@ -1119,8 +1119,8 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
ignore = true;
}
#endif
#ifndef RTABMAP_ORBSLAM2
if(group.compare("OdomORBSLAM2") == 0)
#ifndef RTABMAP_ORB_SLAM
if(group.compare("OdomORBSLAM") == 0)
{
ignore = true;
}

75
corelib/src/odometry/OdometryORBSLAM3.cpp
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UDirectory.h"
#include "rtabmap/utilite/UFile.h"
#include <pcl/common/transforms.h>
#include <opencv2/imgproc/types_c.h>
#include <rtabmap/core/odometry/OdometryORBSLAM3.h>
@@ -116,6 +117,13 @@ bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap::
}
//Load ORB Vocabulary
vocabularyPath = uReplaceChar(vocabularyPath, '~', UDirectory::homeDir());
if(!UFile::exists(vocabularyPath))
{
UERROR("ORB_SLAM vocabulary path \"%s\" doesn't exist! (Parameter name=\"%s\")",
vocabularyPath.c_str(),
rtabmap::Parameters::kOdomORBSLAMVocPath().c_str());
return false;
}
UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str());
// Create configuration file
@@ -240,7 +248,7 @@ bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap::
//# IMU Parameters TODO: hard-coded, not used
//#--------------------------------------------------------------------------------------------
// Transformation from camera 0 to body-frame (imu)
rtabmap::Transform camImuT = model1.localTransform()*imuLocalTransform_;
rtabmap::Transform camImuT = imuLocalTransform_.inverse()*model1.localTransform();
ofs << "IMU.T_b_c1: !!opencv-matrix" << std::endl;
ofs << " rows: 4" << std::endl;
ofs << " cols: 4" << std::endl;
@@ -340,14 +348,16 @@ bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap::
ofs.close();
ORB_SLAM3::System::eSensor sensor =
stereo?(withIMU?ORB_SLAM3::System::IMU_STEREO:ORB_SLAM3::System::STEREO):
(withIMU?ORB_SLAM3::System::IMU_RGBD:ORB_SLAM3::System::RGBD);
UINFO("Initializing ORB_SLAM3 system with sensor %d...", (int)sensor);
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,
sensor,
false);
UINFO("Initializing ORB_SLAM3 system with sensor %d... done!", (int)sensor);
return true;
#else
UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
@@ -373,6 +383,7 @@ Transform OdometryORBSLAM3::computeTransform(
{
if(lastImuStamp_ == 0.0 || lastImuStamp_ < data.stamp())
{
UDEBUG("Adding IMU %f", data.stamp());
orbslamImus_.push_back(ORB_SLAM3::IMU::Point(
data.imu().linearAcceleration().val[0],
data.imu().linearAcceleration().val[1],
@@ -432,6 +443,7 @@ Transform OdometryORBSLAM3::computeTransform(
if(lastImageStamp_ == 0.0)
{
lastImageStamp_ = data.stamp();
UDEBUG("Waiting for another image to initialize...");
return t;
}
@@ -457,6 +469,7 @@ Transform OdometryORBSLAM3::computeTransform(
rightMono = cv::Mat();
cv::cvtColor(data.imageRaw(), rightMono, CV_BGR2GRAY);
}
UDEBUG("Adding Stereo Frame %f", data.stamp());
Tcw = orbslam_->TrackStereo(leftMono, rightMono, data.stamp(), orbslamImus_);
orbslamImus_.clear();
}
@@ -472,15 +485,22 @@ Transform OdometryORBSLAM3::computeTransform(
{
depth = util2d::cvtDepthToFloat(data.depthRaw());
}
UDEBUG("Adding RGBD Frame %f", data.stamp());
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())
std::vector<ORB_SLAM3::MapPoint*> trackedMapPoints = orbslam_->GetTrackedMapPoints();
if(orbslam_->isLost() || trackedMapPoints.empty())
{
covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f;
if(!imuLocalTransform_.isNull()) {
UWARN("ORBSLAM lost tracking! If it is on initialization, try moving the sensor in a circle for a couple of seconds.");
}
else {
UWARN("ORBSLAM lost tracking!");
}
}
else
{
@@ -490,14 +510,16 @@ Transform OdometryORBSLAM3::computeTransform(
if(!p.isNull())
{
if(!localTransform.isNull())
if(!imuLocalTransform_.isNull())
{
if(originLocalTransform_.isNull())
{
originLocalTransform_ = localTransform;
}
// transform in base frame
p = originLocalTransform_ * p.inverse() * localTransform.inverse();
// Transform p from optical-imu system (x->left, y->back and z->up) to ros system, then remove camera local transform
p = Transform(0,0,0,0,0,-M_PI/2) * p.inverse() * localTransform.inverse();
}
else
{
UASSERT(!localTransform.isNull());
// Transform p from optical system (x->right, y->down and z->forward) to ros system, then remove camera local transform
p = CameraModel::opticalRotation() * p.inverse() * localTransform.inverse();
}
t = previousPoseInv*p;
}
@@ -534,12 +556,14 @@ Transform OdometryORBSLAM3::computeTransform(
}
}
size_t mapPointsSize = 0;
if(info)
{
info->lost = t.isNull();
info->type = (int)kTypeORBSLAM;
info->reg.covariance = covariance;
info->localMapSize = mapPoints.size();
std::vector<ORB_SLAM3::MapPoint*> mapPoints = orbslam_->GetAllMapPoints();
info->localMapSize = mapPointsSize = mapPoints.size();
info->localKeyFrames = 0;
if(this->isInfoDataFilled())
@@ -549,20 +573,20 @@ Transform OdometryORBSLAM3::computeTransform(
info->reg.inliersIDs.resize(kpts.size());
int oi = 0;
UASSERT(mapPoints.size() == kpts.size());
UASSERT(trackedMapPoints.size() == kpts.size());
for (unsigned int i = 0; i < kpts.size(); ++i)
{
int wordId;
if(mapPoints[i] != 0)
if(trackedMapPoints[i] != 0)
{
wordId = mapPoints[i]->mnId;
wordId = trackedMapPoints[i]->mnId;
}
else
{
wordId = -(i+1);
}
info->words.insert(std::make_pair(wordId, kpts[i]));
if(mapPoints[i] != 0)
if(trackedMapPoints[i] != 0)
{
info->reg.matchesIDs[oi] = wordId;
info->reg.inliersIDs[oi] = wordId;
@@ -574,7 +598,15 @@ Transform OdometryORBSLAM3::computeTransform(
info->reg.inliers = oi;
info->reg.matches = oi;
Eigen::Affine3f fixRot = (this->getPose()*previousPoseInv*originLocalTransform_).toEigen3f();
Eigen::Affine3f fixRot;
if(!imuLocalTransform_.isNull())
{
fixRot = (this->getPose()*previousPoseInv*Transform(0,0,0,0,0,-M_PI/2)).toEigen3f();
}
else
{
fixRot = (this->getPose()*previousPoseInv*CameraModel::opticalRotation()).toEigen3f();
}
for (unsigned int i = 0; i < mapPoints.size(); ++i)
{
if(mapPoints[i])
@@ -587,7 +619,8 @@ Transform OdometryORBSLAM3::computeTransform(
}
}
UINFO("Odom update time = %fs, map points=%ld, lost=%s", timer.elapsed(), mapPoints.size(), t.isNull()?"true":"false");
UINFO("Odom update time = %fs, tracked points=%ld, map points=%ld, lost=%s",
timer.elapsed(), trackedMapPoints.size(), mapPointsSize, t.isNull()?"true":"false");
#else
UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");