UI: Update preferences with the new imu filter parameters. Camera: added imu filtering option. Updated support for zedm, D435i and T265.

corelib/include/rtabmap/core/CameraThread.h

@@ -45,6 +45,7 @@ class Camera;
 class CameraInfo;
 class SensorData;
 class StereoDense;
+class IMUFilter;
 
 /**
  * Class CameraThread
@@ -68,6 +69,8 @@ public:
 	void setDistortionModel(const std::string & path);
 	void enableBilateralFiltering(float sigmaS, float sigmaR);
 	void disableBilateralFiltering() {_bilateralFiltering = false;}
+	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap());
+	void disableIMUFiltering();
 
 	void setScanParameters(
 			bool fromDepth,
@@ -122,6 +125,7 @@ private:
 	bool _bilateralFiltering;
 	float _bilateralSigmaS;
 	float _bilateralSigmaR;
+	IMUFilter * _imuFilter;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Odometry.h

@@ -38,7 +38,6 @@ namespace rtabmap {
 
 class OdometryInfo;
 class ParticleFilter;
-class IMUFilter;
 
 class RTABMAP_EXP Odometry
 {
@@ -92,7 +91,6 @@ private:
 	bool _holonomic;
 	bool guessFromMotion_;
 	bool guessSmoothingDelay_;
-	int _imuFilteringStrategy;
 	int _filteringStrategy;
 	int _particleSize;
 	float _particleNoiseT;
@@ -117,7 +115,6 @@ private:
 
 	std::vector<ParticleFilter *> particleFilters_;
 	cv::KalmanFilter kalmanFilter_;
-	IMUFilter * imuFilter_;
 
 protected:
 	Odometry(const rtabmap::ParametersMap & parameters);

corelib/include/rtabmap/core/Parameters.h

@@ -409,7 +409,6 @@ 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, 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).");
@@ -732,18 +731,6 @@ class RTABMAP_EXP Parameters
     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();
 

corelib/include/rtabmap/core/camera/CameraRealSense2.h

@@ -82,7 +82,7 @@ public:
 	void pose_callback(rs2::frame frame);
 	void frame_callback(rs2::frame frame);
 	void multiple_message_callback(rs2::frame frame);
-	bool getPoseAndIMU(
+	void getPoseAndIMU(
 			const double & stamp,
 			Transform & pose,
 			unsigned int & poseConfidence,
@@ -110,6 +110,8 @@ private:
 	std::map<double, cv::Vec3f> accBuffer_;
 	std::map<double, cv::Vec3f> gyroBuffer_;
 	std::map<double, std::pair<Transform, unsigned int> > poseBuffer_; // <stamp, <Pose, confidence: 1=lost, 2=low, 3=high> >
+	UMutex poseMutex_;
+	UMutex imuMutex_;
 
 	bool emitterEnabled_;
 	bool irDepth_;

corelib/src/CameraThread.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_filtering.h"
 #include "rtabmap/core/StereoDense.h"
 #include "rtabmap/core/DBReader.h"
+#include "rtabmap/core/IMUFilter.h"
 #include "rtabmap/core/clams/discrete_depth_distortion_model.h"
 #include <opencv2/stitching/detail/exposure_compensate.hpp>
 #include <rtabmap/utilite/UTimer.h>
@@ -65,7 +66,8 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_distortionModel(0),
 		_bilateralFiltering(false),
 		_bilateralSigmaS(10),
-		_bilateralSigmaR(0.1)
+		_bilateralSigmaR(0.1),
+		_imuFilter(0)
 {
 	UASSERT(_camera != 0);
 }
@@ -77,6 +79,7 @@ CameraThread::~CameraThread()
 	delete _camera;
 	delete _distortionModel;
 	delete _stereoDense;
+	delete _imuFilter;
 }
 
 void CameraThread::setImageRate(float imageRate)
@@ -115,6 +118,18 @@ void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
 	_bilateralSigmaR = sigmaR;
 }
 
+void CameraThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters)
+{
+	delete _imuFilter;
+	_imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters);
+}
+
+void CameraThread::disableIMUFiltering()
+{
+	delete _imuFilter;
+	_imuFilter = 0;
+}
+
 void CameraThread::mainLoopBegin()
 {
 	ULogger::registerCurrentThread("Camera");
@@ -386,6 +401,38 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 		// filter the scan after registration
 		data.setLaserScan(util3d::commonFiltering(data.laserScanRaw(), _scanDownsampleStep, _scanRangeMin, _scanRangeMax, _scanVoxelSize, _scanNormalsK, _scanNormalsRadius, _scanForceGroundNormalsUp));
 	}
+
+	// IMU filtering
+	if(_imuFilter && !data.imu().empty())
+	{
+		_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()));
+		UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)",
+					data.imu().orientation()[0],
+					data.imu().orientation()[1],
+					data.imu().orientation()[2],
+					data.imu().orientation()[3],
+					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());
+	}
 }
 
 } // namespace rtabmap

corelib/src/Odometry.cpp

@@ -37,7 +37,6 @@ 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"
@@ -111,7 +110,6 @@ 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()),
@@ -129,8 +127,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_resetCurrentCount(0),
 		previousStamp_(0),
 		distanceTravelled_(0),
-		framesProcessed_(0),
+		framesProcessed_(0)
-		imuFilter_(0)
 {
 	Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
 
@@ -139,7 +136,6 @@ 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);
@@ -182,11 +178,6 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	{
 		initKalmanFilter();
 	}
-
-	if(_imuFilteringStrategy > 0)
-	{
-		imuFilter_ = IMUFilter::create((IMUFilter::Type)(_imuFilteringStrategy-1), parameters);
-	}
 }
 
 Odometry::~Odometry()
@@ -196,7 +187,6 @@ Odometry::~Odometry()
 		delete particleFilters_[i];
 	}
 	particleFilters_.clear();
-	delete imuFilter_;
 }
 
 void Odometry::reset(const Transform & initialPose)
@@ -251,10 +241,6 @@ void Odometry::reset(const Transform & initialPose)
 	{
 		_pose = initialPose;
 	}
-	if(imuFilter_)
-	{
-		imuFilter_->reset();
-	}
 }
 
 const Transform & Odometry::previousVelocityTransform() const
@@ -367,28 +353,6 @@ 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();
@@ -406,7 +370,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		previousVelocities_.clear();
 		velocityGuess_.setNull();
 	}
-
 	if(!velocityGuess_.isNull())
 	{
 		if(guessFromMotion_)

corelib/src/camera/CameraRealSense2.cpp

@@ -67,7 +67,7 @@ CameraRealSense2::CameraRealSense2(
 	emitterEnabled_(true),
 	irDepth_(false),
 	rectifyImages_(true),
-	odometryProvided_(true)
+	odometryProvided_(false)
 #endif
 {
 	UDEBUG("");
@@ -173,12 +173,15 @@ void CameraRealSense2::imu_callback(rs2::frame frame)
 {
 	auto stream = frame.get_profile().stream_type();
 	cv::Vec3f crnt_reading = *reinterpret_cast<const cv::Vec3f*>(frame.get_data());
+	UDEBUG("%s callback! %f (%f %f %f)",
+			stream == RS2_STREAM_GYRO?"GYRO":"ACC",
+			frame.get_timestamp(),
+			crnt_reading[0],
+			crnt_reading[1],
+			crnt_reading[2]);
+	UScopeMutex sm(imuMutex_);
 	if(stream == RS2_STREAM_GYRO)
 	{
-		UDEBUG("GYRO callback! %f (%f %f %f)", frame.get_timestamp(),
-				crnt_reading[0],
-				crnt_reading[1],
-				crnt_reading[2]);
 		gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(frame.get_timestamp(), crnt_reading));
 		if(gyroBuffer_.size() > 10)
 		{
@@ -187,18 +190,12 @@ void CameraRealSense2::imu_callback(rs2::frame frame)
 	}
 	else
 	{
-
-		UDEBUG("ACC callback! %f (%f %f %f)", frame.get_timestamp(),
-				crnt_reading[0],
-				crnt_reading[1],
-				crnt_reading[2]);
 		accBuffer_.insert(accBuffer_.end(), std::make_pair(frame.get_timestamp(), crnt_reading));
 		if(accBuffer_.size() > 10)
 		{
 			accBuffer_.erase(accBuffer_.begin());
 		}
 	}
-
 }
 
 Transform CameraRealSense2::realsense2PoseRotation_ = Transform(
@@ -220,6 +217,8 @@ void CameraRealSense2::pose_callback(rs2::frame frame)
 			pose.rotation.w);
 	poseT = realsense2PoseRotation_ * poseT * realsense2PoseRotationInv_;
 	UDEBUG("POSE callback! %f %s (confidence=%d)", frame.get_timestamp(), poseT.prettyPrint().c_str(), (int)pose.tracker_confidence);
+
+	UScopeMutex sm(poseMutex_);
 	poseBuffer_.insert(poseBuffer_.end(), std::make_pair(frame.get_timestamp(), std::make_pair(poseT, pose.tracker_confidence)));
 	if(poseBuffer_.size() > 10)
 	{
@@ -242,14 +241,17 @@ void CameraRealSense2::multiple_message_callback(rs2::frame frame)
             imu_callback(frame);
             break;
         case RS2_STREAM_POSE:
-            pose_callback(frame);
+        	if(odometryProvided_)
+        	{
+        		pose_callback(frame);
+        	}
             break;
         default:
             frame_callback(frame);
     }
 }
 
-bool CameraRealSense2::getPoseAndIMU(
+void CameraRealSense2::getPoseAndIMU(
 		const double & stamp,
 		Transform & pose,
 		unsigned int & poseConfidence,
@@ -258,109 +260,166 @@ bool CameraRealSense2::getPoseAndIMU(
 	pose.setNull();
 	imu = IMU();
 	poseConfidence = 0;
-	if(poseBuffer_.empty() || accBuffer_.empty() || gyroBuffer_.empty())
+	if(accBuffer_.empty() || gyroBuffer_.empty())
 	{
-		return false;
+		return;
 	}
 
+	int maxWaitTime = 30;
+
 	// Interpolate pose
+	if(!poseBuffer_.empty())
 	{
-		std::map<double, std::pair<Transform, unsigned int> >::const_iterator iterB = poseBuffer_.lower_bound(stamp);
+		poseMutex_.lock();
-		std::map<double, std::pair<Transform, unsigned int> >::const_iterator iterA = iterB;
+		int waitTry = 0;
-		if(iterA != poseBuffer_.begin())
+		while(poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTime)
 		{
-			iterA = --iterA;
+			poseMutex_.unlock();
+			++waitTry;
+			uSleep(1);
+			poseMutex_.lock();
 		}
-		if(iterB == poseBuffer_.end())
+		if(poseBuffer_.rbegin()->first < stamp)
 		{
-			iterB = --iterB;
+			UWARN("Could not find poses to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTime, poseBuffer_.rbegin()->first);
-		}
-		if(iterA == iterB && stamp == iterA->first)
-		{
-			pose = iterA->second.first;
-			poseConfidence = iterA->second.second;
-		}
-		else if(stamp >= iterA->first && stamp <= iterB->first)
-		{
-			pose = iterA->second.first.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second.first);
-			poseConfidence = iterA->second.second;
 		}
 		else
 		{
-			UWARN("Could not find poses to interpolate at time %f", stamp);
+			std::map<double, std::pair<Transform, unsigned int> >::const_iterator iterB = poseBuffer_.lower_bound(stamp);
-			return false;
+			std::map<double, std::pair<Transform, unsigned int> >::const_iterator iterA = iterB;
+			if(iterA != poseBuffer_.begin())
+			{
+				iterA = --iterA;
+			}
+			if(iterB == poseBuffer_.end())
+			{
+				iterB = --iterB;
+			}
+			if(iterA == iterB && stamp == iterA->first)
+			{
+				pose = iterA->second.first;
+				poseConfidence = iterA->second.second;
+			}
+			else if(stamp >= iterA->first && stamp <= iterB->first)
+			{
+				pose = iterA->second.first.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second.first);
+				poseConfidence = iterA->second.second;
+			}
+			else
+			{
+				UWARN("Could not find poses to interpolate at time %f", stamp);
+			}
 		}
+		poseMutex_.unlock();
 	}
 
 	// Interpolate acc
 	cv::Vec3d acc;
 	{
-		std::map<double, cv::Vec3f>::const_iterator iterB = accBuffer_.lower_bound(stamp);
+		imuMutex_.lock();
-		std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
+		int waitTry = 0;
-		if(iterA != accBuffer_.begin())
+		while(accBuffer_.rbegin()->first < stamp && waitTry < maxWaitTime)
 		{
-			iterA = --iterA;
+			imuMutex_.unlock();
+			++waitTry;
+			uSleep(1);
+			imuMutex_.lock();
 		}
-		if(iterB == accBuffer_.end())
+		if(accBuffer_.rbegin()->first < stamp)
 		{
-			iterB = --iterB;
+			UWARN("Could not find acc data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTime, accBuffer_.rbegin()->first);
-		}
+			imuMutex_.unlock();
-		if(iterA == iterB && stamp == iterA->first)
+			return;
-		{
-			acc[0] = iterA->second[0];
-			acc[1] = iterA->second[1];
-			acc[2] = iterA->second[2];
-		}
-		else if(stamp >= iterA->first && stamp <= iterB->first)
-		{
-			float t = (stamp-iterA->first) / (iterB->first-iterA->first);
-			acc[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
-			acc[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
-			acc[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
 		}
 		else
 		{
-			UWARN("Could not find acc data to interpolate at time %f", stamp);
+			std::map<double, cv::Vec3f>::const_iterator iterB = accBuffer_.lower_bound(stamp);
-			return false;
+			std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
+			if(iterA != accBuffer_.begin())
+			{
+				iterA = --iterA;
+			}
+			if(iterB == accBuffer_.end())
+			{
+				iterB = --iterB;
+			}
+			if(iterA == iterB && stamp == iterA->first)
+			{
+				acc[0] = iterA->second[0];
+				acc[1] = iterA->second[1];
+				acc[2] = iterA->second[2];
+			}
+			else if(stamp >= iterA->first && stamp <= iterB->first)
+			{
+				float t = (stamp-iterA->first) / (iterB->first-iterA->first);
+				acc[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
+				acc[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
+				acc[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
+			}
+			else
+			{
+				UWARN("Could not find acc data to interpolate at time %f", stamp);
+				imuMutex_.unlock();
+				return;
+			}
 		}
+		imuMutex_.unlock();
 	}
 
 	// Interpolate gyro
 	cv::Vec3d gyro;
 	{
-		std::map<double, cv::Vec3f>::const_iterator iterB = gyroBuffer_.lower_bound(stamp);
+		imuMutex_.lock();
-		std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
+		int waitTry = 0;
-		if(iterA != gyroBuffer_.begin())
+		while(gyroBuffer_.rbegin()->first < stamp && waitTry < maxWaitTime)
 		{
-			iterA = --iterA;
+			imuMutex_.unlock();
+			++waitTry;
+			uSleep(1);
+			imuMutex_.lock();
 		}
-		if(iterB == gyroBuffer_.end())
+		if(gyroBuffer_.rbegin()->first < stamp)
 		{
-			iterB = --iterB;
+			UWARN("Could not find gyro data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTime, gyroBuffer_.rbegin()->first);
-		}
+			imuMutex_.unlock();
-		if(iterA == iterB && stamp == iterA->first)
+			return;
-		{
-			gyro[0] = iterA->second[0];
-			gyro[1] = iterA->second[1];
-			gyro[2] = iterA->second[2];
-		}
-		else if(stamp >= iterA->first && stamp <= iterB->first)
-		{
-			float t = (stamp-iterA->first) / (iterB->first-iterA->first);
-			gyro[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
-			gyro[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
-			gyro[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
 		}
 		else
 		{
-			UWARN("Could not find gyro data to interpolate at time %f", stamp);
+			std::map<double, cv::Vec3f>::const_iterator iterB = gyroBuffer_.lower_bound(stamp);
-			return false;
+			std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
+			if(iterA != gyroBuffer_.begin())
+			{
+				iterA = --iterA;
+			}
+			if(iterB == gyroBuffer_.end())
+			{
+				iterB = --iterB;
+			}
+			if(iterA == iterB && stamp == iterA->first)
+			{
+				gyro[0] = iterA->second[0];
+				gyro[1] = iterA->second[1];
+				gyro[2] = iterA->second[2];
+			}
+			else if(stamp >= iterA->first && stamp <= iterB->first)
+			{
+				float t = (stamp-iterA->first) / (iterB->first-iterA->first);
+				gyro[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
+				gyro[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
+				gyro[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
+			}
+			else
+			{
+				UWARN("Could not find gyro data to interpolate at time %f", stamp);
+				imuMutex_.unlock();
+				return;
+			}
 		}
+		imuMutex_.unlock();
 	}
 
 	imu = IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_);
-
-	return true;
 }
 #endif
 
@@ -485,18 +544,25 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		auto profiles = sensors[i].get_stream_profiles();
 		bool added = false;
 		UINFO("profiles=%d", (int)profiles.size());
+		if(ULogger::level()>=ULogger::kInfo)
+		{
+			for (auto& profile : profiles)
+			{
+				auto video_profile = profile.as<rs2::video_stream_profile>();
+				UINFO("%s %d %d %d", rs2_format_to_string(
+						video_profile.format()),
+						video_profile.width(),
+						video_profile.height(),
+						video_profile.fps());
+			}
+		}
 		int pi = 0;
 		for (auto& profile : profiles)
 		{
 			auto video_profile = profile.as<rs2::video_stream_profile>();
-			UINFO("%s %d %d %d", rs2_format_to_string(
-					video_profile.format()),
-					video_profile.width(),
-					video_profile.height(),
-					video_profile.fps());
-
 			if(!stereo)
 			{
+				//D400 series:
 				if (video_profile.format() == (i==1?RS2_FORMAT_Z16:irDepth_?RS2_FORMAT_Y8:RS2_FORMAT_RGB8) &&
 					video_profile.width()  == 640 &&
 					video_profile.height() == 480 &&
@@ -522,11 +588,18 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 				}
 				else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F)
 				{
+					//D435i:
+					//MOTION_XYZ32F 0 0 200
+					//MOTION_XYZ32F 0 0 400
+					//MOTION_XYZ32F 0 0 63
+					//MOTION_XYZ32F 0 0 250
 					profilesPerSensor[i].push_back(profile);
+					added = true;
 				}
 			}
 			else if(stereo)
 			{
+				//T265:
 				if(video_profile.format() == RS2_FORMAT_Y8 &&
 					video_profile.width()  == 848 &&
 					video_profile.height() == 800 &&
@@ -551,11 +624,11 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 					}
 					added = true;
 				}
-				//MOTION_XYZ32F 0 0 200
-				//MOTION_XYZ32F 0 0 62
-				//6DOF 0 0 200
 				else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
 				{
+					//MOTION_XYZ32F 0 0 200
+					//MOTION_XYZ32F 0 0 62
+					//6DOF 0 0 200
 					profilesPerSensor[0].push_back(profile);
 					added = true;
 				}
@@ -579,28 +652,16 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		 }
 		 *depthToRGBExtrinsics_ = depthStreamProfile.get_extrinsics_to(rgbStreamProfile);
 
-		 std::function<void(rs2::frame)> imu_callback_function = [this](rs2::frame frame){imu_callback(frame);};
+		 if(profilesPerSensor.size() == 3 && !profilesPerSensor[2].empty() && !profilesPerSensor[0].empty())
-
-		 for (unsigned int i=0; i<sensors.size(); ++i)
 		 {
-			 if(profilesPerSensor[i].size())
+			 rs2_extrinsics leftToIMU = profilesPerSensor[0][0].get_extrinsics_to(profilesPerSensor[2][0]);
-			 {
+			 Transform leftToIMUT(
-				 UINFO("Starting sensor %d with %d profiles", (int)i, (int)profilesPerSensor[i].size());
+					 leftToIMU.rotation[0], leftToIMU.rotation[1], leftToIMU.rotation[2], leftToIMU.translation[0],
-				 sensors[i].open(profilesPerSensor[i]);
+					 leftToIMU.rotation[3], leftToIMU.rotation[4], leftToIMU.rotation[5], leftToIMU.translation[1],
-				 if(sensors[i].is<rs2::depth_sensor>())
+					 leftToIMU.rotation[6], leftToIMU.rotation[7], leftToIMU.rotation[8], leftToIMU.translation[2]);
-				 {
+			 UINFO("leftToIMU = %s", leftToIMUT.prettyPrint().c_str());
-					 auto depth_sensor = sensors[i].as<rs2::depth_sensor>();
+			 imuLocalTransform_ = this->getLocalTransform() * leftToIMUT;
-					 depth_scale_meters_ = depth_sensor.get_depth_scale();
+			 UINFO("imu local transform = %s", imuLocalTransform_.prettyPrint().c_str());
-				 }
-				 if(i == 2) // 2 is ACC/GYRO
-				 {
-					 sensors[i].start(imu_callback_function);
-				 }
-				 else
-				 {
-					 sensors[i].start(*syncer_);
-				 }
-			 }
 		 }
 	 }
 	 else
@@ -636,43 +697,73 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		// 3=ACC
 		// 4=POSE
 		UASSERT(profilesPerSensor[0].size() == 5);
-		rs2_extrinsics poseToLeft = profilesPerSensor[0][4].get_extrinsics_to(profilesPerSensor[0][1]);
+		if(odometryProvided_)
-		rs2_extrinsics poseToIMU = profilesPerSensor[0][4].get_extrinsics_to(profilesPerSensor[0][2]);
+		{
-		Transform realsense2_pose_rotation(0, 0,-1,0,
+			rs2_extrinsics poseToLeft = profilesPerSensor[0][4].get_extrinsics_to(profilesPerSensor[0][1]);
-							-1, 0, 0,0,
+			rs2_extrinsics poseToIMU = profilesPerSensor[0][4].get_extrinsics_to(profilesPerSensor[0][2]);
-							 0, 1, 0,0);
+			Transform realsense2_pose_rotation(0, 0,-1,0,
-		Transform poseToLeftT(
+								-1, 0, 0,0,
-				poseToLeft.rotation[0], poseToLeft.rotation[1], poseToLeft.rotation[2], poseToLeft.translation[0],
+								 0, 1, 0,0);
-				poseToLeft.rotation[3], poseToLeft.rotation[4], poseToLeft.rotation[5], poseToLeft.translation[1],
+			Transform poseToLeftT(
-				poseToLeft.rotation[6], poseToLeft.rotation[7], poseToLeft.rotation[8], poseToLeft.translation[2]);
+					poseToLeft.rotation[0], poseToLeft.rotation[1], poseToLeft.rotation[2], poseToLeft.translation[0],
-		poseToLeftT = realsense2PoseRotation_ * poseToLeftT;
+					poseToLeft.rotation[3], poseToLeft.rotation[4], poseToLeft.rotation[5], poseToLeft.translation[1],
-		UINFO("poseToLeft = %s", poseToLeftT.prettyPrint().c_str());
+					poseToLeft.rotation[6], poseToLeft.rotation[7], poseToLeft.rotation[8], poseToLeft.translation[2]);
+			poseToLeftT = realsense2PoseRotation_ * poseToLeftT;
+			UINFO("poseToLeft = %s", poseToLeftT.prettyPrint().c_str());
 
-		Transform poseToIMUT(
+			Transform poseToIMUT(
-				poseToIMU.rotation[0], poseToIMU.rotation[1], poseToIMU.rotation[2], poseToIMU.translation[0],
+					poseToIMU.rotation[0], poseToIMU.rotation[1], poseToIMU.rotation[2], poseToIMU.translation[0],
-				poseToIMU.rotation[3], poseToIMU.rotation[4], poseToIMU.rotation[5], poseToIMU.translation[1],
+					poseToIMU.rotation[3], poseToIMU.rotation[4], poseToIMU.rotation[5], poseToIMU.translation[1],
-				poseToIMU.rotation[6], poseToIMU.rotation[7], poseToIMU.rotation[8], poseToIMU.translation[2]);
+					poseToIMU.rotation[6], poseToIMU.rotation[7], poseToIMU.rotation[8], poseToIMU.translation[2]);
-		poseToIMUT = realsense2PoseRotation_ * poseToIMUT;
+			poseToIMUT = realsense2PoseRotation_ * poseToIMUT;
-		UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str());
+			UINFO("poseToIMU = %s", poseToIMUT.prettyPrint().c_str());
 
-		stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
+			if(this->getLocalTransform().rotation().r13() == 1.0f &&
-		imuLocalTransform_ = poseToIMUT;
+			   this->getLocalTransform().rotation().r21() == -1.0f &&
+			   this->getLocalTransform().rotation().r32() == -1.0f)
+			{
+				UWARN("Detected optical rotation in local transform, removing it for convenience to match realsense2 poses.");
+				Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
+				this->setLocalTransform(this->getLocalTransform()*opticalTransform.inverse());
+			}
+
+			stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
+			imuLocalTransform_ = poseToIMUT;
+		}
+		else
+		{
+			// Set imu transform based on the left camera instead of pose
+			 rs2_extrinsics leftToIMU = profilesPerSensor[0][1].get_extrinsics_to(profilesPerSensor[0][2]);
+			 Transform leftToIMUT(
+					 leftToIMU.rotation[0], leftToIMU.rotation[1], leftToIMU.rotation[2], leftToIMU.translation[0],
+					 leftToIMU.rotation[3], leftToIMU.rotation[4], leftToIMU.rotation[5], leftToIMU.translation[1],
+					 leftToIMU.rotation[6], leftToIMU.rotation[7], leftToIMU.rotation[8], leftToIMU.translation[2]);
+			 UINFO("leftToIMU = %s", leftToIMUT.prettyPrint().c_str());
+			 imuLocalTransform_ = this->getLocalTransform() * leftToIMUT;
+			 UINFO("imu local transform = %s", imuLocalTransform_.prettyPrint().c_str());
+			 stereoModel_.setLocalTransform(this->getLocalTransform());
+		}
 		if(rectifyImages_ && !stereoModel_.isValidForRectification())
 		{
 			UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
 			return false;
 		}
+	 }
 
-		std::function<void(rs2::frame)> multiple_message_callback_function = [this](rs2::frame frame){multiple_message_callback(frame);};
+	 std::function<void(rs2::frame)> multiple_message_callback_function = [this](rs2::frame frame){multiple_message_callback(frame);};
 
-		 for (unsigned int i=0; i<sensors.size(); ++i)
+	 for (unsigned int i=0; i<sensors.size(); ++i)
+	 {
+		 if(profilesPerSensor[i].size())
 		 {
-			 if(profilesPerSensor[i].size())
+			 UINFO("Starting sensor %d with %d profiles", (int)i, (int)profilesPerSensor[i].size());
+			 sensors[i].open(profilesPerSensor[i]);
+			 if(sensors[i].is<rs2::depth_sensor>())
 			 {
-				 UINFO("Starting sensor %d with %d profiles", (int)i, (int)profilesPerSensor[i].size());
+				 auto depth_sensor = sensors[i].as<rs2::depth_sensor>();
-				 sensors[i].open(profilesPerSensor[i]);
+				 depth_scale_meters_ = depth_sensor.get_depth_scale();
-				 sensors[i].start(multiple_message_callback_function);
 			 }
+			 sensors[i].start(multiple_message_callback_function);
 		 }
 	 }
 
@@ -844,26 +935,26 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 				}
 
 				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), stamp);
-
-				IMU imu;
-				unsigned int confidence = 0;
-				getPoseAndIMU(frameset.get_timestamp(), info->odomPose, confidence, imu);
-
-				if(!info->odomPose.isNull())
-				{
-					info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * 0.0001;
-					info->odomCovariance.rowRange(0,3) *= pow(10, 3-(int)confidence);
-					info->odomCovariance.rowRange(3,6) *= pow(10, 1-(int)confidence);
-				}
-				if(!imu.empty())
-				{
-					data.setIMU(imu);
-				}
 			}
 			else
 			{
 				UERROR("Not received depth and rgb");
 			}
+
+			IMU imu;
+			unsigned int confidence = 0;
+			getPoseAndIMU(frameset.get_timestamp(), info->odomPose, confidence, imu);
+
+			if(odometryProvided_ && !info->odomPose.isNull())
+			{
+				info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * 0.0001;
+				info->odomCovariance.rowRange(0,3) *= pow(10, 3-(int)confidence);
+				info->odomCovariance.rowRange(3,6) *= pow(10, 1-(int)confidence);
+			}
+			if(!imu.empty())
+			{
+				data.setIMU(imu);
+			}
 		}
 		else
 		{

corelib/src/camera/CameraStereoZed.cpp

@@ -163,11 +163,10 @@ IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform
 	Transform orientationT(0,0,0, orientation.ox, orientation.oy, orientation.oz, orientation.ow);
 	orientationT = opticalTransform * orientationT;
 
-	Eigen::Matrix4d opticalTransform4d = opticalTransform.toEigen4d();
+	static double deg2rad = 0.017453293;
-	Eigen::Vector4d accT = opticalTransform4d * Eigen::Vector4d(imuData.linear_acceleration.v[0], imuData.linear_acceleration.v[1], imuData.linear_acceleration.v[2], 1);
+	Eigen::Vector4d accT = 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);
+	Eigen::Vector4d gyrT = Eigen::Vector4d(imuData.angular_velocity.v[0]*deg2rad, imuData.angular_velocity.v[1]*deg2rad, imuData.angular_velocity.v[2]*deg2rad, 1);
 
-	// FIXME covariance should be rotated too: see https://robotics.stackexchange.com/questions/2556/how-to-rotate-covariance
 	cv::Mat orientationCov = (cv::Mat_<double>(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],
@@ -182,6 +181,7 @@ IMU zedIMUtoIMU(const sl::IMUData & imuData, const Transform & imuLocalTransform
 			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,

corelib/src/imufilter/MadgwickFilter.cpp

@@ -312,7 +312,6 @@ void MadgwickFilter::updateImpl(
 	  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;
   }

corelib/src/odometry/OdometryF2M.cpp

@@ -195,7 +195,7 @@ Transform OdometryF2M::computeTransform(
 		info->type = 0;
 	}
 
-	if(!data.imu().empty())
+	if(sba_ && sba_->gravitySigma() > 0.0f && !data.imu().empty())
 	{
 		if(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0)
 		{
@@ -1332,14 +1332,16 @@ Transform OdometryF2M::getClosestIMU(const double & stamp, double & stampDiff) c
 		if(fabs(imuIterA->first - lastFrame_->getStamp()) <
 		   fabs(imuIterB->first - lastFrame_->getStamp()))
 		{
-			imuT = imuIterA->second;
+			//imuT = imuIterA->second;
 			stampDiff = fabs(imuIterA->first - lastFrame_->getStamp());
 		}
 		else
 		{
-			imuT = imuIterB->second;
+			//imuT = imuIterB->second;
 			stampDiff = fabs(imuIterB->first - lastFrame_->getStamp());
 		}
+		//interpolate:
+		imuT = imuIterA->second.interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);
 	}
 	return imuT;
 }

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -102,7 +102,7 @@ public:
 		kSrcStereoZed      = 104,
 		kSrcStereoUsb      = 105,
 		kSrcStereoTara 	   = 106,
-		kSrcRealSense2Stereo = 107,
+		kSrcStereoRealSense2 = 107,
 
 		kSrcRGB            = 200,
 		kSrcUsbDevice      = 200,
@@ -243,6 +243,7 @@ public:
 
 	bool isSourceDatabaseStampsUsed() const;
 	bool isSourceRGBDColorOnly() const;
+	int getIMUFilteringStrategy() const;
 	bool isDepthFilteringAvailable() const;
 	QString getSourceDistortionModel() const;
 	bool isBilateralFiltering() const;

guilib/src/MainWindow.cpp

@@ -4454,7 +4454,7 @@ void MainWindow::updateSelectSourceMenu()
 	_ui->actionStereoZed->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoZed);
     _ui->actionStereoTara->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoTara);
 	_ui->actionStereoUsb->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoUsb);
-	_ui->actionRealSense2_T265->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2Stereo);
+	_ui->actionRealSense2_T265->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcStereoRealSense2);
 }
 
 void MainWindow::changeImgRateSetting()
@@ -4973,6 +4973,10 @@ void MainWindow::startDetection()
 			_preferencesDialog->getSourceScanNormalsK(),
 			_preferencesDialog->getSourceScanNormalsRadius(),
 			_preferencesDialog->isSourceScanForceGroundNormalsUp());
+	if(_preferencesDialog->getIMUFilteringStrategy()>0)
+	{
+		_camera->enableIMUFiltering(_preferencesDialog->getIMUFilteringStrategy()-1, parameters);
+	}
 	if(_preferencesDialog->isDepthFilteringAvailable())
 	{
 		if(_preferencesDialog->isBilateralFiltering())
@@ -6192,7 +6196,7 @@ void MainWindow::selectRealSense2()
 
 void MainWindow::selectRealSense2Stereo()
 {
-	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcRealSense2Stereo);
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoRealSense2);
 }
 
 void MainWindow::selectStereoDC1394()

guilib/src/PreferencesDialog.cpp

@@ -303,7 +303,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	if (!CameraRealSense2::available())
 	{
 		_ui->comboBox_cameraRGBD->setItemData(kSrcRealSense2 - kSrcRGBD, 0, Qt::UserRole - 1);
-		_ui->comboBox_cameraRGBD->setItemData(kSrcRealSense2Stereo - kSrcStereo, 0, Qt::UserRole - 1);
+		_ui->comboBox_cameraRGBD->setItemData(kSrcStereoRealSense2 - kSrcStereo, 0, Qt::UserRole - 1);
 	}
 	if(!CameraStereoDC1394::available())
 	{
@@ -342,6 +342,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->label_markerDetection->setText(_ui->label_markerDetection->text()+" This option works only if OpenCV has been built with \"aruco\" module.");
 #endif
 
+#ifndef RTABMAP_MADGWICK
+	_ui->comboBox_imuFilter_strategy->setItemData(1, 0, Qt::UserRole - 1);
+#endif
+
 	// in case we change the ui, we should not forget to change stuff related to this parameter
 	UASSERT(_ui->odom_registration->count() == 4);
 
@@ -350,6 +354,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
 	connect(_ui->source_comboBox_image_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
 	connect(_ui->comboBox_cameraStereo, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
+	connect(_ui->comboBox_imuFilter_strategy, SIGNAL(currentIndexChanged(int)), this, SLOT(updateSourceGrpVisibility()));
 	this->resetSettings(_ui->groupBox_source0);
 
 	_ui->predictionPlot->showLegend(false);
@@ -668,6 +673,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->toolButton_zedSvoPath, SIGNAL(clicked()), this, SLOT(selectSourceSvoPath()));
 	connect(_ui->lineEdit_zedSvoPath, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 
+	connect(_ui->checkbox_stereoRealSense2_odom, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+
 	connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -685,6 +692,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_bilateral_sigmaS, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_bilateral_sigmaR, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 
+	connect(_ui->comboBox_imuFilter_strategy, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_imuFilter_strategy, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_imuFilter, SLOT(setCurrentIndex(int)));
+	_ui->stackedWidget_imuFilter->setCurrentIndex(_ui->comboBox_imuFilter_strategy->currentIndex());
+
 	connect(_ui->checkBox_source_scanFromDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_source_scanDownsampleStep, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_source_scanRangeMin, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1245,6 +1256,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->ArucoVarianceAngular->setObjectName(Parameters::kMarkerVarianceAngular().c_str());
 	_ui->ArucoCornerRefinementMethod->setObjectName(Parameters::kMarkerCornerRefinementMethod().c_str());
 
+	// IMU filter
+	_ui->doubleSpinBox_imuFilterMadgwickGain->setObjectName(Parameters::kImuFilterMadgwickGain().c_str());
+	_ui->doubleSpinBox_imuFilterMadgwickZeta->setObjectName(Parameters::kImuFilterMadgwickZeta().c_str());
+	_ui->doubleSpinBox_imuFilterComplementaryGainAcc->setObjectName(Parameters::kImuFilterComplementaryGainAcc().c_str());
+	_ui->doubleSpinBox_imuFilterComplementaryBiasAlpha->setObjectName(Parameters::kImuFilterComplementaryBiasAlpha().c_str());
+	_ui->checkBox_imuFilterComplementaryDoAdaptiveGain->setObjectName(Parameters::kImuFilterComplementaryDoAdpativeGain().c_str());
+	_ui->checkBox_imuFilterComplementaryDoBiasEstimation->setObjectName(Parameters::kImuFilterComplementaryDoBiasEstimation().c_str());
+
 	// reset default settings for the gui
 	resetSettings(_ui->groupBox_generalSettingsGui0);
 	resetSettings(_ui->groupBox_cloudRendering1);
@@ -1772,6 +1791,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->spinBox_stereoZed_confidenceThr->setValue(100);
 		_ui->checkbox_stereoZed_odom->setChecked(false);
 		_ui->lineEdit_zedSvoPath->clear();
+		_ui->checkbox_stereoRealSense2_odom->setChecked(false);
 
 		_ui->checkBox_cameraImages_timestamps->setChecked(false);
 		_ui->checkBox_cameraImages_syncTimeStamps->setChecked(true);
@@ -2184,6 +2204,9 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->lineEdit_zedSvoPath->setText(settings.value("svo_path", _ui->lineEdit_zedSvoPath->text()).toString());
 	settings.endGroup(); // StereoZed
 	
+	settings.beginGroup("StereoRealSense2");
+	_ui->checkbox_stereoRealSense2_odom->setChecked(settings.value("odom", _ui->checkbox_stereoRealSense2_odom->isChecked()).toBool());
+	settings.endGroup(); // StereoRealSense2
 
 	settings.beginGroup("Images");
 	_ui->source_images_lineEdit_path->setText(settings.value("path", _ui->source_images_lineEdit_path->text()).toString());
@@ -2613,7 +2636,9 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("svo_path", _ui->lineEdit_zedSvoPath->text());
 	settings.endGroup(); // StereoZed
 
-	
+	settings.beginGroup("StereoRealSense2");
+	settings.setValue("odom", _ui->checkbox_stereoRealSense2_odom->isChecked());
+	settings.endGroup(); // StereoRealSense2
 
 	settings.beginGroup("Images");
 	settings.setValue("path", 			_ui->source_images_lineEdit_path->text());
@@ -4336,7 +4361,7 @@ void PreferencesDialog::updateStereoDisparityVisibility()
               _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoTara - kSrcStereo ||
 			 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoVideo - kSrcStereo ||
 			 _ui->comboBox_cameraStereo->currentIndex() == kSrcDC1394 - kSrcStereo ||
-			 _ui->comboBox_cameraStereo->currentIndex() == kSrcRealSense2Stereo - kSrcStereo);
+			 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoRealSense2 - kSrcStereo);
 	_ui->checkBox_stereo_rectify->setVisible(_ui->checkBox_stereo_rectify->isEnabled());
 	_ui->label_stereo_rectify->setVisible(_ui->checkBox_stereo_rectify->isEnabled());
 }
@@ -4490,12 +4515,13 @@ void PreferencesDialog::updateSourceGrpVisibility()
 		(_ui->comboBox_cameraStereo->currentIndex() == kSrcStereoVideo-kSrcStereo || 
 		 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo ||
 		 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZed - kSrcStereo ||
-          _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoTara - kSrcStereo ||
+		 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoUsb - kSrcStereo ||
-		 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoUsb - kSrcStereo));
+		 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoRealSense2 - kSrcStereo));
 	_ui->groupBox_cameraStereoImages->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo);
 	_ui->groupBox_cameraStereoVideo->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoVideo - kSrcStereo);
 	_ui->groupBox_cameraStereoUsb->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoUsb - kSrcStereo);
 	_ui->groupBox_cameraStereoZed->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZed - kSrcStereo);
+	_ui->groupBox_cameraStereoRealSense2->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoRealSense2 - kSrcStereo);
 
 	_ui->stackedWidget_image->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && (_ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB || _ui->source_comboBox_image_type->currentIndex() == kSrcVideo-kSrcRGB));
 	_ui->source_groupBox_images->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB);
@@ -4511,6 +4537,18 @@ void PreferencesDialog::updateSourceGrpVisibility()
 				_ui->comboBox_sourceType->currentIndex() == 3);  // Database
 	_ui->groupBox_depthImageFiltering->setVisible(_ui->groupBox_depthImageFiltering->isEnabled());
 
+	_ui->groupBox_imuFiltering->setEnabled(
+			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD) ||
+			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo) ||
+			(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB) ||
+			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRealSense2 - kSrcRGBD) || //D435i
+			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoRealSense2 - kSrcStereo) || //T265
+			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZed - kSrcStereo)); // ZEDm
+	_ui->stackedWidget_imuFilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() > 0);
+	_ui->groupBox_madgwickfilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() == 1);
+	_ui->groupBox_complementaryfilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() == 2);
+	_ui->groupBox_imuFiltering->setVisible(_ui->groupBox_imuFiltering->isEnabled());
+
 	//_ui->groupBox_scan->setVisible(_ui->comboBox_sourceType->currentIndex() != 3);
 
 	_ui->groupBox_depthFromScan->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB);
@@ -5039,6 +5077,10 @@ bool PreferencesDialog::isSourceRGBDColorOnly() const
 {
 	return _ui->checkbox_rgbd_colorOnly->isChecked();
 }
+int PreferencesDialog::getIMUFilteringStrategy() const
+{
+	return _ui->comboBox_imuFilter_strategy->currentIndex();
+}
 bool PreferencesDialog::isDepthFilteringAvailable() const
 {
 	return _ui->groupBox_depthImageFiltering->isEnabled();
@@ -5204,7 +5246,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 			((CameraRealSense*)camera)->setRGBSource((CameraRealSense::RGBSource)_ui->comboBox_realsenseRGBSource->currentIndex());
 		}
 	}
-	else if (driver == kSrcRealSense2 || driver == kSrcRealSense2Stereo)
+	else if (driver == kSrcRealSense2 || driver == kSrcStereoRealSense2)
 	{
 		if(driver == kSrcRealSense2 && useRawImages)
 		{
@@ -5219,9 +5261,16 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				this->getSourceDevice().toStdString(),
 				this->getGeneralInputRate(),
 				this->getSourceLocalTransform());
-			((CameraRealSense2*)camera)->setEmitterEnabled(_ui->checkbox_rs2_emitter->isChecked());
+			if(driver == kSrcStereoRealSense2)
-			((CameraRealSense2*)camera)->setIRDepthFormat(_ui->checkbox_rs2_irDepth->isChecked());
+			{
-			((CameraRealSense2*)camera)->setImagesRectified(_ui->checkBox_stereo_rectify->isChecked() && !useRawImages);
+				((CameraRealSense2*)camera)->setImagesRectified(_ui->checkBox_stereo_rectify->isChecked() && !useRawImages);
+				((CameraRealSense2*)camera)->setOdomProvided(_ui->checkbox_stereoRealSense2_odom->isChecked());
+			}
+			else
+			{
+				((CameraRealSense2*)camera)->setEmitterEnabled(_ui->checkbox_rs2_emitter->isChecked());
+				((CameraRealSense2*)camera)->setIRDepthFormat(_ui->checkbox_rs2_irDepth->isChecked());
+			}
 		}
 	}
 	else if(driver == kSrcRGBDImages)
@@ -5709,6 +5758,10 @@ void PreferencesDialog::testOdometry()
 			_ui->spinBox_source_scanNormalsK->value(),
 			_ui->doubleSpinBox_source_scanNormalsRadius->value(),
 			_ui->checkBox_source_scanForceGroundNormalsUp->isChecked());
+	if(_ui->comboBox_imuFilter_strategy->currentIndex()>0)
+	{
+		cameraThread.enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters());
+	}
 	if(isDepthFilteringAvailable())
 	{
 		if(_ui->groupBox_bilateral->isChecked())
@@ -5776,6 +5829,10 @@ void PreferencesDialog::testCamera()
 				_ui->spinBox_source_scanNormalsK->value(),
 				_ui->doubleSpinBox_source_scanNormalsRadius->value(),
 				_ui->checkBox_source_scanForceGroundNormalsUp->isChecked());
+		if(_ui->comboBox_imuFilter_strategy->currentIndex()>0)
+		{
+			cameraThread.enableIMUFiltering(_ui->comboBox_imuFilter_strategy->currentIndex()-1, this->getAllParameters());
+		}
 		if(isDepthFilteringAvailable())
 		{
 			if(_ui->groupBox_bilateral->isChecked())
@@ -6002,7 +6059,7 @@ void PreferencesDialog::calibrate()
 		}
 
 		bool freenect2 = driver == kSrcFreenect2;
-		bool fisheye = driver == kSrcRealSense2Stereo;
+		bool fisheye = driver == kSrcStereoRealSense2;
 		_calibrationDialog->setStereoMode(this->getSourceType() != kSrcRGB && driver != kSrcRealSense, freenect2?"rgb":"left", freenect2?"depth":"right"); // RGB+Depth or left+right
 		_calibrationDialog->setSwitchedImages(freenect2);
 		_calibrationDialog->setFisheyeImages(fisheye);

guilib/src/ui/preferencesDialog.ui

@@ -24,37 +24,6 @@
     <normaloff>:/images/RTAB-Map.ico</normaloff>:/images/RTAB-Map.ico</iconset>
   </property>
   <layout class="QGridLayout" name="gridLayout_49">
-   <item row="1" column="0">
-    <layout class="QHBoxLayout" name="horizontalLayout_3">
-     <item>
-      <widget class="QRadioButton" name="radioButton_basic">
-       <property name="text">
-        <string>Basic</string>
-       </property>
-       <property name="checked">
-        <bool>true</bool>
-       </property>
-      </widget>
-     </item>
-     <item>
-      <widget class="QRadioButton" name="radioButton_advanced">
-       <property name="text">
-        <string>Advanced</string>
-       </property>
-      </widget>
-     </item>
-     <item>
-      <widget class="QDialogButtonBox" name="buttonBox_global">
-       <property name="orientation">
-        <enum>Qt::Horizontal</enum>
-       </property>
-       <property name="standardButtons">
-        <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
-       </property>
-      </widget>
-     </item>
-    </layout>
-   </item>
    <item row="0" column="0">
     <layout class="QHBoxLayout" name="horizontalLayout_2" stretch="0,1">
      <property name="spacing">
@@ -94,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-359</y>
+            <y>-1977</y>
             <width>680</width>
             <height>3082</height>
            </rect>
@@ -2731,7 +2700,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                    <property name="title">
                     <string>Source</string>
                    </property>
-                   <layout class="QVBoxLayout" name="verticalLayout_7" stretch="0,0,0,0,1">
+                   <layout class="QVBoxLayout" name="verticalLayout_7" stretch="0,0,0,0,0,1">
                     <item>
                      <layout class="QGridLayout" name="gridLayout_28" columnstretch="0,1">
                       <item row="1" column="1">
@@ -3122,7 +3091,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_rgbd">
                              <property name="currentIndex">
-                              <number>9</number>
+                              <number>6</number>
                              </property>
                              <widget class="QWidget" name="page_32">
                               <layout class="QVBoxLayout" name="verticalLayout_63">
@@ -4075,7 +4044,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               </layout>
                              </widget>
                              <widget class="QWidget" name="page_55">
-                              <layout class="QVBoxLayout" name="verticalLayout_103">
+                              <layout class="QVBoxLayout" name="verticalLayout_103" stretch="0,1">
                                <item>
                                 <widget class="QGroupBox" name="groupBox_realsense2">
                                  <property name="sizePolicy">
@@ -4152,7 +4121,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <property name="sizeHint" stdset="0">
                                   <size>
                                    <width>20</width>
-                                   <height>338</height>
+                                   <height>0</height>
                                   </size>
                                  </property>
                                 </spacer>
@@ -4320,7 +4289,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_stereo">
                              <property name="currentIndex">
-                              <number>7</number>
+                              <number>5</number>
                              </property>
                              <widget class="QWidget" name="page_49">
                               <layout class="QVBoxLayout" name="verticalLayout_91">
@@ -4868,6 +4837,41 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              </widget>
                              <widget class="QWidget" name="page_80">
                               <layout class="QVBoxLayout" name="verticalLayout_140">
+                               <item>
+                                <widget class="QGroupBox" name="groupBox_cameraStereoRealSense2">
+                                 <property name="sizePolicy">
+                                  <sizepolicy hsizetype="Ignored" vsizetype="Ignored">
+                                   <horstretch>0</horstretch>
+                                   <verstretch>0</verstretch>
+                                  </sizepolicy>
+                                 </property>
+                                 <property name="title">
+                                  <string>RealSense2</string>
+                                 </property>
+                                 <layout class="QGridLayout" name="gridLayout_108" columnstretch="0,1">
+                                  <item row="0" column="0">
+                                   <widget class="QCheckBox" name="checkbox_stereoRealSense2_odom">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="0" column="1">
+                                   <widget class="QLabel" name="label_546">
+                                    <property name="text">
+                                     <string>Use RealSense2 visual inertial odometry.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                 </layout>
+                                </widget>
+                               </item>
                                <item>
                                 <spacer name="verticalSpacer_78">
                                  <property name="orientation">
@@ -4876,7 +4880,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <property name="sizeHint" stdset="0">
                                   <size>
                                    <width>20</width>
-                                   <height>471</height>
+                                   <height>0</height>
                                   </size>
                                  </property>
                                 </spacer>
@@ -6034,6 +6038,307 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                       </layout>
                      </widget>
                     </item>
+                    <item>
+                     <widget class="QGroupBox" name="groupBox_imuFiltering">
+                      <property name="title">
+                       <string>IMU Filtering</string>
+                      </property>
+                      <layout class="QVBoxLayout" name="verticalLayout_141">
+                       <item>
+                        <layout class="QGridLayout" name="gridLayout_109" columnstretch="0,1">
+                         <item row="0" column="0">
+                          <widget class="QComboBox" name="comboBox_imuFilter_strategy">
+                           <item>
+                            <property name="text">
+                             <string>None</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>Madgwick Filter</string>
+                            </property>
+                           </item>
+                           <item>
+                            <property name="text">
+                             <string>Complementary Filter</string>
+                            </property>
+                           </item>
+                          </widget>
+                         </item>
+                         <item row="0" column="1">
+                          <widget class="QLabel" name="label_541">
+                           <property name="text">
+                            <string>IMU filtering strategy used to estimate the orientation.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                        </layout>
+                       </item>
+                       <item>
+                        <widget class="QStackedWidget" name="stackedWidget_imuFilter">
+                         <property name="currentIndex">
+                          <number>2</number>
+                         </property>
+                         <widget class="QWidget" name="page_83">
+                          <layout class="QVBoxLayout" name="verticalLayout_144">
+                           <item>
+                            <spacer name="verticalSpacer_80">
+                             <property name="orientation">
+                              <enum>Qt::Vertical</enum>
+                             </property>
+                             <property name="sizeHint" stdset="0">
+                              <size>
+                               <width>20</width>
+                               <height>0</height>
+                              </size>
+                             </property>
+                            </spacer>
+                           </item>
+                          </layout>
+                         </widget>
+                         <widget class="QWidget" name="page_81">
+                          <layout class="QVBoxLayout" name="verticalLayout_142">
+                           <item>
+                            <widget class="QGroupBox" name="groupBox_madgwickfilter">
+                             <property name="title">
+                              <string>Madgwick Filter</string>
+                             </property>
+                             <layout class="QGridLayout" name="gridLayout_110" columnstretch="0,1">
+                              <item row="0" column="0">
+                               <widget class="QDoubleSpinBox" name="doubleSpinBox_imuFilterMadgwickGain">
+                                <property name="suffix">
+                                 <string/>
+                                </property>
+                                <property name="decimals">
+                                 <number>4</number>
+                                </property>
+                                <property name="minimum">
+                                 <double>0.000000000000000</double>
+                                </property>
+                                <property name="maximum">
+                                 <double>1.000000000000000</double>
+                                </property>
+                                <property name="singleStep">
+                                 <double>0.010000000000000</double>
+                                </property>
+                                <property name="value">
+                                 <double>0.010000000000000</double>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="0" column="1">
+                               <widget class="QLabel" name="label_542">
+                                <property name="text">
+                                 <string>Gain.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="1" column="0">
+                               <widget class="QDoubleSpinBox" name="doubleSpinBox_imuFilterMadgwickZeta">
+                                <property name="suffix">
+                                 <string/>
+                                </property>
+                                <property name="decimals">
+                                 <number>4</number>
+                                </property>
+                                <property name="minimum">
+                                 <double>0.000000000000000</double>
+                                </property>
+                                <property name="maximum">
+                                 <double>1.000000000000000</double>
+                                </property>
+                                <property name="singleStep">
+                                 <double>0.010000000000000</double>
+                                </property>
+                                <property name="value">
+                                 <double>0.010000000000000</double>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="1" column="1">
+                               <widget class="QLabel" name="label_543">
+                                <property name="text">
+                                 <string>Zeta.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                             </layout>
+                            </widget>
+                           </item>
+                           <item>
+                            <spacer name="verticalSpacer_79">
+                             <property name="orientation">
+                              <enum>Qt::Vertical</enum>
+                             </property>
+                             <property name="sizeHint" stdset="0">
+                              <size>
+                               <width>20</width>
+                               <height>0</height>
+                              </size>
+                             </property>
+                            </spacer>
+                           </item>
+                          </layout>
+                         </widget>
+                         <widget class="QWidget" name="page_82">
+                          <layout class="QVBoxLayout" name="verticalLayout_143">
+                           <item>
+                            <widget class="QGroupBox" name="groupBox_complementaryfilter">
+                             <property name="title">
+                              <string>Complementary Filter</string>
+                             </property>
+                             <layout class="QGridLayout" name="gridLayout_111" columnstretch="0,1">
+                              <item row="2" column="1">
+                               <widget class="QLabel" name="label_547">
+                                <property name="text">
+                                 <string>Do bias estimation.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="0" column="1">
+                               <widget class="QLabel" name="label_544">
+                                <property name="text">
+                                 <string>Gain acc.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="2" column="0">
+                               <widget class="QCheckBox" name="checkBox_imuFilterComplementaryDoBiasEstimation">
+                                <property name="text">
+                                 <string/>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="0" column="0">
+                               <widget class="QDoubleSpinBox" name="doubleSpinBox_imuFilterComplementaryGainAcc">
+                                <property name="suffix">
+                                 <string/>
+                                </property>
+                                <property name="decimals">
+                                 <number>4</number>
+                                </property>
+                                <property name="minimum">
+                                 <double>0.000000000000000</double>
+                                </property>
+                                <property name="maximum">
+                                 <double>1.000000000000000</double>
+                                </property>
+                                <property name="singleStep">
+                                 <double>0.010000000000000</double>
+                                </property>
+                                <property name="value">
+                                 <double>0.010000000000000</double>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="1" column="1">
+                               <widget class="QLabel" name="label_545">
+                                <property name="text">
+                                 <string>Bias alpha.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="1" column="0">
+                               <widget class="QDoubleSpinBox" name="doubleSpinBox_imuFilterComplementaryBiasAlpha">
+                                <property name="suffix">
+                                 <string/>
+                                </property>
+                                <property name="decimals">
+                                 <number>4</number>
+                                </property>
+                                <property name="minimum">
+                                 <double>0.000000000000000</double>
+                                </property>
+                                <property name="maximum">
+                                 <double>1.000000000000000</double>
+                                </property>
+                                <property name="singleStep">
+                                 <double>0.010000000000000</double>
+                                </property>
+                                <property name="value">
+                                 <double>0.010000000000000</double>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="3" column="0">
+                               <widget class="QCheckBox" name="checkBox_imuFilterComplementaryDoAdaptiveGain">
+                                <property name="text">
+                                 <string/>
+                                </property>
+                               </widget>
+                              </item>
+                              <item row="3" column="1">
+                               <widget class="QLabel" name="label_548">
+                                <property name="text">
+                                 <string>Do adaptive gain.</string>
+                                </property>
+                                <property name="wordWrap">
+                                 <bool>true</bool>
+                                </property>
+                                <property name="textInteractionFlags">
+                                 <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                </property>
+                               </widget>
+                              </item>
+                             </layout>
+                            </widget>
+                           </item>
+                           <item>
+                            <spacer name="verticalSpacer_81">
+                             <property name="orientation">
+                              <enum>Qt::Vertical</enum>
+                             </property>
+                             <property name="sizeHint" stdset="0">
+                              <size>
+                               <width>20</width>
+                               <height>0</height>
+                              </size>
+                             </property>
+                            </spacer>
+                           </item>
+                          </layout>
+                         </widget>
+                        </widget>
+                       </item>
+                      </layout>
+                     </widget>
+                    </item>
                     <item>
                      <widget class="QGroupBox" name="groupBox_scan">
                       <property name="title">
@@ -20132,6 +20437,37 @@ Lower the ratio -&gt; higher the precision.</string>
      </item>
     </layout>
    </item>
+   <item row="1" column="0">
+    <layout class="QHBoxLayout" name="horizontalLayout_3">
+     <item>
+      <widget class="QRadioButton" name="radioButton_basic">
+       <property name="text">
+        <string>Basic</string>
+       </property>
+       <property name="checked">
+        <bool>true</bool>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QRadioButton" name="radioButton_advanced">
+       <property name="text">
+        <string>Advanced</string>
+       </property>
+      </widget>
+     </item>
+     <item>
+      <widget class="QDialogButtonBox" name="buttonBox_global">
+       <property name="orientation">
+        <enum>Qt::Horizontal</enum>
+       </property>
+       <property name="standardButtons">
+        <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
+       </property>
+      </widget>
+     </item>
+    </layout>
+   </item>
   </layout>
  </widget>
  <customwidgets>

tools/EurocDataset/main.cpp

@@ -63,6 +63,7 @@ void showUsage()
 			"  --exposure_comp    Do exposure compensation between left and right images.\n"
 			"  --disp             Generate full disparity.\n"
 			"  --raw              Use raw images (not rectified, this only works with okvis, msckf or vins odometry).\n"
+			"  --imu #            IMU filter: 0=madgwick, 1=complementary (default).\n"
 			"%s\n"
 			"Example:\n\n"
 			"   $ rtabmap-euroc_dataset \\\n"
@@ -100,6 +101,7 @@ int main(int argc, char * argv[])
 	bool raw = false;
 	bool exposureCompensation = false;
 	bool quiet = false;
+	int imuFilter = 1;
 	if(argc < 2)
 	{
 		showUsage();
@@ -128,6 +130,10 @@ int main(int argc, char * argv[])
 			{
 				raw = true;
 			}
+			else if(std::strcmp(argv[i], "--imu") == 0)
+			{
+				imuFilter = atoi(argv[++i]);
+			}
 			else if(std::strcmp(argv[i], "--exposure_comp") == 0)
 			{
 				exposureCompensation = true;
@@ -191,6 +197,7 @@ int main(int argc, char * argv[])
 	if(!pathImu.empty())
 	{
 		printf("   IMU:              %s\n", pathImu.c_str());
+		printf("   IMU Filter:       %d\n", imuFilter);
 	}
 
 	printf("   Exposure Compensation: %s\n", exposureCompensation?"true":"false");
@@ -373,6 +380,8 @@ int main(int argc, char * argv[])
 					((CameraStereoImages*)cameraThread.camera())->setStartIndex(310);
 				}
 			}
+
+			cameraThread.enableIMUFiltering(imuFilter, parameters);
 		}
 
 		Rtabmap rtabmap;
@@ -429,6 +438,7 @@ int main(int argc, char * argv[])
 					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);
+						cameraThread.postUpdate(&dataImu);
 						UDEBUG("");
 						odom->process(dataImu);
 						UDEBUG("");