diff --git a/CMakeLists.txt b/CMakeLists.txt index a097398d..3a948c9e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 19) -SET(RTABMAP_PATCH_VERSION 2) +SET(RTABMAP_PATCH_VERSION 3) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) @@ -190,6 +190,7 @@ option(WITH_ORB_SLAM2 "Include ORB_SLAM2 support" ON) option(WITH_OKVIS "Include OKVIS support" ON) option(WITH_MSCKF_VIO "Include MSCKF_VIO support" OFF) option(WITH_VINS "Include VINS-Fusion support" ON) +option(WITH_MADGWICK "Include Madgwick IMU filtering support" ON) option(PCL_OMP "With PCL OMP implementations" ON) FIND_PACKAGE(OpenCV REQUIRED QUIET) @@ -734,6 +735,9 @@ ENDIF() IF(VTK_USE_QVTK) SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${QVTK_LIBRARY}) ENDIF(VTK_USE_QVTK) +IF(NOT WITH_MADGWICK) + SET(MADGWICK "//") +ENDIF() IF(NOT (OpenCV_FOUND AND NOT (OpenCV_VERSION_MAJOR LESS 3))) SET(OPENCV3 "//") @@ -957,6 +961,12 @@ ELSE() MESSAGE(STATUS " With ORB OcTree = NO (WITH_ORB_OCTREE=OFF)") ENDIF() +IF(WITH_MADGWICK) +MESSAGE(STATUS " With Madgwick = YES (License: GPL)") +ELSE() +MESSAGE(STATUS " With Madgwick = NO (WITH_MADGWICK=OFF)") +ENDIF() + IF(Freenect_FOUND) MESSAGE(STATUS " With Freenect = YES (License: Apache v2 and/or GPLv2)") ELSEIF(NOT WITH_FREENECT) diff --git a/Version.h.in b/Version.h.in index 66cfd537..1103e2aa 100644 --- a/Version.h.in +++ b/Version.h.in @@ -68,6 +68,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. @VINS@#define RTABMAP_VINS @ORB_SLAM2@#define RTABMAP_ORB_SLAM2 @ORB_OCTREE@#define RTABMAP_ORB_OCTREE +@MADGWICK@#define RTABMAP_MADGWICK + #endif /* VERSION_H_ */ diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 58c29bd6..1123e70c 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -100,7 +100,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters() parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), std::string("false"))); // just keep loop closure detection parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLinearUpdate(), std::string("0.05"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAngularUpdate(), std::string("0.05"))); - parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kArucoMarkerLength(), std::string("0.0"))); + parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMarkerLength(), std::string("0.0"))); if(parameters.find(rtabmap::Parameters::kOptimizerStrategy()) != parameters.end()) { diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java index 6ca9ab6b..1fff771b 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java @@ -840,10 +840,10 @@ public class RTABMapActivity extends Activity implements OnClickListener, OnItem else { RTABMapLib.setMappingParameter("RGBD/MarkerDetection", "true"); - RTABMapLib.setMappingParameter("Aruco/Dictionary", markerDetection); - RTABMapLib.setMappingParameter("Aruco/CornerRefinementMethod", Integer.parseInt(markerDetection) > 16?"3":"0"); + RTABMapLib.setMappingParameter("Marker/Dictionary", markerDetection); + RTABMapLib.setMappingParameter("Marker/CornerRefinementMethod", Integer.parseInt(markerDetection) > 16?"3":"0"); } - RTABMapLib.setMappingParameter("Aruco/MaxDepthError", markerDetectionDepthError); + RTABMapLib.setMappingParameter("Marker/MaxDepthError", markerDetectionDepthError); if(!DISABLE_LOG) Log.d(TAG, "set exporting parameters..."); RTABMapLib.setCloudDensityLevel(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_density), getString(R.string.pref_default_density)))); diff --git a/corelib/include/rtabmap/core/IMU.h b/corelib/include/rtabmap/core/IMU.h index 46340005..fa961d63 100644 --- a/corelib/include/rtabmap/core/IMU.h +++ b/corelib/include/rtabmap/core/IMU.h @@ -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 diff --git a/corelib/include/rtabmap/core/IMUFilter.h b/corelib/include/rtabmap/core/IMUFilter.h new file mode 100644 index 00000000..f93d3f91 --- /dev/null +++ b/corelib/include/rtabmap/core/IMUFilter.h @@ -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 +#include + +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_ */ diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h index d45c859e..a845d80e 100644 --- a/corelib/include/rtabmap/core/Odometry.h +++ b/corelib/include/rtabmap/core/Odometry.h @@ -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 particleFilters_; cv::KalmanFilter kalmanFilter_; + IMUFilter * imuFilter_; protected: Odometry(const rtabmap::ParametersMap & parameters); diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 4e06412d..0d8786bf 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -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(); diff --git a/corelib/include/rtabmap/core/camera/CameraStereoZed.h b/corelib/include/rtabmap/core/camera/CameraStereoZed.h index c5b007b0..9309c60e 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoZed.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoZed.h @@ -83,6 +83,7 @@ private: #ifdef RTABMAP_ZED sl::Camera * zed_; StereoCameraModel stereoModel_; + Transform imuLocalTransform_; CameraVideo::Source src_; int usbDevice_; std::string svoFilePath_; diff --git a/corelib/include/rtabmap/core/odometry/OdometryF2M.h b/corelib/include/rtabmap/core/odometry/OdometryF2M.h index 4837264d..c17fe6bd 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryF2M.h +++ b/corelib/include/rtabmap/core/odometry/OdometryF2M.h @@ -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::Ptr, pcl::IndicesPtr> > scansBuffer_; + std::map imus_; + bool initGravity_; std::map > bundleWordReferences_; //> std::map bundlePoses_; std::multimap bundleLinks_; + std::multimap bundleIMUOrientations_; std::map bundleModels_; std::map bundlePoseReferences_; int bundleSeq_; diff --git a/corelib/include/rtabmap/core/odometry/OdometryMSCKF.h b/corelib/include/rtabmap/core/odometry/OdometryMSCKF.h index 1689b050..94106cf5 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryMSCKF.h +++ b/corelib/include/rtabmap/core/odometry/OdometryMSCKF.h @@ -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); diff --git a/corelib/include/rtabmap/core/odometry/OdometryOkvis.h b/corelib/include/rtabmap/core/odometry/OdometryOkvis.h index 1e5e444b..522d1cfe 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryOkvis.h +++ b/corelib/include/rtabmap/core/odometry/OdometryOkvis.h @@ -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); diff --git a/corelib/include/rtabmap/core/odometry/OdometryVINS.h b/corelib/include/rtabmap/core/odometry/OdometryVINS.h index 68cbe116..bddd95ce 100644 --- a/corelib/include/rtabmap/core/odometry/OdometryVINS.h +++ b/corelib/include/rtabmap/core/odometry/OdometryVINS.h @@ -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); diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 87773751..28052492 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -88,6 +88,8 @@ SET(SRC_FILES odometry/OdometryVINS.cpp IMUThread.cpp + IMUFilter.cpp + imufilter/ComplementaryFilter.cpp Stereo.cpp StereoDense.cpp @@ -470,7 +472,7 @@ IF(GTSAM_FOUND) ) ENDIF() SET(SRC_FILES - ${SRC_FILES} + ${SRC_FILES} optimizer/gtsam/GravityFactor.cpp ) IF(WIN32) @@ -483,6 +485,13 @@ IF(GTSAM_FOUND) ) ENDIF(GTSAM_FOUND) +IF(WITH_MADGWICK) + SET(SRC_FILES + ${SRC_FILES} + imufilter/MadgwickFilter.cpp + ) +ENDIF(WITH_MADGWICK) + #################################### # Generate resources files #################################### diff --git a/corelib/src/IMUFilter.cpp b/corelib/src/IMUFilter.cpp new file mode 100644 index 00000000..a320d360 --- /dev/null +++ b/corelib/src/IMUFilter.cpp @@ -0,0 +1,88 @@ +/* +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. +*/ + + +#include +#include +#include "imufilter/ComplementaryFilter.h" +#ifdef RTABMAP_MADGWICK +#include "imufilter/MadgwickFilter.h" +#endif + +namespace rtabmap { + +IMUFilter * IMUFilter::create(const ParametersMap & parameters) +{ + int type = Parameters::defaultKpDetectorStrategy(); + Parameters::parse(parameters, Parameters::kKpDetectorStrategy(), type); + return create((IMUFilter::Type)type, parameters); +} + +IMUFilter * IMUFilter::create(IMUFilter::Type type, const ParametersMap & parameters) +{ +#ifndef RTABMAP_MADGWICK + if(type == IMUFilter::kMadgwick) + { + UWARN("Madgwick filter cannot be used as RTAB-Map is not built with the option enabled. Complementary filter is used instead."); + type = IMUFilter::kComplementaryFilter; + } +#endif + + IMUFilter * filter = 0; + switch(type) + { +#ifdef RTABMAP_MADGWICK + case IMUFilter::kMadgwick: + filter = new MadgwickFilter(parameters); + break; +#endif + default: + filter = new ComplementaryFilter(parameters); + type = IMUFilter::kComplementaryFilter; + break; + + } + return filter; +} + +void IMUFilter::update( + double gx, double gy, double gz, + double ax, double ay, double az, + double stamp) + { + if(previousStamp_ == 0.0) + { + previousStamp_ = stamp; + } + double dt = stamp - previousStamp_; + + updateImpl(gx, gy, gz, ax, ay, az, dt); + + previousStamp_ = stamp; + } + +} diff --git a/corelib/src/MarkerDetector.cpp b/corelib/src/MarkerDetector.cpp index 8084b45a..c4551ffa 100644 --- a/corelib/src/MarkerDetector.cpp +++ b/corelib/src/MarkerDetector.cpp @@ -34,18 +34,18 @@ namespace rtabmap { MarkerDetector::MarkerDetector(const ParametersMap & parameters) { #ifdef HAVE_OPENCV_ARUCO - markerLength_ = Parameters::defaultArucoMarkerLength(); - maxDepthError_ = Parameters::defaultArucoMaxDepthError(); - dictionaryId_ = Parameters::defaultArucoDictionary(); + markerLength_ = Parameters::defaultMarkerLength(); + maxDepthError_ = Parameters::defaultMarkerMaxDepthError(); + dictionaryId_ = Parameters::defaultMarkerDictionary(); #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2) detectorParams_ = cv::aruco::DetectorParameters::create(); #else detectorParams_.reset(new cv::aruco::DetectorParameters()); #endif #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3) - detectorParams_->cornerRefinementMethod = Parameters::defaultArucoCornerRefinementMethod(); + detectorParams_->cornerRefinementMethod = Parameters::defaultMarkerCornerRefinementMethod(); #else - detectorParams_->doCornerRefinement = Parameters::defaultArucoCornerRefinementMethod()!=0; + detectorParams_->doCornerRefinement = Parameters::defaultMarkerCornerRefinementMethod()!=0; #endif parseParameters(parameters); #endif @@ -69,10 +69,10 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) detectorParams_->minDistanceToBorder = 3; detectorParams_->minMarkerDistanceRate = 0.05; #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=3) - Parameters::parse(parameters, Parameters::kArucoCornerRefinementMethod(), detectorParams_->cornerRefinementMethod); + Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), detectorParams_->cornerRefinementMethod); #else int doCornerRefinement = detectorParams_->doCornerRefinement?1:0; - Parameters::parse(parameters, Parameters::kArucoCornerRefinementMethod(), doCornerRefinement); + Parameters::parse(parameters, Parameters::kMarkerCornerRefinementMethod(), doCornerRefinement); detectorParams_->doCornerRefinement = doCornerRefinement!=0; #endif detectorParams_->cornerRefinementWinSize = 5; @@ -85,18 +85,18 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters) detectorParams_->minOtsuStdDev = 5.0; detectorParams_->errorCorrectionRate = 0.6; - Parameters::parse(parameters, Parameters::kArucoMarkerLength(), markerLength_); - Parameters::parse(parameters, Parameters::kArucoMaxDepthError(), maxDepthError_); - Parameters::parse(parameters, Parameters::kArucoDictionary(), dictionaryId_); + Parameters::parse(parameters, Parameters::kMarkerLength(), markerLength_); + Parameters::parse(parameters, Parameters::kMarkerMaxDepthError(), maxDepthError_); + Parameters::parse(parameters, Parameters::kMarkerDictionary(), dictionaryId_); #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION <4 || (CV_MINOR_VERSION ==4 && CV_SUBMINOR_VERSION<2))) if(dictionaryId_ >= 17) { UERROR("Cannot set AprilTag dictionary. OpenCV version should be at least 3.4.2, " "current version is %s. Setting %s to default (%d)", CV_VERSION, - Parameters::kArucoDictionary().c_str(), - Parameters::defaultArucoDictionary()); - dictionaryId_ = Parameters::defaultArucoDictionary(); + Parameters::kMarkerDictionary().c_str(), + Parameters::defaultMarkerDictionary()); + dictionaryId_ = Parameters::defaultMarkerDictionary(); } #endif #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2) @@ -133,7 +133,7 @@ std::map MarkerDetector::detect(const cv::Mat & image, const Cam { if(depth.empty()) { - UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kArucoMarkerLength().c_str()); + UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str()); return detections; } rgbToDepthFactorX = 1.0f/(model.imageWidth()>0?model.imageWidth()/depth.cols:1); @@ -171,9 +171,9 @@ std::map MarkerDetector::detect(const cv::Mat & image, const Cam "the marker's length. Errors: %f, %f, %f > %fm (%s). Four corners: %f %f %f %f. " "Parameter %s can be set to non-null to skip automatic " "marker length estimation. Detections are ignored.", - fabs(d1-d2), fabs(d1-d3), fabs(d1-d4), maxDepthError_, Parameters::kArucoMaxDepthError().c_str(), + fabs(d1-d2), fabs(d1-d3), fabs(d1-d4), maxDepthError_, Parameters::kMarkerMaxDepthError().c_str(), d1, d2, d3, d4, - Parameters::kArucoMarkerLength().c_str()); + Parameters::kMarkerLength().c_str()); detections.clear(); return detections; } @@ -185,7 +185,7 @@ std::map MarkerDetector::detect(const cv::Mat & image, const Cam "Parameter %s can be set to non-null to skip automatic " "marker length estimation. Detections are ignored.", d1,d2,d3,d4, - Parameters::kArucoMarkerLength().c_str()); + Parameters::kMarkerLength().c_str()); detections.clear(); return detections; } @@ -217,7 +217,7 @@ std::map MarkerDetector::detect(const cv::Mat & image, const Cam "Parameter %s can be set to non-null to skip automatic " "marker length estimation. Detections are ignored.", ids[i], scales[i], ids[0], scales[0], - Parameters::kArucoMarkerLength().c_str()); + Parameters::kMarkerLength().c_str()); detections.clear(); return detections; } diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 3c132ce4..e9cbfc03 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -108,8 +108,8 @@ Memory::Memory(const ParametersMap & parameters) : _rectifyOnlyFeatures(Parameters::defaultRtabmapRectifyOnlyFeatures()), _covOffDiagonalIgnored(Parameters::defaultMemCovOffDiagIgnored()), _detectMarkers(Parameters::defaultRGBDMarkerDetection()), - _markerLinVariance(Parameters::defaultArucoVarianceLinear()), - _markerAngVariance(Parameters::defaultArucoVarianceAngular()), + _markerLinVariance(Parameters::defaultMarkerVarianceLinear()), + _markerAngVariance(Parameters::defaultMarkerVarianceAngular()), _idCount(kIdStart), _idMapCount(kIdStart), _lastSignature(0), @@ -567,8 +567,8 @@ void Memory::parseParameters(const ParametersMap & parameters) Parameters::parse(params, Parameters::kRtabmapRectifyOnlyFeatures(), _rectifyOnlyFeatures); Parameters::parse(params, Parameters::kMemCovOffDiagIgnored(), _covOffDiagonalIgnored); Parameters::parse(params, Parameters::kRGBDMarkerDetection(), _detectMarkers); - Parameters::parse(params, Parameters::kArucoVarianceLinear(), _markerLinVariance); - Parameters::parse(params, Parameters::kArucoVarianceAngular(), _markerAngVariance); + Parameters::parse(params, Parameters::kMarkerVarianceLinear(), _markerLinVariance); + Parameters::parse(params, Parameters::kMarkerVarianceAngular(), _markerAngVariance); UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str()); UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str()); diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index b541bf5d..51d8c2cc 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/odometry/OdometryMSCKF.h" #include "rtabmap/core/odometry/OdometryVINS.h" #include "rtabmap/core/OdometryInfo.h" +#include "rtabmap/core/IMUFilter.h" #include "rtabmap/core/util3d.h" #include "rtabmap/core/util3d_mapping.h" #include "rtabmap/core/util3d_filtering.h" @@ -110,6 +111,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : _holonomic(Parameters::defaultOdomHolonomic()), guessFromMotion_(Parameters::defaultOdomGuessMotion()), guessSmoothingDelay_(Parameters::defaultOdomGuessSmoothingDelay()), + _imuFilteringStrategy(Parameters::defaultOdomImuFilteringStrategy()), _filteringStrategy(Parameters::defaultOdomFilteringStrategy()), _particleSize(Parameters::defaultOdomParticleSize()), _particleNoiseT(Parameters::defaultOdomParticleNoiseT()), @@ -127,7 +129,8 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : _resetCurrentCount(0), previousStamp_(0), distanceTravelled_(0), - framesProcessed_(0) + framesProcessed_(0), + imuFilter_(0) { Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown); @@ -136,6 +139,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_); Parameters::parse(parameters, Parameters::kOdomGuessSmoothingDelay(), guessSmoothingDelay_); Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData); + Parameters::parse(parameters, Parameters::kOdomImuFilteringStrategy(), _imuFilteringStrategy); Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy); Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize); Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT); @@ -178,6 +182,11 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) : { initKalmanFilter(); } + + if(_imuFilteringStrategy > 0) + { + imuFilter_ = IMUFilter::create((IMUFilter::Type)(_imuFilteringStrategy-1), parameters); + } } Odometry::~Odometry() @@ -187,6 +196,7 @@ Odometry::~Odometry() delete particleFilters_[i]; } particleFilters_.clear(); + delete imuFilter_; } void Odometry::reset(const Transform & initialPose) @@ -241,6 +251,10 @@ void Odometry::reset(const Transform & initialPose) { _pose = initialPose; } + if(imuFilter_) + { + imuFilter_->reset(); + } } const Transform & Odometry::previousVelocityTransform() const @@ -353,6 +367,28 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet } } + // Update IMU orientation + if(!data.imu().empty() && imuFilter_ != 0) + { + 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], + 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())); + } + // KITTI datasets start with stamp=0 double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0; Transform guess = dt>0.0 && guessFromMotion_ && !velocityGuess_.isNull()?Transform::getIdentity():Transform(); @@ -370,6 +406,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet previousVelocities_.clear(); velocityGuess_.setNull(); } + if(!velocityGuess_.isNull()) { if(guessFromMotion_) @@ -466,6 +503,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet t = this->computeTransform(data, guess, info); } + if(data.imageRaw().empty() && data.laserScanRaw().isEmpty() && !data.imu().empty()) + { + return Transform(); // Return null on IMU-only updates + } + if(info) { info->timeEstimation = time.ticks(); diff --git a/corelib/src/Parameters.cpp b/corelib/src/Parameters.cpp index 30da747e..3859bae5 100644 --- a/corelib/src/Parameters.cpp +++ b/corelib/src/Parameters.cpp @@ -180,6 +180,9 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, boo (icp && group.compare("Icp") == 0) || (vis && Parameters::isFeatureParameter(iter->first)) || group.compare("Reg") == 0 || + group.compare("Optimizer") == 0 || + group.compare("g2o") == 0 || + group.compare("GTSAM") == 0 || (vis && group.compare("Vis") == 0) || iter->first.compare(kRtabmapPublishRAMUsage())==0) { @@ -234,6 +237,14 @@ const std::map > & Parameters::getRemo { // removed parameters + // 0.19.3 + removedParameters_.insert(std::make_pair("Aruco/Dictionary", std::make_pair(true, Parameters::kMarkerDictionary()))); + removedParameters_.insert(std::make_pair("Aruco/MarkerLength", std::make_pair(true, Parameters::kMarkerLength()))); + removedParameters_.insert(std::make_pair("Aruco/MaxDepthError", std::make_pair(true, Parameters::kMarkerMaxDepthError()))); + removedParameters_.insert(std::make_pair("Aruco/VarianceLinear", std::make_pair(true, Parameters::kMarkerVarianceLinear()))); + removedParameters_.insert(std::make_pair("Aruco/VarianceAngular", std::make_pair(true, Parameters::kMarkerVarianceAngular()))); + removedParameters_.insert(std::make_pair("Aruco/CornerRefinementMethod", std::make_pair(true, Parameters::kMarkerCornerRefinementMethod()))); + // 0.17.5 removedParameters_.insert(std::make_pair("Grid/OctoMapOccupancyThr", std::make_pair(true, Parameters::kGridGlobalOccupancyThr()))); @@ -591,6 +602,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; #else std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; +#endif + str = "With Madgwick:"; +#ifdef RTABMAP_MADGWICK + std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; +#else + std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; #endif str = "With TORO:"; #ifdef RTABMAP_TORO diff --git a/corelib/src/camera/CameraStereoZed.cpp b/corelib/src/camera/CameraStereoZed.cpp index 8176db9b..3d0f1fbe 100644 --- a/corelib/src/camera/CameraStereoZed.cpp +++ b/corelib/src/camera/CameraStereoZed.cpp @@ -126,6 +126,73 @@ CameraStereoZed::~CameraStereoZed() #endif } +#ifdef RTABMAP_ZED +static cv::Mat slMat2cvMat(sl::Mat& input) { + //convert MAT_TYPE to CV_TYPE + int cv_type = -1; + switch (input.getDataType()) { + case sl::MAT_TYPE_32F_C1: cv_type = CV_32FC1; break; + case sl::MAT_TYPE_32F_C2: cv_type = CV_32FC2; break; + case sl::MAT_TYPE_32F_C3: cv_type = CV_32FC3; break; + case sl::MAT_TYPE_32F_C4: cv_type = CV_32FC4; break; + case sl::MAT_TYPE_8U_C1: cv_type = CV_8UC1; break; + case sl::MAT_TYPE_8U_C2: cv_type = CV_8UC2; break; + case sl::MAT_TYPE_8U_C3: cv_type = CV_8UC3; break; + case sl::MAT_TYPE_8U_C4: cv_type = CV_8UC4; break; + default: break; + } + // cv::Mat data requires a uchar* pointer. Therefore, we get the uchar1 pointer from sl::Mat (getPtr()) + //cv::Mat and sl::Mat will share the same memory pointer + return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr(sl::MEM_CPU)); +} + +Transform zedPoseToTransform(const sl::Pose & pose) +{ + return Transform( + pose.pose_data.m[0], pose.pose_data.m[1], pose.pose_data.m[2], pose.pose_data.m[3], + pose.pose_data.m[4], pose.pose_data.m[5], pose.pose_data.m[6], pose.pose_data.m[7], + pose.pose_data.m[8], pose.pose_data.m[9], pose.pose_data.m[10], pose.pose_data.m[11]); +} + +IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform) +{ + sl::Orientation orientation = imuData.pose_data.getOrientation(); + + //Convert zed imu orientation from camera frame to world frame ENU! + Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0); + Transform orientationT(0,0,0, orientation.ox, orientation.oy, orientation.oz, orientation.ow); + orientationT = opticalTransform * orientationT; + + Eigen::Matrix4d opticalTransform4d = opticalTransform.toEigen4d(); + Eigen::Vector4d accT = opticalTransform4d * Eigen::Vector4d(imuData.linear_acceleration.v[0], imuData.linear_acceleration.v[1], imuData.linear_acceleration.v[2], 1); + Eigen::Vector4d gyrT = opticalTransform4d * Eigen::Vector4d(imuData.angular_velocity.v[0], imuData.angular_velocity.v[1], imuData.angular_velocity.v[2], 1); + + // FIXME covariance should be rotated too: see https://robotics.stackexchange.com/questions/2556/how-to-rotate-covariance + cv::Mat orientationCov = (cv::Mat_(3,3)<< + imuData.pose_covariance[21], imuData.pose_covariance[22], imuData.pose_covariance[23], + imuData.pose_covariance[27], imuData.pose_covariance[28], imuData.pose_covariance[29], + imuData.pose_covariance[33], imuData.pose_covariance[34], imuData.pose_covariance[35]); + cv::Mat angCov = (cv::Mat_(3,3)<< + imuData.angular_velocity_convariance.r[0], imuData.angular_velocity_convariance.r[1], imuData.angular_velocity_convariance.r[2], + imuData.angular_velocity_convariance.r[3], imuData.angular_velocity_convariance.r[4], imuData.angular_velocity_convariance.r[5], + imuData.angular_velocity_convariance.r[6], imuData.angular_velocity_convariance.r[7], imuData.angular_velocity_convariance.r[8]); + cv::Mat accCov = (cv::Mat_(3,3)<< + imuData.linear_acceleration_convariance.r[0], imuData.linear_acceleration_convariance.r[1], imuData.linear_acceleration_convariance.r[2], + imuData.linear_acceleration_convariance.r[3], imuData.linear_acceleration_convariance.r[4], imuData.linear_acceleration_convariance.r[5], + imuData.linear_acceleration_convariance.r[6], imuData.linear_acceleration_convariance.r[7], imuData.linear_acceleration_convariance.r[8]); + + Eigen::Quaternionf quat = orientationT.getQuaternionf(); + return IMU( + cv::Vec4d(quat.x(), quat.y(), quat.z(), quat.w()), + orientationCov, + cv::Vec3d(gyrT[0], gyrT[1], gyrT[2]), + angCov, + cv::Vec3d(accT[0], accT[1], accT[2]), + accCov, + imuLocalTransform); +} +#endif + bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName) { UDEBUG(""); @@ -217,6 +284,14 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str (int)res.height, this->getLocalTransform().prettyPrint().c_str()); + if(infos.camera_model == sl::MODEL_ZED_M) + { + imuLocalTransform_ = this->getLocalTransform() * zedPoseToTransform(infos.camera_imu_transform).inverse(); + UINFO("IMU local transform: %s (imu2cam=%s))", + imuLocalTransform_.prettyPrint().c_str(), + zedPoseToTransform(infos.camera_imu_transform).prettyPrint().c_str()); + } + return true; #else UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!"); @@ -252,34 +327,6 @@ bool CameraStereoZed::odomProvided() const return false; #endif } -#ifdef RTABMAP_ZED -static cv::Mat slMat2cvMat(sl::Mat& input) { - //convert MAT_TYPE to CV_TYPE - int cv_type = -1; - switch (input.getDataType()) { - case sl::MAT_TYPE_32F_C1: cv_type = CV_32FC1; break; - case sl::MAT_TYPE_32F_C2: cv_type = CV_32FC2; break; - case sl::MAT_TYPE_32F_C3: cv_type = CV_32FC3; break; - case sl::MAT_TYPE_32F_C4: cv_type = CV_32FC4; break; - case sl::MAT_TYPE_8U_C1: cv_type = CV_8UC1; break; - case sl::MAT_TYPE_8U_C2: cv_type = CV_8UC2; break; - case sl::MAT_TYPE_8U_C3: cv_type = CV_8UC3; break; - case sl::MAT_TYPE_8U_C4: cv_type = CV_8UC4; break; - default: break; - } - // cv::Mat data requires a uchar* pointer. Therefore, we get the uchar1 pointer from sl::Mat (getPtr()) - //cv::Mat and sl::Mat will share the same memory pointer - return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr(sl::MEM_CPU)); -} - -Transform zedPoseToTransform(const sl::Pose & pose) -{ - return Transform( - pose.pose_data.m[0], pose.pose_data.m[1], pose.pose_data.m[2], pose.pose_data.m[3], - pose.pose_data.m[4], pose.pose_data.m[5], pose.pose_data.m[6], pose.pose_data.m[7], - pose.pose_data.m[8], pose.pose_data.m[9], pose.pose_data.m[10], pose.pose_data.m[11]); -} -#endif SensorData CameraStereoZed::captureImage(CameraInfo * info) { @@ -327,6 +374,14 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info) data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now()); } + sl::IMUData imudata; + res = zed_->getIMUData(imudata, sl::TIME_REFERENCE_IMAGE); + if(res == sl::SUCCESS && imudata.valid) + { + //ZED-Mini + data.setIMU(zedIMUtoIMU(imudata, imuLocalTransform_)); + } + if (computeOdometry_ && info) { sl::Pose pose; diff --git a/corelib/src/imufilter/ComplementaryFilter.cpp b/corelib/src/imufilter/ComplementaryFilter.cpp new file mode 100644 index 00000000..c51b0f82 --- /dev/null +++ b/corelib/src/imufilter/ComplementaryFilter.cpp @@ -0,0 +1,482 @@ +/* + @author Roberto G. Valenti + + @section LICENSE + Copyright (c) 2015, City University of New York + CCNY Robotics Lab + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + 1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + 2. 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. + 3. Neither the name of the City College of New York 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 CCNY ROBOTICS LAB 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 "ComplementaryFilter.h" +#include +#include +#include +#include + +namespace rtabmap { + +const double ComplementaryFilter::kGravity = 9.81; +const double ComplementaryFilter::gamma_ = 0.01; +// Bias estimation steady state thresholds +const double ComplementaryFilter::kAngularVelocityThreshold = 0.2; +const double ComplementaryFilter::kAccelerationThreshold = 0.1; +const double ComplementaryFilter::kDeltaAngularVelocityThreshold = 0.01; + +ComplementaryFilter::ComplementaryFilter(const ParametersMap & parameters): + IMUFilter(parameters), + gain_acc_(Parameters::defaultImuFilterComplementaryGainAcc()), + bias_alpha_(Parameters::defaultImuFilterComplementaryBiasAlpha()), + do_bias_estimation_(Parameters::defaultImuFilterComplementaryDoBiasEstimation()), + do_adaptive_gain_(Parameters::defaultImuFilterComplementaryDoAdpativeGain()), + initialized_(false), + steady_state_(false), + q0_(1), q1_(0), q2_(0), q3_(0), + wx_prev_(0), wy_prev_(0), wz_prev_(0), + wx_bias_(0), wy_bias_(0), wz_bias_(0) +{ + parseParameters(parameters); +} + +void ComplementaryFilter::parseParameters(const ParametersMap & parameters) +{ + Parameters::parse(parameters, Parameters::kImuFilterComplementaryGainAcc(), gain_acc_); + Parameters::parse(parameters, Parameters::kImuFilterComplementaryBiasAlpha(), bias_alpha_); + Parameters::parse(parameters, Parameters::kImuFilterComplementaryDoBiasEstimation(), do_bias_estimation_); + Parameters::parse(parameters, Parameters::kImuFilterComplementaryDoAdpativeGain(), do_adaptive_gain_); +} + +void ComplementaryFilter::setDoBiasEstimation(bool do_bias_estimation) +{ + do_bias_estimation_ = do_bias_estimation; +} + +bool ComplementaryFilter::getDoBiasEstimation() const +{ + return do_bias_estimation_; +} + +void ComplementaryFilter::setDoAdaptiveGain(bool do_adaptive_gain) +{ + do_adaptive_gain_ = do_adaptive_gain; +} + +bool ComplementaryFilter::getDoAdaptiveGain() const +{ + return do_adaptive_gain_; +} + +bool ComplementaryFilter::setGainAcc(double gain) +{ + if (gain >= 0 && gain <= 1.0) + { + gain_acc_ = gain; + return true; + } + else + return false; +} + +double ComplementaryFilter::getGainAcc() const +{ + return gain_acc_; +} + +bool ComplementaryFilter::getSteadyState() const +{ + return steady_state_; +} + +bool ComplementaryFilter::setBiasAlpha(double bias_alpha) +{ + if (bias_alpha >= 0 && bias_alpha <= 1.0) + { + bias_alpha_ = bias_alpha; + return true; + } + else + return false; +} + +double ComplementaryFilter::getBiasAlpha() const +{ + return bias_alpha_; +} + +void ComplementaryFilter::reset( + double qx, double qy, double qz, double qw) +{ + // Set the state to inverse (state is fixed wrt body). + invertQuaternion(qw, qx, qy, qz, q0_, q1_, q2_, q3_); + + wx_prev_=0; + wy_prev_=0; + wz_prev_=0; + wx_bias_=0; + wy_bias_=0; + wz_bias_=0; + initialized_ = false; + steady_state_= false; +} + + +double ComplementaryFilter::getAngularVelocityBiasX() const +{ + return wx_bias_; +} + +double ComplementaryFilter::getAngularVelocityBiasY() const +{ + return wy_bias_; +} + +double ComplementaryFilter::getAngularVelocityBiasZ() const +{ + return wz_bias_; +} + +void ComplementaryFilter::updateImpl( + double gx, double gy, double gz, + double ax, double ay, double az, + double dt) +{ + if (!initialized_) + { + // First time - ignore prediction: + getMeasurement(ax, ay, az, + q0_, q1_, q2_, q3_); + initialized_ = true; + return; + } + + if(dt <= 0.0) + { + UERROR("dt=%f <=0.0, orientation will not be updated!", dt); + return; + } + + // Bias estimation. + if (do_bias_estimation_) + updateBiases(ax, ay, az, gx, gy, gz); + + // Prediction. + double q0_pred, q1_pred, q2_pred, q3_pred; + getPrediction(gx, gy, gz, dt, + q0_pred, q1_pred, q2_pred, q3_pred); + + // Correction (from acc): + // q_ = q_pred * [(1-gain) * qI + gain * dq_acc] + // where qI = identity quaternion + double dq0_acc, dq1_acc, dq2_acc, dq3_acc; + getAccCorrection(ax, ay, az, + q0_pred, q1_pred, q2_pred, q3_pred, + dq0_acc, dq1_acc, dq2_acc, dq3_acc); + + double gain; + if (do_adaptive_gain_) + { + gain = getAdaptiveGain(gain_acc_, ax, ay, az); + + } + else + { + gain = gain_acc_; + + } + + scaleQuaternion(gain, dq0_acc, dq1_acc, dq2_acc, dq3_acc); + + quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred, + dq0_acc, dq1_acc, dq2_acc, dq3_acc, + q0_, q1_, q2_, q3_); + + normalizeQuaternion(q0_, q1_, q2_, q3_); +} + +bool ComplementaryFilter::checkState(double ax, double ay, double az, + double wx, double wy, double wz) const +{ + double acc_magnitude = sqrt(ax*ax + ay*ay + az*az); + if (fabs(acc_magnitude - kGravity) > kAccelerationThreshold) + return false; + + if (fabs(wx - wx_prev_) > kDeltaAngularVelocityThreshold || + fabs(wy - wy_prev_) > kDeltaAngularVelocityThreshold || + fabs(wz - wz_prev_) > kDeltaAngularVelocityThreshold) + return false; + + if (fabs(wx - wx_bias_) > kAngularVelocityThreshold || + fabs(wy - wy_bias_) > kAngularVelocityThreshold || + fabs(wz - wz_bias_) > kAngularVelocityThreshold) + return false; + + return true; +} + +void ComplementaryFilter::updateBiases(double ax, double ay, double az, + double wx, double wy, double wz) +{ + steady_state_ = checkState(ax, ay, az, wx, wy, wz); + + if (steady_state_) + { + wx_bias_ += bias_alpha_ * (wx - wx_bias_); + wy_bias_ += bias_alpha_ * (wy - wy_bias_); + wz_bias_ += bias_alpha_ * (wz - wz_bias_); + } + + wx_prev_ = wx; + wy_prev_ = wy; + wz_prev_ = wz; +} + +void ComplementaryFilter::getPrediction( + double wx, double wy, double wz, double dt, + double& q0_pred, double& q1_pred, double& q2_pred, double& q3_pred) const +{ + double wx_unb = wx - wx_bias_; + double wy_unb = wy - wy_bias_; + double wz_unb = wz - wz_bias_; + + q0_pred = q0_ + 0.5*dt*( wx_unb*q1_ + wy_unb*q2_ + wz_unb*q3_); + q1_pred = q1_ + 0.5*dt*(-wx_unb*q0_ - wy_unb*q3_ + wz_unb*q2_); + q2_pred = q2_ + 0.5*dt*( wx_unb*q3_ - wy_unb*q0_ - wz_unb*q1_); + q3_pred = q3_ + 0.5*dt*(-wx_unb*q2_ + wy_unb*q1_ - wz_unb*q0_); + + normalizeQuaternion(q0_pred, q1_pred, q2_pred, q3_pred); +} + +void ComplementaryFilter::getMeasurement( + double ax, double ay, double az, + double mx, double my, double mz, + double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas) +{ + // q_acc is the quaternion obtained from the acceleration vector representing + // the orientation of the Global frame wrt the Local frame with arbitrary yaw + // (intermediary frame). q3_acc is defined as 0. + double q0_acc, q1_acc, q2_acc, q3_acc; + + // Normalize acceleration vector. + normalizeVector(ax, ay, az); + if (az >=0) + { + q0_acc = sqrt((az + 1) * 0.5); + q1_acc = -ay/(2.0 * q0_acc); + q2_acc = ax/(2.0 * q0_acc); + q3_acc = 0; + } + else + { + double X = sqrt((1 - az) * 0.5); + q0_acc = -ay/(2.0 * X); + q1_acc = X; + q2_acc = 0; + q3_acc = ax/(2.0 * X); + } + + // [lx, ly, lz] is the magnetic field reading, rotated into the intermediary + // frame by the inverse of q_acc. + // l = R(q_acc)^-1 m + double lx = (q0_acc*q0_acc + q1_acc*q1_acc - q2_acc*q2_acc)*mx + + 2.0 * (q1_acc*q2_acc)*my - 2.0 * (q0_acc*q2_acc)*mz; + double ly = 2.0 * (q1_acc*q2_acc)*mx + (q0_acc*q0_acc - q1_acc*q1_acc + + q2_acc*q2_acc)*my + 2.0 * (q0_acc*q1_acc)*mz; + + // q_mag is the quaternion that rotates the Global frame (North West Up) into + // the intermediary frame. q1_mag and q2_mag are defined as 0. + double gamma = lx*lx + ly*ly; + double beta = sqrt(gamma + lx*sqrt(gamma)); + double q0_mag = beta / (sqrt(2.0 * gamma)); + double q3_mag = ly / (sqrt(2.0) * beta); + + // The quaternion multiplication between q_acc and q_mag represents the + // quaternion, orientation of the Global frame wrt the local frame. + // q = q_acc times q_mag + quaternionMultiplication(q0_acc, q1_acc, q2_acc, q3_acc, + q0_mag, 0, 0, q3_mag, + q0_meas, q1_meas, q2_meas, q3_meas ); + //q0_meas = q0_acc*q0_mag; + //q1_meas = q1_acc*q0_mag + q2_acc*q3_mag; + //q2_meas = q2_acc*q0_mag - q1_acc*q3_mag; + //q3_meas = q0_acc*q3_mag; +} + + +void ComplementaryFilter::getMeasurement( + double ax, double ay, double az, + double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas) +{ + // q_acc is the quaternion obtained from the acceleration vector representing + // the orientation of the Global frame wrt the Local frame with arbitrary yaw + // (intermediary frame). q3_acc is defined as 0. + + // Normalize acceleration vector. + normalizeVector(ax, ay, az); + + if (az >=0) + { + q0_meas = sqrt((az + 1) * 0.5); + q1_meas = -ay/(2.0 * q0_meas); + q2_meas = ax/(2.0 * q0_meas); + q3_meas = 0; + } + else + { + double X = sqrt((1 - az) * 0.5); + q0_meas = -ay/(2.0 * X); + q1_meas = X; + q2_meas = 0; + q3_meas = ax/(2.0 * X); + } +} + +void ComplementaryFilter::getAccCorrection( + double ax, double ay, double az, + double p0, double p1, double p2, double p3, + double& dq0, double& dq1, double& dq2, double& dq3) +{ + // Normalize acceleration vector. + normalizeVector(ax, ay, az); + + // Acceleration reading rotated into the world frame by the inverse predicted + // quaternion (predicted gravity): + double gx, gy, gz; + rotateVectorByQuaternion(ax, ay, az, + p0, -p1, -p2, -p3, + gx, gy, gz); + + // Delta quaternion that rotates the predicted gravity into the real gravity: + dq0 = sqrt((gz + 1) * 0.5); + dq1 = -gy/(2.0 * dq0); + dq2 = gx/(2.0 * dq0); + dq3 = 0.0; +} + +void ComplementaryFilter::getOrientation( + double& qx, double& qy, double& qz, double& qw) const +{ + // Return the inverse of the state (state is fixed wrt body). + invertQuaternion(q0_, q1_, q2_, q3_, qw,qx,qy,qz); +} + +double ComplementaryFilter::getAdaptiveGain(double alpha, double ax, double ay, double az) +{ + double a_mag = sqrt(ax*ax + ay*ay + az*az); + double error = fabs(a_mag - kGravity)/kGravity; + double factor; + double error1 = 0.1; + double error2 = 0.2; + double m = 1.0/(error1 - error2); + double b = 1.0 - m*error1; + if (error < error1) + factor = 1.0; + else if (error < error2) + factor = m*error + b; + else + factor = 0.0; + //printf("FACTOR: %f \n", factor); + return factor*alpha; +} + +void normalizeVector(double& x, double& y, double& z) +{ + double norm = sqrt(x*x + y*y + z*z); + + x /= norm; + y /= norm; + z /= norm; +} + +void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3) +{ + double norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); + q0 /= norm; + q1 /= norm; + q2 /= norm; + q3 /= norm; +} + +void invertQuaternion( + double q0, double q1, double q2, double q3, + double& q0_inv, double& q1_inv, double& q2_inv, double& q3_inv) +{ + // Assumes quaternion is normalized. + q0_inv = q0; + q1_inv = -q1; + q2_inv = -q2; + q3_inv = -q3; +} + +void scaleQuaternion( + double gain, + double& dq0, double& dq1, double& dq2, double& dq3) +{ + if (dq0 < 0.0)//0.9 + { + // Slerp (Spherical linear interpolation): + double angle = acos(dq0); + double A = sin(angle*(1.0 - gain))/sin(angle); + double B = sin(angle * gain)/sin(angle); + dq0 = A + B * dq0; + dq1 = B * dq1; + dq2 = B * dq2; + dq3 = B * dq3; + } + else + { + // Lerp (Linear interpolation): + dq0 = (1.0 - gain) + gain * dq0; + dq1 = gain * dq1; + dq2 = gain * dq2; + dq3 = gain * dq3; + } + + normalizeQuaternion(dq0, dq1, dq2, dq3); +} + +void quaternionMultiplication( + double p0, double p1, double p2, double p3, + double q0, double q1, double q2, double q3, + double& r0, double& r1, double& r2, double& r3) +{ + // r = p q + r0 = p0*q0 - p1*q1 - p2*q2 - p3*q3; + r1 = p0*q1 + p1*q0 + p2*q3 - p3*q2; + r2 = p0*q2 - p1*q3 + p2*q0 + p3*q1; + r3 = p0*q3 + p1*q2 - p2*q1 + p3*q0; +} + +void rotateVectorByQuaternion( + double x, double y, double z, + double q0, double q1, double q2, double q3, + double& vx, double& vy, double& vz) +{ + vx = (q0*q0 + q1*q1 - q2*q2 - q3*q3)*x + 2*(q1*q2 - q0*q3)*y + 2*(q1*q3 + q0*q2)*z; + vy = 2*(q1*q2 + q0*q3)*x + (q0*q0 - q1*q1 + q2*q2 - q3*q3)*y + 2*(q2*q3 - q0*q1)*z; + vz = 2*(q1*q3 - q0*q2)*x + 2*(q2*q3 + q0*q1)*y + (q0*q0 - q1*q1 - q2*q2 + q3*q3)*z; +} + + +} // namespace rtabmap + diff --git a/corelib/src/imufilter/ComplementaryFilter.h b/corelib/src/imufilter/ComplementaryFilter.h new file mode 100644 index 00000000..5a629b39 --- /dev/null +++ b/corelib/src/imufilter/ComplementaryFilter.h @@ -0,0 +1,161 @@ +/* + @author Roberto G. Valenti + @section LICENSE + Copyright (c) 2015, City University of New York + CCNY Robotics Lab + All rights reserved. + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + 1. Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + 2. 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. + 3. Neither the name of the City College of New York 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 CCNY ROBOTICS LAB 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_SRC_IMUFILTER_COMPLEMENTARYFILTER_H_ +#define CORELIB_SRC_IMUFILTER_COMPLEMENTARYFILTER_H_ + +#include + +namespace rtabmap { + +class ComplementaryFilter : public IMUFilter +{ + public: + ComplementaryFilter(const ParametersMap & parameters = ParametersMap()); + virtual ~ComplementaryFilter() {} + + bool setGainAcc(double gain); + double getGainAcc() const; + + bool setBiasAlpha(double bias_alpha); + double getBiasAlpha() const; + + // When the filter is in the steady state, bias estimation will occur (if the + // parameter is enabled). + bool getSteadyState() const; + + void setDoBiasEstimation(bool do_bias_estimation); + bool getDoBiasEstimation() const; + + void setDoAdaptiveGain(bool do_adaptive_gain); + bool getDoAdaptiveGain() const; + + double getAngularVelocityBiasX() const; + double getAngularVelocityBiasY() const; + double getAngularVelocityBiasZ() const; + + virtual void parseParameters(const ParametersMap & parameters); + virtual IMUFilter::Type type() const {return IMUFilter::kComplementaryFilter;} + virtual void getOrientation(double & qx, double & qy, double & qz, double & qw) const; + virtual void reset(double qx = 0.0, double qy = 0.0, double qz = 0.0, double qw = 1.0); + + // 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); + + private: + static const double kGravity; + static const double gamma_; + // Bias estimation steady state thresholds + static const double kAngularVelocityThreshold; + static const double kAccelerationThreshold; + static const double kDeltaAngularVelocityThreshold; + + // Gain parameter for the complementary filter, belongs in [0, 1]. + double gain_acc_; + + // Bias estimation gain parameter, belongs in [0, 1]. + double bias_alpha_; + + // Parameter whether to do bias estimation or not. + bool do_bias_estimation_; + + // Parameter whether to do adaptive gain or not. + bool do_adaptive_gain_; + + bool initialized_; + bool steady_state_; + + // The orientation as a Hamilton quaternion (q0 is the scalar). Represents + // the orientation of the fixed frame wrt the body frame. + double q0_, q1_, q2_, q3_; + + // Bias in angular velocities; + double wx_prev_, wy_prev_, wz_prev_; + + // Bias in angular velocities; + double wx_bias_, wy_bias_, wz_bias_; + + void updateBiases(double ax, double ay, double az, + double wx, double wy, double wz); + + bool checkState(double ax, double ay, double az, + double wx, double wy, double wz) const; + + void getPrediction( + double wx, double wy, double wz, double dt, + double& q0_pred, double& q1_pred, double& q2_pred, double& q3_pred) const; + + void getMeasurement( + double ax, double ay, double az, + double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas); + + void getMeasurement( + double ax, double ay, double az, + double mx, double my, double mz, + double& q0_meas, double& q1_meas, double& q2_meas, double& q3_meas); + + void getAccCorrection( + double ax, double ay, double az, + double p0, double p1, double p2, double p3, + double& dq0, double& dq1, double& dq2, double& dq3); + + double getAdaptiveGain(double alpha, double ax, double ay, double az); +}; + +// Utility math functions: + +void normalizeVector(double& x, double& y, double& z); + +void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3); + +void scaleQuaternion(double gain, + double& dq0, double& dq1, double& dq2, double& dq3); + +void invertQuaternion( + double q0, double q1, double q2, double q3, + double& q0_inv, double& q1_inv, double& q2_inv, double& q3_inv); + +void quaternionMultiplication(double p0, double p1, double p2, double p3, + double q0, double q1, double q2, double q3, + double& r0, double& r1, double& r2, double& r3); + +void rotateVectorByQuaternion(double x, double y, double z, + double q0, double q1, double q2, double q3, +double& vx, double& vy, double& vz); + +} + + +#endif /* CORELIB_SRC_IMUFILTER_COMPLEMENTARYFILTER_H_ */ diff --git a/corelib/src/imufilter/MadgwickFilter.cpp b/corelib/src/imufilter/MadgwickFilter.cpp new file mode 100644 index 00000000..6b314e6b --- /dev/null +++ b/corelib/src/imufilter/MadgwickFilter.cpp @@ -0,0 +1,364 @@ + +/* + * Copyright (C) 2010, CCNY Robotics Lab + * Ivan Dryanovski + * + * http://robotics.ccny.cuny.edu + * + * Based on implementation of Madgwick's IMU and AHRS algorithms. + * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include "MadgwickFilter.h" +#include + +namespace rtabmap { + +// Fast inverse square-root +// See: http://en.wikipedia.org/wiki/Methods_of_computing_square_roots#Reciprocal_of_the_square_root +static inline float invSqrt(float x) +{ + float xhalf = 0.5f * x; + union + { + float x; + int i; + } u; + u.x = x; + u.i = 0x5f3759df - (u.i >> 1); + /* The next line can be repeated any number of times to increase accuracy */ + u.x = u.x * (1.5f - xhalf * u.x * u.x); + return u.x; +} + +template +static inline void normalizeVectorOpt(T& vx, T& vy, T& vz) +{ + T recipNorm = invSqrt (vx * vx + vy * vy + vz * vz); + vx *= recipNorm; + vy *= recipNorm; + vz *= recipNorm; +} + +template +static inline void normalizeQuaternion(T& q0, T& q1, T& q2, T& q3) +{ + T recipNorm = invSqrt (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; +} + +static inline void rotateAndScaleVector( + float q0, float q1, float q2, float q3, + float _2dx, float _2dy, float _2dz, + float& rx, float& ry, float& rz) { + + // result is half as long as input + rx = _2dx * (0.5f - q2 * q2 - q3 * q3) + + _2dy * (q0 * q3 + q1 * q2) + + _2dz * (q1 * q3 - q0 * q2); + ry = _2dx * (q1 * q2 - q0 * q3) + + _2dy * (0.5f - q1 * q1 - q3 * q3) + + _2dz * (q0 * q1 + q2 * q3); + rz = _2dx * (q0 * q2 + q1 * q3) + + _2dy * (q2 * q3 - q0 * q1) + + _2dz * (0.5f - q1 * q1 - q2 * q2); +} + +static inline void orientationChangeFromGyro( + float q0, float q1, float q2, float q3, + float gx, float gy, float gz, + float& qDot1, float& qDot2, float& qDot3, float& qDot4) +{ + // Rate of change of quaternion from gyroscope + // See EQ 12 + qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); + qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); + qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); + qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); +} + +static inline void addGradientDescentStep( + float q0, float q1, float q2, float q3, + float _2dx, float _2dy, float _2dz, + float mx, float my, float mz, + float& s0, float& s1, float& s2, float& s3) +{ + float f0, f1, f2; + + // Gradient decent algorithm corrective step + // EQ 15, 21 + rotateAndScaleVector(q0,q1,q2,q3, _2dx, _2dy, _2dz, f0, f1, f2); + + f0 -= mx; + f1 -= my; + f2 -= mz; + + + // EQ 22, 34 + // Jt * f + s0 += (_2dy * q3 - _2dz * q2) * f0 + + (-_2dx * q3 + _2dz * q1) * f1 + + (_2dx * q2 - _2dy * q1) * f2; + s1 += (_2dy * q2 + _2dz * q3) * f0 + + (_2dx * q2 - 2.0f * _2dy * q1 + _2dz * q0) * f1 + + (_2dx * q3 - _2dy * q0 - 2.0f * _2dz * q1) * f2; + s2 += (-2.0f * _2dx * q2 + _2dy * q1 - _2dz * q0) * f0 + + (_2dx * q1 + _2dz * q3) * f1 + + (_2dx * q0 + _2dy * q3 - 2.0f * _2dz * q2) * f2; + s3 += (-2.0f * _2dx * q3 + _2dy * q0 + _2dz * q1) * f0 + + (-_2dx * q0 - 2.0f * _2dy * q3 + _2dz * q2) * f1 + + (_2dx * q1 + _2dy * q2) * f2; +} + +template +static inline void crossProduct( + T ax, T ay, T az, + T bx, T by, T bz, + T& rx, T& ry, T& rz) { + rx = ay*bz - az*by; + ry = az*bx - ax*bz; + rz = ax*by - ay*bx; +} + +template +static inline T normalizeVector(T& vx, T& vy, T& vz) { + T norm = sqrt(vx*vx + vy*vy + vz*vz); + T inv = 1.0 / norm; + vx *= inv; + vy *= inv; + vz *= inv; + return norm; + +} + +static inline bool computeOrientation( + Eigen::Vector3f A, + Eigen::Vector3f E, + Eigen::Quaternionf& orientation) { + + float Hx, Hy, Hz; + float Mx, My, Mz; + float normH; + + // A: pointing up + float Ax = A[0], Ay = A[1], Az = A[2]; + + // E: pointing down/north + float Ex = E[0], Ey = E[1], Ez = E[2]; + + // H: vector horizontal, pointing east + // H = E x A + crossProduct(Ex, Ey, Ez, Ax, Ay, Az, Hx, Hy, Hz); + + // normalize H + normH = normalizeVector(Hx, Hy, Hz); + if (normH < 1E-7) { + // device is close to free fall (or in space?), or close to + // magnetic north pole. + // mag in T => Threshold 1E-7, typical values are > 1E-5. + return false; + } + + // normalize A + normalizeVector(Ax, Ay, Az); + + // M: vector horizontal, pointing north + // M = A x H + crossProduct(Ax, Ay, Az, Hx, Hy, Hz, Mx, My, Mz); + + // Create matrix for basis transformation + Eigen::Matrix3f R; + + //case WorldFrame::ENU: + // vector space world W: + // Basis: bwx (1,0,0) east, bwy (0,1,0) north, bwz (0,0,1) up + // vector space local L: + // Basis: H, M , A + // W(1,0,0) => L(H) + // W(0,1,0) => L(M) + // W(0,0,1) => L(A) + + // R: Transform Matrix local => world equals basis of L, because basis of W is I + R(0,0) = Hx; R(0,1) = Mx; R(0,2) = Ax; + R(1,0) = Hy; R(1,1) = My; R(1,2) = Ay; + R(2,0) = Hz; R(2,1) = Mz; R(2,2) = Az; + + // Matrix.getRotation assumes vector rotation, but we're using + // coordinate systems. Thus negate rotation angle (inverse). + Eigen::Quaternionf q(R); + orientation = q.inverse(); + return true; +} + + +static inline bool computeOrientation( + Eigen::Vector3f A, + Eigen::Quaternionf& orientation) { + + // This implementation could be optimized regarding speed. + + // 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) { + 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; + } + + return computeOrientation(A, E, orientation); +} + +MadgwickFilter::MadgwickFilter(const ParametersMap & parameters) : + IMUFilter(parameters), + q0(1.0), q1(0.0), q2(0.0), q3(0.0), + w_bx_(0.0), w_by_(0.0), w_bz_(0.0), + initialized_(false), + gain_ (Parameters::defaultImuFilterMadgwickGain()), + zeta_ (Parameters::defaultImuFilterMadgwickZeta()) +{ + parseParameters(parameters); +} + +void MadgwickFilter::parseParameters(const ParametersMap & parameters) +{ + Parameters::parse(parameters, Parameters::kImuFilterMadgwickGain(), gain_); + Parameters::parse(parameters, Parameters::kImuFilterMadgwickZeta(), zeta_); +} + +/** + * Gain of the filter. Higher values lead to faster convergence but + * more noise. Lower values lead to slower convergence but smoother signal. [0.0, 1.0] + */ +void MadgwickFilter::setAlgorithmGain(double gain) +{ + gain_ = gain; +} + +/** + * Gyro drift gain (approx. rad/s). [-1.0, 1.0] + */ +void MadgwickFilter::setDriftBiasGain(double zeta) +{ + zeta_ = zeta; +} + +void MadgwickFilter::getOrientation(double & qx, double & qy, double & qz, double & qw) const +{ + qx = this->q1; + qy = this->q2; + qz = this->q3; + qw = this->q0; + + // perform precise normalization of the output, using 1/sqrt() + // instead of the fast invSqrt() approximation. Without this, + // TF2 complains that the quaternion is not normalized. + double recipNorm = 1 / sqrt(qx * qx + qy * qy + qz * qz + qw * qw); + qx *= recipNorm; + qy *= recipNorm; + qz *= recipNorm; + qw *= recipNorm; +} + +void MadgwickFilter::reset(double qx, double qy, double qz, double qw) +{ + this->q0 = qw; + this->q1 = qx; + this->q2 = qy; + this->q3 = qz; + + w_bx_ = 0; + w_by_ = 0; + w_bz_ = 0; + + initialized_ = false; +} + +void MadgwickFilter::updateImpl( + double gx, double gy, double gz, + double ax, double ay, double az, + double dt) +{ + if(!initialized_) + { + Eigen::Quaternionf orientation; + Eigen::Vector3f A; + A[0] = ax; + A[1] = ay; + A[2] = az; + computeOrientation(A,orientation); + reset(orientation.x(), orientation.y(), orientation.z(), orientation.w()); + printf("%f %f %f -> %f %f %f %f\n", A[0], A[1], A[2], orientation.x(), orientation.y(), orientation.z(), orientation.w()); + initialized_ = true; + return; + } + + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + + // Rate of change of quaternion from gyroscope + orientationChangeFromGyro (q0, q1, q2, q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4); + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) + { + // Normalise accelerometer measurement + normalizeVectorOpt(ax, ay, az); + + // Gradient decent algorithm corrective step + s0 = 0.0; s1 = 0.0; s2 = 0.0; s3 = 0.0; + //case WorldFrame::ENU: + // Gravity: [0, 0, 1] + addGradientDescentStep(q0, q1, q2, q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3); + + normalizeQuaternion(s0, s1, s2, s3); + + // Apply feedback step + qDot1 -= gain_ * s0; + qDot2 -= gain_ * s1; + qDot3 -= gain_ * s2; + qDot4 -= gain_ * s3; + } + + // Integrate rate of change of quaternion to yield quaternion + if(dt <= 0.0) + { + UERROR("dt=%f <=0.0, orientation will not be updated!", dt); + return; + } + q0 += qDot1 * dt; + q1 += qDot2 * dt; + q2 += qDot3 * dt; + q3 += qDot4 * dt; + + //printf("%fs %f %f %f %f\n", dt, q0, q1, q2, q3); + + // Normalise quaternion + normalizeQuaternion (q0, q1, q2, q3); +} + +} diff --git a/corelib/src/imufilter/MadgwickFilter.h b/corelib/src/imufilter/MadgwickFilter.h new file mode 100644 index 00000000..aedaf9e9 --- /dev/null +++ b/corelib/src/imufilter/MadgwickFilter.h @@ -0,0 +1,82 @@ + +/* + * Copyright (C) 2010, CCNY Robotics Lab + * Ivan Dryanovski + * + * http://robotics.ccny.cuny.edu + * + * Based on implementation of Madgwick's IMU and AHRS algorithms. + * http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms + * + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef CORELIB_SRC_IMUFILTER_MADGWICKFILTER_H_ +#define CORELIB_SRC_IMUFILTER_MADGWICKFILTER_H_ + +#include +#include + +namespace rtabmap { + +class MadgwickFilter : public IMUFilter +{ + public: + + MadgwickFilter(const ParametersMap & parameters = ParametersMap()); + virtual ~MadgwickFilter(){} + + private: + // **** state variables + double q0, q1, q2, q3; // quaternion + float w_bx_, w_by_, w_bz_; // + bool initialized_; + + // **** paramaters + double gain_; // algorithm gain + double zeta_; // gyro drift bias gain + +public: + /** + * Gain of the filter. Higher values lead to faster convergence but + * more noise. Lower values lead to slower convergence but smoother signal. [0.0, 1.0] + */ + void setAlgorithmGain(double gain); + + /** + * Gyro drift gain (approx. rad/s). [-1.0, 1.0] + */ + void setDriftBiasGain(double zeta); + + virtual void parseParameters(const ParametersMap & parameters); + virtual IMUFilter::Type type() const {return IMUFilter::kMadgwick;} + virtual void getOrientation(double & qx, double & qy, double & qz, double & qw) const; + virtual void reset(double qx = 0.0, double qy = 0.0, double qz = 0.0, double qw = 1.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. + void updateImpl( + double gx, double gy, double gz, + double ax, double ay, double az, + double dt); +}; + +} + + +#endif /* CORELIB_SRC_IMUFILTER_MADGWICKFILTER_H_ */ diff --git a/corelib/src/imufilter/README.txt b/corelib/src/imufilter/README.txt new file mode 100644 index 00000000..9e12c5c0 --- /dev/null +++ b/corelib/src/imufilter/README.txt @@ -0,0 +1,28 @@ +IMU filters taken from ROS imu_tools stack: https://github.com/ccny-ros-pkg/imu_tools, please look below for licensing. To avoid GPL license, MadgwickFilter can be disabled on compilation with "cmake -DWITH_MADGWICK=OFF ..". +=================================== + +Overview +----------------------------------- + +IMU-related filters: + + * `MadgwickFilter`: a filter which fuses angular velocities, +accelerations, and (optionally) magnetic readings from a generic IMU +device into an orientation. Based on the work of [1]. + + * `ComplementaryFilter`: a filter which fuses angular velocities, +accelerations, and (optionally) magnetic readings from a generic IMU +device into an orientation quaternion using a novel approach based on a complementary fusion. Based on the work of [2]. + +License +----------------------------------- + + * `MadgwickFilter`: currently licensed as GPL, following the original implementation + + * `ComplementaryFilter`: BSD + +References +----------------------------------- + [1] http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ + + [2] http://www.mdpi.com/1424-8220/15/8/19302 diff --git a/corelib/src/odometry/OdometryF2M.cpp b/corelib/src/odometry/OdometryF2M.cpp index 7b99042c..05aafe3c 100644 --- a/corelib/src/odometry/OdometryF2M.cpp +++ b/corelib/src/odometry/OdometryF2M.cpp @@ -72,6 +72,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) : map_(new Signature(-1)), lastFrame_(new Signature(1)), lastFrameOldestNewId_(0), + initGravity_(false), bundleSeq_(0), sba_(0) { @@ -150,6 +151,7 @@ OdometryF2M::~OdometryF2M() bundleLinks_.clear(); bundleModels_.clear(); bundlePoseReferences_.clear(); + imus_.clear(); delete sba_; delete regPipeline_; UDEBUG(""); @@ -158,26 +160,33 @@ OdometryF2M::~OdometryF2M() void OdometryF2M::reset(const Transform & initialPose) { - UDEBUG("initialPose=%s", initialPose.prettyPrint().c_str()); Odometry::reset(initialPose); - *lastFrame_ = Signature(1); - *map_ = Signature(-1); - scansBuffer_.clear(); - bundleWordReferences_.clear(); - bundlePoses_.clear(); - bundleLinks_.clear(); - bundleModels_.clear(); - bundlePoseReferences_.clear(); - bundleSeq_ = 0; - lastFrameOldestNewId_ = 0; + if(!initGravity_) + { + UDEBUG("initialPose=%s", initialPose.prettyPrint().c_str()); + Odometry::reset(initialPose); + *lastFrame_ = Signature(1); + *map_ = Signature(-1); + scansBuffer_.clear(); + bundleWordReferences_.clear(); + bundlePoses_.clear(); + bundleLinks_.clear(); + bundleModels_.clear(); + bundlePoseReferences_.clear(); + bundleSeq_ = 0; + lastFrameOldestNewId_ = 0; + imus_.clear(); + } + initGravity_ = false; } // return not null transform if odometry is correctly computed Transform OdometryF2M::computeTransform( SensorData & data, - const Transform & guess, + const Transform & guessIn, OdometryInfo * info) { + Transform guess = guessIn; UTimer timer; Transform output; @@ -186,6 +195,39 @@ Transform OdometryF2M::computeTransform( info->type = 0; } + if(!data.imu().empty()) + { + if(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0) + { + UERROR("IMU received doesn't have orientation set, it is ignored."); + } + else + { + Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]); + //UWARN("%fs %s", data.stamp(), orientation.prettyPrint().c_str()); + imus_.insert(std::make_pair(data.stamp(), orientation*data.imu().localTransform().inverse())); + if(imus_.size() > 1000) + { + imus_.erase(imus_.begin()); + } + + if(this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f) + { + Eigen::Quaterniond imuQuat = imus_.rbegin()->second.getQuaterniond(); + Transform previous = this->getPose(); + Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w()); + UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str()); + initGravity_ = true; + this->reset(newFramePose); + } + } + + if(data.imageRaw().empty() && data.laserScanRaw().isEmpty()) + { + return output; + } + } + RegistrationInfo regInfo; int nFeatures = 0; @@ -285,6 +327,21 @@ Transform OdometryF2M::computeTransform( UDEBUG("Registration time = %fs", regInfo.totalTime); if(!transform.isNull()) { + Transform imuT; + if(!imus_.empty()) + { + double stampDiff = 0.0; + imuT = getClosestIMU(lastFrame_->getStamp(), stampDiff); + if(stampDiff < 0.05) + { + bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kPoseOdom, imuT))); + } + else + { + UWARN("IMUs are set, but we could not find one matching the current frame stamp %f (stampDiff=%f > 0.05)", lastFrame_->getStamp(), stampDiff); + } + } + // local bundle adjustment if(bundleAdjustment_>0 && sba_ && regPipeline_->isImageRequired() && @@ -311,6 +368,7 @@ Transform OdometryF2M::computeTransform( bundlePoses = bundlePoses_; bundleLinks = bundleLinks_; bundleModels = bundleModels_; + bundleLinks.insert(bundleIMUOrientations_.begin(), bundleIMUOrientations_.end()); UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(), uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str()); @@ -320,6 +378,11 @@ Transform OdometryF2M::computeTransform( bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, var.inv()))); bundlePoses.insert(std::make_pair(lastFrame_->id(), transform)); + if(!imuT.isNull()) + { + bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kPoseOdom, imuT))); + } + CameraModel model; if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection()) { @@ -445,7 +508,9 @@ Transform OdometryF2M::computeTransform( else { transform = bundlePoses.rbegin()->second; - bundleLinks.find(bundlePoses_.rbegin()->first)->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform); + std::multimap::iterator iter = graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false); + UASSERT(iter != bundleLinks.end()); + iter->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform); } } UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str()); @@ -534,8 +599,14 @@ Transform OdometryF2M::computeTransform( if(bundleAdjustment_>0) { bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), 0)); - UASSERT(graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false) != bundleLinks.end()); - bundleLinks_.insert(*bundleLinks.find(bundlePoses_.rbegin()->first)); + std::multimap::iterator iter = graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false); + UASSERT(iter != bundleLinks.end()); + bundleLinks_.insert(*iter); + iter = graph::findLink(bundleLinks, lastFrame_->id(), lastFrame_->id(), false); + if(iter != bundleLinks.end()) + { + bundleIMUOrientations_.insert(*iter); + } uInsert(bundlePoses_, bundlePoses); UASSERT(bundleModels.find(lastFrame_->id()) != bundleModels.end()); bundleModels_.insert(*bundleModels.find(lastFrame_->id())); @@ -818,6 +889,7 @@ Transform OdometryF2M::computeTransform( UASSERT(bundlePoses_.erase(iter->first) == 1); bundleLinks_.erase(iter->first); bundleModels_.erase(iter->first); + bundleIMUOrientations_.erase(iter->first); bundlePoseReferences_.erase(iter++); } } @@ -1012,6 +1084,7 @@ Transform OdometryF2M::computeTransform( bool frameValid = false; Transform newFramePose = this->getPose(); // initial pose may be not identity... + if(regPipeline_->isImageRequired()) { int ptsWithDepth = 0; @@ -1115,6 +1188,11 @@ Transform OdometryF2M::computeTransform( } bundleModels_.insert(std::make_pair(lastFrame_->id(), model)); bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose)); + + if(!imus_.empty()) + { + bundleIMUOrientations_.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kPoseOdom, newFramePose))); + } } map_->setWords(words); @@ -1229,4 +1307,41 @@ Transform OdometryF2M::computeTransform( return output; } +Transform OdometryF2M::getClosestIMU(const double & stamp, double & stampDiff) const +{ + UASSERT(!imus_.empty()); + std::map::const_iterator imuIterB = imus_.lower_bound(stamp); + std::map::const_iterator imuIterA = imuIterB; + if(imuIterA != imus_.begin()) + { + imuIterA = --imuIterA; + } + if(imuIterB == imus_.end()) + { + imuIterB = --imuIterB; + } + Transform imuT; + stampDiff = 0.0; + if(imuIterB->first == lastFrame_->getStamp() || imuIterA == imuIterB) + { + imuT = imuIterB->second; + stampDiff = fabs(imuIterB->first - lastFrame_->getStamp()); + } + else if(imuIterA != imuIterB) + { + if(fabs(imuIterA->first - lastFrame_->getStamp()) < + fabs(imuIterB->first - lastFrame_->getStamp())) + { + imuT = imuIterA->second; + stampDiff = fabs(imuIterA->first - lastFrame_->getStamp()); + } + else + { + imuT = imuIterB->second; + stampDiff = fabs(imuIterB->first - lastFrame_->getStamp()); + } + } + return imuT; +} + } // namespace rtabmap diff --git a/corelib/src/optimizer/OptimizerG2O.cpp b/corelib/src/optimizer/OptimizerG2O.cpp index b965d300..b920ac30 100644 --- a/corelib/src/optimizer/OptimizerG2O.cpp +++ b/corelib/src/optimizer/OptimizerG2O.cpp @@ -1417,6 +1417,9 @@ std::map OptimizerG2O::optimizeBA( g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1); EdgeSBACamGravity* priorEdge(new EdgeSBACamGravity()); + std::map::const_iterator iterModel = models.find(iter->first); + UASSERT(iterModel != models.end() && !iterModel->second.localTransform().isNull()); + priorEdge->setCameraInvLocalTransform(iterModel->second.localTransform().inverse().toEigen3d().linear()); priorEdge->setMeasurement(m); priorEdge->setInformation(information); priorEdge->vertices()[0] = v1; diff --git a/corelib/src/optimizer/g2o/edge_sbacam_gravity.h b/corelib/src/optimizer/g2o/edge_sbacam_gravity.h index fff9c795..d2fe24cd 100644 --- a/corelib/src/optimizer/g2o/edge_sbacam_gravity.h +++ b/corelib/src/optimizer/g2o/edge_sbacam_gravity.h @@ -48,6 +48,11 @@ class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix(_vertices[0]); @@ -57,7 +62,8 @@ class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrixestimate().rotation().toRotationMatrix(); + // Transform pose from camera frame to world frame + Eigen::Matrix3d t = v1->estimate().rotation().toRotationMatrix() * cameraInvLocalTransform_; ea[0] = atan2(t (2, 1), t (2, 2)); ea[1] = asin(-t (2, 0)); ea[2] = atan2(t (1, 0), t (0, 0)); @@ -69,10 +75,10 @@ class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrixid(), - // measurement[0], measurement[1], measurement[2], - // estimate[0], estimate[1], estimate[2], - // _error[0], _error[1], _error[2]); + /*printf("%d : measured=%f %f %f est=%f %f %f error=%f %f %f\n", v1->id(), + measurement[0], measurement[1], measurement[2], + estimate[0], estimate[1], estimate[2], + _error[0], _error[1], _error[2]);*/ } // 6 values: @@ -82,6 +88,9 @@ class EdgeSBACamGravity : public g2o::BaseUnaryEdge<3, Eigen::Matrix() = m.head<3>().normalized(); _measurement.tail<3>() = m.tail<3>().normalized(); } + + private: + Eigen::Matrix3d cameraInvLocalTransform_; }; } diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 98e64c71..f65daabd 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -1237,12 +1237,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->stereosgbm_mode->setObjectName(Parameters::kStereoSGBMMode().c_str()); // Aruco marker - _ui->ArucoDictionary->setObjectName(Parameters::kArucoDictionary().c_str()); - _ui->ArucoMarkerLength->setObjectName(Parameters::kArucoMarkerLength().c_str()); - _ui->ArucoMaxDepthError->setObjectName(Parameters::kArucoMaxDepthError().c_str()); - _ui->ArucoVarianceLinear->setObjectName(Parameters::kArucoVarianceLinear().c_str()); - _ui->ArucoVarianceAngular->setObjectName(Parameters::kArucoVarianceAngular().c_str()); - _ui->ArucoCornerRefinementMethod->setObjectName(Parameters::kArucoCornerRefinementMethod().c_str()); + _ui->ArucoDictionary->setObjectName(Parameters::kMarkerDictionary().c_str()); + _ui->ArucoMarkerLength->setObjectName(Parameters::kMarkerLength().c_str()); + _ui->ArucoMaxDepthError->setObjectName(Parameters::kMarkerMaxDepthError().c_str()); + _ui->ArucoVarianceLinear->setObjectName(Parameters::kMarkerVarianceLinear().c_str()); + _ui->ArucoVarianceAngular->setObjectName(Parameters::kMarkerVarianceAngular().c_str()); + _ui->ArucoCornerRefinementMethod->setObjectName(Parameters::kMarkerCornerRefinementMethod().c_str()); // reset default settings for the gui resetSettings(_ui->groupBox_generalSettingsGui0); diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index c29b8dec..05c4c7bd 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -94,7 +94,7 @@ 0 - -234 + 0 680 3082 @@ -126,7 +126,7 @@ QFrame::Raised - 5 + 14 diff --git a/package.xml b/package.xml index fe3bd5cc..8aee1c9d 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap - 0.19.2 + 0.19.3 RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe diff --git a/tools/EurocDataset/main.cpp b/tools/EurocDataset/main.cpp index d8ffb15a..919e7a81 100644 --- a/tools/EurocDataset/main.cpp +++ b/tools/EurocDataset/main.cpp @@ -41,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/utilite/UMath.h" #include "rtabmap/utilite/UStl.h" #include "rtabmap/utilite/UProcessInfo.h" +#include "rtabmap/core/IMUFilter.h" #include #include #include @@ -309,60 +310,6 @@ int main(int argc, char * argv[]) mapUpdate = 1; } - std::ifstream imu_file; - if(odomStrategy == Odometry::kTypeOkvis || odomStrategy == Odometry::kTypeMSCKF || odomStrategy == Odometry::kTypeVINS) - { - // open the IMU file - std::string line; - imu_file.open(pathImu.c_str()); - if (!imu_file.good()) { - UERROR("no imu file found at %s",pathImu.c_str()); - return -1; - } - int number_of_lines = 0; - while (std::getline(imu_file, line)) - ++number_of_lines; - printf("No. IMU measurements: %d\n", number_of_lines-1); - if (number_of_lines - 1 <= 0) { - UERROR("no imu messages present in %s", pathImu.c_str()); - return -1; - } - // set reading position to second line - imu_file.clear(); - imu_file.seekg(0, std::ios::beg); - std::getline(imu_file, line); - - if(odomStrategy == Odometry::kTypeMSCKF) - { - if(seq.compare("MH_01_easy") == 0) - { - printf("MH_01_easy detected with MSCFK odometry, ignoring first moving 440 images...\n"); - ((CameraStereoImages*)cameraThread.camera())->setStartIndex(440); - } - else if(seq.compare("MH_02_easy") == 0) - { - printf("MH_02_easy detected with MSCFK odometry, ignoring first moving 525 images...\n"); - ((CameraStereoImages*)cameraThread.camera())->setStartIndex(525); - } - else if(seq.compare("MH_03_medium") == 0) - { - printf("MH_03_medium detected with MSCFK odometry, ignoring first moving 210 images...\n"); - ((CameraStereoImages*)cameraThread.camera())->setStartIndex(210); - } - else if(seq.compare("MH_04_difficult") == 0) - { - printf("MH_04_difficult detected with MSCFK odometry, ignoring first moving 250 images...\n"); - ((CameraStereoImages*)cameraThread.camera())->setStartIndex(250); - } - else if(seq.compare("MH_05_difficult") == 0) - { - printf("MH_05_difficult detected with MSCFK odometry, ignoring first moving 310 images...\n"); - ((CameraStereoImages*)cameraThread.camera())->setStartIndex(310); - } - } - } - - std::string databasePath = output+"/"+outputName+".db"; UFile::erase(databasePath); if(cameraThread.camera()->init(output, outputName+"_calib")) @@ -374,6 +321,60 @@ int main(int argc, char * argv[]) ParametersMap odomParameters = parameters; odomParameters.erase(Parameters::kRtabmapPublishRAMUsage()); // as odometry is in the same process than rtabmap, don't get RAM usage in odometry. Odometry * odom = Odometry::create(odomParameters); + + std::ifstream imu_file; + if(odom->canProcessIMU()) + { + // open the IMU file + std::string line; + imu_file.open(pathImu.c_str()); + if (!imu_file.good()) { + UERROR("no imu file found at %s",pathImu.c_str()); + return -1; + } + int number_of_lines = 0; + while (std::getline(imu_file, line)) + ++number_of_lines; + printf("No. IMU measurements: %d\n", number_of_lines-1); + if (number_of_lines - 1 <= 0) { + UERROR("no imu messages present in %s", pathImu.c_str()); + return -1; + } + // set reading position to second line + imu_file.clear(); + imu_file.seekg(0, std::ios::beg); + std::getline(imu_file, line); + + if(odomStrategy == Odometry::kTypeMSCKF) + { + if(seq.compare("MH_01_easy") == 0) + { + printf("MH_01_easy detected with MSCFK odometry, ignoring first moving 440 images...\n"); + ((CameraStereoImages*)cameraThread.camera())->setStartIndex(440); + } + else if(seq.compare("MH_02_easy") == 0) + { + printf("MH_02_easy detected with MSCFK odometry, ignoring first moving 525 images...\n"); + ((CameraStereoImages*)cameraThread.camera())->setStartIndex(525); + } + else if(seq.compare("MH_03_medium") == 0) + { + printf("MH_03_medium detected with MSCFK odometry, ignoring first moving 210 images...\n"); + ((CameraStereoImages*)cameraThread.camera())->setStartIndex(210); + } + else if(seq.compare("MH_04_difficult") == 0) + { + printf("MH_04_difficult detected with MSCFK odometry, ignoring first moving 250 images...\n"); + ((CameraStereoImages*)cameraThread.camera())->setStartIndex(250); + } + else if(seq.compare("MH_05_difficult") == 0) + { + printf("MH_05_difficult detected with MSCFK odometry, ignoring first moving 310 images...\n"); + ((CameraStereoImages*)cameraThread.camera())->setStartIndex(310); + } + } + } + Rtabmap rtabmap; rtabmap.init(parameters, databasePath); @@ -394,7 +395,7 @@ int main(int argc, char * argv[]) while(data.isValid() && g_forever) { UDEBUG(""); - if(odomStrategy == Odometry::kTypeOkvis || odomStrategy == Odometry::kTypeMSCKF || odomStrategy == Odometry::kTypeVINS) + if(odom->canProcessIMU()) { // get all IMU measurements till then double t_imu = start; @@ -426,6 +427,7 @@ int main(int argc, char * argv[]) t_imu = double(uStr2Int(seconds)) + double(uStr2Int(nanoseconds))*1e-9; if (t_imu - start + 1 > 0) { + SensorData dataImu(IMU(gyr, cv::Mat(3,3,CV_64FC1), acc, cv::Mat(3,3,CV_64FC1), baseToImu), 0, t_imu); UDEBUG(""); odom->process(dataImu); @@ -443,14 +445,6 @@ int main(int argc, char * argv[]) UDEBUG(""); Transform pose = odom->process(data, &odomInfo); UDEBUG(""); - if((odomStrategy == Odometry::kTypeOkvis || odomStrategy == Odometry::kTypeMSCKF || odomStrategy == Odometry::kTypeVINS) && - pose.isNull()) - { - cameraInfo = CameraInfo(); - timer.restart(); - data = cameraThread.camera()->takeImage(&cameraInfo); - continue; - } if(odomInfo.keyFrameAdded) {