i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-05 01:27:46 +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
cmake_modules/FindORB_SLAM.cmake
+4 -3
View File
@@ -11,6 +11,7 @@
find_path(ORB_SLAM_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/include) find_path(ORB_SLAM_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/include)
find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM_ROOT_DIR}/lib) find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM_ROOT_DIR}/lib)
find_library(ORB_SLAM3_LIBRARY NAMES ORB_SLAM3 PATHS $ENV{ORB_SLAM_ROOT_DIR}/lib) find_library(ORB_SLAM3_LIBRARY NAMES ORB_SLAM3 PATHS $ENV{ORB_SLAM_ROOT_DIR}/lib)
find_path(DBoW2_INCLUDE_DIR NAMES DBoW2/BowVector.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/DBoW2 NO_DEFAULT_PATH)
find_path(g2o_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/g2o NO_DEFAULT_PATH) find_path(g2o_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/g2o NO_DEFAULT_PATH)
find_path(sophus_INCLUDE_DIR NAMES sophus/se3.hpp PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/Sophus NO_DEFAULT_PATH) find_path(sophus_INCLUDE_DIR NAMES sophus/se3.hpp PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/Sophus NO_DEFAULT_PATH)
find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/g2o/lib NO_DEFAULT_PATH) find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM_ROOT_DIR}/Thirdparty/g2o/lib NO_DEFAULT_PATH)
@@ -22,9 +23,9 @@ IF(ORB_SLAM2_LIBRARY)
ELSEIF(ORB_SLAM3_LIBRARY) ELSEIF(ORB_SLAM3_LIBRARY)
SET(ORB_SLAM_VERSION 3) SET(ORB_SLAM_VERSION 3)
SET(ORB_SLAM_LIBRARY ${ORB_SLAM3_LIBRARY}) SET(ORB_SLAM_LIBRARY ${ORB_SLAM3_LIBRARY})
IF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR) # ORB_SLAM3 v1 IF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR AND DBoW2_INCLUDE_DIR) # ORB_SLAM3 v1
SET(g2o_INCLUDE_DIR ${g2o_INCLUDE_DIR} ${sophus_INCLUDE_DIR}) SET(g2o_INCLUDE_DIR ${g2o_INCLUDE_DIR} ${sophus_INCLUDE_DIR} ${DBoW2_INCLUDE_DIR} $ENV{ORB_SLAM_ROOT_DIR})
ENDIF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR) ENDIF(g2o_INCLUDE_DIR AND sophus_INCLUDE_DIR AND DBoW2_INCLUDE_DIR)
ENDIF() ENDIF()
IF (ORB_SLAM_INCLUDE_DIR AND ORB_SLAM_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY) IF (ORB_SLAM_INCLUDE_DIR AND ORB_SLAM_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
corelib/include/rtabmap/core/Camera.h
+2 -1
View File
@@ -48,7 +48,7 @@ public:
SensorData takeImage(SensorCaptureInfo * info = 0) {return takeData(info);} SensorData takeImage(SensorCaptureInfo * info = 0) {return takeData(info);}
float getImageRate() const {return getFrameRate();} float getImageRate() const {return getFrameRate();}
void setImageRate(float imageRate) {setFrameRate(imageRate);} void setImageRate(float imageRate) {setFrameRate(imageRate);}
void setInterIMUPublishing(bool enabled, IMUFilter * filter = 0); // Take ownership of filter void setInterIMUPublishing(bool enabled, IMUFilter * filter = 0, bool baseFrameConversion = false); // Take ownership of filter
bool isInterIMUPublishing() const {return publishInterIMU_;} bool isInterIMUPublishing() const {return publishInterIMU_;}
bool initFromFile(const std::string & calibrationPath); bool initFromFile(const std::string & calibrationPath);
@@ -73,6 +73,7 @@ private:
private: private:
IMUFilter * imuFilter_; IMUFilter * imuFilter_;
bool publishInterIMU_; bool publishInterIMU_;
bool imuBaseFrameConversion_;
}; };
corelib/include/rtabmap/core/Parameters.h
+2 -2
View File
@@ -463,7 +463,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(GTSAM, IncRelinearizeSkip, int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info."); RTABMAP_PARAM(GTSAM, IncRelinearizeSkip, int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");
// Odometry // Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D 13=cuVSLAM"); RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D 13=cuVSLAM");
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images where odometry cannot be computed (a value of 0 disables auto-reset). When a reset occurs, odometry resumes from the last successfully computed pose with large covariance to trigger a new map. If external odometry is used, it will also be reset based on the motion estimated relative to the last computed pose but no large covariance will be received, so that a new map won't be triggered."); RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images where odometry cannot be computed (a value of 0 disables auto-reset). When a reset occurs, odometry resumes from the last successfully computed pose with large covariance to trigger a new map. If external odometry is used, it will also be reset based on the motion estimated relative to the last computed pose but no large covariance will be received, so that a new map won't be triggered.");
RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw))."); RTABMAP_PARAM(Odom, Holonomic, bool, true, "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features)."); RTABMAP_PARAM(Odom, FillInfoData, bool, true, "Fill info with data (inliers/outliers features).");
@@ -558,7 +558,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(OdomViso2, BucketWidth, double, 50, "Width of bucket."); RTABMAP_PARAM(OdomViso2, BucketWidth, double, 50, "Width of bucket.");
RTABMAP_PARAM(OdomViso2, BucketHeight, double, 50, "Height of bucket."); RTABMAP_PARAM(OdomViso2, BucketHeight, double, 50, "Height of bucket.");
// Odometry ORB_SLAM2 // Odometry ORB_SLAM
RTABMAP_PARAM_STR(OdomORBSLAM, VocPath, "", "Path to ORB vocabulary (*.txt)."); RTABMAP_PARAM_STR(OdomORBSLAM, VocPath, "", "Path to ORB vocabulary (*.txt).");
RTABMAP_PARAM(OdomORBSLAM, Bf, double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used."); RTABMAP_PARAM(OdomORBSLAM, Bf, double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used.");
RTABMAP_PARAM(OdomORBSLAM, ThDepth, double, 40.0, "Close/Far threshold. Baseline times."); RTABMAP_PARAM(OdomORBSLAM, ThDepth, double, 40.0, "Close/Far threshold. Baseline times.");
corelib/src/Camera.cpp
+12 -3
View File
@@ -39,7 +39,8 @@ namespace rtabmap
Camera::Camera(float imageRate, const Transform & localTransform) : Camera::Camera(float imageRate, const Transform & localTransform) :
SensorCapture(imageRate, localTransform*CameraModel::opticalRotation()), SensorCapture(imageRate, localTransform*CameraModel::opticalRotation()),
imuFilter_(0), imuFilter_(0),
publishInterIMU_(false) publishInterIMU_(false),
imuBaseFrameConversion_(false)
{} {}
Camera::~Camera() Camera::~Camera()
@@ -52,15 +53,23 @@ bool Camera::initFromFile(const std::string & calibrationPath)
return init(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()); 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; publishInterIMU_ = enabled;
delete imuFilter_; delete imuFilter_;
imuFilter_ = filter; 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_) if(imuFilter_)
{ {
imuFilter_->update( imuFilter_->update(
corelib/src/Parameters.cpp
+2 -2
View File
@@ -1119,8 +1119,8 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
ignore = true; ignore = true;
} }
#endif #endif
#ifndef RTABMAP_ORBSLAM2 #ifndef RTABMAP_ORB_SLAM
if(group.compare("OdomORBSLAM2") == 0) if(group.compare("OdomORBSLAM") == 0)
{ {
ignore = true; ignore = true;
} }
corelib/src/odometry/OdometryORBSLAM3.cpp
+54 -21
View File
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UDirectory.h" #include "rtabmap/utilite/UDirectory.h"
#include "rtabmap/utilite/UFile.h"
#include <pcl/common/transforms.h> #include <pcl/common/transforms.h>
#include <opencv2/imgproc/types_c.h> #include <opencv2/imgproc/types_c.h>
#include <rtabmap/core/odometry/OdometryORBSLAM3.h> #include <rtabmap/core/odometry/OdometryORBSLAM3.h>
@@ -116,6 +117,13 @@ bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap::
} }
//Load ORB Vocabulary //Load ORB Vocabulary
vocabularyPath = uReplaceChar(vocabularyPath, '~', UDirectory::homeDir()); 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()); UWARN("Loading ORB Vocabulary: \"%s\". This could take a while...", vocabularyPath.c_str());
// Create configuration file // Create configuration file
@@ -240,7 +248,7 @@ bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap::
//# IMU Parameters TODO: hard-coded, not used //# IMU Parameters TODO: hard-coded, not used
//#-------------------------------------------------------------------------------------------- //#--------------------------------------------------------------------------------------------
// Transformation from camera 0 to body-frame (imu) // 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 << "IMU.T_b_c1: !!opencv-matrix" << std::endl;
ofs << " rows: 4" << std::endl; ofs << " rows: 4" << std::endl;
ofs << " cols: 4" << std::endl; ofs << " cols: 4" << std::endl;
@@ -340,14 +348,16 @@ bool OdometryORBSLAM3::init(const rtabmap::CameraModel & model1, const rtabmap::
ofs.close(); 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( orbslam_ = new ORB_SLAM3::System(
vocabularyPath, vocabularyPath,
configPath, configPath,
stereo && withIMU?ORB_SLAM3::System::IMU_STEREO: sensor,
stereo?ORB_SLAM3::System::STEREO:
withIMU?ORB_SLAM3::System::IMU_RGBD:
ORB_SLAM3::System::RGBD,
false); false);
UINFO("Initializing ORB_SLAM3 system with sensor %d... done!", (int)sensor);
return true; return true;
#else #else
UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach."); 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()) if(lastImuStamp_ == 0.0 || lastImuStamp_ < data.stamp())
{ {
UDEBUG("Adding IMU %f", data.stamp());
orbslamImus_.push_back(ORB_SLAM3::IMU::Point( orbslamImus_.push_back(ORB_SLAM3::IMU::Point(
data.imu().linearAcceleration().val[0], data.imu().linearAcceleration().val[0],
data.imu().linearAcceleration().val[1], data.imu().linearAcceleration().val[1],
@@ -432,6 +443,7 @@ Transform OdometryORBSLAM3::computeTransform(
if(lastImageStamp_ == 0.0) if(lastImageStamp_ == 0.0)
{ {
lastImageStamp_ = data.stamp(); lastImageStamp_ = data.stamp();
UDEBUG("Waiting for another image to initialize...");
return t; return t;
} }
@@ -457,6 +469,7 @@ Transform OdometryORBSLAM3::computeTransform(
rightMono = cv::Mat(); rightMono = cv::Mat();
cv::cvtColor(data.imageRaw(), rightMono, CV_BGR2GRAY); cv::cvtColor(data.imageRaw(), rightMono, CV_BGR2GRAY);
} }
UDEBUG("Adding Stereo Frame %f", data.stamp());
Tcw = orbslam_->TrackStereo(leftMono, rightMono, data.stamp(), orbslamImus_); Tcw = orbslam_->TrackStereo(leftMono, rightMono, data.stamp(), orbslamImus_);
orbslamImus_.clear(); orbslamImus_.clear();
} }
@@ -472,15 +485,22 @@ Transform OdometryORBSLAM3::computeTransform(
{ {
depth = util2d::cvtDepthToFloat(data.depthRaw()); depth = util2d::cvtDepthToFloat(data.depthRaw());
} }
UDEBUG("Adding RGBD Frame %f", data.stamp());
Tcw = orbslam_->TrackRGBD(data.imageRaw(), depth, data.stamp(), orbslamImus_); Tcw = orbslam_->TrackRGBD(data.imageRaw(), depth, data.stamp(), orbslamImus_);
orbslamImus_.clear(); orbslamImus_.clear();
} }
Transform previousPoseInv = previousPose_.inverse(); Transform previousPoseInv = previousPose_.inverse();
std::vector<ORB_SLAM3::MapPoint*> mapPoints = orbslam_->GetTrackedMapPoints(); std::vector<ORB_SLAM3::MapPoint*> trackedMapPoints = orbslam_->GetTrackedMapPoints();
if(orbslam_->isLost() || mapPoints.empty()) if(orbslam_->isLost() || trackedMapPoints.empty())
{ {
covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0f; 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 else
{ {
@@ -490,14 +510,16 @@ Transform OdometryORBSLAM3::computeTransform(
if(!p.isNull()) if(!p.isNull())
{ {
if(!localTransform.isNull()) if(!imuLocalTransform_.isNull())
{ {
if(originLocalTransform_.isNull()) // 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();
originLocalTransform_ = localTransform; }
} else
// transform in base frame {
p = originLocalTransform_ * p.inverse() * localTransform.inverse(); 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; t = previousPoseInv*p;
} }
@@ -534,12 +556,14 @@ Transform OdometryORBSLAM3::computeTransform(
} }
} }
size_t mapPointsSize = 0;
if(info) if(info)
{ {
info->lost = t.isNull(); info->lost = t.isNull();
info->type = (int)kTypeORBSLAM; info->type = (int)kTypeORBSLAM;
info->reg.covariance = covariance; info->reg.covariance = covariance;
info->localMapSize = mapPoints.size(); std::vector<ORB_SLAM3::MapPoint*> mapPoints = orbslam_->GetAllMapPoints();
info->localMapSize = mapPointsSize = mapPoints.size();
info->localKeyFrames = 0; info->localKeyFrames = 0;
if(this->isInfoDataFilled()) if(this->isInfoDataFilled())
@@ -549,20 +573,20 @@ Transform OdometryORBSLAM3::computeTransform(
info->reg.inliersIDs.resize(kpts.size()); info->reg.inliersIDs.resize(kpts.size());
int oi = 0; int oi = 0;
UASSERT(mapPoints.size() == kpts.size()); UASSERT(trackedMapPoints.size() == kpts.size());
for (unsigned int i = 0; i < kpts.size(); ++i) for (unsigned int i = 0; i < kpts.size(); ++i)
{ {
int wordId; int wordId;
if(mapPoints[i] != 0) if(trackedMapPoints[i] != 0)
{ {
wordId = mapPoints[i]->mnId; wordId = trackedMapPoints[i]->mnId;
} }
else else
{ {
wordId = -(i+1); wordId = -(i+1);
} }
info->words.insert(std::make_pair(wordId, kpts[i])); info->words.insert(std::make_pair(wordId, kpts[i]));
if(mapPoints[i] != 0) if(trackedMapPoints[i] != 0)
{ {
info->reg.matchesIDs[oi] = wordId; info->reg.matchesIDs[oi] = wordId;
info->reg.inliersIDs[oi] = wordId; info->reg.inliersIDs[oi] = wordId;
@@ -574,7 +598,15 @@ Transform OdometryORBSLAM3::computeTransform(
info->reg.inliers = oi; info->reg.inliers = oi;
info->reg.matches = 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) for (unsigned int i = 0; i < mapPoints.size(); ++i)
{ {
if(mapPoints[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 #else
UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach."); UERROR("RTAB-Map is not built with ORB_SLAM support! Select another visual odometry approach.");
guilib/src/PreferencesDialog.cpp
+8 -4
View File
@@ -6761,7 +6761,8 @@ Camera * PreferencesDialog::createCamera(
camera->setInterIMUPublishing( camera->setInterIMUPublishing(
_ui->checkbox_publishInterIMU->isChecked(), _ui->checkbox_publishInterIMU->isChecked(),
_ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0? _ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0?
IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0); IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0,
getIMUFilteringBaseFrameConversion());
} }
else if (driver == kSrcRealSense) else if (driver == kSrcRealSense)
{ {
@@ -6803,7 +6804,8 @@ Camera * PreferencesDialog::createCamera(
camera->setInterIMUPublishing( camera->setInterIMUPublishing(
_ui->checkbox_publishInterIMU->isChecked(), _ui->checkbox_publishInterIMU->isChecked(),
_ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0? _ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0?
IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0); IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0,
getIMUFilteringBaseFrameConversion());
if(driver == kSrcStereoRealSense2) if(driver == kSrcStereoRealSense2)
{ {
((CameraRealSense2*)camera)->setImagesRectified((_ui->checkBox_stereo_rectify->isEnabled() && _ui->checkBox_stereo_rectify->isChecked()) && !useRawImages); ((CameraRealSense2*)camera)->setImagesRectified((_ui->checkBox_stereo_rectify->isEnabled() && _ui->checkBox_stereo_rectify->isChecked()) && !useRawImages);
@@ -7021,7 +7023,8 @@ Camera * PreferencesDialog::createCamera(
camera->setInterIMUPublishing( camera->setInterIMUPublishing(
_ui->checkbox_publishInterIMU->isChecked(), _ui->checkbox_publishInterIMU->isChecked(),
_ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0? _ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0?
IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0); IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0,
getIMUFilteringBaseFrameConversion());
((CameraStereoZed*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked()); ((CameraStereoZed*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked());
} }
else if (driver == kSrcStereoZedOC) else if (driver == kSrcStereoZedOC)
@@ -7070,7 +7073,8 @@ Camera * PreferencesDialog::createCamera(
camera->setInterIMUPublishing( camera->setInterIMUPublishing(
_ui->checkbox_publishInterIMU->isChecked(), _ui->checkbox_publishInterIMU->isChecked(),
_ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0? _ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0?
IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0); IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0,
getIMUFilteringBaseFrameConversion());
} }
else if(driver == kSrcUsbDevice) else if(driver == kSrcUsbDevice)
{ {
guilib/src/ui/preferencesDialog.ui
+35 -16
View File
@@ -6,7 +6,7 @@
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>1009</width> <width>980</width>
<height>925</height> <height>925</height>
</rect> </rect>
</property> </property>
@@ -63,9 +63,9 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>-949</y> <y>-1237</y>
<width>713</width> <width>684</width>
<height>4922</height> <height>5110</height>
</rect> </rect>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_16"> <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>12</number> <number>19</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0"> <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -8308,7 +8308,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item row="1" column="1"> <item row="1" column="1">
<widget class="QLabel" name="label_563"> <widget class="QLabel" name="label_563">
<property name="text"> <property name="text">
<string>Publish inter IMU messages from the camera. IMU received between images will be published as separate topic. Orientation overridden (if any) if IMU filtering is used.</string> <string>Publish inter IMU messages from the camera. IMU received between images will be published as separate topic. Orientation overridden (if any) if IMU filtering is used. </string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>
@@ -16349,7 +16349,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item> <item>
<widget class="QStackedWidget" name="stackedWidget_odometryType"> <widget class="QStackedWidget" name="stackedWidget_odometryType">
<property name="currentIndex"> <property name="currentIndex">
<number>0</number> <number>5</number>
</property> </property>
<widget class="QWidget" name="page_52"> <widget class="QWidget" name="page_52">
<layout class="QVBoxLayout" name="verticalLayout_77"> <layout class="QVBoxLayout" name="verticalLayout_77">
@@ -18580,23 +18580,39 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item> <item>
<widget class="QGroupBox" name="groupBox_OdomORBSLAMInertial"> <widget class="QGroupBox" name="groupBox_OdomORBSLAMInertial">
<property name="title"> <property name="title">
<string>Enable IMU. Only supported with ORB_SLAM3.</string> <string>Enable IMU.</string>
</property> </property>
<property name="checkable"> <property name="checkable">
<bool>true</bool> <bool>true</bool>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_169"> <layout class="QVBoxLayout" name="verticalLayout_169">
<item>
<widget class="QLabel" name="label_621">
<property name="text">
<string>Only supported with ORB_SLAM3. Inter IMU publishing should be enabled (see Source panel under IMU Filtering section).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item> <item>
<layout class="QGridLayout" name="gridLayout_129" columnstretch="0,1"> <layout class="QGridLayout" name="gridLayout_129" columnstretch="0,1">
<item row="0" column="0"> <item row="0" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_ORBSLAMGyroNoise"> <widget class="QDoubleSpinBox" name="doubleSpinBox_ORBSLAMGyroNoise">
<property name="decimals"> <property name="decimals">
<number>8</number> <number>6</number>
</property> </property>
<property name="minimum"> <property name="minimum">
<double>0.010000000000000</double> <double>0.000001000000000</double>
</property> </property>
<property name="maximum"> <property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double> <double>0.010000000000000</double>
</property> </property>
</widget> </widget>
@@ -18643,12 +18659,15 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="1" column="0"> <item row="1" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_ORBSLAMAccNoise"> <widget class="QDoubleSpinBox" name="doubleSpinBox_ORBSLAMAccNoise">
<property name="decimals"> <property name="decimals">
<number>8</number> <number>6</number>
</property> </property>
<property name="minimum"> <property name="minimum">
<double>0.100000000000000</double> <double>0.000001000000000</double>
</property> </property>
<property name="maximum"> <property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double> <double>0.100000000000000</double>
</property> </property>
</widget> </widget>
@@ -18682,10 +18701,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="2" column="0"> <item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_ORBSLAMGyroWalk"> <widget class="QDoubleSpinBox" name="doubleSpinBox_ORBSLAMGyroWalk">
<property name="decimals"> <property name="decimals">
<number>8</number> <number>6</number>
</property> </property>
<property name="minimum"> <property name="minimum">
<double>0.000000000000000</double> <double>0.000001000000000</double>
</property> </property>
<property name="maximum"> <property name="maximum">
<double>1.000000000000000</double> <double>1.000000000000000</double>
@@ -18701,10 +18720,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="3" column="0"> <item row="3" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_ORBSLAMAccWalk"> <widget class="QDoubleSpinBox" name="doubleSpinBox_ORBSLAMAccWalk">
<property name="decimals"> <property name="decimals">
<number>8</number> <number>6</number>
</property> </property>
<property name="minimum"> <property name="minimum">
<double>0.000000000000000</double> <double>0.000001000000000</double>
</property> </property>
<property name="maximum"> <property name="maximum">
<double>1.000000000000000</double> <double>1.000000000000000</double>