0.16.3: Added OKVIS support (tested only on EuRoC dataset). Added IMU/IMUThread classes. Added OdomOKVIS/ConfigPath and Rtabmap/ImagesAlreadyRectified parameters. MainWindow, limited odom local feature map to maximum 50 meters from current pose (to avoid VTK glitching with near/far clipping plane).

corelib/include/rtabmap/core/IMU.h

@@ -0,0 +1,109 @@
+/*
+ * IMU.h
+ *
+ *  Created on: 2018-03-05
+ *      Author: mathieu
+ */
+
+#ifndef IMU_H_
+#define IMU_H_
+
+#include <opencv2/core.hpp>
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+
+// Correspondence class to sensor_msgs/IMU
+class IMU
+{
+public:
+	IMU() {}
+	IMU(const Eigen::Quaterniond & orientation,
+		const cv::Mat & orientationCovariance,
+		const Eigen::Vector3d & angularVelocity,
+		const cv::Mat & angularVelocityCovariance,
+		const Eigen::Vector3d & linearAcceleration,
+		const cv::Mat & linearAccelerationCovariance,
+		const Transform & localTransform = Transform::getIdentity()) :
+			orientation_(orientation),
+			orientationCovariance_(orientationCovariance),
+			angularVelocity_(angularVelocity),
+			angularVelocityCovariance_(angularVelocityCovariance),
+			linearAcceleration_(linearAcceleration),
+			linearAccelerationCovariance_(linearAccelerationCovariance),
+			localTransform_(localTransform)
+	{
+		UASSERT(!orientationCovariance.empty() && orientationCovariance.cols == 3 && orientationCovariance.rows == 3 && orientationCovariance.type() == CV_64FC1);
+		UASSERT(!angularVelocityCovariance.empty() && angularVelocityCovariance.cols == 3 && angularVelocityCovariance.rows == 3 && angularVelocityCovariance.type() == CV_64FC1);
+		UASSERT(!linearAccelerationCovariance.empty() && linearAccelerationCovariance.cols == 3 && linearAccelerationCovariance.rows == 3 && linearAccelerationCovariance.type() == CV_64FC1);
+	}
+	IMU(const Eigen::Vector3d & angularVelocity,
+		const cv::Mat & angularVelocityCovariance,
+		const Eigen::Vector3d & linearAcceleration,
+		const cv::Mat & linearAccelerationCovariance,
+		const Transform & localTransform = Transform::getIdentity()) :
+			angularVelocity_(angularVelocity),
+			angularVelocityCovariance_(angularVelocityCovariance),
+			linearAcceleration_(linearAcceleration),
+			linearAccelerationCovariance_(linearAccelerationCovariance),
+			localTransform_(localTransform)
+	{
+		UASSERT(!angularVelocityCovariance.empty() && angularVelocityCovariance.cols == 3 && angularVelocityCovariance.rows == 3 && angularVelocityCovariance.type() == CV_64FC1);
+		UASSERT(!linearAccelerationCovariance.empty() && linearAccelerationCovariance.cols == 3 && linearAccelerationCovariance.rows == 3 && linearAccelerationCovariance.type() == CV_64FC1);
+	}
+
+	const Eigen::Quaterniond & 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
+
+	const Eigen::Vector3d & angularVelocity() const {return angularVelocity_;}
+	const cv::Mat & angularVelocityCovariance() const {return angularVelocityCovariance_;} // 3x3 double Row major about x, y, z axes, empty if angularVelocity is not set
+
+	const Eigen::Vector3d linearAcceleration() const {return linearAcceleration_;}
+	const cv::Mat & linearAccelerationCovariance() const {return linearAccelerationCovariance_;} // 3x3 double Row major x, y z, empty if linearAcceleration is not set
+
+	const Transform & localTransform() const {return localTransform_;}
+
+	bool empty() const
+	{
+		return orientationCovariance_.empty() && angularVelocityCovariance_.empty() && linearAccelerationCovariance_.empty();
+	}
+
+
+private:
+	Eigen::Quaterniond orientation_;
+	cv::Mat orientationCovariance_; // 3x3 double Row major about x, y, z axes, empty if orientation is not set
+
+	Eigen::Vector3d angularVelocity_;
+	cv::Mat angularVelocityCovariance_; // 3x3 double Row major about x, y, z axes, empty if angularVelocity is not set
+
+	Eigen::Vector3d linearAcceleration_;
+	cv::Mat linearAccelerationCovariance_; // 3x3 double Row major x, y z, empty if linearAcceleration is not set
+
+	Transform localTransform_;
+};
+
+class IMUEvent : public UEvent
+{
+public:
+	IMUEvent() :
+		stamp_(0.0)
+	{}
+	IMUEvent(const IMU & data, double stamp) :
+		data_(data),
+		stamp_(stamp)
+	{
+	}
+	virtual std::string getClassName() const {return "IMUEvent";}
+	const IMU & getData() const {return data_;}
+	double getStamp() const {return stamp_;}
+
+private:
+	IMU data_;
+	double stamp_;
+};
+
+}
+
+
+#endif /* IMU_H_ */

corelib/include/rtabmap/core/IMUThread.h

@@ -0,0 +1,70 @@
+/*
+Copyright (c) 2010-2016, 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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#include <fstream>
+
+namespace rtabmap
+{
+
+/**
+ * Class IMUThread
+ *
+ */
+class RTABMAP_EXP IMUThread :
+	public UThread,
+	public UEventsSender
+{
+public:
+	IMUThread(int rate, const Transform & localTransform);
+	virtual ~IMUThread();
+
+	bool init(const std::string & path);
+	void setRate(int rate);
+
+private:
+	virtual void mainLoopBegin();
+	virtual void mainLoop();
+
+private:
+	int rate_;
+	Transform localTransform_;
+	std::ifstream imuFile_;
+	UTimer frameRateTimer_;
+	double captureDelay_;
+	double previousStamp_;
+};
+
+} // namespace rtabmap

corelib/include/rtabmap/core/Memory.h

@@ -297,6 +297,7 @@ private:
 	bool _createOccupancyGrid;
 	int _visMaxFeatures;
 	int _visCorType;
+	bool _imagesAlreadyRectified;
 
 	int _idCount;
 	int _idMapCount;

corelib/include/rtabmap/core/Odometry.h

@@ -49,7 +49,8 @@ public:
 		kTypeFovis = 2,
 		kTypeViso2 = 3,
 		kTypeDVO = 4,
-		kTypeORBSLAM2 = 5
+		kTypeORBSLAM2 = 5,
+		kTypeOkvis = 6
 	};
 
 public:
@@ -62,6 +63,7 @@ public:
 	Transform process(SensorData & data, const Transform & guess, OdometryInfo * info = 0);
 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
 	virtual Odometry::Type getType() = 0;
+	virtual bool canProcessRawImages() const {return false;}
 
 	//getters
 	const Transform & getPose() const {return _pose;}
@@ -69,6 +71,7 @@ public:
 	const Transform & previousVelocityTransform() const {return previousVelocityTransform_;}
 	double previousStamp() const {return previousStamp_;}
 	unsigned int framesProcessed() const {return framesProcessed_;}
+	bool imagesAlreadyRectified() const {return _imagesAlreadyRectified;}
 
 private:
 	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
@@ -94,6 +97,7 @@ private:
 	int _imageDecimation;
 	bool _alignWithGround;
 	bool _publishRAMUsage;
+	bool _imagesAlreadyRectified;
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;

corelib/include/rtabmap/core/OdometryOkvis.h

@@ -0,0 +1,64 @@
+/*
+Copyright (c) 2010-2016, 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 ODOMETRYOKVIS_H_
+#define ODOMETRYOKVIS_H_
+
+#include <rtabmap/core/Odometry.h>
+
+namespace okvis {
+	class ThreadedKFVio;
+}
+
+namespace rtabmap {
+
+class OkvisCallbackHandler;
+class RTABMAP_EXP OdometryOkvis : public Odometry
+{
+public:
+	OdometryOkvis(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryOkvis();
+
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() {return Odometry::kTypeOkvis;}
+	virtual bool canProcessRawImages() const {return true;}
+
+private:
+	virtual Transform computeTransform(SensorData & image, const Transform & guess = Transform(), OdometryInfo * info = 0);
+
+private:
+	std::string configFilename_;
+	OkvisCallbackHandler * okvisCallbackHandler_;
+	okvis::ThreadedKFVio * okvisEstimator_;
+	ParametersMap okvisParameters_;
+	IMU lastImu_; // only used for initialization
+	int imagesProcessed_;
+};
+
+}
+
+#endif /* ODOMETRYOKVIS_H_ */

corelib/include/rtabmap/core/OdometryThread.h

@@ -62,10 +62,13 @@ private:
 	USemaphore _dataAdded;
 	UMutex _dataMutex;
 	std::list<SensorData> _dataBuffer;
+	std::list<SensorData> _imuBuffer;
 	Odometry * _odometry;
 	unsigned int _dataBufferMaxSize;
 	bool _resetOdometry;
 	Transform _resetPose;
+	double _lastImuStamp;
+	double _imuEstimatedDelay;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Parameters.h

@@ -186,6 +186,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Rtabmap, StatisticLogged,              bool, false, "Logging enabled.");
     RTABMAP_PARAM(Rtabmap, StatisticLoggedHeaders,       bool, true,  "Add column header description to log files.");
     RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.");
+    RTABMAP_PARAM(Rtabmap, ImagesAlreadyRectified,       bool, true,  "Images are already rectified. By default RTAB-Map assumes that received images are rectified. If they are not, they can be rectified by RTAB-Map if this parameter is false.");
 
     // Hypotheses selection
     RTABMAP_PARAM(Rtabmap, LoopThr,           float, 0.11,      "Loop closing threshold.");
@@ -485,6 +486,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(OdomORBSLAM2, MaxFeatures,     int,    1000,  "Maximum ORB features extracted per frame.");
     RTABMAP_PARAM(OdomORBSLAM2, MapSize,         int,    3000,  "Maximum size of the feature map (0 means infinite).");
 
+    // Odometry OKVIS
+    RTABMAP_PARAM_STR(OdomOKVIS, ConfigPath,     "",  "Path of OKVIS config file.");
+
     // Common registration parameters
     RTABMAP_PARAM(Reg, RepeatOnce,               bool, true,    "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration (like on loop closure). It can be useful if the registration approach used can use a guess to get better matches.");
     RTABMAP_PARAM(Reg, Strategy,                 int, 0,        "0=Vis, 1=Icp, 2=VisIcp");

corelib/include/rtabmap/core/SensorData.h

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <rtabmap/core/LaserScan.h>
+#include <rtabmap/core/IMU.h>
 
 namespace rtabmap
 {
@@ -122,6 +123,12 @@ public:
 			double stamp = 0.0,
 			const cv::Mat & userData = cv::Mat());
 
+	// IMU constructor
+	SensorData(
+			const IMU & imu,
+			int id = 0,
+			double stamp = 0.0);
+
 	virtual ~SensorData();
 
 	bool isValid() const {
@@ -138,7 +145,8 @@ public:
 			_userDataRaw.empty() &&
 			_userDataCompressed.empty() &&
 			_keypoints.size() == 0 &&
-			_descriptors.empty());
+			_descriptors.empty() &&
+			imu_.empty());
 	}
 
 	int id() const {return _id;}
@@ -233,6 +241,12 @@ public:
 	}
 	const GPS & gps() const {return gps_;}
 
+	void setIMU(const IMU & imu)
+	{
+		imu_ = imu;
+	}
+	const IMU & imu() const {return imu_;}
+
 	long getMemoryUsed() const; // Return memory usage in Bytes
 	void clearCompressedData() {_imageCompressed=cv::Mat(); _depthOrRightCompressed=cv::Mat(); _laserScanCompressed.clear(); _userDataCompressed=cv::Mat();}
 
@@ -278,6 +292,8 @@ private:
 	cv::Mat globalPoseCovariance_; // 6x6 double
 
 	GPS gps_;
+
+	IMU imu_;
 };
 
 }

corelib/include/rtabmap/core/Statistics.h

@@ -132,6 +132,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Subpixel, ms);
 	RTABMAP_STATS(TimingMem, Stereo_correspondences, ms);
 	RTABMAP_STATS(TimingMem, Descriptors_extraction, ms);
+	RTABMAP_STATS(TimingMem, Rectification, ms);
 	RTABMAP_STATS(TimingMem, Keypoints_3D, ms);
 	RTABMAP_STATS(TimingMem, Joining_dictionary_update, ms);
 	RTABMAP_STATS(TimingMem, Add_new_words, ms);