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

Added imu to odom bundle adjustment. Added IMUFilter classes. Changed Aruco parameter prefix to Marker. Zed: publishing IMU data.

Browse Source
This commit is contained in:
matlabbe
2019-05-07 18:57:53 -04:00
parent 4675240d6e
commit e6f471d88e
32 changed files with 1733 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -19,7 +19,7 @@ class IMU
{
public:
IMU() {}
IMU(const cv::Vec4d & orientation,
IMU(const cv::Vec4d & orientation, // qx qy qz qw
const cv::Mat & orientationCovariance,
const cv::Vec3d & angularVelocity,
const cv::Mat & angularVelocityCovariance,
@@ -48,6 +48,7 @@ public:
{
}
// qx qy qz qw
const cv::Vec4d & orientation() const {return orientation_;}
const cv::Mat & orientationCovariance() const {return orientationCovariance_;} // 3x3 double Row major about x, y, z axes, empty if orientation is not set

79
corelib/include/rtabmap/core/IMUFilter.h Normal file
View File

@@ -0,0 +1,79 @@
/*
Copyright (c) 2010-2019, 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.
*/
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_
#include <rtabmap/core/Parameters.h>
#include <Eigen/Geometry>
namespace rtabmap {
class IMUFilter
{
public:
enum Type {
kMadgwick=0,
kComplementaryFilter=1};
public:
static IMUFilter * create(const ParametersMap & parameters = ParametersMap());
static IMUFilter * create(IMUFilter::Type type, const ParametersMap & parameters = ParametersMap());
public:
virtual void parseParameters(const ParametersMap & parameters) {}
virtual ~IMUFilter(){}
void update(
double gx, double gy, double gz,
double ax, double ay, double az,
double stamp);
virtual IMUFilter::Type type() const = 0;
virtual void getOrientation(double & qx, double & qy, double & qz, double & qw) const = 0;
virtual void reset(double qx = 0.0, double qy = 0.0, double qz = 0.0, double qw = 1.0) = 0;
protected:
IMUFilter(const ParametersMap & parameters = ParametersMap()) : previousStamp_(0) {}
private:
// Update from accelerometer and gyroscope data.
// [gx, gy, gz]: Angular veloctiy, in rad / s.
// [ax, ay, az]: Normalized gravity vector.
// dt: time delta, in seconds.
virtual void updateImpl(
double gx, double gy, double gz,
double ax, double ay, double az,
double dt) = 0;
private:
double previousStamp_;
};
}
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_ */

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

@@ -38,6 +38,7 @@ namespace rtabmap {
class OdometryInfo;
class ParticleFilter;
class IMUFilter;
class RTABMAP_EXP Odometry
{
@@ -67,6 +68,7 @@ public:
virtual void reset(const Transform & initialPose = Transform::getIdentity());
virtual Odometry::Type getType() = 0;
virtual bool canProcessRawImages() const {return false;}
virtual bool canProcessIMU() const {return false;}
//getters
const Transform & getPose() const {return _pose;}
@@ -90,6 +92,7 @@ private:
bool _holonomic;
bool guessFromMotion_;
bool guessSmoothingDelay_;
int _imuFilteringStrategy;
int _filteringStrategy;
int _particleSize;
float _particleNoiseT;
@@ -114,6 +117,7 @@ private:
std::vector<ParticleFilter *> particleFilters_;
cv::KalmanFilter kalmanFilter_;
IMUFilter * imuFilter_;
protected:
Odometry(const rtabmap::ParametersMap & parameters);

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

@@ -355,7 +355,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, false, "Extract features even if there are some already in the nodes.");
RTABMAP_PARAM(RGBD, LocalBundleOnLoopClosure, bool, false, "Do local bundle adjustment with neighborhood of the loop closure.");
RTABMAP_PARAM(RGBD, CreateOccupancyGrid, bool, false, "Create local occupancy grid maps. See \"Grid\" group for parameters.");
RTABMAP_PARAM(RGBD, MarkerDetection, bool, false, "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Aruco\" group for parameters.");
RTABMAP_PARAM(RGBD, MarkerDetection, bool, false, "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Marker\" group for parameters.");
RTABMAP_PARAM(RGBD, LoopCovLimited, bool, false, "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
RTABMAP_PARAM(RGBD, MaxOdomCacheSize, int, 0, uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false) and when %s!=0. This is used to verify localization transforms to make sure we don't teleport to a location very similar to one we previously localized on. When the cache is full, the whole cache is cleared and the next localization is automatically accepted without verification. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str()));
@@ -409,7 +409,8 @@ class RTABMAP_EXP Parameters
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, ImageBufferSize, unsigned int, 1, "Data buffer size (0 min inf).");
RTABMAP_PARAM(Odom, FilteringStrategy, int, 0, "0=No filtering 1=Kalman filtering 2=Particle filtering");
RTABMAP_PARAM(Odom, ImuFilteringStrategy, int, 0, "0=No filtering 1=Madgwick Filter 2=Complementary Filter. This is used to estimate the quaternion from acceleration and angular velocities of IMU before doing odometry updates. IMU data should be in ENU coordinates.");
RTABMAP_PARAM(Odom, FilteringStrategy, int, 0, "0=No filtering 1=Kalman filtering 2=Particle filtering. This filter is used to smooth the odometry output.");
RTABMAP_PARAM(Odom, ParticleSize, unsigned int, 400, "Number of particles of the filter.");
RTABMAP_PARAM(Odom, ParticleNoiseT, float, 0.002, "Noise (m) of translation components (x,y,z).");
RTABMAP_PARAM(Odom, ParticleLambdaT, float, 100, "Lambda of translation components (x,y,z).");
@@ -716,12 +717,32 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(GridGlobal, ProbClampingMin, float, 0.1192, "Probability clamping minimum (value between 0 and 1).");
RTABMAP_PARAM(GridGlobal, ProbClampingMax, float, 0.971, "Probability clamping maximum (value between 0 and 1).");
RTABMAP_PARAM(Aruco, Dictionary, int, 0, "Dictionary to use: DICT_4X4_50=0, DICT_4X4_100=1, DICT_4X4_250=2, DICT_4X4_1000=3, DICT_5X5_50=4, DICT_5X5_100=5, DICT_5X5_250=6, DICT_5X5_1000=7, DICT_6X6_50=8, DICT_6X6_100=9, DICT_6X6_250=10, DICT_6X6_1000=11, DICT_7X7_50=12, DICT_7X7_100=13, DICT_7X7_250=14, DICT_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20");
RTABMAP_PARAM(Aruco, MarkerLength, float, 0, "The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).");
RTABMAP_PARAM(Aruco, MaxDepthError, float, 0.01, uFormat("Maximum depth error between all corners of a marker when estimating the marker length (when %s is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.", kArucoMarkerLength().c_str()));
RTABMAP_PARAM(Aruco, VarianceLinear, float, 0.001, "Linear variance to set on marker detections.");
RTABMAP_PARAM(Aruco, VarianceAngular, float, 0.01, "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization.");
RTABMAP_PARAM(Aruco, CornerRefinementMethod, int, 0, "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag 2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
RTABMAP_PARAM(Marker, Dictionary, int, 0, "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20");
RTABMAP_PARAM(Marker, Length, float, 0, "The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).");
RTABMAP_PARAM(Marker, MaxDepthError, float, 0.01, uFormat("Maximum depth error between all corners of a marker when estimating the marker length (when %s is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.", kMarkerLength().c_str()));
RTABMAP_PARAM(Marker, VarianceLinear, float, 0.001, "Linear variance to set on marker detections.");
RTABMAP_PARAM(Marker, VarianceAngular, float, 0.01, "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization.");
RTABMAP_PARAM(Marker, CornerRefinementMethod, int, 0, "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
RTABMAP_PARAM(ImuFilter, MadgwickGain, double, 0.1, "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal, belongs in [0, 1].");
RTABMAP_PARAM(ImuFilter, MadgwickZeta, double, 0.0, "Gyro drift gain (approx. rad/s), belongs in [-1, 1].");
RTABMAP_PARAM(ImuFilter, ComplementaryGainAcc, double, 0.01, "Gain parameter for the complementary filter, belongs in [0, 1].");
RTABMAP_PARAM(ImuFilter, ComplementaryBiasAlpha, double, 0.01, "Bias estimation gain parameter, belongs in [0, 1].");
RTABMAP_PARAM(ImuFilter, ComplementaryDoBiasEstimation, bool, true, "Parameter whether to do bias estimation or not.");
RTABMAP_PARAM(ImuFilter, ComplementaryDoAdpativeGain, bool, true, "Parameter whether to do adaptive gain or not.");
//
double gain_acc_;
//
double bias_alpha_;
//
bool do_bias_estimation_;
//
bool do_adaptive_gain_;
public:
virtual ~Parameters();

1
corelib/include/rtabmap/core/camera/CameraStereoZed.h
View File

@@ -83,6 +83,7 @@ private:
#ifdef RTABMAP_ZED
sl::Camera * zed_;
StereoCameraModel stereoModel_;
Transform imuLocalTransform_;
CameraVideo::Source src_;
int usbDevice_;
std::string svoFilePath_;

5
corelib/include/rtabmap/core/odometry/OdometryF2M.h
View File

@@ -50,11 +50,13 @@ public:
virtual void reset(const Transform & initialPose = Transform::getIdentity());
const Signature & getMap() const {return *map_;}
const Signature & getLastFrame() const {return *lastFrame_;}
virtual bool canProcessIMU() const {return true;}
virtual Odometry::Type getType() {return Odometry::kTypeF2M;}
private:
virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0);
Transform getClosestIMU(const double & stamp, double & stampDiff) const;
private:
//Parameters
@@ -75,10 +77,13 @@ private:
Signature * lastFrame_;
int lastFrameOldestNewId_;
std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
std::map<double, Transform> imus_;
bool initGravity_;
std::map<int, std::map<int, FeatureBA> > bundleWordReferences_; //<WordId, <FrameId, pt2D+depth>>
std::map<int, Transform> bundlePoses_;
std::multimap<int, Link> bundleLinks_;
std::multimap<int, Link> bundleIMUOrientations_;
std::map<int, CameraModel> bundleModels_;
std::map<int, int> bundlePoseReferences_;
int bundleSeq_;

1
corelib/include/rtabmap/core/odometry/OdometryMSCKF.h
View File

@@ -44,6 +44,7 @@ public:
virtual void reset(const Transform & initialPose = Transform::getIdentity());
virtual Odometry::Type getType() {return Odometry::kTypeMSCKF;}
virtual bool canProcessRawImages() const {return true;}
virtual bool canProcessIMU() const {return true;}
private:
virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);

1
corelib/include/rtabmap/core/odometry/OdometryOkvis.h
View File

@@ -46,6 +46,7 @@ public:
virtual void reset(const Transform & initialPose = Transform::getIdentity());
virtual Odometry::Type getType() {return Odometry::kTypeOkvis;}
virtual bool canProcessRawImages() const {return true;}
virtual bool canProcessIMU() const {return true;}
private:
virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);

1
corelib/include/rtabmap/core/odometry/OdometryVINS.h
View File

@@ -43,6 +43,7 @@ public:
virtual void reset(const Transform & initialPose = Transform::getIdentity());
virtual Odometry::Type getType() {return Odometry::kTypeVINS;}
virtual bool canProcessRawImages() const {return true;}
virtual bool canProcessIMU() const {return true;}
private:
virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);