Added IMU support for camera images source with odom approach not supporting async imu

corelib/include/rtabmap/core/IMUThread.h

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Parameters.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEventsSender.h>
 #include <rtabmap/utilite/UTimer.h>
@@ -39,6 +40,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
+class IMUFilter;
+
 /**
  * Class IMUThread
  *
@@ -53,6 +56,8 @@ public:
 
 	bool init(const std::string & path);
 	void setRate(int rate);
+	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
+	void disableIMUFiltering();
 
 private:
 	virtual void mainLoopBegin();
@@ -65,6 +70,8 @@ private:
 	UTimer frameRateTimer_;
 	double captureDelay_;
 	double previousStamp_;
+	IMUFilter * _imuFilter;
+	bool _imuBaseFrameConversion;
 };
 
 } // namespace rtabmap

corelib/src/IMUThread.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/IMUThread.h"
 #include "rtabmap/core/IMU.h"
+#include "rtabmap/core/IMUFilter.h"
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -38,13 +39,16 @@ IMUThread::IMUThread(int rate, const Transform & localTransform) :
 		rate_(rate),
 		localTransform_(localTransform),
 		captureDelay_(0.0),
-		previousStamp_(0.0)
+		previousStamp_(0.0),
+		_imuFilter(0),
+		_imuBaseFrameConversion(false)
 {
 }
 
 IMUThread::~IMUThread()
 {
 	imuFile_.close();
+	delete _imuFilter;
 }
 
 bool IMUThread::init(const std::string & path)
@@ -81,6 +85,19 @@ void IMUThread::setRate(int rate)
 	rate_ = rate;
 }
 
+void IMUThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion)
+{
+	delete _imuFilter;
+	_imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters);
+	_imuBaseFrameConversion = baseFrameConversion;
+}
+
+void IMUThread::disableIMUFiltering()
+{
+	delete _imuFilter;
+	_imuFilter = 0;
+}
+
 void IMUThread::mainLoopBegin()
 {
 	ULogger::registerCurrentThread("IMU");
@@ -141,6 +158,60 @@ void IMUThread::mainLoop()
 		previousStamp_ = stamp;
 
 		IMU imu(gyr, cv::Mat(), acc, cv::Mat(), localTransform_);
+
+		// IMU filtering
+		if(_imuFilter && !imu.empty())
+		{
+			if(imu.angularVelocity()[0] == 0 &&
+			   imu.angularVelocity()[1] == 0 &&
+			   imu.angularVelocity()[2] == 0 &&
+			   imu.linearAcceleration()[0] == 0 &&
+			   imu.linearAcceleration()[1] == 0 &&
+			   imu.linearAcceleration()[2] == 0)
+			{
+				UWARN("IMU's acc and gyr values are null! Please disable IMU filtering.");
+			}
+			else
+			{
+				// Transform IMU data in base_link to correctly initialize yaw
+				if(_imuBaseFrameConversion)
+				{
+					UASSERT(!imu.localTransform().isNull());
+					imu.convertToBaseFrame();
+
+				}
+				_imuFilter->update(
+						imu.angularVelocity()[0],
+						imu.angularVelocity()[1],
+						imu.angularVelocity()[2],
+						imu.linearAcceleration()[0],
+						imu.linearAcceleration()[1],
+						imu.linearAcceleration()[2],
+						stamp);
+				double qx,qy,qz,qw;
+				_imuFilter->getOrientation(qx,qy,qz,qw);
+
+				imu = IMU(
+						cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1),
+						imu.angularVelocity(), imu.angularVelocityCovariance(),
+						imu.linearAcceleration(), imu.linearAccelerationCovariance(),
+						imu.localTransform());
+
+				UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)",
+							imu.orientation()[0],
+							imu.orientation()[1],
+							imu.orientation()[2],
+							imu.orientation()[3],
+							imu.angularVelocity()[0],
+							imu.angularVelocity()[1],
+							imu.angularVelocity()[2],
+							imu.linearAcceleration()[0],
+							imu.linearAcceleration()[1],
+							imu.linearAcceleration()[2],
+							stamp);
+			}
+		}
+
 		this->post(new IMUEvent(imu, stamp));
 	}
 	else if(!this->isKilled())

corelib/src/Odometry.cpp

@@ -324,6 +324,10 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 				imus_.erase(imus_.begin());
 			}
 		}
+		else
+		{
+			UWARN("Received IMU doesn't have orientation set! It is ignored.");
+		}
 	}
 
 

corelib/src/odometry/OdometryF2M.cpp

@@ -214,6 +214,11 @@ Transform OdometryF2M::computeTransform(
 	if(sba_ && sba_->gravitySigma() > 0.0f && !imus().empty())
 	{
 		imuT = Transform::getTransform(imus(), data.stamp());
+		if(data.imu().empty())
+		{
+			Eigen::Quaternionf q = imuT.getQuaternionf();
+			data.setIMU(IMU(cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat(), cv::Vec3d(), cv::Mat(), cv::Vec3d(), cv::Mat()));
+		}
 	}
 
 	RegistrationInfo regInfo;