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;

guilib/src/MainWindow.cpp

@@ -5843,27 +5843,20 @@ void MainWindow::startDetection()
 					_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcImages) &&
 				   !_preferencesDialog->getIMUPath().isEmpty())
 				{
-					if( odomStrategy != Odometry::kTypeOkvis &&
+					_imuThread = new IMUThread(_preferencesDialog->getIMURate(), _preferencesDialog->getIMULocalTransform());
-						odomStrategy != Odometry::kTypeMSCKF &&
+					if(_preferencesDialog->getIMUFilteringStrategy()>0)
-						odomStrategy != Odometry::kTypeVINS &&
+					{
-						odomStrategy != Odometry::kTypeOpenVINS)
+						_imuThread->enableIMUFiltering(_preferencesDialog->getIMUFilteringStrategy()-1, parameters, _preferencesDialog->getIMUFilteringBaseFrameConversion());
+					}
+					if(!_imuThread->init(_preferencesDialog->getIMUPath().toStdString()))
 					{
 						QMessageBox::warning(this, tr("Source IMU Path"),
-								tr("IMU path is set but odometry chosen doesn't support asynchronous IMU, ignoring IMU..."), QMessageBox::Ok);
+							tr("Initialization of IMU data has failed! Path=%1.").arg(_preferencesDialog->getIMUPath()), QMessageBox::Ok);
-					}
+						delete _camera;
-					else
+						_camera = 0;
-					{
+						delete _imuThread;
-						_imuThread = new IMUThread(_preferencesDialog->getIMURate(), _preferencesDialog->getIMULocalTransform());
+						_imuThread = 0;
-						if(!_imuThread->init(_preferencesDialog->getIMUPath().toStdString()))
+						return;
-						{
-							QMessageBox::warning(this, tr("Source IMU Path"),
-								tr("Initialization of IMU data has failed! Path=%1.").arg(_preferencesDialog->getIMUPath()), QMessageBox::Ok);
-							delete _camera;
-							_camera = 0;
-							delete _imuThread;
-							_imuThread = 0;
-							return;
-						}
 					}
 				}
 				Odometry * odom = Odometry::create(odomParameters);

guilib/src/PreferencesDialog.cpp

@@ -6609,35 +6609,28 @@ void PreferencesDialog::testOdometry()
 		return;
 	}
 
+	ParametersMap parameters = this->getAllParameters();
 	IMUThread * imuThread = 0;
 	if((this->getSourceDriver() == kSrcStereoImages ||
 	   this->getSourceDriver() == kSrcRGBDImages ||
 	   this->getSourceDriver() == kSrcImages) &&
 	   !_ui->lineEdit_cameraImages_path_imu->text().isEmpty())
 	{
-		if(this->getOdomStrategy() != Odometry::kTypeOkvis &&
+		imuThread = new IMUThread(_ui->spinBox_cameraImages_max_imu_rate->value(), this->getIMULocalTransform());
-		   this->getOdomStrategy() != Odometry::kTypeMSCKF &&
+		if(getIMUFilteringStrategy()>0)
-		   this->getOdomStrategy() != Odometry::kTypeVINS &&
+		{
-		   this->getOdomStrategy() != Odometry::kTypeOpenVINS)
+			imuThread->enableIMUFiltering(getIMUFilteringStrategy()-1, parameters, getIMUFilteringBaseFrameConversion());
+		}
+		if(!imuThread->init(_ui->lineEdit_cameraImages_path_imu->text().toStdString()))
 		{
 			QMessageBox::warning(this, tr("Source IMU Path"),
-					tr("IMU path is set but odometry chosen doesn't support asynchronous IMU, ignoring IMU..."), QMessageBox::Ok);
+				tr("Initialization of IMU data has failed! Path=%1.").arg(_ui->lineEdit_cameraImages_path_imu->text()), QMessageBox::Ok);
-		}
+			delete camera;
-		else
+			delete imuThread;
-		{
+			return;
-			imuThread = new IMUThread(_ui->spinBox_cameraImages_max_imu_rate->value(), this->getIMULocalTransform());
-			if(!imuThread->init(_ui->lineEdit_cameraImages_path_imu->text().toStdString()))
-			{
-				QMessageBox::warning(this, tr("Source IMU Path"),
-					tr("Initialization of IMU data has failed! Path=%1.").arg(_ui->lineEdit_cameraImages_path_imu->text()), QMessageBox::Ok);
-				delete camera;
-				delete imuThread;
-				return;
-			}
 		}
 	}
 
-	ParametersMap parameters = this->getAllParameters();
 	if(getOdomRegistrationApproach() < 3)
 	{
 		uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), uNumber2Str(getOdomRegistrationApproach())));