Update package.xml

Bump package version
RegistrationIcp: Fixed invalid variance set when structural complexity is low and input doesn't have normals. DBViewer: added check on angular error after optimizing the graph when adding a new constraint. Memory: added warning if odom angular variance detected is very high.
2026-09-13 06:50:19 +08:00 · 2018-09-29 11:58:58 -04:00 · 2018-09-28 22:02:07 -04:00 · 2018-09-28 15:37:24 -04:00 · 2018-09-28 09:45:43 -04:00 · 2018-09-27 16:08:55 -04:00
41 changed files with 1430 additions and 527 deletions
@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 17)
-SET(RTABMAP_PATCH_VERSION 5)
+SET(RTABMAP_PATCH_VERSION 6)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -681,9 +681,9 @@ IF(VTK_USE_QVTK)
   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${QVTK_LIBRARY}) 
 ENDIF(VTK_USE_QVTK)

-IF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
+IF(NOT (OpenCV_FOUND AND NOT (OpenCV_VERSION_MAJOR LESS 3)))
   SET(OPENCV3 "//")
-ENDIF(NOT (OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3))
+ENDIF(NOT (OpenCV_FOUND AND NOT (OpenCV_VERSION_MAJOR LESS 3)))
 CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)

 ADD_SUBDIRECTORY( utilite )
@@ -868,18 +868,19 @@ ENDIF(APPLE OR WIN32)
 MESSAGE(STATUS "  CMAKE_CXX_FLAGS = ${CMAKE_CXX_FLAGS}")
 MESSAGE(STATUS "  PCL_DEFINITIONS = ${PCL_DEFINITIONS}")

+MESSAGE(STATUS "Optional dependencies ('*' affects some default parameters) :")
 IF(OpenCV_FOUND)
  IF(OpenCV_VERSION_MAJOR EQUAL 2)
    IF(OPENCV_NONFREE_FOUND)
-      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = YES (License: Non commercial)")
    ELSE()
-      MESSAGE(STATUS "  With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV 2 nonfree module (SIFT/SURF) = NO (not found, License: BSD)")
    ENDIF()
  ELSE()
    IF(OPENCV_XFEATURES2D_FOUND)
-      MESSAGE(STATUS "  With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)")
+      MESSAGE(STATUS " *With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = YES (License: Non commercial)")
    ELSE()
-      MESSAGE(STATUS "  With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found, License: BSD)")
+      MESSAGE(STATUS " *With OpenCV 3 xfeatures2d module (SIFT/SURF/BRIEF/FREAK) = NO (not found, License: BSD)")
    ENDIF()
  ENDIF()
 ENDIF(OpenCV_FOUND)
@@ -945,19 +946,19 @@ MESSAGE(STATUS "  With TORO                 = NO (WITH_TORO=OFF)")
 ENDIF()

 IF(G2O_FOUND)
-MESSAGE(STATUS "  With g2o                  = YES (License: BSD)")
+MESSAGE(STATUS " *With g2o                  = YES (License: BSD)")
 ELSEIF(NOT WITH_G2O)
-MESSAGE(STATUS "  With g2o                  = NO (WITH_G2O=OFF)")
+MESSAGE(STATUS " *With g2o                  = NO (WITH_G2O=OFF)")
 ELSE()
-MESSAGE(STATUS "  With g2o                  = NO (g2o not found)")
+MESSAGE(STATUS " *With g2o                  = NO (g2o not found)")
 ENDIF()

 IF(GTSAM_FOUND)
-MESSAGE(STATUS "  With GTSAM                = YES (License: BSD)")
+MESSAGE(STATUS " *With GTSAM                = YES (License: BSD)")
 ELSEIF(NOT WITH_GTSAM)
-MESSAGE(STATUS "  With GTSAM                = NO (WITH_GTSAM=OFF)")
+MESSAGE(STATUS " *With GTSAM                = NO (WITH_GTSAM=OFF)")
 ELSE()
-MESSAGE(STATUS "  With GTSAM                = NO (GTSAM not found)")
+MESSAGE(STATUS " *With GTSAM                = NO (GTSAM not found)")
 ENDIF()

 IF(G2O_FOUND OR GTSAM_FOUND)
@@ -979,11 +980,11 @@ MESSAGE(STATUS "  With cvsba                = NO (cvsba not found)")
 ENDIF()

 IF(libpointmatcher_FOUND)
-MESSAGE(STATUS "  With libpointmatcher      = YES (License: BSD)")
+MESSAGE(STATUS " *With libpointmatcher      = YES (License: BSD)")
 ELSEIF(NOT WITH_POINTMATCHER)
-MESSAGE(STATUS "  With libpointmatcher      = NO (WITH_POINTMATCHER=OFF)")
+MESSAGE(STATUS " *With libpointmatcher      = NO (WITH_POINTMATCHER=OFF)")
 ELSE()
-MESSAGE(STATUS "  With libpointmatcher      = NO (libpointmatcher not found)")
+MESSAGE(STATUS " *With libpointmatcher      = NO (libpointmatcher not found)")
 ENDIF()

 IF(loam_velodyne_FOUND)
@@ -75,6 +75,7 @@ public:
 	virtual ~CameraModel() {}

 	void initRectificationMap();
+	bool isRectificationMapInitialized() {return !mapX_.empty() && !mapY_.empty();}

 	bool isValidForProjection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0;}
 	bool isValidForReprojection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0 && imageWidth()>0 && imageHeight()>0;}
@@ -379,6 +379,7 @@ class RTABMAP_EXP CameraRealSense :
 {
 public:
 	static bool available();
+	enum RGBSource {kColor, kInfrared, kFishEye};

 public:
 	// default local transform z in, x right, y down));
@@ -391,11 +392,8 @@ public:
 		const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraRealSense();

-	void setDepthScaledToRGBSize(bool enabled) {
-#ifdef RTABMAP_REALSENSE
-		depthScaledToRGBSize_ = enabled;
-#endif
-		}
+	void setDepthScaledToRGBSize(bool enabled);
+	void setRGBSource(RGBSource source);
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
@@ -413,6 +411,9 @@ private:
 	int presetDepth_;
 	bool computeOdometry_;
 	bool depthScaledToRGBSize_;
+	RGBSource rgbSource_;
+	CameraModel cameraModel_;
+	std::vector<int> rsRectificationTable_;

 	int motionSeq_[2];
 	rs::slam::slam * slam_;
@@ -291,7 +291,7 @@ private:
 	float _laserScanDownsampleStepSize;
 	float _laserScanVoxelSize;
 	int _laserScanNormalK;
-	int _laserScanNormalRadius;
+	float _laserScanNormalRadius;
 	bool _reextractLoopClosureFeatures;
 	bool _localBundleOnLoopClosure;
 	float _rehearsalMaxDistance;
@@ -302,6 +302,7 @@ private:
 	int _visMaxFeatures;
 	int _visCorType;
 	bool _imagesAlreadyRectified;
+	bool _rectifyOnlyFeatures;
 	bool _covOffDiagonalIgnored;

 	int _idCount;
@@ -312,6 +313,8 @@ private:
 	bool _linksChanged; // False by default, become true when links are modified.
 	int _signaturesAdded;
 	GPS _gpsOrigin;
+	std::vector<CameraModel> _rectCameraModels;
+	StereoCameraModel _rectStereoCameraModel;

 	std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
 	std::set<int> _stMem; // id
@@ -55,12 +55,12 @@ private:
 #ifdef RTABMAP_OKVIS
 	OkvisCallbackHandler * okvisCallbackHandler_;
 	okvis::ThreadedKFVio * okvisEstimator_;
+	int imagesProcessed_;
+	bool initGravity_;
 #endif
 	ParametersMap okvisParameters_;
 	IMU lastImu_; // only used for initialization
-	int imagesProcessed_;
 	Transform previousPose_;
-	bool initGravity_;
 };

 }
@@ -175,8 +175,8 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Rtabmap, PublishRAMUsage,              bool, false, "Publishing RAM usage in statistics (may add a small overhead to get info from the system).");
    RTABMAP_PARAM(Rtabmap, ComputeRMSE,                  bool, true,  "Compute root mean square error (RMSE) and publish it in statistics, if ground truth is provided.");
    RTABMAP_PARAM(Rtabmap, SaveWMState,                  bool, false, "Save working memory state after each update in statistics.");
-    RTABMAP_PARAM(Rtabmap, TimeThr,                      float, 0,    "Maximum time allowed for the detector (ms) (0 means infinity).");
-    RTABMAP_PARAM(Rtabmap, MemoryThr,                    int, 0,      "Maximum signatures in the Working Memory (ms) (0 means infinity).");
+    RTABMAP_PARAM(Rtabmap, TimeThr,                      float, 0,    "Maximum time allowed for map update (ms) (0 means infinity). When map update time exceeds this fixed time threshold, some nodes in Working Memory (WM) are transferred to Long-Term Memory to limit the size of the WM and decrease the update time.");
+    RTABMAP_PARAM(Rtabmap, MemoryThr,                    int, 0,      uFormat("Maximum nodes in the Working Memory (0 means infinity). Similar to \"%s\", when the number of nodes in Working Memory (WM) exceeds this treshold, some nodes are transferred to Long-Term Memory to keep WM size fixed.", kRtabmapTimeThr().c_str()));
    RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1,    "Detection rate (Hz). RTAB-Map will filter input images to satisfy this rate.");
    RTABMAP_PARAM(Rtabmap, ImageBufferSize,          unsigned int, 1, "Data buffer size (0 min inf).");
    RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
@@ -188,6 +188,7 @@ class RTABMAP_EXP Parameters
    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, StartNewMapOnGoodSignature,   bool, false, uFormat("Start a new map only if the first signature is not bad (i.e., has enough features, see %s).", kKpBadSignRatio().c_str()));
    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.");
+    RTABMAP_PARAM(Rtabmap, RectifyOnlyFeatures,          bool, false,  uFormat("If \"%s\" is false and this parameter is true, the whole RGB image will not be rectified, only the features. Warning: As projection of RGB-D image to point cloud is assuming that images are rectified, the generated point cloud map will have wrong colors if this parameter is true.", kRtabmapImagesAlreadyRectified().c_str()));

    // Hypotheses selection
    RTABMAP_PARAM(Rtabmap, LoopThr,           float, 0.11,      "Loop closing threshold.");
@@ -219,7 +220,7 @@ class RTABMAP_EXP Parameters
    RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,         "If > 1, downsample the laser scans when creating a signature.");
    RTABMAP_PARAM(Mem, LaserScanVoxelSize,          float, 0.0,     uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()).c_str());
    RTABMAP_PARAM(Mem, LaserScanNormalK,            int, 0,         "If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature.");
-    RTABMAP_PARAM(Mem, LaserScanNormalRadius,       int, 0,         "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
+    RTABMAP_PARAM(Mem, LaserScanNormalRadius,       float, 0.0,     "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
    RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, true,     "Use odometry features.");
    RTABMAP_PARAM(Mem, CovOffDiagIgnored,           bool, true,     "Ignore off diagonal values of the covariance matrix.");

@@ -250,7 +250,7 @@ private:
 	float _proximityAngle;
 	std::string _databasePath;
 	bool _optimizeFromGraphEnd;
-	float _optimizationMaxLinearError;
+	float _optimizationMaxError;
 	bool _startNewMapOnLoopClosure;
 	bool _startNewMapOnGoodSignature;
 	float _goalReachedRadius; // meters
@@ -137,6 +137,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Descriptors_extraction, ms);
 	RTABMAP_STATS(TimingMem, Rectification, ms);
 	RTABMAP_STATS(TimingMem, Keypoints_3D, ms);
+	RTABMAP_STATS(TimingMem, Keypoints_3D_motion, ms);
 	RTABMAP_STATS(TimingMem, Joining_dictionary_update, ms);
 	RTABMAP_STATS(TimingMem, Add_new_words, ms);
 	RTABMAP_STATS(TimingMem, Compressing_data, ms);
@@ -86,6 +86,7 @@ public:
 	bool isValidForRectification() const {return left_.isValidForRectification() && right_.isValidForRectification();}

 	void initRectificationMap() {left_.initRectificationMap(); right_.initRectificationMap();}
+	bool isRectificationMapInitialized() {return left_.isRectificationMapInitialized() && right_.isRectificationMapInitialized();}

 	void setName(const std::string & name, const std::string & leftSuffix = "left", const std::string & rightSuffix = "right");
 	const std::string & name() const {return name_;}
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UStl.h>
 #include <opencv2/imgproc/imgproc.hpp>

 namespace rtabmap {
@@ -57,7 +58,7 @@ CameraModel::CameraModel(
 		localTransform_(localTransform)
 {
 	UASSERT(K_.empty() || (K_.rows == 3 && K_.cols == 3 && K_.type() == CV_64FC1));
-	UASSERT(D_.empty() || (D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8) && D_.type() == CV_64FC1));
+	UASSERT(D_.empty() || (D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8) && D_.type() == CV_64FC1));
 	UASSERT(R_.empty() || (R_.rows == 3 && R_.cols == 3 && R_.type() == CV_64FC1));
 	UASSERT(P_.empty() || (P_.rows == 3 && P_.cols == 4 && P_.type() == CV_64FC1));
 }
@@ -153,12 +154,33 @@ CameraModel::CameraModel(
 void CameraModel::initRectificationMap()
 {
 	UASSERT(imageSize_.height > 0 && imageSize_.width > 0);
-	UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8));
+	UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8));
 	UASSERT(R_.rows == 3 && R_.cols == 3);
 	UASSERT(P_.rows == 3 && P_.cols == 4);
 	// init rectification map
 	UINFO("Initialize rectify map");
-	cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+	if(D_.cols == 6)
+	{
+#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)))
+		// Equidistant / FishEye
+		// get only k parameters (k1,k2,p1,p2,k3,k4)
+		cv::Mat D(1, 4, CV_64FC1);
+		D.at<double>(0,0) = D_.at<double>(0,1);
+		D.at<double>(0,1) = D_.at<double>(0,2);
+		D.at<double>(0,2) = D_.at<double>(0,4);
+		D.at<double>(0,3) = D_.at<double>(0,5);
+		cv::fisheye::initUndistortRectifyMap(K_, D, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+	}
+	else
+#else
+		UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+				CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+	}
+#endif
+	{
+		// RadialTangential
+		cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+	}
 }

 void CameraModel::setImageSize(const cv::Size & size)
@@ -263,6 +285,27 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
 				UWARN("Missing \"distorsion_coefficients\" field in \"%s\"", filePath.c_str());
 			}

+			n = fs["distortion_model"];
+			if(n.type() != cv::FileNode::NONE)
+			{
+				std::string distortionModel = (std::string)n;
+				if(D_.cols>=4 &&
+				  (uStrContains(distortionModel, "fisheye") ||
+				   uStrContains(distortionModel, "equidistant")))
+				{
+					cv::Mat D = cv::Mat::zeros(1,6,CV_64FC1);
+					D.at<double>(0,0) = D_.at<double>(0,0);
+					D.at<double>(0,1) = D_.at<double>(0,1);
+					D.at<double>(0,4) = D_.at<double>(0,2);
+					D.at<double>(0,5) = D_.at<double>(0,3);
+					D_ = D;
+				}
+			}
+			else
+			{
+				UWARN("Missing \"distortion_model\" field in \"%s\"", filePath.c_str());
+			}
+
 			n = fs["rectification_matrix"];
 			if(n.type() != cv::FileNode::NONE)
 			{
@@ -347,20 +390,33 @@ bool CameraModel::save(const std::string & directory) const

 		if(!D_.empty())
 		{
+			cv::Mat D = D_;
+			if(D_.cols == 6)
+			{
+				D = cv::Mat(1,4,CV_64FC1);
+				D.at<double>(0,0) = D_.at<double>(0,0);
+				D.at<double>(0,1) = D_.at<double>(0,1);
+				D.at<double>(0,2) = D_.at<double>(0,4);
+				D.at<double>(0,3) = D_.at<double>(0,5);
+			}
 			fs << "distortion_coefficients" << "{";
-			fs << "rows" << D_.rows;
-			fs << "cols" << D_.cols;
-			fs << "data" << std::vector<double>((double*)D_.data, ((double*)D_.data)+(D_.rows*D_.cols));
+			fs << "rows" << D.rows;
+			fs << "cols" << D.cols;
+			fs << "data" << std::vector<double>((double*)D.data, ((double*)D.data)+(D.rows*D.cols));
 			fs << "}";

 			// compaibility with ROS
-			if(D_.cols > 5)
+			if(D_.cols == 6)
 			{
-				fs << "distortion_model" << "rational_polynomial";
+				fs << "distortion_model" << "equidistant"; // equidistant, fisheye
+			}
+			else if(D.cols > 5)
+			{
+				fs << "distortion_model" << "rational_polynomial"; // rad tan
 			}
 			else
 			{
-				fs << "distortion_model" << "plumb_bob";
+				fs << "distortion_model" << "plumb_bob"; // rad tan
 			}
 		}

@@ -37,6 +37,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>

 #include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/videoio/videoio_c.h>
+#endif

 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
@@ -39,6 +39,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UMath.h>

+#include <opencv2/imgproc/types_c.h>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/videoio/videoio_c.h>
+#endif
+
 #ifdef RTABMAP_OPENNI
 #include <pcl/io/openni_grabber.h>
 #include <pcl/io/oni_grabber.h>
@@ -2613,6 +2618,7 @@ CameraRealSense::CameraRealSense(
 	presetDepth_(presetDepth),
 	computeOdometry_(computeOdometry),
 	depthScaledToRGBSize_(false),
+	rgbSource_(kColor),
 	slam_(0)
 #endif
 {
@@ -2626,14 +2632,18 @@ CameraRealSense::~CameraRealSense()
 	UDEBUG("");
 	if(dev_)
 	{
-		if(slam_!=0)
+		try
 		{
-			dev_->stop(rs::source::all_sources);
-		}
-		else
-		{
-			dev_->stop();
+			if(slam_!=0)
+			{
+				dev_->stop(rs::source::all_sources);
+			}
+			else
+			{
+				dev_->stop();
+			}
 		}
+		catch(const rs::error & error){UWARN("%s", error.what());}
 		dev_ = 0;
 	}
 	UDEBUG("");
@@ -2680,6 +2690,118 @@ bool setStreamConfigIntrin(
 }
 #endif

+void CameraRealSense::setDepthScaledToRGBSize(bool enabled) {
+#ifdef RTABMAP_REALSENSE
+	depthScaledToRGBSize_ = enabled;
+#endif
+}
+void CameraRealSense::setRGBSource(RGBSource source)
+{
+#ifdef RTABMAP_REALSENSE
+	rgbSource_ = source;
+#endif
+}
+
+#ifdef RTABMAP_REALSENSE
+template<class GET_DEPTH, class TRANSFER_PIXEL> void align_images(const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, GET_DEPTH get_depth, TRANSFER_PIXEL transfer_pixel)
+{
+	// Iterate over the pixels of the depth image
+#pragma omp parallel for schedule(dynamic)
+	for(int depth_y = 0; depth_y < depth_intrin.height; ++depth_y)
+	{
+		int depth_pixel_index = depth_y * depth_intrin.width;
+		for(int depth_x = 0; depth_x < depth_intrin.width; ++depth_x, ++depth_pixel_index)
+		{
+			// Skip over depth pixels with the value of zero, we have no depth data so we will not write anything into our aligned images
+			if(float depth = get_depth(depth_pixel_index))
+			{
+				// Map the top-left corner of the depth pixel onto the other image
+				float depth_pixel[2] = {depth_x-0.5f, depth_y-0.5f}, depth_point[3], other_point[3], other_pixel[2];
+				rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
+				rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
+				rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
+				const int other_x0 = static_cast<int>(other_pixel[0] + 0.5f);
+				const int other_y0 = static_cast<int>(other_pixel[1] + 0.5f);
+
+				// Map the bottom-right corner of the depth pixel onto the other image
+				depth_pixel[0] = depth_x+0.5f; depth_pixel[1] = depth_y+0.5f;
+				rs_deproject_pixel_to_point(depth_point, &depth_intrin, depth_pixel, depth);
+				rs_transform_point_to_point(other_point, &depth_to_other, depth_point);
+				rs_project_point_to_pixel(other_pixel, &other_intrin, other_point);
+				const int other_x1 = static_cast<int>(other_pixel[0] + 0.5f);
+				const int other_y1 = static_cast<int>(other_pixel[1] + 0.5f);
+
+				if(other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height) continue;
+
+				// Transfer between the depth pixels and the pixels inside the rectangle on the other image
+				for(int y=other_y0; y<=other_y1; ++y) for(int x=other_x0; x<=other_x1; ++x) transfer_pixel(depth_pixel_index, y * other_intrin.width + x);
+			}
+		}
+	}
+}
+typedef uint8_t byte;
+
+void align_z_to_other(byte * z_aligned_to_other, const uint16_t * z_pixels, float z_scale, const rs_intrinsics & z_intrin, const rs_extrinsics & z_to_other, const rs_intrinsics & other_intrin)
+{
+	auto out_z = (uint16_t *)(z_aligned_to_other);
+	align_images(z_intrin, z_to_other, other_intrin,
+		[z_pixels, z_scale](int z_pixel_index) { return z_scale * z_pixels[z_pixel_index]; },
+		[out_z, z_pixels](int z_pixel_index, int other_pixel_index) { out_z[other_pixel_index] = out_z[other_pixel_index] ? std::min(out_z[other_pixel_index],z_pixels[z_pixel_index]) : z_pixels[z_pixel_index]; });
+}
+
+void align_disparity_to_other(byte * disparity_aligned_to_other, const uint16_t * disparity_pixels, float disparity_scale, const rs_intrinsics & disparity_intrin, const rs_extrinsics & disparity_to_other, const rs_intrinsics & other_intrin)
+{
+	auto out_disparity = (uint16_t *)(disparity_aligned_to_other);
+	align_images(disparity_intrin, disparity_to_other, other_intrin,
+		[disparity_pixels, disparity_scale](int disparity_pixel_index) { return disparity_scale / disparity_pixels[disparity_pixel_index]; },
+		[out_disparity, disparity_pixels](int disparity_pixel_index, int other_pixel_index) { out_disparity[other_pixel_index] = disparity_pixels[disparity_pixel_index]; });
+}
+
+template<int N> struct bytes { char b[N]; };
+template<int N, class GET_DEPTH> void align_other_to_depth_bytes(byte * other_aligned_to_depth, GET_DEPTH get_depth, const rs_intrinsics & depth_intrin, const rs_extrinsics & depth_to_other, const rs_intrinsics & other_intrin, const byte * other_pixels)
+{
+	auto in_other = (const bytes<N> *)(other_pixels);
+	auto out_other = (bytes<N> *)(other_aligned_to_depth);
+	align_images(depth_intrin, depth_to_other, other_intrin, get_depth,
+		[out_other, in_other](int depth_pixel_index, int other_pixel_index) { out_other[depth_pixel_index] = in_other[other_pixel_index]; });
+}
+
+/////////////////////////
+// Image rectification //
+/////////////////////////
+
+std::vector<int> compute_rectification_table(const rs_intrinsics & rect_intrin, const rs_extrinsics & rect_to_unrect, const rs_intrinsics & unrect_intrin)
+{
+	std::vector<int> rectification_table;
+	rectification_table.resize(rect_intrin.width * rect_intrin.height);
+	align_images(rect_intrin, rect_to_unrect, unrect_intrin, [](int) { return 1.0f; },
+		[&rectification_table](int rect_pixel_index, int unrect_pixel_index) { rectification_table[rect_pixel_index] = unrect_pixel_index; });
+	return rectification_table;
+}
+
+template<class T> void rectify_image_pixels(T * rect_pixels, const std::vector<int> & rectification_table, const T * unrect_pixels)
+{
+	for(auto entry : rectification_table) *rect_pixels++ = unrect_pixels[entry];
+}
+
+void rectify_image(uint8_t * rect_pixels, const std::vector<int> & rectification_table, const uint8_t * unrect_pixels, rs_format format)
+{
+	switch(format)
+	{
+	case RS_FORMAT_Y8:
+		return rectify_image_pixels((bytes<1> *)rect_pixels, rectification_table, (const bytes<1> *)unrect_pixels);
+	case RS_FORMAT_Y16: case RS_FORMAT_Z16:
+		return rectify_image_pixels((bytes<2> *)rect_pixels, rectification_table, (const bytes<2> *)unrect_pixels);
+	case RS_FORMAT_RGB8: case RS_FORMAT_BGR8:
+		return rectify_image_pixels((bytes<3> *)rect_pixels, rectification_table, (const bytes<3> *)unrect_pixels);
+	case RS_FORMAT_RGBA8: case RS_FORMAT_BGRA8:
+		return rectify_image_pixels((bytes<4> *)rect_pixels, rectification_table, (const bytes<4> *)unrect_pixels);
+	default:
+		assert(false); // NOTE: rectify_image_pixels(...) is not appropriate for RS_FORMAT_YUYV images, no logic prevents U/V channels from being written to one another
+	}
+}
+#endif
+
 bool CameraRealSense::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	UDEBUG("");
@@ -2687,10 +2809,22 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str

 	if(dev_)
 	{
-		dev_->stop(rs::source::all_sources);
+		try
+		{
+			if(slam_!=0)
+			{
+				dev_->stop(rs::source::all_sources);
+			}
+			else
+			{
+				dev_->stop();
+			}
+		}
+		catch(const rs::error & error){UWARN("%s", error.what());}
 		dev_ = 0;
 	}
 	bufferedFrames_.clear();
+	rsRectificationTable_.clear();

 #ifdef RTABMAP_REALSENSE_SLAM
 	motionSeq_[0] = motionSeq_[1] = 0;
@@ -2730,29 +2864,78 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 	UINFO("    Preset RGB: %d", presetRGB_);
 	UINFO("    Preset Depth: %d", presetDepth_);

-	bool computeOdometry = false;
-#ifdef RTABMAP_REALSENSE_SLAM
-	if (name.find("ZR300") != std::string::npos && computeOdometry_)
-	{
-		// Only enable ZR300 functionality if fisheye stream is enabled.
-		// Accel/Gyro automatically enabled when fisheye requested
-		computeOdometry = true;
-	}
+#ifndef RTABMAP_REALSENSE_SLAM
+	computeOdometry_ = false;
 #endif

+	if (name.find("ZR300") == std::string::npos)
+	{
+		// Only enable ZR300 functionality odometry if fisheye stream is enabled.
+		// Accel/Gyro automatically enabled when fisheye requested
+		computeOdometry_ = false;
+		// Only ZR300 has fisheye
+		if(rgbSource_ == kFishEye)
+		{
+			UWARN("Fisheye cannot be used with %s camera, using color instead...", name.c_str());
+			rgbSource_ = kColor;
+		}
+	}
+
+	rs::intrinsics depth_intrin;
+	rs::intrinsics fisheye_intrin;
+	rs::intrinsics color_intrin;
 	// Configure depth and color to run with the device's preferred settings
 	UINFO("Enabling streams...");
 	// R200: 
 	//    0=640x480   vs 480x360
 	//    1=1920x1080 vs 640x480
 	//    2=640x480   vs 320x240
-	dev_->enable_stream(rs::stream::depth, (rs::preset)presetDepth_);
-	dev_->enable_stream(rs::stream::color, (rs::preset)presetRGB_);
+	try {

-	rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
-	rs::intrinsics color_intrin = dev_->get_stream_intrinsics(rs::stream::color);
-	UINFO("    RGB:   %dx%d", color_intrin.width, color_intrin.height);
-	UINFO("    Depth: %dx%d", depth_intrin.width, depth_intrin.height);
+		// left/rgb stream
+		if(rgbSource_==kFishEye || computeOdometry_)
+		{
+			dev_->enable_stream(rs::stream::fisheye, 640, 480, rs::format::raw8, 30);
+			if(computeOdometry_)
+			{
+				// Needed to align image timestamps to common clock-domain with the motion events
+				dev_->set_option(rs::option::fisheye_strobe, 1);
+			}
+			// This option causes the fisheye image to be acquired in-sync with the depth image.
+			dev_->set_option(rs::option::fisheye_external_trigger, 1);
+			dev_->set_option(rs::option::fisheye_color_auto_exposure, 1);
+			fisheye_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
+			UINFO("    Fisheye:   %dx%d", fisheye_intrin.width, fisheye_intrin.height);
+			if(rgbSource_==kFishEye)
+			{
+				color_intrin = fisheye_intrin; // not rectified
+			}
+		}
+		if(rgbSource_!=kFishEye)
+		{
+			dev_->enable_stream(rs::stream::color, (rs::preset)presetRGB_);
+			color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color); // rectified
+			UINFO("    RGB:   %dx%d", color_intrin.width, color_intrin.height);
+
+			if(rgbSource_==kInfrared)
+			{
+				dev_->enable_stream(rs::stream::infrared, (rs::preset)presetRGB_);
+				color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared); // rectified
+				UINFO("    IR left:   %dx%d", color_intrin.width, color_intrin.height);
+			}
+		}
+
+		dev_->enable_stream(rs::stream::depth, (rs::preset)presetDepth_);
+		depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth); // rectified
+		UINFO("    Depth: %dx%d", depth_intrin.width, depth_intrin.height);
+	}
+	catch(const rs::error & error)
+	{
+		UERROR("Failed starting the streams: %s", error.what());
+		return false;
+	}
+
+	Transform imu2Camera = Transform::getIdentity();

 #ifdef RTABMAP_REALSENSE_SLAM
 	UDEBUG("Setup frame callback");
@@ -2782,9 +2965,50 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 		  frame.get_stride()
 		};
 		cv::Mat image;
-		if(frame.get_format() == rs::format::raw8)
+		if(frame.get_format() == rs::format::raw8 || frame.get_format() == rs::format::y8)
 		{
 			image = cv::Mat(height, width, CV_8UC1, (unsigned char*)frame.get_data());
+			if(frame.get_stream_type() == rs::stream::fisheye)
+			{
+				// fisheye always received just after the depth image (doesn't have exact timestamp with depth)
+				if(bufferedFrames_.size())
+				{
+					bufferedFrames_.rbegin()->second.first = image.clone();
+					UScopeMutex lock(dataMutex_);
+					bool notify = lastSyncFrames_.first.empty();
+					lastSyncFrames_ = bufferedFrames_.rbegin()->second;
+					if(notify)
+					{
+						dataReady_.release();
+					}
+					bufferedFrames_.clear();
+				}
+			}
+			else if(frame.get_stream_type() == rs::stream::infrared) // infrared (does have exact timestamp with depth)
+			{
+				if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
+				{
+					bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
+					UScopeMutex lock(dataMutex_);
+					bool notify = lastSyncFrames_.first.empty();
+					lastSyncFrames_ = bufferedFrames_.find(frame.get_timestamp())->second;
+					if(notify)
+					{
+						dataReady_.release();
+					}
+					bufferedFrames_.erase(frame.get_timestamp());
+				}
+				else
+				{
+					bufferedFrames_.insert(std::make_pair(frame.get_timestamp(), std::make_pair(image.clone(), cv::Mat())));
+				}
+				if(bufferedFrames_.size()>5)
+				{
+					UWARN("Frames cannot be synchronized!");
+					bufferedFrames_.clear();
+				}
+				return;
+			}
 		}
 		else if(frame.get_format() == rs::format::z16)
 		{
@@ -2813,7 +3037,15 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 		}
 		else if(frame.get_format() == rs::format::rgb8)
 		{
-			image = cv::Mat(height, width, CV_8UC3, (unsigned char*)frame.get_data());
+			if(rsRectificationTable_.size())
+			{
+				image = cv::Mat(height, width, CV_8UC3);
+				rectify_image(image.data, rsRectificationTable_, (unsigned char*)frame.get_data(), (rs_format)frame.get_format());
+			}
+			else
+			{
+				image = cv::Mat(height, width, CV_8UC3, (unsigned char*)frame.get_data());
+			}
 			if(bufferedFrames_.find(frame.get_timestamp()) != bufferedFrames_.end())
 			{
 				bufferedFrames_.find(frame.get_timestamp())->second.first = image.clone();
@@ -2863,26 +3095,27 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 		}
 	};

+	UDEBUG("");
 	// Setup stream callback for stream
-	if(computeOdometry)
+	if(computeOdometry_ || rgbSource_ == kFishEye)
 	{
 		dev_->set_frame_callback(rs::stream::fisheye, frameCallback);
 	}
-	dev_->set_frame_callback(rs::stream::depth, frameCallback);
-	dev_->set_frame_callback(rs::stream::color, frameCallback);
-
-	if (computeOdometry)
+	if(rgbSource_ == kInfrared)
 	{
-		dev_->enable_stream(rs::stream::fisheye, 640, 480, rs::format::raw8, 30);
-		rs::intrinsics fisheye_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
-		UINFO("    Fish: %dx%d", fisheye_intrin.width, fisheye_intrin.height);
+		dev_->set_frame_callback(rs::stream::infrared, frameCallback);
+	}
+	else if(rgbSource_ == kColor)
+	{
+		dev_->set_frame_callback(rs::stream::color, frameCallback);
+	}

-		// Needed to align image timestamps to common clock-domain with the motion events
-		dev_->set_option(rs::option::fisheye_strobe, 1);
-		// This option causes the fisheye image to be aquired in-sync with the depth image.
-		dev_->set_option(rs::option::fisheye_external_trigger, 1);
-		dev_->set_option(rs::option::fisheye_color_auto_exposure, 1);
+	dev_->set_frame_callback(rs::stream::depth, frameCallback);

+	UDEBUG("");
+
+	if (computeOdometry_)
+	{
 		UDEBUG("Setup motion callback");
 		//define callback to the motion events and set it.
 		std::function<void(rs::motion_data)> motion_callback;
@@ -2990,23 +3223,137 @@ bool CameraRealSense::init(const std::string & calibrationFolder, const std::str
 			return false;
 		}

-		dev_->start(rs::source::all_sources);
+		rs::extrinsics fisheye2imu = dev_->get_motion_extrinsics_from(rs::stream::fisheye);
+		imu2Camera = Transform(
+				fisheye2imu.rotation[0], fisheye2imu.rotation[1], fisheye2imu.rotation[2], fisheye2imu.translation[0],
+				fisheye2imu.rotation[3], fisheye2imu.rotation[4], fisheye2imu.rotation[5], fisheye2imu.translation[1],
+				fisheye2imu.rotation[6], fisheye2imu.rotation[7], fisheye2imu.rotation[8], fisheye2imu.translation[2]).inverse();
+
+		if(rgbSource_ == kInfrared)
+		{
+			rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::infrared);
+			Transform fisheye2Color = Transform(
+					color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
+					color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
+					color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
+			imu2Camera *= fisheye2Color;
+		}
+		else if(rgbSource_ == kColor)
+		{
+			rs::extrinsics color2Fisheye = dev_->get_extrinsics(rs::stream::fisheye, rs::stream::rectified_color);
+			Transform fisheye2Color = Transform(
+					color2Fisheye.rotation[0], color2Fisheye.rotation[1], color2Fisheye.rotation[2], color2Fisheye.translation[0],
+					color2Fisheye.rotation[3], color2Fisheye.rotation[4], color2Fisheye.rotation[5], color2Fisheye.translation[1],
+					color2Fisheye.rotation[6], color2Fisheye.rotation[7], color2Fisheye.rotation[8], color2Fisheye.translation[2]).inverse();
+			imu2Camera *= fisheye2Color;
+		}
+
+		UDEBUG("start device!");
+		try
+		{
+			dev_->start(rs::source::all_sources);
+		}
+		catch(const rs::error & error)
+		{
+			UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
+			return false;
+		}
 	}
 	else
 	{
-		dev_->start();
+		UDEBUG("start device!");
+		try
+		{
+			dev_->start();
+		}
+		catch(const rs::error & error)
+		{
+			UERROR("Failed starting the device: %s (try to unplug/plug the camera)", error.what());
+			return false;
+		}
 	}
 #else
-	dev_->start();
 	try {
+		dev_->start();
 		dev_->wait_for_frames();
 	}
 	catch (const rs::error & e)
 	{
-		UERROR("Exception: %s", e.what());
+		UERROR("Exception: %s (try to unplug/plug the camera)", e.what());
 	}
 #endif

+	cv::Mat D;
+	if(rgbSource_ == kFishEye)
+	{
+		// ftheta/equidistant model
+		D = cv::Mat::zeros(1,6,CV_64FC1);
+		D.at<double>(0,0) = color_intrin.coeffs[0];
+		D.at<double>(0,1) = color_intrin.coeffs[1];
+		D.at<double>(0,4) = color_intrin.coeffs[2];
+		D.at<double>(0,5) = color_intrin.coeffs[3];
+	}
+	else
+	{
+		// Brown-Conrady / radtan
+		D = cv::Mat::zeros(1,5,CV_64FC1);
+		D.at<double>(0,0) = color_intrin.coeffs[0];
+		D.at<double>(0,1) = color_intrin.coeffs[1];
+		D.at<double>(0,2) = color_intrin.coeffs[2];
+		D.at<double>(0,3) = color_intrin.coeffs[3];
+		D.at<double>(0,4) = color_intrin.coeffs[4];
+	}
+	cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
+	K.at<double>(0,0) = color_intrin.fx;
+	K.at<double>(1,1) = color_intrin.fy;
+	K.at<double>(0,2) = color_intrin.ppx;
+	K.at<double>(1,2) = color_intrin.ppy;
+	cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+	cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
+	K(cv::Range(0,2), cv::Range(0,3)).copyTo(P(cv::Range(0,2), cv::Range(0,3)));
+	cameraModel_ = CameraModel(
+			dev_->get_name(),
+			cv::Size(color_intrin.width, color_intrin.height),
+			K,
+			D,
+			R,
+			P,
+			this->getLocalTransform()*imu2Camera);
+
+	UDEBUG("");
+
+	if(rgbSource_ == kColor)
+	{
+		rs::extrinsics rect_to_unrect = dev_->get_extrinsics(rs::stream::rectified_color, rs::stream::color);
+		rs::intrinsics unrect_intrin = dev_->get_stream_intrinsics(rs::stream::color);
+		rsRectificationTable_ = compute_rectification_table(color_intrin, rect_to_unrect, unrect_intrin);
+	}
+	else if(rgbSource_ == kFishEye)
+	{
+		UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
+		if(!calibrationFolder.empty() && !cameraName.empty())
+		{
+			CameraModel model;
+			if(!model.load(calibrationFolder, cameraName))
+			{
+				UWARN("Failed to load calibration \"%s\" in \"%s\" folder, you should calibrate the camera!",
+						cameraName.c_str(), calibrationFolder.c_str());
+			}
+			else
+			{
+				UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
+						model.fx(),
+						model.fy(),
+						model.cx(),
+						model.cy());
+				cameraModel_ = model;
+				cameraModel_.setName(cameraName);
+				cameraModel_.initRectificationMap();
+				cameraModel_.setLocalTransform(this->getLocalTransform()*imu2Camera);
+			}
+		}
+	}
+
 	uSleep(1000); // ignore the first frames
 	UINFO("Enabling streams...done!");

@@ -3068,8 +3415,23 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)

 		// Retrieve camera parameters for mapping between depth and color
 		rs::intrinsics depth_intrin = dev_->get_stream_intrinsics(rs::stream::depth);
-		rs::extrinsics depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::color);
-		rs::intrinsics color_intrin = dev_->get_stream_intrinsics(rs::stream::color);
+		rs::extrinsics depth_to_color;
+		rs::intrinsics color_intrin;
+		if(rgbSource_ == kFishEye)
+		{
+			depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::fisheye);
+			color_intrin = dev_->get_stream_intrinsics(rs::stream::fisheye);
+		}
+		else if(rgbSource_ == kInfrared)
+		{
+			depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::infrared);
+			color_intrin = dev_->get_stream_intrinsics(rs::stream::infrared);
+		}
+		else // color
+		{
+			depth_to_color = dev_->get_extrinsics(rs::stream::depth, rs::stream::rectified_color);
+			color_intrin = dev_->get_stream_intrinsics(rs::stream::rectified_color);
+		}

 #ifdef RTABMAP_REALSENSE_SLAM
 		if(!dataReady_.acquire(1, 5000))
@@ -3077,6 +3439,7 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 			UWARN("Not received new frames since 5 seconds, end of stream reached!");
 			return data;
 		}
+
 		{
 			UScopeMutex lock(dataMutex_);
 			rgb = lastSyncFrames_.first;
@@ -3101,84 +3464,124 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)

 		// Retrieve our images
 		depthIn = cv::Mat(depth_intrin.height, depth_intrin.width, CV_16UC1, (unsigned char*)dev_->get_frame_data(rs::stream::depth));
-		rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC3, (unsigned char*)dev_->get_frame_data(rs::stream::color));
-#endif
-
-		float scale = dev_->get_depth_scale();
-
-		// factory registration...
-		cv::Mat bgr;
-		cv::cvtColor(rgb, bgr, CV_RGB2BGR);
-
-		CameraModel model(
-			color_intrin.fx, //fx
-			color_intrin.fy, //fy
-			color_intrin.ppx,  //cx
-			color_intrin.ppy,  //cy
-			this->getLocalTransform(),
-			0,
-			bgr.size());
-
-		cv::Mat depth;
-		if (color_intrin.width % depth_intrin.width == 0 && color_intrin.height % depth_intrin.height == 0 &&
-			depth_intrin.width < color_intrin.width &&
-			depth_intrin.height < color_intrin.height &&
-			!depthScaledToRGBSize_)
+		if(rgbSource_ == kFishEye)
 		{
-			//we can keep the depth image size as is
-			depth = cv::Mat::zeros(cv::Size(depth_intrin.width, depth_intrin.height), CV_16UC1);
-			float scaleX = float(depth_intrin.width) / float(color_intrin.width);
-			float scaleY = float(depth_intrin.height) / float(color_intrin.height);
-			color_intrin.fx *= scaleX;
-			color_intrin.fy *= scaleY;
-			color_intrin.ppx *= scaleX;
-			color_intrin.ppy *= scaleY;
-			color_intrin.height = depth_intrin.height;
-			color_intrin.width = depth_intrin.width;
+			rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::fisheye));
+		}
+		else if(rgbSource_ == kInfrared)
+		{
+			rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC1, (unsigned char*)dev_->get_frame_data(rs::stream::infrared));
 		}
 		else
 		{
-			//depth to color
-			depth = cv::Mat::zeros(bgr.size(), CV_16UC1);
+			rgb = cv::Mat(color_intrin.height, color_intrin.width, CV_8UC3, (unsigned char*)dev_->get_frame_data(rs::stream::color));
 		}
-		for (int dy = 0; dy < depth_intrin.height; ++dy)
+#endif
+
+		// factory registration...
+		cv::Mat bgr;
+		if(rgbSource_ != kColor)
 		{
-			for (int dx = 0; dx < depth_intrin.width; ++dx)
+			bgr = rgb;
+		}
+		else
+		{
+			cv::cvtColor(rgb, bgr, CV_RGB2BGR);
+		}
+
+		bool rectified = false;
+		if(rgbSource_ == kFishEye && cameraModel_.isRectificationMapInitialized())
+		{
+			bgr = cameraModel_.rectifyImage(bgr);
+			rectified = true;
+			color_intrin.fx = cameraModel_.fx();
+			color_intrin.fy = cameraModel_.fy();
+			color_intrin.ppx = cameraModel_.cx();
+			color_intrin.ppy = cameraModel_.cy();
+			UASSERT_MSG(color_intrin.width == cameraModel_.imageWidth() && color_intrin.height == cameraModel_.imageHeight(),
+					uFormat("color_intrin=%dx%d cameraModel_=%dx%d",
+							color_intrin.width, color_intrin.height, cameraModel_.imageWidth(), cameraModel_.imageHeight()).c_str());
+			((rs_intrinsics*)&color_intrin)->model = RS_DISTORTION_NONE;
+		}
+#ifndef RTABMAP_REALSENSE_SLAM
+		else if(rgbSource_ != kColor)
+		{
+			bgr = bgr.clone();
+		}
+#endif
+
+		cv::Mat depth;
+		if(rgbSource_ != kFishEye || rectified)
+		{
+			if (color_intrin.width % depth_intrin.width == 0 && color_intrin.height % depth_intrin.height == 0 &&
+				depth_intrin.width < color_intrin.width &&
+				depth_intrin.height < color_intrin.height &&
+				!depthScaledToRGBSize_)
 			{
-				// Retrieve the 16-bit depth value and map it into a depth in meters
-				uint16_t depth_value = depthIn.at<unsigned short>(dy,dx);
-				float depth_in_meters = depth_value * scale;
+				//we can keep the depth image size as is
+				depth = cv::Mat::zeros(cv::Size(depth_intrin.width, depth_intrin.height), CV_16UC1);
+				float scaleX = float(depth_intrin.width) / float(color_intrin.width);
+				float scaleY = float(depth_intrin.height) / float(color_intrin.height);
+				color_intrin.fx *= scaleX;
+				color_intrin.fy *= scaleY;
+				color_intrin.ppx *= scaleX;
+				color_intrin.ppy *= scaleY;
+				color_intrin.height = depth_intrin.height;
+				color_intrin.width = depth_intrin.width;
+			}
+			else
+			{
+				//depth to color
+				depth = cv::Mat::zeros(bgr.size(), CV_16UC1);
+			}

-				// Skip over pixels with a depth value of zero, which is used to indicate no data
-				if (depth_value == 0 || depth_in_meters>10.0f) continue;
-
-				// Map from pixel coordinates in the depth image to pixel coordinates in the color image
-				rs::float2 depth_pixel = { (float)dx, (float)dy };
-				rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
-				rs::float3 color_point = depth_to_color.transform(depth_point);
-				rs::float2 color_pixel = color_intrin.project(color_point);
-
-				int pdx = color_pixel.x;
-				int pdy = color_pixel.y;
-				if (uIsInBounds(pdx, 0, depth.cols) && uIsInBounds(pdy, 0, depth.rows))
+			float scale = dev_->get_depth_scale();
+			for (int dy = 0; dy < depth_intrin.height; ++dy)
+			{
+				for (int dx = 0; dx < depth_intrin.width; ++dx)
 				{
-					depth.at<unsigned short>(pdy, pdx) = (unsigned short)(depth_in_meters*1000.0f); // convert to mm
+					// Retrieve the 16-bit depth value and map it into a depth in meters
+					uint16_t depth_value = depthIn.at<unsigned short>(dy,dx);
+					float depth_in_meters = depth_value * scale;
+
+					// Skip over pixels with a depth value of zero, which is used to indicate no data
+					if (depth_value == 0 || depth_in_meters>10.0f) continue;
+
+					// Map from pixel coordinates in the depth image to pixel coordinates in the color image
+					int pdx = dx;
+					int pdy = dy;
+					if(rgbSource_ == kColor || rgbSource_ == kFishEye)
+					{
+						rs::float2 depth_pixel = { (float)dx, (float)dy };
+						rs::float3 depth_point = depth_intrin.deproject(depth_pixel, depth_in_meters);
+						rs::float3 color_point = depth_to_color.transform(depth_point);
+						rs::float2 color_pixel = color_intrin.project(color_point);
+
+						pdx = color_pixel.x;
+						pdy = color_pixel.y;
+					}
+					//else infrared is already registered
+
+					if (uIsInBounds(pdx, 0, depth.cols) && uIsInBounds(pdy, 0, depth.rows))
+					{
+						depth.at<unsigned short>(pdy, pdx) = (unsigned short)(depth_in_meters*1000.0f); // convert to mm
+					}
 				}
 			}
+
+			if (color_intrin.width > depth_intrin.width)
+			{
+				// Fill holes
+				UTimer time;
+				util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);
+				util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);//second pass
+				UDEBUG("Filling depth holes: %fs", time.ticks());
+			}
 		}

-		if (color_intrin.width > depth_intrin.width)
+		if (!bgr.empty() && ((rgbSource_==kFishEye && !rectified) || !depth.empty()))
 		{
-			// Fill holes
-			UTimer time;
-			util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);
-			util2d::fillRegisteredDepthHoles(depth, true, true, color_intrin.width > depth_intrin.width * 2);//second pass
-			UDEBUG("Filling depth holes: %fs", time.ticks());
-		}
-		
-		if (!bgr.empty() && !depth.empty())
-		{
-			data = SensorData(bgr, depth, model, this->getNextSeqID(), UTimer::now());
+			data = SensorData(bgr, depth, cameraModel_, this->getNextSeqID(), UTimer::now());
 #ifdef RTABMAP_REALSENSE_SLAM
 			if(info && slam_)
 			{
@@ -3186,8 +3589,20 @@ SensorData CameraRealSense::captureImage(CameraInfo * info)
 				rs::slam::PoseMatrix4f pose;
 				if(slam_->get_camera_pose(pose) == rs::core::status_no_error)
 				{
-					Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
-					info->odomPose = opticalRotation * rsPoseToTransform(pose) * opticalRotation.inverse();
+					/*rs::slam::tracking_accuracy accuracy = slam_->get_tracking_accuracy();
+					if( accuracy == rs::slam::tracking_accuracy::low ||
+						accuracy == rs::slam::tracking_accuracy::medium ||
+						accuracy == rs::slam::tracking_accuracy::high)*/
+					{
+						// the pose is in camera link or IMU frame, get pose of the color camera
+						Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+						info->odomPose = opticalRotation * rsPoseToTransform(pose) * opticalRotation.inverse();
+						info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005;
+					}
+					/*else
+					{
+						UERROR("Odometry failed: accuracy=%d", accuracy);
+					}*/
 				}
 				else
 				{
@@ -39,6 +39,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UMath.h>

+#include <opencv2/imgproc/types_c.h>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/videoio/videoio_c.h>
+#endif
+
 #ifdef RTABMAP_DC1394
 #include <dc1394/dc1394.h>
 #endif
@@ -61,6 +61,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/common.h>
 #include <rtabmap/core/OccupancyGrid.h>
+#include <opencv2/imgproc/types_c.h>

 namespace rtabmap {

@@ -103,6 +104,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
 	_visCorType(Parameters::defaultVisCorType()),
 	_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
+	_rectifyOnlyFeatures(Parameters::defaultRtabmapRectifyOnlyFeatures()),
 	_covOffDiagonalIgnored(Parameters::defaultMemCovOffDiagIgnored()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
@@ -485,6 +487,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		uInsert(params, ParametersPair(Parameters::kVisCorType(), "0"));
 	}
 	Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
+	Parameters::parse(params, Parameters::kRtabmapRectifyOnlyFeatures(), _rectifyOnlyFeatures);
 	Parameters::parse(params, Parameters::kMemCovOffDiagIgnored(), _covOffDiagonalIgnored);


@@ -798,6 +801,15 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 				   !_signatures.at(*_stMem.rbegin())->getPose().isNull())
 				{
 					UASSERT(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
+					double maxAngVar = uMax3(covariance.at<double>(3,3), covariance.at<double>(4,4), covariance.at<double>(5,5));
+					if(maxAngVar != 1.0 && maxAngVar > 0.1)
+					{
+						UWARN("Very large angular variance (%f) detected! Please fix odometry "
+								"twist covariance, otherwise poor graph optimizations are "
+								"expected and wrong loop closure detections creating a lot "
+								"of errors in the map could be accepted.", maxAngVar);
+					}
+
 					cv::Mat infMatrix;
 					if(_covOffDiagonalIgnored)
 					{
@@ -837,16 +849,19 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
 						*_stMem.rbegin(), signature->id(),
 						_signatures.at(*_stMem.rbegin())->mapId(), signature->mapId());

-				//Tag the first node of the map
-				std::string tag = uFormat("map%d", signature->mapId());
-				if(getSignatureIdByLabel(tag, false) == 0)
+				if(_mapLabelsAdded && isIncremental())
 				{
-					UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
-					signature->setLabel(tag);
+					//Tag the first node of the map
+					std::string tag = uFormat("map%d", signature->mapId());
+					if(getSignatureIdByLabel(tag, false) == 0)
+					{
+						UINFO("Tagging node %d with label \"%s\"", signature->id(), tag.c_str());
+						signature->setLabel(tag);
+					}
 				}
 			}
 		}
-		else if(_mapLabelsAdded)
+		else if(_mapLabelsAdded && isIncremental())
 		{
 			//Tag the first node of the map
 			std::string tag = uFormat("map%d", signature->mapId());
@@ -1502,6 +1517,8 @@ void Memory::clear()
 	_memoryChanged = false;
 	_linksChanged = false;
 	_gpsOrigin = GPS();
+	_rectCameraModels.clear();
+	_rectStereoCameraModel = StereoCameraModel();

 	if(_dbDriver)
 	{
@@ -3681,25 +3698,41 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 	}

-	if(!_imagesAlreadyRectified && !data.imageRaw().empty())
+	bool imagesRectified = _imagesAlreadyRectified;
+	// Stereo must be always rectified because of the stereo correspondence approach
+	if(!imagesRectified && !data.imageRaw().empty() && !(_rectifyOnlyFeatures && data.rightRaw().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;
-		}
+		// we assume that once rtabmap is receiving data, the calibration won't change over time
 		if(data.cameraModels().size())
 		{
+			// Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
 			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());
+			bool initRectMaps = _rectCameraModels.empty();
+			if(initRectMaps)
+			{
+				_rectCameraModels.resize(data.cameraModels().size());
+			}
 			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)));
+					if(initRectMaps)
+					{
+						_rectCameraModels[i] = data.cameraModels()[i];
+						if(!_rectCameraModels[i].isRectificationMapInitialized())
+						{
+							UWARN("Initializing rectification maps for camera %d (only done for the first image received)...", i);
+							_rectCameraModels[i].initRectificationMap();
+							UWARN("Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
+						}
+					}
+					UASSERT(_rectCameraModels[i].imageWidth() == data.cameraModels()[i].imageWidth() &&
+							_rectCameraModels[i].imageHeight() == data.cameraModels()[i].imageHeight());
+					cv::Mat rectifiedImage = _rectCameraModels[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)));
+					imagesRectified = true;
 				}
 				else
 				{
@@ -3714,8 +3747,21 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 		else if(data.stereoCameraModel().isValidForRectification())
 		{
-			data.setImageRaw(data.stereoCameraModel().left().rectifyImage(data.imageRaw()));
-			data.setDepthOrRightRaw(data.stereoCameraModel().right().rectifyImage(data.rightRaw()));
+			if(!_rectStereoCameraModel.isValidForRectification())
+			{
+				_rectStereoCameraModel = data.stereoCameraModel();
+				if(!_rectStereoCameraModel.isRectificationMapInitialized())
+				{
+					UWARN("Initializing rectification maps (only done for the first image received)...");
+					_rectStereoCameraModel.initRectificationMap();
+					UWARN("Initializing rectification maps (only done for the first image received)...done!");
+				}
+			}
+			UASSERT(_rectStereoCameraModel.left().imageWidth() == data.stereoCameraModel().left().imageWidth());
+			UASSERT(_rectStereoCameraModel.left().imageHeight() == data.stereoCameraModel().left().imageHeight());
+			data.setImageRaw(_rectStereoCameraModel.left().rectifyImage(data.imageRaw()));
+			data.setDepthOrRightRaw(_rectStereoCameraModel.right().rectifyImage(data.rightRaw()));
+			imagesRectified = true;
 		}
 		else
 		{
@@ -3744,11 +3790,12 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor

 	int preDecimation = 1;
 	std::vector<cv::Point3f> keypoints3D;
+	SensorData decimatedData;
 	if(!_useOdometryFeatures || data.keypoints().empty() || (int)data.keypoints().size() != data.descriptors().rows)
 	{
 		if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
 		{
-			SensorData decimatedData = data;
+			decimatedData = data;
 			if(_imagePreDecimation > 1)
 			{
 				preDecimation = _imagePreDecimation;
@@ -3784,7 +3831,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			}

 			cv::Mat depthMask;
-			if(!decimatedData.depthRaw().empty() && _depthAsMask)
+			if(imagesRectified && !decimatedData.depthRaw().empty() && _depthAsMask)
 			{
 				if(imageMono.rows % decimatedData.depthRaw().rows == 0 &&
 					imageMono.cols % decimatedData.depthRaw().cols == 0 &&
@@ -3828,13 +3875,134 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				descriptors = cv::Mat();
 			}
-			else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
-					(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
+			else
 			{
-				keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
-				t = timer.ticks();
-				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
-				UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
+				if(!imagesRectified && decimatedData.cameraModels().size())
+				{
+					std::vector<cv::KeyPoint> keypointsValid;
+					keypointsValid.reserve(keypoints.size());
+					cv::Mat descriptorsValid;
+					descriptorsValid.reserve(descriptors.rows);
+
+					//undistort keypoints before projection (RGB-D)
+					if(decimatedData.cameraModels().size() == 1)
+					{
+						std::vector<cv::Point2f> pointsIn, pointsOut;
+						cv::KeyPoint::convert(keypoints,pointsIn);
+						if(decimatedData.cameraModels()[0].D_raw().cols == 6)
+						{
+#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)))
+							// Equidistant / FishEye
+							// get only k parameters (k1,k2,p1,p2,k3,k4)
+							cv::Mat D(1, 4, CV_64FC1);
+							D.at<double>(0,0) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
+							D.at<double>(0,1) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
+							D.at<double>(0,2) = decimatedData.cameraModels()[0].D_raw().at<double>(0,4);
+							D.at<double>(0,3) = decimatedData.cameraModels()[0].D_raw().at<double>(0,5);
+							cv::fisheye::undistortPoints(pointsIn, pointsOut,
+									decimatedData.cameraModels()[0].K_raw(),
+									D,
+									decimatedData.cameraModels()[0].R(),
+									decimatedData.cameraModels()[0].P());
+						}
+						else
+#else
+							UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+									CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+						}
+#endif
+						{
+							//RadialTangential
+							cv::undistortPoints(pointsIn, pointsOut,
+									decimatedData.cameraModels()[0].K_raw(),
+									decimatedData.cameraModels()[0].D_raw(),
+									decimatedData.cameraModels()[0].R(),
+									decimatedData.cameraModels()[0].P());
+						}
+						UASSERT(pointsOut.size() == keypoints.size());
+						for(unsigned int i=0; i<pointsOut.size(); ++i)
+						{
+							if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<decimatedData.cameraModels()[0].imageWidth() &&
+							   pointsOut.at(i).y>=0 && pointsOut.at(i).y<decimatedData.cameraModels()[0].imageHeight())
+							{
+								keypointsValid.push_back(keypoints.at(i));
+								keypointsValid.back().pt.x = pointsOut.at(i).x;
+								keypointsValid.back().pt.y = pointsOut.at(i).y;
+								descriptorsValid.push_back(descriptors.row(i));
+							}
+						}
+					}
+					else
+					{
+						UASSERT(int((decimatedData.imageRaw().cols/decimatedData.cameraModels().size())*decimatedData.cameraModels().size()) == decimatedData.imageRaw().cols);
+						float subImageWidth = decimatedData.imageRaw().cols/decimatedData.cameraModels().size();
+						for(unsigned int i=0; i<keypoints.size(); ++i)
+						{
+							int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
+							UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)decimatedData.cameraModels().size(),
+									uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
+											cameraIndex, (int)decimatedData.cameraModels().size(), keypoints[i].pt.x, subImageWidth, decimatedData.cameraModels()[0].imageWidth()).c_str());
+
+							std::vector<cv::Point2f> pointsIn, pointsOut;
+							pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
+							if(decimatedData.cameraModels()[cameraIndex].D_raw().cols == 6)
+							{
+#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)))
+								// Equidistant / FishEye
+								// get only k parameters (k1,k2,p1,p2,k3,k4)
+								cv::Mat D(1, 4, CV_64FC1);
+								D.at<double>(0,0) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
+								D.at<double>(0,1) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
+								D.at<double>(0,2) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
+								D.at<double>(0,3) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
+								cv::fisheye::undistortPoints(pointsIn, pointsOut,
+										decimatedData.cameraModels()[cameraIndex].K_raw(),
+										D,
+										decimatedData.cameraModels()[cameraIndex].R(),
+										decimatedData.cameraModels()[cameraIndex].P());
+							}
+							else
+#else
+								UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
+										CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
+							}
+#endif
+							{
+								//RadialTangential
+								cv::undistortPoints(pointsIn, pointsOut,
+										decimatedData.cameraModels()[cameraIndex].K_raw(),
+										decimatedData.cameraModels()[cameraIndex].D_raw(),
+										decimatedData.cameraModels()[cameraIndex].R(),
+										decimatedData.cameraModels()[cameraIndex].P());
+							}
+
+							if(pointsOut[0].x>=0 && pointsOut[0].x<decimatedData.cameraModels()[cameraIndex].imageWidth() &&
+							   pointsOut[0].y>=0 && pointsOut[0].y<decimatedData.cameraModels()[cameraIndex].imageHeight())
+							{
+								keypointsValid.push_back(keypoints.at(i));
+								keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
+								keypointsValid.back().pt.y = pointsOut[0].y;
+								descriptorsValid.push_back(descriptors.row(i));
+							}
+						}
+					}
+
+					keypoints = keypointsValid;
+					descriptors = descriptorsValid;
+
+					t = timer.ticks();
+					if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
+					UDEBUG("time rectification = %fs", t);
+				}
+
+				if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
+				   (!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
+				{
+					keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
+					t = timer.ticks();
+					if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+					UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
+				}
 			}
 		}
 		else if(data.imageRaw().empty())
@@ -3886,6 +4054,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				imageMono = data.imageRaw();
 			}

+			UASSERT_MSG(imagesRectified, "Cannot extract descriptors on not rectified image from keypoints which assumed to be undistorted");
+
 			descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
 		}
 		t = timer.ticks();
@@ -4034,7 +4204,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UASSERT(wordIds.size() == keypoints.size());
 		UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size());
 		unsigned int i=0;
-		float decimationRatio = preDecimation / _imagePostDecimation;
+		float decimationRatio = float(preDecimation) / float(_imagePostDecimation);
 		double log2value = log(double(preDecimation))/log(2.0);
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
 		{
@@ -4060,9 +4230,47 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 	}

+	cv::Mat image = data.imageRaw();
+	cv::Mat depthOrRightImage = data.depthOrRightRaw();
+	std::vector<CameraModel> cameraModels = data.cameraModels();
+	StereoCameraModel stereoCameraModel = data.stereoCameraModel();
+
+	// apply decimation?
+	if(_imagePostDecimation > 1 && !isIntermediateNode)
+	{
+		if(_imagePostDecimation == preDecimation && decimatedData.isValid())
+		{
+			image = decimatedData.imageRaw();
+			depthOrRightImage = decimatedData.depthOrRightRaw();
+			cameraModels = decimatedData.cameraModels();
+			stereoCameraModel = decimatedData.stereoCameraModel();
+		}
+		else
+		{
+			if(!data.rightRaw().empty() ||
+				(data.depthRaw().rows == image.rows && data.depthRaw().cols == image.cols))
+			{
+				depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
+			}
+			image = util2d::decimate(image, _imagePostDecimation);
+			for(unsigned int i=0; i<cameraModels.size(); ++i)
+			{
+				cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
+			}
+			if(stereoCameraModel.isValidForProjection())
+			{
+				stereoCameraModel.scale(1.0/double(_imagePostDecimation));
+			}
+		}
+
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemPost_decimation(), t*1000.0f);
+		UDEBUG("time post-decimation = %fs", t);
+	}
+
 	bool triangulateWordsWithoutDepth = !_depthAsMask;
 	if(!pose.isNull() &&
-		data.cameraModels().size() == 1 &&
+		cameraModels.size() == 1 &&
 		words.size() &&
 		(words3D.size() == 0 || (triangulateWordsWithoutDepth && words.size() == words3D.size())) &&
 		_registrationPipeline->isImageRequired() &&
@@ -4087,7 +4295,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			Signature cpCurrent(1);
 			uniqueWords = uMultimapToMapUnique(words);
 			uniqueWordsDescriptors = uMultimapToMapUnique(wordsDescriptors);
-			cpCurrent.sensorData().setCameraModels(data.cameraModels());
+			cpCurrent.sensorData().setCameraModels(cameraModels);
 			cpCurrent.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
 			cpCurrent.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
 			// This will force comparing descriptors between both images directly
@@ -4098,14 +4306,15 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
 					uMultimapToMapUnique(cpCurrent.getWords()),
 					uMultimapToMapUnique(cpPrevious.getWords()),
-					data.cameraModels()[0],
+					cameraModels[0],
 					cameraTransform);

 			UDEBUG("inliers=%d", (int)inliers.size());

-			// words3D should have the same size than words
+			// words3D should have the same size than words if not empty
 			float bad_point = std::numeric_limits<float>::quiet_NaN ();
-			UASSERT(words.size() == words3D.size());
+			UASSERT(words3D.size() == 0 || words.size() == words3D.size());
+			bool words3DWasEmpty = words3D.empty();
 			int added3DPointsWithoutDepth = 0;
 			for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 			{
@@ -4128,45 +4337,22 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					iter3D->second = jter->second;
 					++added3DPointsWithoutDepth;
 				}
+				else if(words3DWasEmpty && jter == inliers.end())
+				{
+					// duplicate
+					words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
+				}
 			}
 			UDEBUG("added3DPointsWithoutDepth=%d", added3DPointsWithoutDepth);
 			if(stats) stats->addStatistic(Statistics::kMemoryTriangulated_points(), (float)added3DPointsWithoutDepth);

 			t = timer.ticks();
 			UASSERT(words3D.size() == words.size());
-			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
-			UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
+			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D_motion(), t*1000.0f);
+			UDEBUG("time keypoints 3D by motion (%d) = %fs", (int)words3D.size(), t);
 		}
 	}

-	cv::Mat image = data.imageRaw();
-	cv::Mat depthOrRightImage = data.depthOrRightRaw();
-	std::vector<CameraModel> cameraModels = data.cameraModels();
-	StereoCameraModel stereoCameraModel = data.stereoCameraModel();
-
-	// apply decimation?
-	if(_imagePostDecimation > 1 && !isIntermediateNode)
-	{
-		if(!data.rightRaw().empty() ||
-			(data.depthRaw().rows == image.rows && data.depthRaw().cols == image.cols))
-		{
-			depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
-		}
-		image = util2d::decimate(image, _imagePostDecimation);
-		for(unsigned int i=0; i<cameraModels.size(); ++i)
-		{
-			cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
-		}
-		if(stereoCameraModel.isValidForProjection())
-		{
-			stereoCameraModel.scale(1.0/double(_imagePostDecimation));
-		}
-
-		t = timer.ticks();
-		if(stats) stats->addStatistic(Statistics::kTimingMemPost_decimation(), t*1000.0f);
-		UDEBUG("time post-decimation = %fs", t);
-	}
-
 	// Filter the laser scan?
 	LaserScan laserScan = data.laserScanRaw();
 	if(!isIntermediateNode && laserScan.size())
@@ -131,11 +131,11 @@ OdometryOkvis::OdometryOkvis(const ParametersMap & parameters) :
 #ifdef RTABMAP_OKVIS
 	okvisCallbackHandler_(new OkvisCallbackHandler),
 	okvisEstimator_(0),
+	imagesProcessed_(0),
+	initGravity_(false),
 #endif
 	okvisParameters_(parameters),
-	imagesProcessed_(0),
-	previousPose_(Transform::getIdentity()),
-	initGravity_(false)
+	previousPose_(Transform::getIdentity())
 {
 #ifdef RTABMAP_OKVIS
 	Parameters::parse(parameters, Parameters::kOdomOKVISConfigPath(), configFilename_);
@@ -1046,17 +1046,29 @@ Transform RegistrationIcp::computeTransformationImpl(
 							t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
 							icpT = guess * t.inverse() * guessInv;

-							// we were using normals, so compute correspondences using normals
-							pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
-							pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
+							if(fromScan.hasNormals() && toScan.hasNormals())
+							{
+								// we were using normals, so compute correspondences using normals
+								pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
+								pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());

-							util3d::computeVarianceAndCorrespondences(
-									fromCloudNormalsRegistered,
-									toCloudNormals,
-									_maxCorrespondenceDistance,
-									_maxRotation,
-									variance,
-									correspondences);
+								util3d::computeVarianceAndCorrespondences(
+										fromCloudNormalsRegistered,
+										toCloudNormals,
+										_maxCorrespondenceDistance,
+										_maxRotation,
+										variance,
+										correspondences);
+							}
+							else
+							{
+								util3d::computeVarianceAndCorrespondences(
+										fromCloudRegistered,
+										toCloudFiltered,
+										_maxCorrespondenceDistance,
+										variance,
+										correspondences);
+							}
 						}
 						else
 						{
@@ -1097,6 +1109,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					// verify if there are enough correspondences (using "To" by default if set, in case if "From" is merged from multiple scans)
 					int maxLaserScans = maxLaserScansTo?maxLaserScansTo:maxLaserScansFrom;
 					UDEBUG("Max scans=%d (from=%d, to=%d)", maxLaserScans, maxLaserScansFrom, maxLaserScansTo);
+
 					if(maxLaserScans)
 					{
 						correspondencesRatio = float(correspondences)/float(maxLaserScans);
@@ -1125,10 +1138,17 @@ Transform RegistrationIcp::computeTransformationImpl(
 							info.icpTranslation,
 							info.icpRotation);

-					info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
+					if(correspondences == 0)
+					{
+						UERROR("Transform is found but no correspondences has been found!? Variance is unknown!");
+					}
+					else
+					{
+						info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
+					}
 					info.icpInliersRatio = correspondencesRatio;

-					if(correspondencesRatio < _correspondenceRatio)
+					if(correspondencesRatio <= _correspondenceRatio)
 					{
 						msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f maxLaserScans=%d)",
 								correspondences, correspondencesRatio, _correspondenceRatio, maxLaserScans);
@@ -114,7 +114,7 @@ Rtabmap::Rtabmap() :
 	_proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f),
 	_databasePath(""),
 	_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
-	_optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()),
+	_optimizationMaxError(Parameters::defaultRGBDOptimizeMaxError()),
 	_startNewMapOnLoopClosure(Parameters::defaultRtabmapStartNewMapOnLoopClosure()),
 	_startNewMapOnGoodSignature(Parameters::defaultRtabmapStartNewMapOnGoodSignature()),
 	_goalReachedRadius(Parameters::defaultRGBDGoalReachedRadius()),
@@ -455,7 +455,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 		_proximityAngle *= M_PI/180.0f;
 	}
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
-	Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError);
+	Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxError);
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnGoodSignature(), _startNewMapOnGoodSignature);
 	Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
@@ -1203,6 +1203,7 @@ bool Rtabmap::process(
 						UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
 						if(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(0,0) != 1.0 && info.covariance.at<double>(5,5) > 0.0 && info.covariance.at<double>(5,5) != 1.0)
 						{
+							std::cout << info.covariance << std::endl;
 							_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
 						}
 					}
@@ -2244,6 +2245,8 @@ bool Rtabmap::process(
 	//============================================================
 	float maxLinearError = 0.0f;
 	float maxLinearErrorRatio = 0.0f;
+	float maxAngularError = 0.0f;
+	float maxAngularErrorRatio = 0.0f;
 	double optimizationError = 0.0;
 	int optimizationIterations = 0;
 	cv::Mat localizationCovariance;
@@ -2340,12 +2343,13 @@ bool Rtabmap::process(
 				rejectedHypothesis = true;
 			}
 			else if(_memory->isIncremental() && // FIXME: not tested in localization mode, so do it only in mapping mode
-			  _optimizationMaxLinearError > 0.0f &&
+			  _optimizationMaxError > 0.0f &&
 			  loopClosureLinksAdded.size() &&
 			  optimizationIterations > 0 &&
 			  constraints.size())
 			{
 				const Link * maxLinearLink = 0;
+				const Link * maxAngularLink = 0;
 				for(std::multimap<int, Link>::iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
 				{
 					// ignore links with high variance
@@ -2358,27 +2362,43 @@ bool Rtabmap::process(
 								fabs(iter->second.transform().x() - t.x()),
 								fabs(iter->second.transform().y() - t.y()),
 								fabs(iter->second.transform().z() - t.z()));
-						float stddev = sqrt(iter->second.transVariance());
-						float linearErrorRatio = linearError/stddev;
+						float opt_roll,opt__pitch,opt__yaw;
+						float link_roll,link_pitch,link_yaw;
+						t.getEulerAngles(opt_roll, opt__pitch, opt__yaw);
+						iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
+						float angularError = uMax3(
+								fabs(opt_roll - link_roll),
+								fabs(opt__pitch - link_pitch),
+								fabs(opt__yaw - link_yaw));
+						float stddevLinear = sqrt(iter->second.transVariance());
+						float linearErrorRatio = linearError/stddevLinear;
 						if(linearErrorRatio > maxLinearErrorRatio)
 						{
 							maxLinearError = linearError;
 							maxLinearErrorRatio = linearErrorRatio;
 							maxLinearLink = &iter->second;
 						}
+						float stddevAngular = sqrt(iter->second.rotVariance());
+						float angularErrorRatio = angularError/stddevAngular;
+						if(angularErrorRatio > maxAngularErrorRatio)
+						{
+							maxAngularError = angularError;
+							maxAngularErrorRatio = angularErrorRatio;
+							maxAngularLink = &iter->second;
+						}
 					}
 				}
+				bool reject = false;
 				if(maxLinearLink)
 				{
-					UINFO("Max optimization error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
-
-					if(maxLinearErrorRatio > _optimizationMaxLinearError)
+					UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
+					if(maxLinearErrorRatio > _optimizationMaxError)
 					{
 						UWARN("Rejecting all added loop closures (%d) in this "
 							  "iteration because a wrong loop closure has been "
 							  "detected after graph optimization, resulting in "
-							  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f, stddev=%f). The "
-							  "maximum error ratio parameter is %f of std deviation.",
+							  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f m, stddev=%f). The "
+							  "maximum error ratio parameter \"%s\" is %f of std deviation.",
 							  (int)loopClosureLinksAdded.size(),
 							  maxLinearErrorRatio,
 							  maxLinearLink->from(),
@@ -2386,18 +2406,46 @@ bool Rtabmap::process(
 							  maxLinearLink->type(),
 							  maxLinearError,
 							  sqrt(maxLinearLink->transVariance()),
-							  _optimizationMaxLinearError);
-						for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
-						{
-							_memory->removeLink(iter->first, iter->second);
-							UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
-						}
-						updateConstraints = false;
-						_loopClosureHypothesis.first = 0;
-						lastProximitySpaceClosureId = 0;
-						rejectedHypothesis = true;
+							  Parameters::kRGBDOptimizeMaxError().c_str(),
+							  _optimizationMaxError);
+						reject = true;
 					}
 				}
+				if(maxAngularLink)
+				{
+					UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
+					if(maxAngularErrorRatio > _optimizationMaxError)
+					{
+						UWARN("Rejecting all added loop closures (%d) in this "
+							  "iteration because a wrong loop closure has been "
+							  "detected after graph optimization, resulting in "
+							  "a maximum graph error ratio of %f (edge %d->%d, type=%d, abs error=%f deg, stddev=%f). The "
+							  "maximum error ratio parameter \"%s\" is %f of std deviation.",
+							  (int)loopClosureLinksAdded.size(),
+							  maxAngularErrorRatio,
+							  maxAngularLink->from(),
+							  maxAngularLink->to(),
+							  maxAngularLink->type(),
+							  maxAngularError*180.0f/CV_PI,
+							  sqrt(maxAngularLink->rotVariance()),
+							  Parameters::kRGBDOptimizeMaxError().c_str(),
+							  _optimizationMaxError);
+						reject = true;
+					}
+				}
+
+				if(reject)
+				{
+					for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
+					{
+						_memory->removeLink(iter->first, iter->second);
+						UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
+					}
+					updateConstraints = false;
+					_loopClosureHypothesis.first = 0;
+					lastProximitySpaceClosureId = 0;
+					rejectedHypothesis = true;
+				}
 			}

 			if(updateConstraints)
@@ -3877,7 +3925,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 					if(!t.isNull())
 					{
 						bool updateConstraints = true;
-						if(_optimizationMaxLinearError > 0.0f)
+						if(_optimizationMaxError > 0.0f)
 						{
 							//optimize the graph to see if the new constraint is globally valid

@@ -3951,7 +3999,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 									UINFO("Max optimization angular error = %f deg (link %d->%d)", maxAngularError*180.0f/M_PI, maxAngularLink->from(), maxAngularLink->to());
 								}

-								if(maxLinearError > _optimizationMaxLinearError)
+								if(maxLinearError > _optimizationMaxError)
 								{
 									msg = uFormat("Rejecting edge %d->%d because "
 											  "graph error is too large after optimization (%f m for edge %d->%d, %f deg for edge %d->%d). "
@@ -3965,7 +4013,7 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 											  maxAngularLink?maxAngularLink->from():0,
 											  maxAngularLink?maxAngularLink->to():0,
 											  Parameters::kRGBDOptimizeMaxError().c_str(),
-											  _optimizationMaxLinearError);
+											  _optimizationMaxError);
 								}
 							}
 							else
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/ULogger.h>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>

 namespace rtabmap {

@@ -198,7 +198,7 @@ namespace clams

 	cv::Mat roiA(targetImage, cv::Rect( 0, 0, overview.cols/2, overview.rows ));
 	cv::Mat(overview, cv::Rect( 0, 0, overview.cols/2, overview.rows )).copyTo(roiA);
-	cv::Mat roiB( targetImage, cvRect( overview.cols/2, 0, pub.cols, pub.rows ) );
+	cv::Mat roiB( targetImage, cv::Rect( overview.cols/2, 0, pub.cols, pub.rows ) );
 	pub.copyTo(roiB);

    return targetImage;
@@ -46,6 +46,9 @@

 #include <opencv2/core/core.hpp>
 #include <opencv2/calib3d/calib3d.hpp>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/calib3d/calib3d_c.h>
+#endif

 namespace cv3 {

@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/video/tracking.hpp>
 #include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/types_c.h>
 #include <map>
 #include <Eigen/Core>

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/io/ply_io.h>
 #include <pcl/common/transforms.h>
 #include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>

 namespace rtabmap
 {
@@ -37,6 +37,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UMath.h"
 #include "rtabmap/utilite/UTimer.h"
+#include <opencv2/core/core_c.h>
+#include <opencv2/imgproc/types_c.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/surface/gp3.h>
 #include <pcl/features/normal_3d_omp.h>
@@ -327,6 +327,7 @@ private:
 	bool _savedMaximized;
 	QStringList _waypoints;
 	int _waypointsIndex;
+	std::vector<CameraModel> _rectCameraModels;

 	QMap<int, Signature> _cachedSignatures;
 	long _cachedMemoryUsage;
@@ -30,7 +30,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include <opencv2/core/core.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/imgproc_c.h>
 #include <opencv2/calib3d/calib3d.hpp>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/calib3d/calib3d_c.h>
+#endif
 #include <opencv2/highgui/highgui.hpp>

 #include <QFileDialog>
@@ -97,6 +101,9 @@ CalibrationDialog::CalibrationDialog(bool stereo, const QString & savingDirector

 	ui_->checkBox_switchImages->setChecked(switchImages);

+	ui_->checkBox_fisheye->setChecked(false);
+	ui_->checkBox_fisheye->setEnabled(false);
+
 	this->setStereoMode(stereo_);
 }

@@ -623,6 +630,7 @@ void CalibrationDialog::restart()
 	maxIrs_[1] = 0x7fff;

 	ui_->pushButton_calibrate->setEnabled(ui_->checkBox_unlock->isChecked());
+	ui_->checkBox_fisheye->setEnabled(ui_->checkBox_unlock->isChecked());
 	ui_->pushButton_save->setEnabled(false);
 	ui_->radioButton_raw->setChecked(true);
 	ui_->radioButton_rectified->setEnabled(false);
@@ -666,6 +674,7 @@ void CalibrationDialog::restart()
 void CalibrationDialog::unlock()
 {
 	ui_->pushButton_calibrate->setEnabled(true);
+	ui_->checkBox_fisheye->setEnabled(true);
 }

 void CalibrationDialog::calibrate()
@@ -702,13 +711,44 @@ void CalibrationDialog::calibrate()
 		K = cv::Mat::eye(3,3,CV_64FC1);
 		UINFO("calibrate!");
 		//Find intrinsic and extrinsic camera parameters
-		double rms = cv::calibrateCamera(objectPoints,
-				imagePoints_[id],
-				imageSize_[id],
-				K,
-				D,
-				rvecs,
-				tvecs);
+		double rms = 0.0;
+#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 = ui_->checkBox_fisheye->isChecked();
+
+		if(fishEye)
+		{
+			try
+			{
+				rms = cv::fisheye::calibrate(objectPoints,
+					imagePoints_[id],
+					imageSize_[id],
+					K,
+					D,
+					rvecs,
+					tvecs,
+					cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC |
+					cv::fisheye::CALIB_CHECK_COND |
+					cv::fisheye::CALIB_FIX_SKEW);
+			}
+			catch(const cv::Exception & e)
+			{
+				UERROR("Error: %s (try restarting the calibration)", e.what());
+				QMessageBox::warning(this, tr("Calibration failed!"), tr("Error: %1 (try restarting the calibration)").arg(e.what()));
+				processingData_ = false;
+				return;
+			}
+		}
+		else
+#endif
+		{
+			rms = cv::calibrateCamera(objectPoints,
+					imagePoints_[id],
+					imageSize_[id],
+					K,
+					D,
+					rvecs,
+					tvecs);
+		}

 		UINFO("Re-projection error reported by calibrateCamera: %f", rms);

@@ -720,7 +760,16 @@ void CalibrationDialog::calibrate()

 		for( i = 0; i < (int)objectPoints.size(); ++i )
 		{
-			cv::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], K, D, imagePoints2);
+#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)))
+			if(fishEye)
+			{
+				cv::fisheye::projectPoints( cv::Mat(objectPoints[i]), imagePoints2, rvecs[i], tvecs[i], K, D);
+			}
+			else
+#endif
+			{
+				cv::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], K, D, imagePoints2);
+			}
 			err = cv::norm(cv::Mat(imagePoints_[id][i]), cv::Mat(imagePoints2), CV_L2);

 			int n = (int)objectPoints[i].size();
@@ -737,6 +786,19 @@ void CalibrationDialog::calibrate()
 		P.at<double>(2,3) = 1;
 		K.copyTo(P.colRange(0,3).rowRange(0,3));

+#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)))
+		if(fishEye)
+		{
+			// Convert to unified distortion model (k1,k2,p1,p2,k3,k4)
+			cv::Mat newD = cv::Mat::zeros(1,6,CV_64FC1);
+			newD.at<double>(0,0) = D.at<double>(0,0);
+			newD.at<double>(0,1) = D.at<double>(0,1);
+			newD.at<double>(0,4) = D.at<double>(0,2);
+			newD.at<double>(0,5) = D.at<double>(0,3);
+			D = newD;
+		}
+#endif
+
 		std::cout << "K = " << K << std::endl;
 		std::cout << "D = " << D << std::endl;
 		std::cout << "width = " << imageSize_[id].width << std::endl;
@@ -871,28 +933,59 @@ StereoCameraModel CalibrationDialog::stereoCalibration(const CameraModel & left,
 			objectPoints[0].push_back(cv::Point3f(float(j*squareSize), float(i*squareSize), 0));
 	objectPoints.resize(stereoImagePoints_[0].size(), objectPoints[0]);

+	double rms = 0.0;
+#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 CV_MAJOR_VERSION < 3
-	double rms = cv::stereoCalibrate(
-			objectPoints,
-			stereoImagePoints_[0],
-			stereoImagePoints_[1],
-			left.K_raw(), left.D_raw(),
-			right.K_raw(), right.D_raw(),
-			imageSize, R, T, E, F,
-			cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
-			cv::CALIB_FIX_INTRINSIC);
-#else
-	double rms = cv::stereoCalibrate(
-			objectPoints,
-			stereoImagePoints_[0],
-			stereoImagePoints_[1],
-			left.K_raw(), left.D_raw(),
-			right.K_raw(), right.D_raw(),
-			imageSize, R, T, E, F,
-			cv::CALIB_FIX_INTRINSIC,
-			cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
+	if(fishEye)
+	{
+		cv::Mat D_left(1,4,CV_64FC1);
+		D_left.at<double>(0,0) = left.D_raw().at<double>(0,0);
+		D_left.at<double>(0,1) = left.D_raw().at<double>(0,1);
+		D_left.at<double>(0,2) = left.D_raw().at<double>(0,4);
+		D_left.at<double>(0,3) = left.D_raw().at<double>(0,5);
+		cv::Mat D_right(1,4,CV_64FC1);
+		UASSERT(right.D_raw().cols == 6);
+		D_right.at<double>(0,0) = right.D_raw().at<double>(0,0);
+		D_right.at<double>(0,1) = right.D_raw().at<double>(0,1);
+		D_right.at<double>(0,2) = right.D_raw().at<double>(0,4);
+		D_right.at<double>(0,3) = right.D_raw().at<double>(0,5);
+
+		rms = cv::fisheye::stereoCalibrate(
+				objectPoints,
+				stereoImagePoints_[0],
+				stereoImagePoints_[1],
+				left.K_raw(), D_left,
+				right.K_raw(), D_right,
+				imageSize, R, T,
+				cv::CALIB_FIX_INTRINSIC,
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
+	}
+	else
 #endif
+	{
+#if CV_MAJOR_VERSION < 3
+		rms = cv::stereoCalibrate(
+				objectPoints,
+				stereoImagePoints_[0],
+				stereoImagePoints_[1],
+				left.K_raw(), left.D_raw(),
+				right.K_raw(), right.D_raw(),
+				imageSize, R, T, E, F,
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
+				cv::CALIB_FIX_INTRINSIC);
+#else
+		rms = cv::stereoCalibrate(
+				objectPoints,
+				stereoImagePoints_[0],
+				stereoImagePoints_[1],
+				left.K_raw(), left.D_raw(),
+				right.K_raw(), right.D_raw(),
+				imageSize, R, T, E, F,
+				cv::CALIB_FIX_INTRINSIC,
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
+#endif
+	}
 	UINFO("stereo calibration... done with RMS error=%f", rms);

 	std::cout << "R = " << R << std::endl;
@@ -988,8 +1081,7 @@ bool CalibrationDialog::save()
 	else
 	{
 		UASSERT(stereoModel_.left().isValidForRectification() &&
-				stereoModel_.right().isValidForRectification() &&
-				(!ui_->label_baseline->isVisible() || stereoModel_.baseline() > 0.0));
+				stereoModel_.right().isValidForRectification());
 		QString cameraName = stereoModel_.name().c_str();
 		QString filePath = QFileDialog::getSaveFileName(this, tr("Export"), savingDirectory_ + "/" + cameraName, "*.yaml");
 		QString name = QFileInfo(filePath).baseName();
@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <opencv2/core/core_c.h>
+#include <opencv2/imgproc/types_c.h>
 #include <opencv2/highgui/highgui.hpp>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UFile.h>
@@ -6276,7 +6277,9 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 		std::multimap<int, Link> linksIn = updateLinksWithModifications(links_);
 		linksIn.insert(std::make_pair(newLink.from(), newLink));
 		const Link * maxLinearLink = 0;
+		const Link * maxAngularLink = 0;
 		float maxLinearErrorRatio = 0.0f;
+		float maxAngularErrorRatio = 0.0f;
 		Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> links;
@@ -6292,38 +6295,52 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 		std::string msg;
 		if(poses.size())
 		{
+			float maxLinearError = 0.0f;
+			float maxAngularError = 0.0f;
 			for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
 				// ignore links with high variance
 				if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
 				{
-					UASSERT(poses.find(iter->second.from())!=poses.end());
-					UASSERT(poses.find(iter->second.to())!=poses.end());
-					Transform t1 = poses.at(iter->second.from());
-					Transform t2 = poses.at(iter->second.to());
-					UASSERT(!t1.isNull() && !t2.isNull());
+					Transform t1 = uValue(poses, iter->second.from(), Transform());
+					Transform t2 = uValue(poses, iter->second.to(), Transform());
 					Transform t = t1.inverse()*t2;
 					float linearError = uMax3(
 							fabs(iter->second.transform().x() - t.x()),
 							fabs(iter->second.transform().y() - t.y()),
 							fabs(iter->second.transform().z() - t.z()));
-					float stddev = sqrt(iter->second.transVariance());
-					float linearErrorRatio = linearError/stddev;
+					float opt_roll,opt__pitch,opt__yaw;
+					float link_roll,link_pitch,link_yaw;
+					t.getEulerAngles(opt_roll, opt__pitch, opt__yaw);
+					iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
+					float angularError = uMax3(
+							fabs(opt_roll - link_roll),
+							fabs(opt__pitch - link_pitch),
+							fabs(opt__yaw - link_yaw));
+					float stddevLinear = sqrt(iter->second.transVariance());
+					float linearErrorRatio = linearError/stddevLinear;
 					if(linearErrorRatio > maxLinearErrorRatio)
 					{
+						maxLinearError = linearError;
 						maxLinearErrorRatio = linearErrorRatio;
 						maxLinearLink = &iter->second;
 					}
+					float stddevAngular = sqrt(iter->second.rotVariance());
+					float angularErrorRatio = angularError/stddevAngular;
+					if(angularErrorRatio > maxAngularErrorRatio)
+					{
+						maxAngularError = angularError;
+						maxAngularErrorRatio = angularErrorRatio;
+						maxAngularLink = &iter->second;
+					}
 				}
 			}
 			if(maxLinearLink)
 			{
-				UINFO("Max optimization linear error ratio = %f (link %d->%d)", maxLinearErrorRatio, maxLinearLink->from(), maxLinearLink->to());
-			}
-
-			if(maxLinearErrorRatio > maxOptimizationError)
-			{
-				msg = uFormat("Rejecting edge %d->%d because "
+				UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
+				if(maxLinearErrorRatio > maxOptimizationError)
+				{
+					msg = uFormat("Rejecting edge %d->%d because "
 						  "graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f). "
 						  "\"%s\" is %f.",
 						  newLink.from(),
@@ -6334,6 +6351,25 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 						  sqrt(maxLinearLink->transVariance()),
 						  Parameters::kRGBDOptimizeMaxError().c_str(),
 						  maxOptimizationError);
+				}
+			}
+			if(maxAngularLink)
+			{
+				UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
+				if(maxAngularErrorRatio > maxOptimizationError)
+				{
+					msg = uFormat("Rejecting edge %d->%d because "
+						  "graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f). "
+						  "\"%s\" is %f.",
+						  newLink.from(),
+						  newLink.to(),
+						  maxAngularErrorRatio,
+						  maxAngularLink->from(),
+						  maxAngularLink->to(),
+						  sqrt(maxAngularLink->rotVariance()),
+						  Parameters::kRGBDOptimizeMaxError().c_str(),
+						  maxOptimizationError);
+				}
 			}
 		}
 		else
@@ -148,6 +148,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->spinBox_randomPoints, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_dilationVoxelSize, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->spinBox_dilationSteps, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_mls_outputVoxelSize, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	_ui->stackedWidget_upsampling->setCurrentIndex(_ui->comboBox_upsamplingMethod->currentIndex());
 	connect(_ui->comboBox_upsamplingMethod, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_upsampling, SLOT(setCurrentIndex(int)));
 	connect(_ui->comboBox_upsamplingMethod, SIGNAL(currentIndexChanged(int)), this, SLOT(updateMLSGrpVisibility()));
@@ -335,6 +336,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("mls_point_density", _ui->spinBox_randomPoints->value());
 	settings.setValue("mls_dilation_voxel_size", _ui->doubleSpinBox_dilationVoxelSize->value());
 	settings.setValue("mls_dilation_iterations", _ui->spinBox_dilationSteps->value());
+	settings.setValue("mls_output_voxel_size", _ui->doubleSpinBox_mls_outputVoxelSize->value());

 	settings.setValue("gain", _ui->checkBox_gainCompensation->isChecked());
 	settings.setValue("gain_radius", _ui->doubleSpinBox_gainRadius->value());
@@ -469,6 +471,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->spinBox_randomPoints->setValue(settings.value("mls_point_density", _ui->spinBox_randomPoints->value()).toInt());
 	_ui->doubleSpinBox_dilationVoxelSize->setValue(settings.value("mls_dilation_voxel_size", _ui->doubleSpinBox_dilationVoxelSize->value()).toDouble());
 	_ui->spinBox_dilationSteps->setValue(settings.value("mls_dilation_iterations", _ui->spinBox_dilationSteps->value()).toInt());
+	_ui->doubleSpinBox_mls_outputVoxelSize->setValue(settings.value("mls_output_voxel_size", _ui->doubleSpinBox_mls_outputVoxelSize->value()).toInt());

 	_ui->checkBox_gainCompensation->setChecked(settings.value("gain", _ui->checkBox_gainCompensation->isChecked()).toBool());
 	_ui->doubleSpinBox_gainRadius->setValue(settings.value("gain_radius", _ui->doubleSpinBox_gainRadius->value()).toDouble());
@@ -596,10 +599,11 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->spinBox_polygonialOrder->setValue(2);
 	_ui->comboBox_upsamplingMethod->setCurrentIndex(0);
 	_ui->doubleSpinBox_sampleRadius->setValue(0.01);
-	_ui->doubleSpinBox_sampleStep->setValue(0.0);
-	_ui->spinBox_randomPoints->setValue(0);
-	_ui->doubleSpinBox_dilationVoxelSize->setValue(0.01);
-	_ui->spinBox_dilationSteps->setValue(0);
+	_ui->doubleSpinBox_sampleStep->setValue(0.005);
+	_ui->spinBox_randomPoints->setValue(10);
+	_ui->doubleSpinBox_dilationVoxelSize->setValue(0.005);
+	_ui->spinBox_dilationSteps->setValue(1);
+	_ui->doubleSpinBox_mls_outputVoxelSize->setValue(0);

 	_ui->checkBox_gainCompensation->setChecked(false);
 	_ui->doubleSpinBox_gainRadius->setValue(0.02);
@@ -1691,16 +1695,16 @@ bool ExportCloudsDialog::getExportedClouds(
 				if(_ui->checkBox_assemble->isChecked())
 				{
 					// Re-voxelize to make sure to have uniform density
-					if(_ui->doubleSpinBox_voxelSize_assembled->value())
+                    if(_ui->doubleSpinBox_mls_outputVoxelSize->value())
 					{
 						_progressDialog->appendText(tr("Voxelize cloud (%1 points, voxel size = %2 m)...")
 								.arg(cloudWithNormals->size())
-								.arg(_ui->doubleSpinBox_voxelSize_assembled->value()));
+								.arg(_ui->doubleSpinBox_mls_outputVoxelSize->value()));
 						QApplication::processEvents();

 						cloudWithNormals = util3d::voxelize(
 								cloudWithNormals,
-								_ui->doubleSpinBox_voxelSize_assembled->value());
+								_ui->doubleSpinBox_mls_outputVoxelSize->value());
 					}
 				
 					_progressDialog->appendText(tr("Update %1 normals with %2 camera views...").arg(cloudWithNormals->size()).arg(poses.size()));
@@ -34,5 +34,6 @@
        <file>images/r200.png</file>
        <file>images/zr300.png</file>
        <file>images/d435.png</file>
+        <file>images/d415.png</file>
    </qresource>
 </RCC>
@@ -429,6 +429,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	connect(_ui->actionKinect_for_Windows_SDK_v2, SIGNAL(triggered()), this, SLOT(selectK4W2()));
 	connect(_ui->actionRealSense_R200, SIGNAL(triggered()), this, SLOT(selectRealSense()));
 	connect(_ui->actionRealSense_ZR300, SIGNAL(triggered()), this, SLOT(selectRealSense()));
+	connect(_ui->actionRealSense2_D415, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
 	connect(_ui->actionRealSense2_D435, SIGNAL(triggered()), this, SLOT(selectRealSense2()));
 	connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394()));
 	connect(_ui->actionStereoFlyCapture2, SIGNAL(triggered()), this, SLOT(selectStereoFlyCapture2()));
@@ -444,6 +445,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent, bool sh
 	_ui->actionKinect_for_Windows_SDK_v2->setEnabled(CameraK4W2::available());
 	_ui->actionRealSense_R200->setEnabled(CameraRealSense::available());
 	_ui->actionRealSense_ZR300->setEnabled(CameraRealSense::available());
+	_ui->actionRealSense2_D415->setEnabled(CameraRealSense2::available());
 	_ui->actionRealSense2_D435->setEnabled(CameraRealSense2::available());
 	_ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available());
 	_ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available());
@@ -2779,6 +2781,57 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 		cv::Mat image, depth;
 		SensorData data = iter->sensorData();
 		data.uncompressData(&image, &depth, 0);
+		ParametersMap allParameters = _preferencesDialog->getAllParameters();
+		bool rectifyOnlyFeatures = Parameters::defaultRtabmapRectifyOnlyFeatures();
+		bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified();
+		Parameters::parse(allParameters, Parameters::kRtabmapRectifyOnlyFeatures(), rectifyOnlyFeatures);
+		Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified);
+		if(rectifyOnlyFeatures && !imagesAlreadyRectified)
+		{
+			if(data.cameraModels().size())
+			{
+				UTimer time;
+				// Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
+				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().clone();
+				bool initRectMaps = _rectCameraModels.empty();
+				if(initRectMaps)
+				{
+					_rectCameraModels.resize(data.cameraModels().size());
+				}
+				for(unsigned int i=0; i<data.cameraModels().size(); ++i)
+				{
+					if(data.cameraModels()[i].isValidForRectification())
+					{
+						if(initRectMaps)
+						{
+							_rectCameraModels[i] = data.cameraModels()[i];
+							if(!_rectCameraModels[i].isRectificationMapInitialized())
+							{
+								UWARN("Initializing rectification maps for camera %d (only done for the first image received)...", i);
+								_rectCameraModels[i].initRectificationMap();
+								UWARN("Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
+							}
+						}
+						UASSERT(_rectCameraModels[i].imageWidth() == data.cameraModels()[i].imageWidth() &&
+								_rectCameraModels[i].imageHeight() == data.cameraModels()[i].imageHeight());
+						cv::Mat rectifiedImage = _rectCameraModels[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
+					{
+						UWARN("Camera %d of data %d is not valid for rectification (%dx%d).",
+								i, data.id(),
+								data.cameraModels()[i].imageWidth(),
+								data.cameraModels()[i].imageHeight());
+					}
+				}
+				UINFO("Time rectification: %fs", time.ticks());
+				data.setImageRaw(rectifiedImages);
+				image = rectifiedImages;
+			}
+		}

 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 		pcl::IndicesPtr indices(new std::vector<int>);
@@ -2790,7 +2843,7 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 				_preferencesDialog->getCloudMaxDepth(0),
 				_preferencesDialog->getCloudMinDepth(0),
 				indices.get(),
-				_preferencesDialog->getAllParameters(),
+				allParameters,
 				_preferencesDialog->getCloudRoiRatios(0));

 		// view point
@@ -4283,6 +4336,7 @@ void MainWindow::updateSelectSourceMenu()
 	_ui->actionKinect_for_Windows_SDK_v2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcK4W2);
 	_ui->actionRealSense_R200->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
 	_ui->actionRealSense_ZR300->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense);
+	_ui->actionRealSense2_D415->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
 	_ui->actionRealSense2_D435->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcRealSense2);
 	_ui->actionStereoDC1394->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcDC1394);
 	_ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->getSourceDriver() == PreferencesDialog::kSrcFlyCapture2);
@@ -6290,6 +6344,7 @@ void MainWindow::clearTheCache()
 	_octomap = new OctoMap(_preferencesDialog->getAllParameters());
 #endif
 	_occupancyGrid->clear();
+	_rectCameraModels.clear();
 }

 void MainWindow::openHelp()
@@ -190,7 +190,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		_ui->reextract_type->setItemData(0, 0, Qt::UserRole - 1);
 		_ui->reextract_type->setItemData(1, 0, Qt::UserRole - 1);

-#if CV_MAJOR_VERSION == 3
+#if CV_MAJOR_VERSION >= 3
 		_ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1);
 		_ui->comboBox_detector_strategy->setItemData(1, 0, Qt::UserRole - 1);
 		_ui->comboBox_detector_strategy->setItemData(3, 0, Qt::UserRole - 1);
@@ -574,6 +574,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->comboBox_realsensePresetDepth, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_realsenseOdom, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_realsenseDepthScaledToRGBSize, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_realsenseRGBSource, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_rs2_emitter, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_rs2_irDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));

@@ -703,6 +704,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_checkBox_startNewMapOnLoopClosure->setObjectName(Parameters::kRtabmapStartNewMapOnLoopClosure().c_str());
 	_ui->general_checkBox_startNewMapOnGoodSignature->setObjectName(Parameters::kRtabmapStartNewMapOnGoodSignature().c_str());
 	_ui->general_checkBox_imagesAlreadyRectified->setObjectName(Parameters::kRtabmapImagesAlreadyRectified().c_str());
+	_ui->general_checkBox_rectifyOnlyFeatures->setObjectName(Parameters::kRtabmapRectifyOnlyFeatures().c_str());
 	_ui->lineEdit_workingDirectory->setObjectName(Parameters::kRtabmapWorkingDirectory().c_str());
 	connect(_ui->toolButton_workingDirectory, SIGNAL(clicked()), this, SLOT(changeWorkingDirectory()));

@@ -1661,6 +1663,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->comboBox_realsensePresetDepth->setCurrentIndex(2);
 		_ui->checkbox_realsenseOdom->setChecked(false);
 		_ui->checkbox_realsenseDepthScaledToRGBSize->setChecked(false);
+		_ui->comboBox_realsenseRGBSource->setCurrentIndex(0);
 		_ui->checkbox_rs2_emitter->setChecked(true);
 		_ui->checkbox_rs2_irDepth->setChecked(false);
 		_ui->lineEdit_openniOniPath->clear();
@@ -2060,6 +2063,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->comboBox_realsensePresetDepth->setCurrentIndex(settings.value("presetDepth", _ui->comboBox_realsensePresetDepth->currentIndex()).toInt());
 	_ui->checkbox_realsenseOdom->setChecked(settings.value("odom", _ui->checkbox_realsenseOdom->isChecked()).toBool());
 	_ui->checkbox_realsenseDepthScaledToRGBSize->setChecked(settings.value("depthScaled", _ui->checkbox_realsenseDepthScaledToRGBSize->isChecked()).toBool());
+	_ui->comboBox_realsenseRGBSource->setCurrentIndex(settings.value("rgbSource", _ui->comboBox_realsenseRGBSource->currentIndex()).toInt());
 	settings.endGroup(); // RealSense

 	settings.beginGroup("RealSense2");
@@ -2483,6 +2487,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("presetDepth",         _ui->comboBox_realsensePresetDepth->currentIndex());
 	settings.setValue("odom",                _ui->checkbox_realsenseOdom->isChecked());
 	settings.setValue("depthScaled",         _ui->checkbox_realsenseDepthScaledToRGBSize->isChecked());
+	settings.setValue("rgbSource",           _ui->comboBox_realsenseRGBSource->currentIndex());
 	settings.endGroup(); // RealSense

 	settings.beginGroup("RealSense2");
@@ -5020,10 +5025,10 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 	}
 	else if (driver == kSrcRealSense)
 	{
-		if(useRawImages)
+		if(useRawImages && _ui->comboBox_realsenseRGBSource->currentIndex()!=2)
 		{
 			QMessageBox::warning(this, tr("Calibration"),
-					tr("Using raw images for \"RealSense\" driver is not yet supported. "
+					tr("Using raw images for \"RealSense\" driver is not yet supported for color and infrared streams. "
 						"Factory calibration loaded from RealSense is used."), QMessageBox::Ok);
 			return 0;
 		}
@@ -5037,6 +5042,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 				this->getGeneralInputRate(),
 				this->getSourceLocalTransform());
 			((CameraRealSense*)camera)->setDepthScaledToRGBSize(_ui->checkbox_realsenseDepthScaledToRGBSize->isChecked());
+			((CameraRealSense*)camera)->setRGBSource((CameraRealSense::RGBSource)_ui->comboBox_realsenseRGBSource->currentIndex());
 		}
 	}
 	else if (driver == kSrcRealSense2)
@@ -5823,7 +5829,7 @@ void PreferencesDialog::calibrate()
 		}

 		bool freenect2 = driver == kSrcFreenect2;
-		_calibrationDialog->setStereoMode(this->getSourceType() != kSrcRGB, freenect2?"rgb":"left", freenect2?"depth":"right"); // RGB+Depth or left+right
+		_calibrationDialog->setStereoMode(this->getSourceType() != kSrcRGB && driver != kSrcRealSense, freenect2?"rgb":"left", freenect2?"depth":"right"); // RGB+Depth or left+right
 		_calibrationDialog->setSwitchedImages(freenect2);
 		_calibrationDialog->setSavingDirectory(this->getCameraInfoDir());
 		_calibrationDialog->registerToEventsManager();
@@ -17,16 +17,7 @@
   <property name="spacing">
    <number>0</number>
   </property>
-   <property name="leftMargin">
-    <number>0</number>
-   </property>
-   <property name="topMargin">
-    <number>0</number>
-   </property>
-   <property name="rightMargin">
-    <number>0</number>
-   </property>
-   <property name="bottomMargin">
+   <property name="margin">
    <number>0</number>
   </property>
   <item>
@@ -154,21 +145,12 @@
         <rect>
          <x>0</x>
          <y>0</y>
-          <width>413</width>
+          <width>379</width>
          <height>816</height>
         </rect>
        </property>
        <layout class="QVBoxLayout" name="verticalLayout">
-         <property name="leftMargin">
-          <number>0</number>
-         </property>
-         <property name="topMargin">
-          <number>0</number>
-         </property>
-         <property name="rightMargin">
-          <number>0</number>
-         </property>
-         <property name="bottomMargin">
+         <property name="margin">
          <number>0</number>
         </property>
         <item>
@@ -299,7 +281,14 @@
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_6">
            <item>
-             <layout class="QGridLayout" name="gridLayout" columnstretch="0,1,1">
+             <layout class="QGridLayout" name="gridLayout" columnstretch="0,0,0">
+              <item row="5" column="0">
+               <widget class="QLabel" name="label_4">
+                <property name="text">
+                 <string>Skew</string>
+                </property>
+               </widget>
+              </item>
              <item row="0" column="0">
               <widget class="QLabel" name="label_19">
                <property name="text">
@@ -435,13 +424,6 @@
                </property>
               </widget>
              </item>
-              <item row="5" column="0">
-               <widget class="QLabel" name="label_4">
-                <property name="text">
-                 <string>Skew</string>
-                </property>
-               </widget>
-              </item>
              <item row="5" column="1">
               <widget class="QProgressBar" name="progressBar_skew">
                <property name="value">
@@ -462,7 +444,7 @@
             </layout>
            </item>
            <item>
-             <layout class="QHBoxLayout" name="horizontalLayout_5" stretch="1,0">
+             <layout class="QHBoxLayout" name="horizontalLayout_5" stretch="1,0,0">
              <item>
               <widget class="QPushButton" name="pushButton_calibrate">
                <property name="text">
@@ -470,6 +452,13 @@
                </property>
               </widget>
              </item>
+              <item>
+               <widget class="QCheckBox" name="checkBox_fisheye">
+                <property name="text">
+                 <string>Fish eye</string>
+                </property>
+               </widget>
+              </item>
              <item>
               <widget class="QCheckBox" name="checkBox_unlock">
                <property name="text">
@@ -485,6 +474,13 @@
               <string>Camera intrinsic parameters</string>
              </property>
              <layout class="QGridLayout" name="gridLayout_2">
+               <item row="8" column="2">
+                <widget class="QLineEdit" name="lineEdit_P_2">
+                 <property name="readOnly">
+                  <bool>true</bool>
+                 </property>
+                </widget>
+               </item>
               <item row="0" column="1">
                <widget class="QLabel" name="label_fx">
                 <property name="text">
@@ -699,13 +695,6 @@
                 </property>
                </widget>
               </item>
-               <item row="8" column="2">
-                <widget class="QLineEdit" name="lineEdit_P_2">
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
               <item row="4" column="0">
                <widget class="QLabel" name="label_baseline_name">
                 <property name="text">
@@ -23,9 +23,9 @@
      <property name="geometry">
       <rect>
        <x>0</x>
-        <y>0</y>
-        <width>778</width>
-        <height>4697</height>
+        <y>-1112</y>
+        <width>773</width>
+        <height>4339</height>
       </rect>
      </property>
      <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -890,12 +890,29 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
              </property>
             </widget>
            </item>
+            <item row="3" column="1">
+             <widget class="QLabel" name="label_90">
+              <property name="text">
+               <string>Output voxel size. Used to filter superposed points after upsampling.</string>
+              </property>
+              <property name="wordWrap">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="0">
+             <widget class="QDoubleSpinBox" name="doubleSpinBox_mls_outputVoxelSize">
+              <property name="decimals">
+               <number>3</number>
+              </property>
+             </widget>
+            </item>
           </layout>
          </item>
          <item>
           <widget class="QStackedWidget" name="stackedWidget_upsampling">
            <property name="currentIndex">
-             <number>0</number>
+             <number>4</number>
            </property>
            <widget class="QWidget" name="page">
             <layout class="QVBoxLayout" name="verticalLayout_3">
@@ -1000,7 +1017,11 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                    </widget>
                   </item>
                   <item row="0" column="0">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBox_sampleRadius"/>
+                    <widget class="QDoubleSpinBox" name="doubleSpinBox_sampleRadius">
+                     <property name="decimals">
+                      <number>3</number>
+                     </property>
+                    </widget>
                   </item>
                   <item row="0" column="1">
                    <widget class="QLabel" name="label_7">
@@ -1013,7 +1034,11 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                    </widget>
                   </item>
                   <item row="1" column="0">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBox_sampleStep"/>
+                    <widget class="QDoubleSpinBox" name="doubleSpinBox_sampleStep">
+                     <property name="decimals">
+                      <number>3</number>
+                     </property>
+                    </widget>
                   </item>
                  </layout>
                 </item>
@@ -1119,7 +1144,11 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                    </widget>
                   </item>
                   <item row="0" column="0">
-                    <widget class="QDoubleSpinBox" name="doubleSpinBox_dilationVoxelSize"/>
+                    <widget class="QDoubleSpinBox" name="doubleSpinBox_dilationVoxelSize">
+                     <property name="decimals">
+                      <number>3</number>
+                     </property>
+                    </widget>
                   </item>
                   <item row="1" column="1">
                    <widget class="QLabel" name="label_11">
@@ -197,12 +197,23 @@
       </property>
       <addaction name="actionRealSense2_D435"/>
      </widget>
+      <widget class="QMenu" name="menuRealSense_D415">
+       <property name="title">
+        <string>RealSense D415</string>
+       </property>
+       <property name="icon">
+        <iconset resource="../GuiLib.qrc">
+         <normaloff>:/images/d415.png</normaloff>:/images/d415.png</iconset>
+       </property>
+       <addaction name="actionRealSense2_D415"/>
+      </widget>
      <addaction name="menuKinect_for_Xbox_360"/>
      <addaction name="menuXtion_PRO_LIVE"/>
      <addaction name="menuSense_3D_scanner"/>
      <addaction name="menuKinect_v2"/>
      <addaction name="menuRealSense_R200"/>
      <addaction name="menuRealSense_ZR300"/>
+      <addaction name="menuRealSense_D415"/>
      <addaction name="menuRealSense_D435"/>
     </widget>
     <widget class="QMenu" name="menuStereo_camera">
@@ -331,16 +342,7 @@
     <property name="spacing">
      <number>0</number>
     </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
      <number>0</number>
     </property>
     <item>
@@ -378,16 +380,7 @@
     <property name="spacing">
      <number>0</number>
     </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
      <number>0</number>
     </property>
     <item>
@@ -630,16 +623,7 @@
     <property name="spacing">
      <number>0</number>
     </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
      <number>0</number>
     </property>
     <item>
@@ -660,16 +644,7 @@
     <property name="spacing">
      <number>0</number>
     </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
      <number>0</number>
     </property>
     <item>
@@ -690,16 +665,7 @@
     <property name="spacing">
      <number>0</number>
     </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
      <number>0</number>
     </property>
     <item>
@@ -720,16 +686,7 @@
     <property name="spacing">
      <number>0</number>
     </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
      <number>0</number>
     </property>
    </layout>
@@ -747,16 +704,7 @@
     <property name="spacing">
      <number>0</number>
     </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
      <number>0</number>
     </property>
    </layout>
@@ -777,16 +725,7 @@
     <property name="spacing">
      <number>0</number>
     </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
      <number>0</number>
     </property>
     <item>
@@ -807,16 +746,7 @@
     <property name="spacing">
      <number>0</number>
     </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
      <number>0</number>
     </property>
     <item>
@@ -840,16 +770,7 @@
     <property name="spacing">
      <number>0</number>
     </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
      <number>0</number>
     </property>
     <item>
@@ -919,16 +840,7 @@
     <property name="spacing">
      <number>0</number>
     </property>
-     <property name="leftMargin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
+     <property name="margin">
      <number>0</number>
     </property>
     <item>
@@ -1485,6 +1397,14 @@
    <string>RealSense2</string>
   </property>
  </action>
+  <action name="actionRealSense2_D415">
+   <property name="checkable">
+    <bool>true</bool>
+   </property>
+   <property name="text">
+    <string>RealSense2</string>
+   </property>
+  </action>
 </widget>
 <customwidgets>
  <customwidget>
@@ -94,7 +94,7 @@
          <property name="geometry">
           <rect>
            <x>0</x>
-            <y>0</y>
+            <y>-287</y>
            <width>681</width>
            <height>2943</height>
           </rect>
@@ -103,16 +103,7 @@
           <property name="spacing">
            <number>0</number>
           </property>
-           <property name="leftMargin">
-            <number>0</number>
-           </property>
-           <property name="topMargin">
-            <number>0</number>
-           </property>
-           <property name="rightMargin">
-            <number>0</number>
-           </property>
-           <property name="bottomMargin">
+           <property name="margin">
            <number>0</number>
           </property>
           <item>
@@ -126,7 +117,7 @@
                <enum>QFrame::Raised</enum>
               </property>
               <property name="currentIndex">
-                <number>16</number>
+                <number>5</number>
               </property>
               <widget class="QWidget" name="page_22">
                <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -3609,7 +3600,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <string>RealSense</string>
                                 </property>
                                 <layout class="QGridLayout" name="gridLayout_73" columnstretch="0,1">
-                                  <item row="4" column="0">
+                                  <item row="5" column="0">
                                   <spacer name="verticalSpacer_51">
                                    <property name="orientation">
                                     <enum>Qt::Vertical</enum>
@@ -3715,6 +3706,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                    </property>
                                   </widget>
                                  </item>
+                                  <item row="3" column="0">
+                                   <widget class="QCheckBox" name="checkbox_realsenseDepthScaledToRGBSize">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                    <property name="checked">
+                                     <bool>false</bool>
+                                    </property>
+                                   </widget>
+                                  </item>
                                  <item row="3" column="1">
                                   <widget class="QLabel" name="label_realsenseOdom_2">
                                    <property name="text">
@@ -3728,13 +3729,38 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                    </property>
                                   </widget>
                                  </item>
-                                  <item row="3" column="0">
-                                   <widget class="QCheckBox" name="checkbox_realsenseDepthScaledToRGBSize">
-                                    <property name="text">
-                                     <string/>
+                                  <item row="4" column="0">
+                                   <widget class="QComboBox" name="comboBox_realsenseRGBSource">
+                                    <property name="sizeAdjustPolicy">
+                                     <enum>QComboBox::AdjustToContents</enum>
                                    </property>
-                                    <property name="checked">
-                                     <bool>false</bool>
+                                    <item>
+                                     <property name="text">
+                                      <string>Color</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>Infrared</string>
+                                     </property>
+                                    </item>
+                                    <item>
+                                     <property name="text">
+                                      <string>Fisheye</string>
+                                     </property>
+                                    </item>
+                                   </widget>
+                                  </item>
+                                  <item row="4" column="1">
+                                   <widget class="QLabel" name="label_realsenseOdom_3">
+                                    <property name="text">
+                                     <string>RGB camera source. For fisheye has depth available only if it is calibrated.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                                    </property>
                                   </widget>
                                  </item>
@@ -5093,16 +5119,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                       <string>Directory of images (optional settings)</string>
                      </property>
                      <layout class="QVBoxLayout" name="verticalLayout_93">
-                       <property name="leftMargin">
-                        <number>0</number>
-                       </property>
-                       <property name="topMargin">
-                        <number>0</number>
-                       </property>
-                       <property name="rightMargin">
-                        <number>0</number>
-                       </property>
-                       <property name="bottomMargin">
+                       <property name="margin">
                        <number>0</number>
                       </property>
                       <item>
@@ -6290,6 +6307,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                   <layout class="QVBoxLayout" name="verticalLayout_15">
                    <item>
                     <layout class="QGridLayout" name="gridLayout_43" columnstretch="0,1">
+                      <item row="1" column="0">
+                       <widget class="QDoubleSpinBox" name="general_doubleSpinBox_detectionRate">
+                        <property name="suffix">
+                         <string> Hz</string>
+                        </property>
+                        <property name="decimals">
+                         <number>3</number>
+                        </property>
+                        <property name="value">
+                         <double>1.000000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                      <item row="0" column="0">
                       <widget class="QCheckBox" name="general_checkBox_SLAM_mode">
                        <property name="text">
@@ -6383,19 +6413,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="1" column="0">
-                       <widget class="QDoubleSpinBox" name="general_doubleSpinBox_detectionRate">
-                        <property name="suffix">
-                         <string> Hz</string>
-                        </property>
-                        <property name="decimals">
-                         <number>3</number>
-                        </property>
-                        <property name="value">
-                         <double>1.000000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
                      <item row="3" column="1">
                       <widget class="QLabel" name="label_165">
                        <property name="text">
@@ -6455,6 +6472,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_498">
+                        <property name="text">
+                         <string>Rectify only features. If images are not already rectified (see parameter above) and this parameter is true, the whole RGB image will not be rectified, only the features. Warning: As projection of RGB-D image to point cloud is assuming that images are rectified, the generated point cloud map will have wrong colors if this parameter is true.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_rectifyOnlyFeatures">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
                     </layout>
                    </item>
                    <item>
@@ -15276,16 +15316,7 @@ Lower the ratio -&gt; higher the precision.</string>
                        <property name="spacing">
                         <number>0</number>
                        </property>
-                        <property name="leftMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
+                        <property name="margin">
                         <number>0</number>
                        </property>
                        <item>
@@ -15365,16 +15396,7 @@ Lower the ratio -&gt; higher the precision.</string>
                        <property name="spacing">
                         <number>0</number>
                        </property>
-                        <property name="leftMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
+                        <property name="margin">
                         <number>0</number>
                        </property>
                        <item>
@@ -15486,16 +15508,7 @@ Lower the ratio -&gt; higher the precision.</string>
                        <property name="spacing">
                         <number>0</number>
                        </property>
-                        <property name="leftMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
+                        <property name="margin">
                         <number>0</number>
                        </property>
                        <item>
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
  <name>rtabmap</name>
-  <version>0.17.5</version>
+  <version>0.17.6</version>
  <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
  <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
  <author>Mathieu Labbe</author>
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UDirectory.h"
 #include "rtabmap/utilite/UConversion.h"
 #include <opencv2/highgui/highgui.hpp>
+#include <opencv2/highgui/highgui_c.h>
 #include <stdio.h>

 void showUsage()
@@ -37,6 +37,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UConversion.h"
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/types_c.h>
+#if CV_MAJOR_VERSION >= 3
+#include <opencv2/videoio/videoio_c.h>
+#endif
 #include <pcl/visualization/cloud_viewer.h>
 #include <stdio.h>
 #include <signal.h>
@@ -138,7 +138,7 @@ int main(int argc, char * argv[])

 			cv::Mat roiA(targetImage, cv::Rect( 0, 0, sizeA.width, sizeA.height ));
 			imageA.copyTo(roiA);
-			cv::Mat roiB( targetImage, cvRect( sizeA.width, 0, sizeB.width, sizeB.height ) );
+			cv::Mat roiB( targetImage, cv::Rect( sizeA.width, 0, sizeB.width, sizeB.height ) );
 			imageB.copyTo(roiB);

 			if(!cv::imwrite(targetFilePath.c_str(), targetImage))
@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UMath.h>
+#include <opencv2/imgproc/types_c.h>
 #include <fstream>
 #include <string>
Author	SHA1	Message	Date
matlabbe	db3b901063	Update package.xml Bump package version	2018-09-29 11:58:58 -04:00
matlabbe	8b055752aa	RegistrationIcp: Fixed invalid variance set when structural complexity is low and input doesn't have normals. DBViewer: added check on angular error after optimizing the graph when adding a new constraint. Memory: added warning if odom angular variance detected is very high.	2018-09-28 22:02:07 -04:00
matlabbe	dbb9cfa77a	Parameters: Updated description of Rtabmap/TimeThr and Rtabmap/MemoryThr (https://github.com/introlab/rtabmap_ros/issues/276 )	2018-09-28 15:37:24 -04:00
matlabbe	95e87fed14	MLS: added output voxel size parameter	2018-09-28 09:45:43 -04:00
matlabbe	02fdd677cf	💄	2018-09-27 16:08:55 -04:00
matlabbe	3b74534567	Parameters: Mem/LaserScanNormalRadius should be a float	2018-09-27 14:28:51 -04:00
matlabbe	790b0e5cf7	Memory: undistortion of features can be done also for RGB-only case	2018-09-27 10:19:28 -04:00
matlabbe	5e08da51aa	RealSense: fixed color rectification. Fixed ZR300 odometry frame.	2018-09-26 20:51:12 -04:00
matlabbe	0c2287df77	CameraModel fisheye model: fixed typo	2018-09-25 14:36:59 -04:00
matlabbe	b581a62c89	Fixed build with opencv < 2.4.10 for missing fisheye model	2018-09-24 15:09:12 -04:00
matlabbe	c341648a44	Added parameter "Rtabmap/RectifyOnlyFeatures". Added support of fisheye distortion model.	2018-09-24 14:36:54 -04:00
matlabbe	f903ffb927	Fixed regression errors for OpencV2 (#309 )	2018-09-18 23:03:40 -04:00
matlabbe	124543c57d	Fixed compilation errors with OpenCV 4.0.0 (#309 )	2018-09-18 22:43:18 -04:00
matlabbe	829f05e2fb	Implemented missing max angular optimization error check for RGBD/OptimizeMaxError	2018-09-18 22:34:15 -04:00
matlabbe	d936b2d35a	Added '*' to cmake info to show dependencies affecting default parameters	2018-09-18 17:00:37 -04:00
matlabbe	84a8e5830e	fixed https://github.com/introlab/rtabmap_ros/issues/274	2018-09-18 12:30:01 -04:00
matlabbe	eedc68c360	fixed #312	2018-09-18 12:20:15 -04:00
matlabbe	0cf37fbbf1	Added D415 icon	2018-09-15 11:37:02 -04:00