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).

2026-09-02 17:40:23 +08:00 · 2018-03-07 19:43:30 -05:00
parent 2fad881202
commit 4969ece356
30 changed files with 1700 additions and 102 deletions
--- a/corelib/src/CMakeLists.txt
+++ b/corelib/src/CMakeLists.txt
@@ -66,8 +66,11 @@ SET(SRC_FILES
 	OdometryFovis.cpp
 	OdometryViso2.cpp
 	OdometryDVO.cpp
+	OdometryOkvis.cpp
 	OdometryORBSLAM2.cpp
 	
+	IMUThread.cpp
+	
 	Stereo.cpp
 	StereoDense.cpp
 	StereoCameraModel.cpp
@@ -345,6 +348,23 @@ IF(dvo_core_FOUND)
 	)
 ENDIF(dvo_core_FOUND)

+IF(okvis_FOUND)
+    SET(INCLUDE_DIRS
+		${OKVIS_INCLUDE_DIRS}
+		${BRISK_INCLUDE_DIRS}
+		${OPENGV_INCLUDE_DIRS}
+		${CERES_INCLUDE_DIRS}
+		${INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${OKVIS_LIBRARIES}
+		${BRISK_LIBRARIES}
+		${OPENGV_LIBRARIES}
+		${CERES_LIBRARIES}
+		${LIBRARIES}
+	)
+ENDIF(okvis_FOUND)
+
 IF(ORB_SLAM2_FOUND)
    SET(INCLUDE_DIRS
        ${ORB_SLAM2_INCLUDE_DIRS} #before so that g2o includes are taken from ORB_SLAM2 directory before the official g2o one
--- a/corelib/src/CameraStereo.cpp
+++ b/corelib/src/CameraStereo.cpp
@@ -379,7 +379,7 @@ bool CameraStereoDC1394::init(const std::string & calibrationFolder, const std::
 			// look for calibration files
 			if(!calibrationFolder.empty())
 			{
-				if(!stereoModel_.load(calibrationFolder, cameraName.empty()?device_->guid():cameraName))
+				if(!stereoModel_.load(calibrationFolder, cameraName.empty()?device_->guid():cameraName, false))
 				{
 					UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
 							cameraName.empty()?device_->guid().c_str():cameraName.c_str(), calibrationFolder.c_str());
@@ -1147,7 +1147,7 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 	// look for calibration files
 	if(!calibrationFolder.empty() && !cameraName.empty())
 	{
-		if(!stereoModel_.load(calibrationFolder, cameraName))
+		if(!stereoModel_.load(calibrationFolder, cameraName, false))
 		{
 			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
 					cameraName.c_str(), calibrationFolder.c_str());
@@ -1372,7 +1372,7 @@ bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::s
 	// look for calibration files
 	if(!calibrationFolder.empty() && !cameraName_.empty())
 	{
-		if(!stereoModel_.load(calibrationFolder, cameraName_))
+		if(!stereoModel_.load(calibrationFolder, cameraName_, false))
 		{
 			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
 				cameraName_.c_str(), calibrationFolder.c_str());
--- a/corelib/src/IMUThread.cpp
+++ b/corelib/src/IMUThread.cpp
@@ -0,0 +1,153 @@
+/*
+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.
+*/
+
+#include "rtabmap/core/IMUThread.h"
+#include "rtabmap/core/IMU.h"
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap
+{
+
+IMUThread::IMUThread(int rate, const Transform & localTransform) :
+		rate_(rate),
+		localTransform_(localTransform),
+		captureDelay_(0.0),
+		previousStamp_(0.0)
+{
+}
+
+IMUThread::~IMUThread()
+{
+	imuFile_.close();
+}
+
+bool IMUThread::init(const std::string & path)
+{
+	imuFile_.close();
+	captureDelay_ = 0.0;
+	previousStamp_ = 0.0;
+
+	// open the IMU file
+	std::string line;
+	imuFile_.open(path.c_str());
+	if (!imuFile_.good()) {
+		UERROR("no imu file found at %s",path.c_str());
+		return false;
+	}
+	int number_of_lines = 0;
+	while (std::getline(imuFile_, line))
+		++number_of_lines;
+	printf("No. IMU measurements: %d\n", number_of_lines-1);
+	if (number_of_lines - 1 <= 0) {
+		UERROR("no imu messages present in %s", path.c_str());
+		return false;
+	}
+	// set reading position to second line
+	imuFile_.clear();
+	imuFile_.seekg(0, std::ios::beg);
+	std::getline(imuFile_, line);
+
+	return true;
+}
+
+void IMUThread::setRate(int rate)
+{
+	rate_ = rate;
+}
+
+void IMUThread::mainLoopBegin()
+{
+	ULogger::registerCurrentThread("IMU");
+	frameRateTimer_.start();
+}
+
+void IMUThread::mainLoop()
+{
+	UTimer totalTime;
+	UDEBUG("");
+
+	if(rate_>0 || captureDelay_)
+	{
+		double delay = rate_>0?1000.0/double(rate_):1000.0f*captureDelay_;
+		int sleepTime = delay - 1000.0f*frameRateTimer_.getElapsedTime();
+		if(sleepTime > 2)
+		{
+			uSleep(sleepTime-2);
+		}
+
+		// Add precision at the cost of a small overhead
+		delay/=1000.0;
+		while(frameRateTimer_.getElapsedTime() < delay-0.000001)
+		{
+			//
+		}
+
+		frameRateTimer_.start();
+	}
+	captureDelay_ = 0.0;
+
+	std::string line;
+	if (std::getline(imuFile_, line))
+	{
+		std::stringstream stream(line);
+		std::string s;
+		std::getline(stream, s, ',');
+		std::string nanoseconds = s.substr(s.size() - 9, 9);
+		std::string seconds = s.substr(0, s.size() - 9);
+
+		Eigen::Vector3d gyr;
+		for (int j = 0; j < 3; ++j) {
+			std::getline(stream, s, ',');
+			gyr[j] = std::stof(s);
+		}
+
+		Eigen::Vector3d acc;
+		for (int j = 0; j < 3; ++j) {
+			std::getline(stream, s, ',');
+			acc[j] = std::stof(s);
+		}
+
+		double stamp = double(std::stoi(seconds)) + double(std::stoi(nanoseconds))*1e-9;
+		if(previousStamp_>0 && stamp > previousStamp_)
+		{
+			captureDelay_ = stamp - previousStamp_;
+		}
+		previousStamp_ = stamp;
+
+		IMU imu(gyr, cv::Mat(3,3,CV_64FC1), acc, cv::Mat(3,3,CV_64FC1), localTransform_);
+		this->post(new IMUEvent(imu, stamp));
+	}
+	else if(!this->isKilled())
+	{
+		UWARN("no more imu data...");
+		this->kill();
+		this->post(new IMUEvent());
+	}
+}
+
+} // namespace rtabmap
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -100,6 +100,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
 	_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
 	_visCorType(Parameters::defaultVisCorType()),
+	_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -481,6 +482,8 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		uInsert(parameters_, ParametersPair(Parameters::kVisCorType(), "0"));
 		uInsert(params, ParametersPair(Parameters::kVisCorType(), "0"));
 	}
+	Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
+

 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -3372,9 +3375,10 @@ private:
 	VWDictionary * _vwp;
 };

-Signature * Memory::createSignature(const SensorData & data, const Transform & pose, Statistics * stats)
+Signature * Memory::createSignature(const SensorData & inputData, const Transform & pose, Statistics * stats)
 {
 	UDEBUG("");
+	SensorData data = inputData;
 	UASSERT(data.imageRaw().empty() ||
 			data.imageRaw().type() == CV_8UC1 ||
 			data.imageRaw().type() == CV_8UC3);
@@ -3398,7 +3402,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		!data.stereoCameraModel().isValidForProjection() &&
 		!pose.isNull())
 	{
-		UERROR("Rectified images required! Calibrate your camera.");
+		UERROR("Camera calibration not valid, calibrate your camera!");
 		return 0;
 	}
 	UASSERT(_feature2D != 0);
@@ -3442,6 +3446,54 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		}
 	}

+	if(!_imagesAlreadyRectified && !data.imageRaw().empty())
+	{
+		if(!data.depthRaw().empty())
+		{
+			UERROR("RGB-D images should be already rectified! Make sure they are and set %s parameter back to true.",
+					Parameters::kRtabmapImagesAlreadyRectified().c_str());
+			return 0;
+		}
+		if(data.cameraModels().size())
+		{
+			UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
+			int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
+			cv::Mat rectifiedImages(data.imageRaw().size(), data.imageRaw().type());
+			for(unsigned int i=0; i<data.cameraModels().size(); ++i)
+			{
+				if(data.cameraModels()[i].isValidForRectification())
+				{
+					cv::Mat rectifiedImage = data.cameraModels()[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+					rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+				}
+				else
+				{
+					UERROR("Calibration for camera %d cannot be used to rectify the image. Make sure to do a "
+						"full calibration. If images are already rectified, set %s parameter back to true.",
+						(int)i,
+						Parameters::kRtabmapImagesAlreadyRectified().c_str());
+					return 0;
+				}
+			}
+			data.setImageRaw(rectifiedImages);
+		}
+		else if(data.stereoCameraModel().isValidForRectification())
+		{
+			data.setImageRaw(data.stereoCameraModel().left().rectifyImage(data.imageRaw()));
+			data.setDepthOrRightRaw(data.stereoCameraModel().right().rectifyImage(data.rightRaw()));
+		}
+		else
+		{
+			UERROR("Stereo calibration cannot be used to rectify images. Make sure to do a "
+					"full stereo calibration. If images are already rectified, set %s parameter back to true.",
+					Parameters::kRtabmapImagesAlreadyRectified().c_str());
+			return 0;
+		}
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
+		UDEBUG("time rectification = %fs", t);
+	}
+
 	int treeSize= int(_workingMem.size() + _stMem.size());
 	int meanWordsPerLocation = 0;
 	if(treeSize > 0)
--- a/corelib/src/Odometry.cpp
+++ b/corelib/src/Odometry.cpp
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryFovis.h"
 #include "rtabmap/core/OdometryViso2.h"
 #include "rtabmap/core/OdometryDVO.h"
+#include "rtabmap/core/OdometryOkvis.h"
 #include "rtabmap/core/OdometryORBSLAM2.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util3d.h"
@@ -76,6 +77,9 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 	case Odometry::kTypeF2F:
 		odometry = new OdometryF2F(parameters);
 		break;
+	case Odometry::kTypeOkvis:
+		odometry = new OdometryOkvis(parameters);
+		break;
 	default:
 		odometry = new OdometryF2M(parameters);
 		type = Odometry::kTypeF2M;
@@ -101,6 +105,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_imageDecimation(Parameters::defaultOdomImageDecimation()),
 		_alignWithGround(Parameters::defaultOdomAlignWithGround()),
 		_publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
+		_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
 		_pose(Transform::getIdentity()),
 		_resetCurrentCount(0),
 		previousStamp_(0),
@@ -128,6 +133,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomImageDecimation(), _imageDecimation);
 	Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround);
 	Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
+	Parameters::parse(parameters, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);

 	if(_imageDecimation == 0)
 	{
@@ -227,6 +233,13 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 {
 	UASSERT_MSG(data.id() >= 0, uFormat("Input data should have ID greater or equal than 0 (id=%d)!", data.id()).c_str());

+	if(!_imagesAlreadyRectified && !this->canProcessRawImages())
+	{
+		UERROR("Odometry approach chosen cannot process raw images (not rectified images). Make sure images "
+				"are rectified, and set %s parameter back to true.",
+				Parameters::kRtabmapImagesAlreadyRectified().c_str());
+	}
+
 	// Ground alignment
 	if(_pose.isIdentity() && _alignWithGround)
 	{
@@ -337,7 +350,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet

 	UTimer time;
 	Transform t;
-	if(_imageDecimation > 1)
+	if(_imageDecimation > 1 && !data.imageRaw().empty())
 	{
 		// Decimation of images with calibrations
 		SensorData decimatedData = data;
--- a/corelib/src/OdometryOkvis.cpp
+++ b/corelib/src/OdometryOkvis.cpp
@@ -0,0 +1,486 @@
+/*
+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.
+*/
+
+#include "rtabmap/core/OdometryOkvis.h"
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UStl.h"
+#include "rtabmap/utilite/UThread.h"
+
+#ifdef RTABMAP_OKVIS
+#include <iostream>
+#include <fstream>
+#include <stdlib.h>
+#include <memory>
+#include <functional>
+#include <atomic>
+
+#include <Eigen/Core>
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
+#pragma GCC diagnostic ignored "-Woverloaded-virtual"
+#include <opencv2/opencv.hpp>
+#pragma GCC diagnostic pop
+#include <okvis/VioParametersReader.hpp>
+#include <okvis/ThreadedKFVio.hpp>
+#include <okvis/cameras/PinholeCamera.hpp>
+#include <okvis/cameras/NoDistortion.hpp>
+#include <okvis/cameras/RadialTangentialDistortion.hpp>
+#include <okvis/cameras/EquidistantDistortion.hpp>
+#include <okvis/cameras/RadialTangentialDistortion8.hpp>
+#include <boost/filesystem.hpp>
+#endif
+
+namespace rtabmap {
+
+#ifdef RTABMAP_OKVIS
+class OkvisCallbackHandler
+{
+public:
+	OkvisCallbackHandler()
+	{
+	}
+
+	Transform getLastTransform()
+	{
+		Transform tf;
+		mutex_.lock();
+		tf = transform_;
+		mutex_.unlock();
+
+		return tf;
+	}
+
+	std::map<int, cv::Point3f> getLastLandmarks()
+	{
+		std::map<int, cv::Point3f> landmarks;
+		mutexLandmarks_.lock();
+		landmarks = landmarks_;
+		mutexLandmarks_.unlock();
+		return landmarks;
+	}
+
+public:
+	void fullStateCallback(
+	      const okvis::Time & t, const okvis::kinematics::Transformation & T_WS,
+	      const Eigen::Matrix<double, 9, 1> & /*speedAndBiases*/,
+	      const Eigen::Matrix<double, 3, 1> & /*omega_S*/)
+	{
+		UDEBUG("");
+		Transform tf = Transform::fromEigen4d(T_WS.T());
+
+		mutex_.lock();
+		transform_ = tf;
+		mutex_.unlock();
+	}
+
+	void landmarksCallback(const okvis::Time & t,
+			const okvis::MapPointVector & landmarksVector,
+			const okvis::MapPointVector & /*transferredLandmarks*/)
+	{
+		bool notify = true;
+		mutexLandmarks_.lock();
+		if(landmarks_.size())
+		{
+			notify = false;
+		}
+		landmarks_.clear();
+		for(unsigned int i=0; i<landmarksVector.size(); ++i)
+		{
+			landmarks_.insert(std::make_pair((int)landmarksVector[i].id, cv::Point3f(landmarksVector[i].point[0], landmarksVector[i].point[1], landmarksVector[i].point[2])));
+		}
+		mutexLandmarks_.unlock();
+		if(notify)
+		{
+			semLandmarks_.release();
+		}
+	}
+
+
+
+private:
+	Transform transform_;
+	std::map<int, cv::Point3f> landmarks_;
+	UMutex mutex_;
+	UMutex mutexLandmarks_;
+	USemaphore semTf_;
+	USemaphore semLandmarks_;
+};
+#endif
+
+OdometryOkvis::OdometryOkvis(const ParametersMap & parameters) :
+	Odometry(parameters),
+#ifdef RTABMAP_OKVIS
+	okvisCallbackHandler_(new OkvisCallbackHandler),
+#else
+	okvisCallbackHandler_(0),
+#endif
+	okvisEstimator_(0),
+	okvisParameters_(parameters),
+	imagesProcessed_(0)
+{
+#ifdef RTABMAP_OKVIS
+	Parameters::parse(parameters, Parameters::kOdomOKVISConfigPath(), configFilename_);
+	if(configFilename_.empty())
+	{
+		UERROR("OKVIS config file is empty (%s)!", Parameters::kOdomOKVISConfigPath().c_str());
+	}
+#endif
+}
+
+OdometryOkvis::~OdometryOkvis()
+{
+#ifdef RTABMAP_OKVIS
+	if(okvisEstimator_)
+	{
+		delete okvisEstimator_;
+	}
+	delete okvisCallbackHandler_;
+#endif
+}
+
+void OdometryOkvis::reset(const Transform & initialPose)
+{
+	Odometry::reset(initialPose);
+#ifdef RTABMAP_OKVIS
+	if(okvisEstimator_)
+	{
+		delete okvisEstimator_;
+		okvisEstimator_ = 0;
+	}
+	lastImu_ = IMU();
+
+	delete okvisCallbackHandler_;
+	okvisCallbackHandler_ = new OkvisCallbackHandler();
+#endif
+	imagesProcessed_ = 0;
+}
+
+// return not null transform if odometry is correctly computed
+Transform OdometryOkvis::computeTransform(
+		SensorData & data,
+		const Transform & guess,
+		OdometryInfo * info)
+{
+	UDEBUG("");
+	Transform t;
+#ifdef RTABMAP_OKVIS
+	UTimer timer;
+
+	okvis::Time timeOkvis = okvis::Time(data.stamp());
+
+	bool imuUpdated = false;
+	if(!data.imu().empty())
+	{
+		UDEBUG("IMU update stamp=%f acc=%f %f %f gyr=%f %f %f", data.stamp(),
+				data.imu().linearAcceleration()[0],
+				data.imu().linearAcceleration()[1],
+				data.imu().linearAcceleration()[2],
+				data.imu().angularVelocity()[0],
+				data.imu().angularVelocity()[1],
+				data.imu().angularVelocity()[2]);
+		if(okvisEstimator_ != 0)
+		{
+			imuUpdated = okvisEstimator_->addImuMeasurement(timeOkvis, data.imu().linearAcceleration(), data.imu().angularVelocity());
+		}
+		else
+		{
+			UWARN("Ignoring IMU, waiting for an image to initialize...");
+			lastImu_ = data.imu();
+		}
+	}
+
+	bool imageUpdated = false;
+	if(!data.imageRaw().empty())
+	{
+		UDEBUG("Image update stamp=%f", data.stamp());
+		std::vector<cv::Mat> images;
+		std::vector<CameraModel> models;
+		if(data.stereoCameraModel().isValidForProjection())
+		{
+			images.push_back(data.imageRaw());
+			images.push_back(data.rightRaw());
+			CameraModel mleft = data.stereoCameraModel().left();
+			// should be transform between IMU and camera
+			mleft.setLocalTransform(lastImu_.localTransform().inverse()*mleft.localTransform());
+			models.push_back(mleft);
+			CameraModel mright = data.stereoCameraModel().right();
+
+			// To support not rectified images
+			if(!imagesAlreadyRectified())
+			{
+				cv::Mat R = data.stereoCameraModel().R();
+				cv::Mat T = data.stereoCameraModel().T();
+				UASSERT(R.cols==3 && R.rows == 3);
+				UASSERT(T.cols==1 && T.rows == 3);
+				Transform extrinsics(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0,0),
+									R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1,0),
+									R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2,0));
+				mright.setLocalTransform(mleft.localTransform() * extrinsics.inverse());
+			}
+			else
+			{
+				Transform extrinsics(1, 0, 0, 0,
+									 0, 1, 0, data.stereoCameraModel().baseline(),
+									 0, 0, 1, 0);
+				mright.setLocalTransform(extrinsics * mleft.localTransform());
+			}
+
+
+			models.push_back(mright);
+		}
+		else
+		{
+			UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
+			int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
+			for(unsigned int i=0; i<data.cameraModels().size(); ++i)
+			{
+				if(data.cameraModels()[i].isValidForProjection())
+				{
+					images.push_back(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+					CameraModel m = data.cameraModels()[i];
+					// should be transform between IMU and camera
+					m.setLocalTransform(lastImu_.localTransform().inverse()*m.localTransform());
+					models.push_back(m);
+				}
+			}
+		}
+
+		if(images.size())
+		{
+			// initialization
+			if(okvisEstimator_ == 0)
+			{
+				UDEBUG("Initialization");
+				if(lastImu_.empty())
+				{
+					UWARN("Ignoring Image, waiting for imu to initialize...");
+					return t;
+				}
+
+				okvis::VioParameters parameters;
+				if(configFilename_.empty())
+				{
+					UERROR("OKVIS config file is empty (%s)!", Parameters::kOdomOKVISConfigPath().c_str());
+					return t;
+				}
+				else
+				{
+					okvis::VioParametersReader vio_parameters_reader(configFilename_);
+					vio_parameters_reader.getParameters(parameters);
+					if(parameters.nCameraSystem.numCameras() > 0)
+					{
+						UWARN("Camera calibration included in OKVIS is ignored as calibration from received images will be used instead.");
+					}
+					parameters.nCameraSystem = okvis::cameras::NCameraSystem();
+				}
+
+				parameters.publishing.publishRate = parameters.imu.rate; // rate at which odometry updates are published only works properly if imu_rate/publish_rate is an integer!!
+				parameters.publishing.publishLandmarks = true; // select, if you want to publish landmarks at all
+				parameters.publishing.publishImuPropagatedState = true; // Should the state that is propagated with IMU messages be published? Or just the optimized ones?
+				parameters.publishing.landmarkQualityThreshold = 1.0e-2; // landmark with lower quality will not be published
+				parameters.publishing.maxLandmarkQuality = 0.05; // landmark with higher quality will be published with the maximum colour intensity
+				parameters.publishing.trackedBodyFrame = okvis::FrameName::B; // B or S, the frame of reference that will be expressed relative to the selected worldFrame
+				parameters.publishing.velocitiesFrame = okvis::FrameName::B; // Wc, B or S,  the frames in which the velocities of the selected trackedBodyFrame will be expressed in
+
+				// non-hard coded parameters
+				parameters.imu.T_BS = okvis::kinematics::Transformation(lastImu_.localTransform().toEigen4d());
+				for(unsigned int i=0; i<models.size(); ++i)
+				{
+					okvis::cameras::NCameraSystem::DistortionType distType = okvis::cameras::NCameraSystem::NoDistortion;
+					std::shared_ptr<const okvis::cameras::CameraBase> cam;
+					if(!imagesAlreadyRectified())
+					{
+						if(models[i].D_raw().cols == 8)
+						{
+							okvis::cameras::RadialTangentialDistortion8 dist(
+									models[i].D_raw().at<double>(0,0),
+									models[i].D_raw().at<double>(0,1),
+									models[i].D_raw().at<double>(0,2),
+									models[i].D_raw().at<double>(0,3),
+									models[i].D_raw().at<double>(0,4),
+									models[i].D_raw().at<double>(0,5),
+									models[i].D_raw().at<double>(0,6),
+									models[i].D_raw().at<double>(0,7));
+							cam.reset(
+									new okvis::cameras::PinholeCamera<okvis::cameras::RadialTangentialDistortion8>(
+											models[i].imageWidth(),
+											models[i].imageHeight(),
+											models[i].K_raw().at<double>(0,0),
+											models[i].K_raw().at<double>(1,1),
+											models[i].K_raw().at<double>(0,2),
+											models[i].K_raw().at<double>(1,2),
+											dist));
+							distType = okvis::cameras::NCameraSystem::RadialTangential8;
+							UINFO("RadialTangential8");
+						}
+						else if(models[i].D_raw().cols == 6)
+						{
+							okvis::cameras::EquidistantDistortion dist(
+									models[i].D_raw().at<double>(0,0),
+									models[i].D_raw().at<double>(0,1),
+									models[i].D_raw().at<double>(0,4),
+									models[i].D_raw().at<double>(0,5));
+							cam.reset(new okvis::cameras::PinholeCamera<okvis::cameras::EquidistantDistortion>(
+											models[i].imageWidth(),
+											models[i].imageHeight(),
+											models[i].K_raw().at<double>(0,0),
+											models[i].K_raw().at<double>(1,1),
+											models[i].K_raw().at<double>(0,2),
+											models[i].K_raw().at<double>(1,2),
+											dist));
+							distType = okvis::cameras::NCameraSystem::Equidistant;
+							UINFO("Equidistant");
+						}
+						else if(models[i].D_raw().cols >= 4)
+						{
+							// To support not rectified images
+							okvis::cameras::RadialTangentialDistortion dist(
+									models[i].D_raw().at<double>(0,0),
+									models[i].D_raw().at<double>(0,1),
+									models[i].D_raw().at<double>(0,2),
+									models[i].D_raw().at<double>(0,3));
+							cam.reset(
+									new okvis::cameras::PinholeCamera<okvis::cameras::RadialTangentialDistortion>(
+											models[i].imageWidth(),
+											models[i].imageHeight(),
+											models[i].K_raw().at<double>(0,0),
+											models[i].K_raw().at<double>(1,1),
+											models[i].K_raw().at<double>(0,2),
+											models[i].K_raw().at<double>(1,2),
+											dist));
+							distType = okvis::cameras::NCameraSystem::RadialTangential;
+							UINFO("RadialTangential");
+						}
+					}
+					else // no distortion, rectified images
+					{
+						okvis::cameras::RadialTangentialDistortion dist(0,0,0,0);
+						cam.reset(
+								new okvis::cameras::PinholeCamera<okvis::cameras::RadialTangentialDistortion>(
+										models[i].imageWidth(),
+										models[i].imageHeight(),
+										models[i].K().at<double>(0,0),
+										models[i].K().at<double>(1,1),
+										models[i].K().at<double>(0,2),
+										models[i].K().at<double>(1,2),
+										dist));
+						distType = okvis::cameras::NCameraSystem::RadialTangential;
+					}
+
+					if(cam.get())
+					{
+						UINFO("model %d: %s", i, models[i].localTransform().prettyPrint().c_str());
+
+						Eigen::Vector3d r(models[i].localTransform().x(), models[i].localTransform().y(), models[i].localTransform().z());
+						parameters.nCameraSystem.addCamera(
+								std::shared_ptr<const okvis::kinematics::Transformation>(new okvis::kinematics::Transformation(r, models[i].localTransform().getQuaterniond().normalized())),
+								cam,
+								distType);
+					}
+				}
+
+				okvisEstimator_ = new okvis::ThreadedKFVio(parameters);
+
+				okvisEstimator_->setFullStateCallback(
+				  std::bind(&OkvisCallbackHandler::fullStateCallback, okvisCallbackHandler_,
+							std::placeholders::_1, std::placeholders::_2,
+							std::placeholders::_3, std::placeholders::_4));
+
+				okvisEstimator_->setLandmarksCallback(
+				  std::bind(&OkvisCallbackHandler::landmarksCallback, okvisCallbackHandler_,
+							std::placeholders::_1, std::placeholders::_2,
+							std::placeholders::_3));
+
+				okvisEstimator_->setBlocking(true);
+			}
+
+			for(unsigned int i=0; i<images.size(); ++i)
+			{
+				cv::Mat gray;
+				if(images[i].type() == CV_8UC3)
+				{
+					cv::cvtColor(images[i], gray, CV_BGR2GRAY);
+				}
+				else if(images[i].type() == CV_8UC1)
+				{
+					gray = images[i];
+				}
+				else
+				{
+					UFATAL("Not supported color type!");
+				}
+
+				imageUpdated = okvisEstimator_->addImage(timeOkvis, i, gray);
+				if(!imageUpdated)
+				{
+					UWARN("Image update with stamp %f delayed...", data.stamp());
+				}
+			}
+			if(imageUpdated)
+			{
+				++imagesProcessed_;
+			}
+		}
+	}
+
+	if((imageUpdated || imuUpdated) && imagesProcessed_ > 10)
+	{
+		Transform fixPos(-1,0,0,0, 0,-1,0,0, 0,0,1,0);
+		Transform fixRot(0,0,1,0, 0,-1,0,0, 1,0,0,0);
+		Transform p = okvisCallbackHandler_->getLastTransform();
+		if(!p.isNull())
+		{
+			p = fixPos * p * fixRot;
+			// make it incremental
+			t = this->getPose().inverse()*p;
+
+			if(info)
+			{
+				info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1);
+				info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001;
+
+				// FIXME: the scale of landmarks doesn't seem to fit well the environment...
+				/*info->localMap = okvisCallbackHandler_->getLastLandmarks();
+				info->localMapSize = info->localMap.size();
+				for(std::map<int, cv::Point3f>::iterator iter=info->localMap.begin(); iter!=info->localMap.end(); ++iter)
+				{
+					iter->second = util3d::transformPoint(iter->second, fixPos);
+				}*/
+			}
+		}
+		UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
+	}
+#else
+	UERROR("RTAB-Map is not built with OKVIS support! Select another visual odometry approach.");
+#endif
+	return t;
+}
+
+} // namespace rtabmap
--- a/corelib/src/OdometryThread.cpp
+++ b/corelib/src/OdometryThread.cpp
@@ -40,7 +40,9 @@ OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSi
 	_odometry(odometry),
 	_dataBufferMaxSize(dataBufferMaxSize),
 	_resetOdometry(false),
-	_resetPose(Transform::getIdentity())
+	_resetPose(Transform::getIdentity()),
+	_lastImuStamp(0.0),
+	_imuEstimatedDelay(0.0)
 {
 	UASSERT(_odometry != 0);
 }
@@ -68,6 +70,14 @@ bool OdometryThread::handleEvent(UEvent * event)
 				this->addData(cameraEvent->data());
 			}
 		}
+		else if(event->getClassName().compare("IMUEvent") == 0)
+		{
+			IMUEvent * imuEvent = (IMUEvent*)event;
+			if(!imuEvent->getData().empty())
+			{
+				this->addData(SensorData(imuEvent->getData(), 0, imuEvent->getStamp()));
+			}
+		}
 	}
 	if(event->getClassName().compare("OdometryResetEvent") == 0)
 	{
@@ -109,41 +119,64 @@ void OdometryThread::mainLoop()
 		OdometryInfo info;
 		UDEBUG("Processing data...");
 		Transform pose = _odometry->process(data, &info);
-		// a null pose notify that odometry could not be computed
 		UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
-		this->post(new OdometryEvent(data, pose, info));
+		if(!data.imageRaw().empty() || pose.isNull())
+		{
+			// a null pose notify that odometry could not be computed
+			this->post(new OdometryEvent(data, pose, info));
+		}
 	}
 }

 void OdometryThread::addData(const SensorData & data)
 {
-	if(dynamic_cast<OdometryMono*>(_odometry) == 0)
+	if(data.imu().empty())
 	{
-		if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
+		if(dynamic_cast<OdometryMono*>(_odometry) == 0)
 		{
-			ULOGGER_ERROR("Missing some information (images empty or missing calibration)!?");
-			return;
+			if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
+			{
+				ULOGGER_ERROR("Missing some information (images empty or missing calibration)!?");
+				return;
+			}
 		}
-	}
-	else
-	{
-		// Mono can accept RGB only
-		if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
+		else
 		{
-			ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
-			return;
+			// Mono can accept RGB only
+			if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
+			{
+				ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
+				return;
+			}
 		}
 	}

 	bool notify = true;
 	_dataMutex.lock();
 	{
-		_dataBuffer.push_back(data);
-		while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
+		if(data.imu().empty())
 		{
-			UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
-			_dataBuffer.pop_front();
-			notify = false;
+			_dataBuffer.push_back(data);
+			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
+			{
+				UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
+				_dataBuffer.erase(_dataBuffer.begin());
+				notify = false;
+			}
+			if(notify && _imuEstimatedDelay>0.0 && data.stamp() > (_lastImuStamp+_imuEstimatedDelay))
+			{
+				// Don't notify if IMU data before this image has not been received yet
+				notify = false;
+			}
+		}
+		else
+		{
+			_imuBuffer.push_back(data);
+			if(_lastImuStamp != 0.0 && data.stamp() > _lastImuStamp)
+			{
+				_imuEstimatedDelay = data.stamp() - _lastImuStamp;
+			}
+			_lastImuStamp = data.stamp();
 		}
 	}
 	_dataMutex.unlock();
@@ -160,10 +193,19 @@ bool OdometryThread::getData(SensorData & data)
 	_dataAdded.acquire();
 	_dataMutex.lock();
 	{
-		if(!_dataBuffer.empty())
+		if(!_dataBuffer.empty() || !_imuBuffer.empty())
 		{
-			data = _dataBuffer.front();
-			_dataBuffer.pop_front();
+			if(_dataBuffer.empty() ||
+				(!_dataBuffer.empty() && !_imuBuffer.empty() && _imuBuffer.front().stamp() <= _dataBuffer.front().stamp()))
+			{
+				data = _imuBuffer.front();
+				_imuBuffer.pop_front();
+			}
+			else
+			{
+				data = _dataBuffer.front();
+				_dataBuffer.pop_front();
+			}
 			dataFilled = true;
 		}
 	}
--- a/corelib/src/Parameters.cpp
+++ b/corelib/src/Parameters.cpp
@@ -664,11 +664,17 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 						ignore = true;
 					}
 #endif
-#ifndef RTABMAP_VISO2
+#ifndef RTABMAP_ORBSLAM2
 					if(group.compare("OdomORBSLAM2") == 0)
 					{
 						ignore = true;
 					}
+#endif
+#ifndef RTABMAP_OKVIS
+					if(group.compare("OdomOKVIS") == 0)
+					{
+						ignore = true;
+					}
 #endif
 					if(!ignore)
 					{
--- a/corelib/src/SensorData.cpp
+++ b/corelib/src/SensorData.cpp
@@ -418,6 +418,17 @@ SensorData::SensorData(
 	}
 }

+SensorData::SensorData(
+	const IMU & imu,
+	int id,
+	double stamp) :
+		_id(id),
+		_stamp(stamp),
+		_cellSize(0.0f)
+{
+	imu_ = imu;
+}
+
 SensorData::~SensorData()
 {
 }
--- a/corelib/src/StereoCameraModel.cpp
+++ b/corelib/src/StereoCameraModel.cpp
@@ -224,9 +224,9 @@ bool StereoCameraModel::load(const std::string & directory, const std::string &
 				UWARN("Missing \"rotation_matrix\" field in \"%s\"", filePath.c_str());
 			}

+			n = fs["translation_matrix"];
 			if(n.type() != cv::FileNode::NONE)
 			{
-				n = fs["translation_matrix"];
 				int rows = (int)n["rows"];
 				int cols = (int)n["cols"];
 				std::vector<double> data;
@@ -240,9 +240,9 @@ bool StereoCameraModel::load(const std::string & directory, const std::string &
 				UWARN("Missing \"translation_matrix\" field in \"%s\"", filePath.c_str());
 			}

+			n = fs["essential_matrix"];
 			if(n.type() != cv::FileNode::NONE)
 			{
-				n = fs["essential_matrix"];
 				int rows = (int)n["rows"];
 				int cols = (int)n["cols"];
 				std::vector<double> data;