Added MYNT EYE S support. Fixed StereoCameraModel rectification not done with non-square images, also updated to support fisheye model.

CMakeLists.txt

@@ -183,6 +183,7 @@ option(WITH_ZED           "Include ZED sdk support"              ON)
 option(WITH_REALSENSE     "Include RealSense support"            ON)
 option(WITH_REALSENSE_SLAM "Include RealSenseSlam support"       ON)
 option(WITH_REALSENSE2    "Include RealSense support"            ON)
+option(WITH_MYNTEYE       "Include mynteye-s support"            ON)
 option(WITH_OCTOMAP       "Include Octomap support"              ON)
 option(WITH_CPUTSDF       "Include CPUTSDF support"              ON)
 option(WITH_OPENCHISEL    "Include open_chisel support"          ON)
@@ -470,6 +471,13 @@ IF(WITH_REALSENSE2)
     ENDIF(realsense2_FOUND)
 ENDIF(WITH_REALSENSE2)
 
+IF(WITH_MYNTEYE)
+    FIND_PACKAGE(mynteye QUIET)
+    IF(mynteye_FOUND)
+       MESSAGE(STATUS "Found mynteye-s: ${mynteye_INCLUDE_DIRS}")
+    ENDIF(mynteye_FOUND)
+ENDIF(WITH_MYNTEYE)
+
 IF(WITH_OCTOMAP)
     FIND_PACKAGE(octomap QUIET)
     IF(octomap_FOUND)
@@ -758,6 +766,9 @@ IF(NOT realsense2_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${realsense2_LIBRARIES}) 
 ENDIF()
+IF(NOT mynteye_FOUND)
+   SET(MYNTEYE "//")
+ENDIF(NOT mynteye_FOUND)
 IF(NOT octomap_FOUND)
    SET(OCTOMAP "//")
 ELSE()
@@ -1270,6 +1281,14 @@ ELSE()
 MESSAGE(STATUS "  With RealSense2           = NO (librealsense2 not found)")
 ENDIF()
 
+IF(mynteye_FOUND)
+MESSAGE(STATUS "  With MyntEyeS             = YES (License: Apache-2)")
+ELSEIF(NOT WITH_MYNTEYE)
+MESSAGE(STATUS "  With MyntEyeS             = NO (WITH_MYNTEYE=OFF)")
+ELSE()
+MESSAGE(STATUS "  With MyntEyeS             = NO (mynteye s sdk not found)")
+ENDIF()
+
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Odometry Approaches:")
 IF(loam_velodyne_FOUND)

Version.h.in

@@ -60,6 +60,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @REALSENSE@#define RTABMAP_REALSENSE
 @REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
 @REALSENSE2@#define RTABMAP_REALSENSE2
+@MYNTEYE@#define RTABMAP_MYNTEYE
 @OCTOMAP@#define RTABMAP_OCTOMAP
 @CPUTSDF@#define RTABMAP_CPUTSDF
 @ALICE_VISION@#define RTABMAP_ALICE_VISION

corelib/include/rtabmap/core/CameraStereo.h

@@ -33,3 +33,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/camera/CameraStereoVideo.h>
 #include <rtabmap/core/camera/CameraStereoZed.h>
 #include <rtabmap/core/camera/CameraStereoTara.h>
+#include <rtabmap/core/camera/CameraMyntEye.h>

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -125,6 +125,9 @@ public:
 	const std::string & getLeftSuffix() const {return leftSuffix_;}
 	const std::string & getRightSuffix() const {return rightSuffix_;}
 
+private:
+	void updateStereoRectification();
+
 private:
 	std::string leftSuffix_;
 	std::string rightSuffix_;

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

@@ -0,0 +1,104 @@
+/*
+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/Camera.h"
+#include "rtabmap/core/Version.h"
+#include "rtabmap/utilite/USemaphore.h"
+#include <memory>
+
+namespace mynteye {
+class Device;
+class API;
+}
+
+namespace rtabmap
+{
+
+/**
+ * Class CameraMyntEye
+ *
+ */
+class RTABMAP_EXP CameraMyntEye : public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraMyntEye(const std::string & device = "", bool apiRectification = false, bool apiDepth = false, float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraMyntEye();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	virtual bool odomProvided() const { return false; }
+
+	void publishInterIMU(bool enabled);
+
+protected:
+	/**
+	 * returned rgb and depth images should be already rectified if calibration was loaded
+	 */
+	virtual SensorData captureImage(CameraInfo * info = 0);
+
+private:
+#ifdef RTABMAP_MYNTEYE
+	double hardTimeToSoftTime(std::uint64_t hardTime);
+	void getPoseAndIMU(const double & stamp, IMU & imu, int maxWaitTimeMs = 35) const;
+	double checkUpTimeStamp(std::uint64_t _hard_time, std::uint8_t stream);
+
+	std::shared_ptr<mynteye::Device> device_;
+	std::shared_ptr<mynteye::API> api_;
+	StereoCameraModel stereoModel_;
+	std::string deviceName_;
+	bool apiRectification_;
+	bool apiDepth_;
+	USemaphore dataReady_;
+	UMutex dataMutex_;
+	cv::Mat leftFrameBuffer_;
+	cv::Mat rightFrameBuffer_;
+	std::pair<cv::Mat, cv::Mat> lastFrames_;
+	double lastFramesStamp_;
+	std::uint64_t stamp_;
+	bool publishInterIMU_;
+	Transform imuLocalTransform_;
+	std::map<double, std::pair<cv::Vec3f, cv::Vec3f> > imuBuffer_;
+	UMutex imuMutex_;
+
+	double softTimeBegin_;
+	std::uint64_t hardTimeBegin_;
+	std::uint64_t unitHardTime;
+	std::vector<std::uint64_t> lastHardTimes_;
+	std::vector<std::uint64_t> acc_;
+#endif
+};
+
+
+} // namespace rtabmap

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

@@ -83,6 +83,7 @@ public:
 	void setOdomProvided(bool enabled);
 
 #ifdef RTABMAP_REALSENSE2
+private:
 	void imu_callback(rs2::frame frame);
 	void pose_callback(rs2::frame frame);
 	void frame_callback(rs2::frame frame);

corelib/include/rtabmap/core/stereo/stereoRectifyFisheye.h

@@ -29,8 +29,8 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#ifndef GUILIB_SRC_OPENCV_STEREORECTIFYFISHEYE_H_
+#ifndef CORELIB_SRC_OPENCV_STEREORECTIFYFISHEYE_H_
-#define GUILIB_SRC_OPENCV_STEREORECTIFYFISHEYE_H_
+#define CORELIB_SRC_OPENCV_STEREORECTIFYFISHEYE_H_
 
 #include <opencv2/calib3d/calib3d.hpp>
 #if CV_MAJOR_VERSION >= 3
@@ -1240,4 +1240,4 @@ void stereoRectifyFisheye( cv::InputArray _cameraMatrix1, cv::InputArray _distCo
 }
 
 
-#endif /* GUILIB_SRC_OPENCV_STEREORECTIFYFISHEYE_H_ */
+#endif /* CORELIB_SRC_OPENCV_STEREORECTIFYFISHEYE_H_ */

corelib/src/CMakeLists.txt

@@ -35,6 +35,7 @@ SET(SRC_FILES
     camera/CameraStereoZed.cpp
     camera/CameraStereoTara.cpp
     camera/CameraVideo.cpp
+    camera/CameraMyntEye.cpp
     
     EpipolarGeometry.cpp
     VisualWord.cpp
@@ -306,6 +307,13 @@ IF(FlyCapture2_FOUND)
 	)
 ENDIF(FlyCapture2_FOUND)
 
+IF(mynteye_FOUND)
+	SET(LIBRARIES
+		${LIBRARIES}
+		mynteye # target
+	)
+ENDIF(mynteye_FOUND)
+
 IF(WITH_TORO)
 	SET(SRC_FILES
     	${SRC_FILES}

corelib/src/StereoCameraModel.cpp

@@ -33,6 +33,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <opencv2/imgproc/imgproc.hpp>
 
+#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+#include <rtabmap/core/stereo/stereoRectifyFisheye.h>
+#endif
+
 namespace rtabmap {
 
 StereoCameraModel::StereoCameraModel(
@@ -88,17 +92,9 @@ StereoCameraModel::StereoCameraModel(
 	{
 		UASSERT(leftCameraModel.isValidForRectification() && rightCameraModel.isValidForRectification());
 
-		if(left_.imageWidth() == right_.imageHeight())
+		if(left_.imageWidth() == right_.imageWidth() && left_.imageHeight() == right_.imageHeight())
 		{
-			cv::Mat R1,R2,P1,P2,Q;
+			updateStereoRectification();
-			cv::stereoRectify(
-					left_.K_raw(), left_.D_raw(),
-					right_.K_raw(), right_.D_raw(),
-					left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
-					cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
-
-			left_ = CameraModel(left_.name(), left_.imageSize(), left_.K_raw(), left_.D_raw(), R1, P1, left_.localTransform());
-			right_ = CameraModel(right_.name(), right_.imageSize(), right_.K_raw(), right_.D_raw(), R2, P2, right_.localTransform());
 		}
 	}
 }
@@ -124,17 +120,9 @@ StereoCameraModel::StereoCameraModel(
 		extrinsics.rotationMatrix().convertTo(R_, CV_64FC1);
 		extrinsics.translationMatrix().convertTo(T_, CV_64FC1);
 
-		if(left_.imageWidth() == right_.imageHeight())
+		if(left_.imageWidth() == right_.imageWidth() && left_.imageHeight() == right_.imageHeight())
 		{
-			cv::Mat R1,R2,P1,P2,Q;
+			updateStereoRectification();
-			cv::stereoRectify(
-					left_.K_raw(), left_.D_raw(),
-					right_.K_raw(), right_.D_raw(),
-					left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
-					cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
-
-			left_ = CameraModel(left_.name(), left_.imageSize(), left_.K_raw(), left_.D_raw(), R1, P1, left_.localTransform());
-			right_ = CameraModel(right_.name(), right_.imageSize(), right_.K_raw(), right_.D_raw(), R2, P2, right_.localTransform());
 		}
 	}
 }
@@ -181,6 +169,57 @@ void StereoCameraModel::setName(const std::string & name, const std::string & le
 	right_.setName(name_+"_"+getRightSuffix());
 }
 
+void StereoCameraModel::updateStereoRectification()
+{
+	cv::Mat R1,R2,P1,P2,Q;
+	#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
+	bool fishEye = left_.D_raw().cols == 6;
+	// calibrate extrinsic
+	if(fishEye)
+	{
+		cv::Vec4d D_left(left_.D_raw().at<double>(0,0), left_.D_raw().at<double>(0,1), left_.D_raw().at<double>(0,4), left_.D_raw().at<double>(0,5));
+		cv::Vec4d D_right(right_.D_raw().at<double>(0,0), right_.D_raw().at<double>(0,1), right_.D_raw().at<double>(0,4), right_.D_raw().at<double>(0,5));
+
+		stereoRectifyFisheye(
+				left_.K_raw(), D_left,
+				right_.K_raw(), D_right,
+				left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
+				cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
+
+		// Re-zoom to original focal distance
+		if(P1.at<double>(0,0) < 0)
+		{
+			P1.at<double>(0,0) *= -1;
+			P1.at<double>(1,1) *= -1;
+		}
+		if(P2.at<double>(0,0) < 0)
+		{
+			P2.at<double>(0,0) *= -1;
+			P2.at<double>(1,1) *= -1;
+		}
+		if(P2.at<double>(0,3) > 0)
+		{
+			P2.at<double>(0,3) *= -1;
+		}
+		P2.at<double>(0,3) = P2.at<double>(0,3) * left_.K_raw().at<double>(0,0) / P2.at<double>(0,0);
+		P1.at<double>(0,0) = P1.at<double>(1,1) = left_.K_raw().at<double>(0,0);
+		P2.at<double>(0,0) = P2.at<double>(1,1) = left_.K_raw().at<double>(0,0);
+	}
+	else
+#endif
+	{
+
+		cv::stereoRectify(
+				left_.K_raw(), left_.D_raw(),
+				right_.K_raw(), right_.D_raw(),
+				left_.imageSize(), R_, T_, R1, R2, P1, P2, Q,
+				cv::CALIB_ZERO_DISPARITY, 0, left_.imageSize());
+	}
+
+	left_ = CameraModel(left_.name(), left_.imageSize(), left_.K_raw(), left_.D_raw(), R1, P1, left_.localTransform());
+	right_ = CameraModel(right_.name(), right_.imageSize(), right_.K_raw(), right_.D_raw(), R2, P2, right_.localTransform());
+}
+
 bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
 {
 	name_ = cameraName;

corelib/src/camera/CameraMyntEye.cpp

@@ -0,0 +1,611 @@
+/*
+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/utilite/UThread.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UEventsManager.h"
+#include <rtabmap/core/camera/CameraMyntEye.h>
+#ifdef RTABMAP_MYNTEYE
+#include <mynteye/api.h>
+#include <mynteye/device.h>
+#include <mynteye/context.h>
+#endif
+
+namespace rtabmap
+{
+
+CameraMyntEye::CameraMyntEye(const std::string & device, bool apiRectification, bool apiDepth, float imageRate, const Transform & localTransform) :
+			Camera(imageRate, localTransform)
+#ifdef RTABMAP_MYNTEYE
+			,
+			deviceName_(device),
+			apiRectification_(apiRectification),
+			apiDepth_(apiDepth),
+			dataReady_(0),
+			lastFramesStamp_(0.0),
+			stamp_(0),
+			publishInterIMU_(false),
+			softTimeBegin_(0.0),
+			hardTimeBegin_(0),
+			unitHardTime(std::numeric_limits<std::uint32_t>::max()*10)
+#endif
+{
+#ifdef RTABMAP_MYNTEYE
+	lastHardTimes_ = std::vector<std::uint64_t>((size_t)mynteye::Stream::LAST+1, 0);
+	acc_ = std::vector<std::uint64_t>((size_t)mynteye::Stream::LAST+1, 0);
+#endif
+}
+CameraMyntEye::~CameraMyntEye()
+{
+#ifdef RTABMAP_MYNTEYE
+	if (api_) {
+		api_->Stop(mynteye::Source::ALL);
+	}
+	dataReady_.release();
+#endif
+}
+
+#ifdef RTABMAP_MYNTEYE
+std::shared_ptr<mynteye::IntrinsicsBase> getDefaultIntrinsics() {
+	auto res = std::make_shared<mynteye::IntrinsicsPinhole>();
+	res->width = 640;
+	res->height = 400;
+	res->model = 0;
+	res->fx = 3.6220059643202876e+02;
+	res->fy = 3.6350065250745848e+02;
+	res->cx = 4.0658699068023441e+02;
+	res->cy = 2.3435161110061483e+02;
+	double codffs[5] = {
+			-2.5034765682756088e-01,
+			5.0579399202897619e-02,
+			-7.0536676161976066e-04,
+			-8.5255451307033846e-03,
+			0.
+	};
+	for (unsigned int i = 0; i < 5; i++) {
+		res->coeffs[i] = codffs[i];
+	}
+	return res;
+}
+
+std::shared_ptr<mynteye::Extrinsics> getDefaultExtrinsics() {
+	auto res = std::make_shared<mynteye::Extrinsics>();
+	double rotation[9] = {
+			9.9867908939669447e-01,  -6.3445566137485428e-03, 5.0988459509619687e-02,
+			5.9890316389333252e-03,  9.9995670037792639e-01,  7.1224201868366971e-03,
+			-5.1031440326695092e-02, -6.8076406092671274e-03, 9.9867384471984544e-01
+	};
+	double translation[3] = {-1.2002489764113250e+02, -1.1782637409050747e+00,
+			-5.2058205159996538e+00};
+	for (unsigned int i = 0; i < 3; i++) {
+		for (unsigned int j = 0; j < 3; j++) {
+			res->rotation[i][j] = rotation[i*3 + j];
+		}
+	}
+	for (unsigned int i = 0; i < 3; i++) {
+		res->translation[i] = translation[i];
+	}
+	return res;
+}
+
+double CameraMyntEye::hardTimeToSoftTime(std::uint64_t hardTime) {
+	if (hardTimeBegin_==0) {
+		softTimeBegin_ = UTimer::now();
+		hardTimeBegin_ = hardTime;
+	}
+
+	std::uint64_t time_ns_detal = (hardTime - hardTimeBegin_);
+	std::uint64_t time_ns_detal_s = time_ns_detal / 1000000;
+	std::uint64_t time_ns_detal_ns = time_ns_detal % 1000000;
+	double time_sec_double = static_cast<double>(time_ns_detal_s) + 1e-9*static_cast<double>(time_ns_detal_ns * 1000);
+
+	return softTimeBegin_ + time_sec_double;
+}
+
+inline bool is_overflow(std::uint64_t now, std::uint64_t pre, std::uint64_t unit_hard_time) {
+
+	return (now < pre) && ((pre - now) > (unit_hard_time / 2));
+}
+
+double CameraMyntEye::checkUpTimeStamp(std::uint64_t _hard_time, std::uint8_t stream) {
+	UASSERT(stream < (std::uint8_t)mynteye::Stream::LAST+1);
+	if (is_overflow(_hard_time, lastHardTimes_[stream], unitHardTime)) {
+		acc_[stream]++;
+	}
+
+	lastHardTimes_[stream] = _hard_time;
+
+	return hardTimeToSoftTime(acc_[stream] * unitHardTime + _hard_time);
+}
+#endif
+
+void CameraMyntEye::publishInterIMU(bool enabled)
+{
+#ifdef RTABMAP_MYNTEYE
+	publishInterIMU_ = enabled;
+#endif
+}
+
+bool CameraMyntEye::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef RTABMAP_MYNTEYE
+	mynteye::Context context;
+	auto &&devices = context.devices();
+
+	softTimeBegin_ = 0.0;
+	hardTimeBegin_ = 0;
+	imuBuffer_.clear();
+	lastHardTimes_ = std::vector<std::uint64_t>((size_t)mynteye::Stream::LAST+1, 0);
+	acc_ = std::vector<std::uint64_t>((size_t)mynteye::Stream::LAST+1, 0);
+
+	size_t n = devices.size();
+	if(n==0)
+	{
+		UERROR("No Mynt Eye devices detected!");
+		return false;
+	}
+	UINFO("MYNT EYE devices:");
+
+	for (size_t i = 0; i < n; i++) {
+		auto &&device = devices[i];
+		auto &&name = device->GetInfo(mynteye::Info::DEVICE_NAME);
+		auto &&serial_number = device->GetInfo(mynteye::Info::SERIAL_NUMBER);
+		if(!deviceName_.empty() && serial_number.compare(deviceName_) == 0)
+		{
+			device_ = devices[i];
+		}
+		UINFO("  index: %d, name: %s, serial number: %s", (int)i, name.c_str(), serial_number.c_str());
+	}
+
+	if(device_.get() == 0)
+	{
+		//take first one by default
+		device_ = devices[0];
+	}
+
+	UINFO("");
+	api_ = mynteye::API::Create(device_);
+
+	UINFO("");
+	auto in_left_base = api_->GetIntrinsicsBase(mynteye::Stream::LEFT);
+	auto in_right_base = api_->GetIntrinsicsBase(mynteye::Stream::RIGHT);
+	UINFO("");
+	auto ex = api_->GetExtrinsics(mynteye::Stream::RIGHT, mynteye::Stream::LEFT);
+
+	UINFO("");
+	if(!in_left_base || !in_right_base)
+	{
+		UERROR("Unknown calibration model! Using default ones");
+		in_left_base = getDefaultIntrinsics();
+		in_right_base = getDefaultIntrinsics();
+		ex = *(getDefaultExtrinsics());
+	}
+	cv::Size size{in_left_base->width, in_left_base->height};
+
+	cv::Mat K1, K2, D1, D2;
+
+	if(in_left_base->calib_model() == mynteye::CalibrationModel::PINHOLE &&
+			in_right_base->calib_model() == mynteye::CalibrationModel::PINHOLE)
+	{
+		auto in_left = *std::dynamic_pointer_cast<mynteye::IntrinsicsPinhole>(in_left_base);
+		auto in_right = *std::dynamic_pointer_cast<mynteye::IntrinsicsPinhole>(in_right_base);
+
+		K1 = (cv::Mat_<double>(3, 3) << in_left.fx, 0, in_left.cx, 0, in_left.fy, in_left.cy, 0, 0, 1);
+		K2 = (cv::Mat_<double>(3, 3) << in_right.fx, 0, in_right.cx, 0, in_right.fy, in_right.cy, 0, 0, 1);
+		D1 = cv::Mat(1, 5, CV_64F, in_left.coeffs).clone();
+		D2 = cv::Mat(1, 5, CV_64F, in_right.coeffs).clone();
+	}
+	else if(in_left_base->calib_model() == mynteye::CalibrationModel::KANNALA_BRANDT &&
+			in_right_base->calib_model() == mynteye::CalibrationModel::KANNALA_BRANDT)
+	{
+		//equidistant
+		auto in_left = *std::dynamic_pointer_cast<mynteye::IntrinsicsEquidistant>(in_left_base);
+		auto in_right = *std::dynamic_pointer_cast<mynteye::IntrinsicsEquidistant>(in_right_base);
+
+		/** The distortion coefficients: k2,k3,k4,k5,mu,mv,u0,v0 */
+		UINFO("left coeff = %f %f %f %f %f %f %f %f",
+				in_left.coeffs[0], in_left.coeffs[1], in_left.coeffs[2], in_left.coeffs[3],
+				in_left.coeffs[4], in_left.coeffs[5], in_left.coeffs[6], in_left.coeffs[7]);
+		UINFO("right coeff = %f %f %f %f %f %f %f %f",
+				in_left.coeffs[0], in_left.coeffs[1], in_left.coeffs[2], in_left.coeffs[3],
+				in_left.coeffs[4], in_left.coeffs[5], in_left.coeffs[6], in_left.coeffs[7]);
+
+		K1 = (cv::Mat_<double>(3, 3) << in_left.coeffs[4], 0, in_left.coeffs[6], 0, in_left.coeffs[5], in_left.coeffs[7], 0, 0, 1);
+		K2 = (cv::Mat_<double>(3, 3) << in_right.coeffs[4], 0, in_right.coeffs[6], 0, in_right.coeffs[5], in_right.coeffs[7], 0, 0, 1);
+		// convert to (k1,k2,p1,p2,k3,k4)
+		D1 = (cv::Mat_<double>(1, 6) << in_left.coeffs[0], in_left.coeffs[1], 0, 0, in_left.coeffs[2], in_left.coeffs[3]);
+		D2 = (cv::Mat_<double>(1, 6) << in_right.coeffs[0], in_right.coeffs[1], 0, 0, in_right.coeffs[2], in_right.coeffs[3]);
+	}
+
+	bool is_data_use_mm_instead_of_m =
+			abs(ex.translation[0]) > 1.0 ||
+			abs(ex.translation[1]) > 1.0 ||
+			abs(ex.translation[2]) > 1.0;
+	if(is_data_use_mm_instead_of_m)
+	{
+		ex.translation[0] *= 0.001;
+		ex.translation[1] *= 0.001;
+		ex.translation[2] *= 0.001;
+	}
+
+	Transform extrinsics(
+			ex.rotation[0][0], ex.rotation[0][1], ex.rotation[0][2], ex.translation[0],
+			ex.rotation[1][0], ex.rotation[1][1], ex.rotation[1][2], ex.translation[1],
+			ex.rotation[2][0], ex.rotation[2][1], ex.rotation[2][2], ex.translation[2]);
+
+	cv::Mat P1 = cv::Mat::eye(3, 4, CV_64FC1);
+	P1.at<double>(0,0) = K1.at<double>(0,0);
+	P1.at<double>(1,1) = K1.at<double>(1,1);
+	P1.at<double>(0,2) = K1.at<double>(0,2);
+	P1.at<double>(1,2) = K1.at<double>(1,2);
+
+	cv::Mat P2 = cv::Mat::eye(3, 4, CV_64FC1);
+	P2.at<double>(0,0) = K2.at<double>(0,0);
+	P2.at<double>(1,1) = K2.at<double>(1,1);
+	P2.at<double>(0,2) = K2.at<double>(0,2);
+	P2.at<double>(1,2) = K2.at<double>(1,2);
+
+	CameraModel leftModel(this->getSerial(), size, K1, D1, cv::Mat::eye(3, 3, CV_64F), P1, this->getLocalTransform());
+	CameraModel rightModel(this->getSerial(), size, K2, D2, cv::Mat::eye(3, 3, CV_64F), P2, this->getLocalTransform());
+	UINFO("raw: fx=%f fy=%f cx=%f cy=%f",
+				leftModel.fx(),
+				leftModel.fy(),
+				leftModel.cx(),
+				leftModel.cy());
+
+	UINFO("stereo extrinsics = %s", extrinsics.prettyPrint().c_str());
+	stereoModel_ = StereoCameraModel(this->getSerial(), leftModel, rightModel, extrinsics);
+
+	if(!stereoModel_.isValidForRectification())
+	{
+		UERROR("Could not initialize stereo rectification.");
+		return false;
+	}
+
+	stereoModel_.initRectificationMap();
+	UINFO("baseline = %f rectified: fx=%f fy=%f cx=%f cy=%f",
+			stereoModel_.baseline(),
+			stereoModel_.left().fx(),
+			stereoModel_.left().fy(),
+			stereoModel_.left().cx(),
+			stereoModel_.left().cy());
+
+	// get left to imu camera transform
+	auto &&exImu = api_->GetMotionExtrinsics(mynteye::Stream::LEFT);
+	is_data_use_mm_instead_of_m =
+			abs(exImu.translation[0]) > 1.0 ||
+			abs(exImu.translation[1]) > 1.0 ||
+			abs(exImu.translation[2]) > 1.0;
+
+	if (is_data_use_mm_instead_of_m) {
+		exImu.translation[0] *= 0.001;
+		exImu.translation[1] *= 0.001;
+		exImu.translation[2] *= 0.001;
+	}
+
+	if (exImu.rotation[0][0] == 0 && exImu.rotation[2][2] == 0)
+	{
+		imuLocalTransform_ = Transform(
+				0, 0, 1, exImu.translation[0],
+				0,-1, 0, exImu.translation[1],
+				1, 0, 0, exImu.translation[2]);
+	}
+	else
+	{
+		imuLocalTransform_ = Transform(
+				exImu.rotation[0][0], exImu.rotation[0][1], exImu.rotation[0][2], exImu.translation[0],
+				exImu.rotation[1][0], exImu.rotation[1][1], exImu.rotation[1][2], exImu.translation[1],
+				exImu.rotation[2][0], exImu.rotation[2][1], exImu.rotation[2][2], exImu.translation[2]);
+	}
+	UINFO("imu extrinsics = %s", imuLocalTransform_.prettyPrint().c_str());
+
+	for(int i=0;i<(int)mynteye::Stream::LAST; ++i)
+	{
+		UINFO("Support stream %d = %s", i, api_->Supports((mynteye::Stream)i)?"true":"false");
+	}
+
+	if (api_->Supports(apiRectification_?mynteye::Stream::LEFT_RECTIFIED:mynteye::Stream::LEFT) &&
+		api_->Supports(apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT))
+	{
+		api_->EnableStreamData(apiRectification_?mynteye::Stream::LEFT_RECTIFIED:mynteye::Stream::LEFT);
+		api_->SetStreamCallback(apiRectification_?mynteye::Stream::LEFT_RECTIFIED:mynteye::Stream::LEFT,
+				[&](const mynteye::api::StreamData &data)
+		{
+			double stamp = checkUpTimeStamp(data.img->timestamp, (std::uint8_t)(apiRectification_?mynteye::Stream::LEFT_RECTIFIED:mynteye::Stream::LEFT));
+
+			UScopeMutex s(dataMutex_);
+			bool notify = false;
+			leftFrameBuffer_ = data.frame;
+			if(stamp_>0 && stamp_ == data.img->timestamp && !rightFrameBuffer_.empty())
+			{
+				notify = lastFrames_.first.empty();
+				lastFrames_.first = leftFrameBuffer_;
+				lastFrames_.second = rightFrameBuffer_;
+				lastFramesStamp_ = stamp;
+				leftFrameBuffer_ = cv::Mat();
+				rightFrameBuffer_ = cv::Mat();
+				stamp_ = 0;
+			}
+			else
+			{
+				stamp_ = data.img->timestamp;
+			}
+			if(notify)
+			{
+				dataReady_.release();
+			}
+		});
+
+		if(apiRectification_ && apiDepth_ && api_->Supports(mynteye::Stream::DEPTH))
+		{
+			api_->EnableStreamData(mynteye::Stream::DEPTH);
+		}
+		else
+		{
+			api_->EnableStreamData(apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT);
+			apiDepth_ = false;
+		}
+		api_->SetStreamCallback(
+				apiDepth_?mynteye::Stream::DEPTH:apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT,
+				[&](const mynteye::api::StreamData &data)
+		{
+			double stamp = checkUpTimeStamp(data.img->timestamp, (std::uint8_t)(apiDepth_?mynteye::Stream::DEPTH:apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT));
+
+			UScopeMutex s(dataMutex_);
+			bool notify = false;
+			rightFrameBuffer_ = data.frame;
+			if(stamp_>0 && stamp_ == data.img->timestamp && !leftFrameBuffer_.empty())
+			{
+				notify = lastFrames_.first.empty();
+				lastFrames_.first = leftFrameBuffer_;
+				lastFrames_.second = rightFrameBuffer_;
+				lastFramesStamp_ = stamp;
+				leftFrameBuffer_ = cv::Mat();
+				rightFrameBuffer_ = cv::Mat();
+				stamp_ = 0;
+			}
+			else
+			{
+				stamp_ = data.img->timestamp;
+			}
+			if(notify)
+			{
+				dataReady_.release();
+			}
+		});
+
+		api_->SetMotionCallback([this](const mynteye::api::MotionData &data) {
+
+			double stamp = checkUpTimeStamp(data.imu->timestamp, (std::uint8_t)mynteye::Stream::LAST);
+			if(data.imu->flag == 0)
+			{
+				//UWARN("%f %f %f %f %f %f",
+				//		data.imu->gyro[0] * M_PI / 180, data.imu->gyro[1] * M_PI / 180, data.imu->gyro[2] * M_PI / 180,
+				//		data.imu->accel[0] * 9.8, data.imu->accel[1] * 9.8, data.imu->accel[2] * 9.8);
+				cv::Vec3d gyro(data.imu->gyro[0] * M_PI / 180, data.imu->gyro[1] * M_PI / 180, data.imu->gyro[2] * M_PI / 180);
+				cv::Vec3d acc(data.imu->accel[0] * 9.8, data.imu->accel[1] * 9.8, data.imu->accel[2] * 9.8);
+				if(publishInterIMU_)
+				{
+					IMU imu(gyro, cv::Mat::eye(3,3,CV_64FC1),
+							acc, cv::Mat::eye(3,3,CV_64FC1),
+							imuLocalTransform_);
+					UEventsManager::post(new IMUEvent(imu, stamp));
+				}
+				else
+				{
+					UScopeMutex lock(imuMutex_);
+					imuBuffer_.insert(imuBuffer_.end(), std::make_pair(stamp, std::make_pair(gyro, acc)));
+					if(imuBuffer_.size() > 1000)
+					{
+						imuBuffer_.erase(imuBuffer_.begin());
+					}
+				}
+			}
+
+			uSleep(1);
+
+		});
+
+		api_->Start(mynteye::Source::ALL);
+		uSleep(500); // To buffer some imus before sending images
+		return true;
+	}
+	UERROR("Streams missing.");
+	return false;
+#else
+	UERROR("Not built with Mynt Eye support!");
+	return false;
+#endif
+}
+
+bool CameraMyntEye::isCalibrated() const
+{
+	return true;
+}
+
+std::string CameraMyntEye::getSerial() const
+{
+#ifdef RTABMAP_MYNTEYE
+	if(device_.get())
+	{
+		return device_->GetInfo(mynteye::Info::SERIAL_NUMBER);
+	}
+#endif
+	return "";
+}
+
+bool CameraMyntEye::available()
+{
+#ifdef RTABMAP_MYNTEYE
+	return true;
+#else
+	return false;
+#endif
+}
+
+#ifdef RTABMAP_MYNTEYE
+void CameraMyntEye::getPoseAndIMU(
+		const double & stamp,
+		IMU & imu,
+		int maxWaitTimeMs) const
+{
+	imu = IMU();
+	if(imuBuffer_.empty())
+	{
+		return;
+	}
+
+	// Interpolate gyro,acc
+	cv::Vec3d gyro;
+	cv::Vec3d acc;
+	{
+		imuMutex_.lock();
+		int waitTry = 0;
+		while(maxWaitTimeMs > 0 && imuBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
+		{
+			imuMutex_.unlock();
+			++waitTry;
+			uSleep(1);
+			imuMutex_.lock();
+		}
+		if(imuBuffer_.rbegin()->first < stamp)
+		{
+			if(maxWaitTimeMs>0)
+			{
+				UWARN("Could not find gyro/acc data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, imuBuffer_.rbegin()->first);
+			}
+			imuMutex_.unlock();
+			return;
+		}
+		else
+		{
+			std::map<double, std::pair<cv::Vec3f, cv::Vec3f> >::const_iterator iterB = imuBuffer_.lower_bound(stamp);
+			std::map<double, std::pair<cv::Vec3f, cv::Vec3f> >::const_iterator iterA = iterB;
+			if(iterA != imuBuffer_.begin())
+			{
+				iterA = --iterA;
+			}
+			if(iterB == imuBuffer_.end())
+			{
+				iterB = --iterB;
+			}
+			if(iterA == iterB && stamp == iterA->first)
+			{
+				gyro[0] = iterA->second.first[0];
+				gyro[1] = iterA->second.first[1];
+				gyro[2] = iterA->second.first[2];
+				acc[0] = iterA->second.second[0];
+				acc[1] = iterA->second.second[1];
+				acc[2] = iterA->second.second[2];
+			}
+			else if(stamp >= iterA->first && stamp <= iterB->first)
+			{
+				float t = (stamp-iterA->first) / (iterB->first-iterA->first);
+				gyro[0] = iterA->second.first[0] + t*(iterB->second.first[0] - iterA->second.first[0]);
+				gyro[1] = iterA->second.first[1] + t*(iterB->second.first[1] - iterA->second.first[1]);
+				gyro[2] = iterA->second.first[2] + t*(iterB->second.first[2] - iterA->second.first[2]);
+				acc[0] = iterA->second.second[0] + t*(iterB->second.second[0] - iterA->second.second[0]);
+				acc[1] = iterA->second.second[1] + t*(iterB->second.second[1] - iterA->second.second[1]);
+				acc[2] = iterA->second.second[2] + t*(iterB->second.second[2] - iterA->second.second[2]);
+			}
+			else
+			{
+				if(stamp < iterA->first)
+				{
+					UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
+				}
+				else
+				{
+					UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
+				}
+				imuMutex_.unlock();
+				return;
+			}
+		}
+		imuMutex_.unlock();
+	}
+
+	imu = IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_);
+}
+#endif
+
+SensorData CameraMyntEye::captureImage(CameraInfo * info)
+{
+	SensorData data;
+#ifdef RTABMAP_MYNTEYE
+	if(!dataReady_.acquire(1, 3000))
+	{
+		UERROR("Did not receive frame since 3 seconds...");
+		return data;
+	}
+
+	cv::Mat left;
+	cv::Mat right;
+	double stamp = 0.0;
+
+	dataMutex_.lock();
+	if(!lastFrames_.first.empty())
+	{
+		left = lastFrames_.first;
+		right = lastFrames_.second;
+		stamp = lastFramesStamp_;
+		lastFrames_ = std::pair<cv::Mat, cv::Mat>();
+	}
+	dataMutex_.unlock();
+
+	if(!left.empty() && !right.empty())
+	{
+		if(right.type() == CV_16UC1)
+		{
+			data = SensorData(left, right, stereoModel_.left(), 0, stamp);
+		}
+		else
+		{
+			if(!apiRectification_)
+			{
+				left = stereoModel_.left().rectifyImage(left);
+				right = stereoModel_.right().rectifyImage(right);
+			}
+			data = SensorData(left, right, stereoModel_, 0, stamp);
+		}
+		if(!publishInterIMU_ && imuBuffer_.size())
+		{
+			IMU imu;
+			getPoseAndIMU(stamp, imu, 60);
+			if(!imu.empty())
+			{
+				data.setIMU(imu);
+			}
+		}
+	}
+#else
+	UERROR("Not built with Mynt Eye support!");
+#endif
+	return data;
+}
+
+} // namespace rtabmap

guilib/include/rtabmap/gui/MainWindow.h

@@ -180,6 +180,7 @@ protected Q_SLOTS:
 	void selectStereoZed();
 	void selectStereoTara();
 	void selectStereoUsb();
+	void selectMyntEyeS();
 	void dumpTheMemory();
 	void dumpThePrediction();
 	void sendGoal();

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -104,6 +104,7 @@ public:
 		kSrcStereoUsb      = 105,
 		kSrcStereoTara 	   = 106,
 		kSrcStereoRealSense2 = 107,
+		kSrcStereoMyntEye  = 108,
 
 		kSrcRGB            = 200,
 		kSrcUsbDevice      = 200,

guilib/src/CalibrationDialog.cpp

@@ -37,7 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #endif
 #include <opencv2/highgui/highgui.hpp>
 #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
-#include "opencv/stereoRectifyFisheye.h"
+#include <rtabmap/core/stereo/stereoRectifyFisheye.h>
 #endif
 
 #include <QFileDialog>

guilib/src/GuiLib.qrc

@@ -38,5 +38,6 @@
         <file>images/tara.png</file>
         <file>images/t265.png</file>
         <file>images/sr300.png</file>
+        <file>images/mynteyes.png</file>
     </qresource>
 </RCC>

guilib/src/MainWindow.cpp

@@ -442,6 +442,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
      connect(_ui->actionStereoTara, SIGNAL(triggered()), this, SLOT(selectStereoTara()));
 	connect(_ui->actionStereoUsb, SIGNAL(triggered()), this, SLOT(selectStereoUsb()));
 	connect(_ui->actionRealSense2_T265, SIGNAL(triggered()), this, SLOT(selectRealSense2Stereo()));
+	connect(_ui->actionMYNT_EYE_S_SDK, SIGNAL(triggered()), this, SLOT(selectMyntEyeS()));
 	_ui->actionFreenect->setEnabled(CameraFreenect::available());
 	_ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available());
 	_ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
@@ -460,6 +461,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available());
 	_ui->actionStereoZed->setEnabled(CameraStereoZed::available());
     _ui->actionStereoTara->setEnabled(CameraStereoTara::available());
+    _ui->actionMYNT_EYE_S_SDK->setEnabled(CameraMyntEye::available());
 	this->updateSelectSourceMenu();
 
 	connect(_ui->actionPreferences, SIGNAL(triggered()), this, SLOT(openPreferences()));
@@ -6367,6 +6369,11 @@ void MainWindow::selectStereoUsb()
 	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoUsb);
 }
 
+void MainWindow::selectMyntEyeS()
+{
+	_preferencesDialog->selectSourceDriver(PreferencesDialog::kSrcStereoMyntEye);
+}
+
 void MainWindow::dumpTheMemory()
 {
 	this->post(new RtabmapEventCmd(RtabmapEventCmd::kCmdDumpMemory));

guilib/src/PreferencesDialog.cpp

@@ -335,6 +335,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
     if (!CameraStereoTara::available())
     {
         _ui->comboBox_cameraStereo->setItemData(kSrcStereoTara - kSrcStereo, 0, Qt::UserRole - 1);
+    }
+    if (!CameraMyntEye::available())
+    {
+        _ui->comboBox_cameraStereo->setItemData(kSrcStereoMyntEye - kSrcStereo, 0, Qt::UserRole - 1);
     }
 	_ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable());
 	_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
@@ -716,6 +720,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	connect(_ui->checkbox_stereoRealSense2_odom, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
+	connect(_ui->checkbox_stereoMyntEye_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->checkbox_stereoMyntEye_depth, 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()));
@@ -1872,6 +1879,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkbox_stereoZed_odom->setChecked(false);
 		_ui->lineEdit_zedSvoPath->clear();
 		_ui->checkbox_stereoRealSense2_odom->setChecked(false);
+		_ui->checkbox_stereoMyntEye_rectify->setChecked(false);
+		_ui->checkbox_stereoMyntEye_depth->setChecked(false);
 
 		_ui->checkBox_cameraImages_timestamps->setChecked(false);
 		_ui->checkBox_cameraImages_syncTimeStamps->setChecked(true);
@@ -2317,6 +2326,11 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->checkbox_stereoRealSense2_odom->setChecked(settings.value("odom", _ui->checkbox_stereoRealSense2_odom->isChecked()).toBool());
 	settings.endGroup(); // StereoRealSense2
 
+	settings.beginGroup("MyntEye");
+	_ui->checkbox_stereoMyntEye_rectify->setChecked(settings.value("rectify", _ui->checkbox_stereoMyntEye_rectify->isChecked()).toBool());
+	_ui->checkbox_stereoMyntEye_depth->setChecked(settings.value("depth", _ui->checkbox_stereoMyntEye_depth->isChecked()).toBool());
+	settings.endGroup(); // StereoRealSense2
+
 	settings.beginGroup("Images");
 	_ui->source_images_lineEdit_path->setText(settings.value("path", _ui->source_images_lineEdit_path->text()).toString());
 	_ui->source_images_spinBox_startPos->setValue(settings.value("startPos",_ui->source_images_spinBox_startPos->value()).toInt());
@@ -2784,6 +2798,11 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("odom", _ui->checkbox_stereoRealSense2_odom->isChecked());
 	settings.endGroup(); // StereoRealSense2
 
+	settings.beginGroup("MyntEye");
+	settings.setValue("rectify", _ui->checkbox_stereoMyntEye_rectify->isChecked());
+	settings.setValue("depth", _ui->checkbox_stereoMyntEye_depth->isChecked());
+	settings.endGroup(); // MyntEye
+
 	settings.beginGroup("Images");
 	settings.setValue("path", 			_ui->source_images_lineEdit_path->text());
 	settings.setValue("startPos", 		_ui->source_images_spinBox_startPos->value());
@@ -4741,12 +4760,14 @@ void PreferencesDialog::updateSourceGrpVisibility()
 		 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo ||
 		 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZed - kSrcStereo ||
 		 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoUsb - kSrcStereo ||
-		 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoRealSense2 - kSrcStereo));
+		 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoRealSense2 - kSrcStereo ||
+		 _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoMyntEye - 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->groupBox_cameraMyntEye->setVisible(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoMyntEye - kSrcStereo);
 
 	_ui->stackedWidget_image->setVisible(_ui->comboBox_sourceType->currentIndex() == 2 &&
 			(_ui->source_comboBox_image_type->currentIndex() == kSrcImages-kSrcRGB ||
@@ -4773,7 +4794,8 @@ void PreferencesDialog::updateSourceGrpVisibility()
 			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcK4A - kSrcRGBD) || //K4A
 			(_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->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZed - kSrcStereo) || // ZEDm, ZED2
+			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoMyntEye - kSrcStereo));  // MYNT EYE S
 	_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);
@@ -5545,6 +5567,26 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 			}
 		}
 	}
+	else if (driver == kSrcStereoMyntEye)
+	{
+		if(driver == kSrcStereoMyntEye && useRawImages)
+		{
+			QMessageBox::warning(this, tr("Calibration"),
+					tr("Using raw images for \"RealSense\" driver is not yet supported. "
+						"Factory calibration loaded from RealSense2 is used."), QMessageBox::Ok);
+			return 0;
+		}
+		else
+		{
+			camera = new CameraMyntEye(
+				this->getSourceDevice().toStdString(),
+				_ui->checkbox_stereoMyntEye_rectify->isChecked(),
+				_ui->checkbox_stereoMyntEye_depth->isChecked(),
+				this->getGeneralInputRate(),
+				this->getSourceLocalTransform());
+			((CameraMyntEye*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked());
+		}
+	}
 	else if(driver == kSrcRGBDImages)
 	{
 		camera = new CameraRGBDImages(

guilib/src/ui/mainWindow.ui

@@ -278,10 +278,21 @@
        </property>
        <addaction name="actionRealSense2_T265"/>
       </widget>
+      <widget class="QMenu" name="menuMynt_Eye_S">
+       <property name="title">
+        <string>Mynt Eye S</string>
+       </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/mynteyes.png</normaloff>:/images/mynteyes.png</iconset>
+       </property>
+       <addaction name="actionMYNT_EYE_S_SDK"/>
+      </widget>
       <addaction name="menuBumblebee2_2"/>
       <addaction name="menuZed_camera"/>
       <addaction name="menuTara_Camera"/>
       <addaction name="menuRealSense_T265"/>
+      <addaction name="menuMynt_Eye_S"/>
      </widget>
      <addaction name="menuRGB_D_camera"/>
      <addaction name="menuStereo_camera"/>
@@ -1579,6 +1590,11 @@
     <string>RealSense2</string>
    </property>
   </action>
+  <action name="actionMYNT_EYE_S_SDK">
+   <property name="text">
+    <string>MYNT EYE S SDK</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-224</y>
+            <y>-250</y>
             <width>680</width>
-            <height>3310</height>
+            <height>3497</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -3109,7 +3109,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_src">
                       <property name="currentIndex">
-                       <number>3</number>
+                       <number>1</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -4596,6 +4596,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <string>RealSense2</string>
                                 </property>
                                </item>
+                               <item>
+                                <property name="text">
+                                 <string>MyntEyeS</string>
+                                </property>
+                               </item>
                               </widget>
                              </item>
                              <item row="0" column="2">
@@ -4676,7 +4681,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>4</number>
+                              <number>8</number>
                              </property>
                              <widget class="QWidget" name="page_49">
                               <layout class="QVBoxLayout" name="verticalLayout_91">
@@ -5294,6 +5299,78 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                </item>
                               </layout>
                              </widget>
+                             <widget class="QWidget" name="page_86">
+                              <layout class="QVBoxLayout" name="verticalLayout_148">
+                               <item>
+                                <widget class="QGroupBox" name="groupBox_cameraMyntEye">
+                                 <property name="sizePolicy">
+                                  <sizepolicy hsizetype="Ignored" vsizetype="Ignored">
+                                   <horstretch>0</horstretch>
+                                   <verstretch>0</verstretch>
+                                  </sizepolicy>
+                                 </property>
+                                 <property name="title">
+                                  <string>MYNT EYE S</string>
+                                 </property>
+                                 <layout class="QGridLayout" name="gridLayout_115" columnstretch="0,1">
+                                  <item row="1" column="0">
+                                   <widget class="QCheckBox" name="checkbox_stereoMyntEye_depth">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                  <item row="1" column="1">
+                                   <widget class="QLabel" name="label_576">
+                                    <property name="text">
+                                     <string>Compute depth image with MYNT EYE S SDK. Ignored if images are not rectified from MYNT EYE S SDK.</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_581">
+                                    <property name="text">
+                                     <string>Rectify images using MYNT EYE S SDK.</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="0">
+                                   <widget class="QCheckBox" name="checkbox_stereoMyntEye_rectify">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
+                                  </item>
+                                 </layout>
+                                </widget>
+                               </item>
+                               <item>
+                                <spacer name="verticalSpacer_85">
+                                 <property name="orientation">
+                                  <enum>Qt::Vertical</enum>
+                                 </property>
+                                 <property name="sizeHint" stdset="0">
+                                  <size>
+                                   <width>20</width>
+                                   <height>596</height>
+                                  </size>
+                                 </property>
+                                </spacer>
+                               </item>
+                              </layout>
+                             </widget>
                             </widget>
                            </item>
                           </layout>

tools/CameraRGBD/main.cpp

@@ -62,6 +62,7 @@ void showUsage()
 			"                                     10=Kinect for Windows 2 SDK\n"
 			"                                     11=RealSense2\n"
 			"                                     12=Kinect for Azure SDK\n"
+			"                                     13=MYNT EYE S\n"
 			"  Options:\n"
 			"      -rate #.#                      Input rate Hz (default 0=inf)\n"
 			"      -device #                      Device ID (number or string)\n"
@@ -173,9 +174,9 @@ int main(int argc, char * argv[])
 
 			// last
 			driver = atoi(argv[i]);
-			if(driver < 0 || driver > 12)
+			if(driver < 0 || driver > 13)
 			{
-				UERROR("driver should be between 0 and 12.");
+				UERROR("driver should be between 0 and 13.");
 				showUsage();
 			}
 		}
@@ -299,11 +300,20 @@ int main(int argc, char * argv[])
 	{
 		if (!rtabmap::CameraK4A::available())
 		{
-			UERROR("Not built with Kienct for Azure SDK support...");
+			UERROR("Not built with Kinect for Azure SDK support...");
 			exit(-1);
 		}
 		camera = new rtabmap::CameraK4A(deviceId);
 	}
+	else if (driver == 13)
+	{
+		if (!rtabmap::CameraMyntEye::available())
+		{
+			UERROR("Not built with Mynt Eye S support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraMyntEye(deviceId);
+	}
 	else
 	{
 		UFATAL("");