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

Added ORB_SLAM3 support. IMU Filter: added base frame conversion option. (#698)

Browse Source 
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
2021-03-09 16:00:41 -05:00
committed by GitHub
parent 736c8aceae
commit db43479e44
26 changed files with 637 additions and 315 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/CameraThread.h
View File

@@ -69,7 +69,7 @@ public:
void setDistortionModel(const std::string & path);
void enableBilateralFiltering(float sigmaS, float sigmaR);
void disableBilateralFiltering() {_bilateralFiltering = false;}
void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap());
void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
void disableIMUFiltering();
RTABMAP_DEPRECATED(void setScanParameters(
@@ -125,6 +125,7 @@ private:
float _bilateralSigmaS;
float _bilateralSigmaR;
IMUFilter * _imuFilter;
bool _imuBaseFrameConversion;
};
} // namespace rtabmap

6
corelib/include/rtabmap/core/IMU.h
View File

@@ -9,7 +9,8 @@
#define IMU_H_
#include <opencv2/core/core.hpp>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UEvent.h>
#include <rtabmap/core/Transform.h>
namespace rtabmap {
@@ -60,6 +61,9 @@ public:
const Transform & localTransform() const {return localTransform_;}
// apply local transform rotation to data, and set Identity rotation for local transform
void convertToBaseFrame();
bool empty() const
{
return localTransform_.isNull();

2
corelib/include/rtabmap/core/Odometry.h
View File

@@ -49,7 +49,7 @@ public:
kTypeFovis = 2,
kTypeViso2 = 3,
kTypeDVO = 4,
kTypeORBSLAM2 = 5,
kTypeORBSLAM = 5,
kTypeOkvis = 6,
kTypeLOAM = 7,
kTypeMSCKF = 8,

20
corelib/include/rtabmap/core/Parameters.h
View File

@@ -418,7 +418,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Optimizer, GravitySigma, float, 0.0, uFormat("Gravity sigma value (>=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with g2o and GTSAM optimization strategies (see %s).", kOptimizerStrategy().c_str()));
#endif
#ifdef RTABMAP_ORB_SLAM2
#ifdef RTABMAP_ORB_SLAM
RTABMAP_PARAM(g2o, Solver, int, 3, "0=csparse 1=pcg 2=cholmod 3=Eigen");
#else
RTABMAP_PARAM(g2o, Solver, int, 0, "0=csparse 1=pcg 2=cholmod 3=Eigen");
@@ -460,7 +460,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(OdomF2M, ScanSubtractAngle, float, 45, uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
RTABMAP_PARAM(OdomF2M, ScanRange, float, 0, "[Geometry] Distance Range used to filter points of local map (when > 0). 0 means local map is updated using time and not range.");
RTABMAP_PARAM(OdomF2M, ValidDepthRatio, float, 0.75, "If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map.");
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 1, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
#else
RTABMAP_PARAM(OdomF2M, BundleAdjustment, int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
@@ -521,12 +521,12 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(OdomViso2, BucketHeight, double, 50, "Height of bucket.");
// Odometry ORB_SLAM2
RTABMAP_PARAM_STR(OdomORBSLAM2, VocPath, "", "Path to ORB vocabulary (*.txt).");
RTABMAP_PARAM(OdomORBSLAM2, Bf, double, 0.076, "Fake IR projector baseline (m) used only when stereo is not used.");
RTABMAP_PARAM(OdomORBSLAM2, ThDepth, double, 40.0, "Close/Far threshold. Baseline times.");
RTABMAP_PARAM(OdomORBSLAM2, Fps, float, 0.0, "Camera FPS.");
RTABMAP_PARAM(OdomORBSLAM2, MaxFeatures, int, 1000, "Maximum ORB features extracted per frame.");
RTABMAP_PARAM(OdomORBSLAM2, MapSize, int, 3000, "Maximum size of the feature map (0 means infinite).");
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, ThDepth, double, 40.0, "Close/Far threshold. Baseline times.");
RTABMAP_PARAM(OdomORBSLAM, Fps, float, 0.0, "Camera FPS.");
RTABMAP_PARAM(OdomORBSLAM, MaxFeatures, int, 1000, "Maximum ORB features extracted per frame.");
RTABMAP_PARAM(OdomORBSLAM, MapSize, int, 3000, "Maximum size of the feature map (0 means infinite).");
// Odometry OKVIS
RTABMAP_PARAM_STR(OdomOKVIS, ConfigPath, "", "Path of OKVIS config file.");
@@ -582,7 +582,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, RefineIterations, int, 5, uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, PnPReprojError, float, 2, uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, PnPFlags, int, 0, uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str()));
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
RTABMAP_PARAM(Vis, PnPRefineIterations, int, 0, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
#else
RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
@@ -619,7 +619,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, CorFlowIterations, int, 30, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
RTABMAP_PARAM(Vis, CorFlowMaxLevel, int, 3, uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
RTABMAP_PARAM(Vis, BundleAdjustment, int, 1, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
#else
RTABMAP_PARAM(Vis, BundleAdjustment, int, 0, "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");

27
corelib/include/rtabmap/core/odometry/OdometryORBSLAM2.h → corelib/include/rtabmap/core/odometry/OdometryORBSLAM.h
View File

@@ -25,41 +25,48 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef ODOMETRYORBSLAM2_H_
#define ODOMETRYORBSLAM2_H_
#ifndef ODOMETRYORBSLAM_H_
#define ODOMETRYORBSLAM_H_
#include <rtabmap/core/Odometry.h>
#if RTABMAP_ORB_SLAM == 3
namespace ORB_SLAM3 {
#else
namespace ORB_SLAM2 {
#endif
class System;
}
class ORBSLAM2System;
class ORBSLAMSystem;
namespace rtabmap {
class RTABMAP_EXP OdometryORBSLAM2 : public Odometry
class RTABMAP_EXP OdometryORBSLAM : public Odometry
{
public:
OdometryORBSLAM2(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
virtual ~OdometryORBSLAM2();
OdometryORBSLAM(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
virtual ~OdometryORBSLAM();
virtual void reset(const Transform & initialPose = Transform::getIdentity());
virtual Odometry::Type getType() {return Odometry::kTypeORBSLAM2;}
virtual Odometry::Type getType() {return Odometry::kTypeORBSLAM;}
virtual bool canProcessAsyncIMU() const;
private:
virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
private:
#ifdef RTABMAP_ORB_SLAM2
ORBSLAM2System * orbslam2_;
#ifdef RTABMAP_ORB_SLAM
ORBSLAMSystem * orbslam_;
bool firstFrame_;
Transform originLocalTransform_;
Transform previousPose_;
bool useIMU_;
Transform imuLocalTransform_;
#endif
};
}
#endif /* ODOMETRYORBSLAM2_H_ */
#endif /* ODOMETRYORBSLAM_H_ */