Added parameter "Rtabmap/RectifyOnlyFeatures". Added support of fisheye distortion model.

CMakeLists.txt

@@ -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})
   

corelib/include/rtabmap/core/CameraModel.h

@@ -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;}

corelib/include/rtabmap/core/Memory.h

@@ -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

corelib/include/rtabmap/core/Parameters.h

@@ -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.");

corelib/include/rtabmap/core/Statistics.h

@@ -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);

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -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_;}

corelib/src/CameraModel.cpp

@@ -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,27 @@ 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)
+	{
+		// 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,1);
+		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
+	{
+		// RadialTangential
+		cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+	}
 }
 
 void CameraModel::setImageSize(const cv::Size & size)
@@ -256,7 +272,20 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
 				n["data"] >> data;
 				UASSERT(rows*cols == (int)data.size());
 				UASSERT(rows == 1 && (cols == 4 || cols == 5 || cols == 8));
-				D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+				std::string distortionModel = (std::string)n["distortion_model"];
+				if(uStrContains(distortionModel, "fisheye") ||
+				   uStrContains(distortionModel, "equidistant"))
+				{
+					D_ = cv::Mat::zeros(1,6,CV_64FC1);
+					D_.at<double>(0,0) = data[0];
+					D_.at<double>(0,1) = data[1];
+					D_.at<double>(0,4) = data[2];
+					D_.at<double>(0,5) = data[3];
+				}
+				else
+				{
+					D_ = cv::Mat(rows, cols, CV_64FC1, data.data()).clone();
+				}
 			}
 			else
 			{
@@ -347,20 +376,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 << "rows" << D.rows;
-			fs << "cols" << D_.cols;
+			fs << "cols" << D.cols;
-			fs << "data" << std::vector<double>((double*)D_.data, ((double*)D_.data)+(D_.rows*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" << "fisheye"; // equidistant
+			}
+			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
 			}
 		}
 

corelib/src/Memory.cpp

@@ -104,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),
@@ -486,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);
 
 
@@ -1506,6 +1508,8 @@ void Memory::clear()
 	_memoryChanged = false;
 	_linksChanged = false;
 	_gpsOrigin = GPS();
+	_rectCameraModels.clear();
+	_rectStereoCameraModel = StereoCameraModel();
 
 	if(_dbDriver)
 	{
@@ -3685,25 +3689,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())
+		// we assume that once rtabmap is receiving data, the calibration won't change over time
-		{
-			UERROR("RGB-D images should be already rectified! Make sure they are and set %s parameter back to true.",
-					Parameters::kRtabmapImagesAlreadyRectified().c_str());
-			return 0;
-		}
 		if(data.cameraModels().size())
 		{
+			// 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
 				{
@@ -3718,8 +3738,21 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 		else if(data.stereoCameraModel().isValidForRectification())
 		{
-			data.setImageRaw(data.stereoCameraModel().left().rectifyImage(data.imageRaw()));
+			if(!_rectStereoCameraModel.isValidForRectification())
-			data.setDepthOrRightRaw(data.stereoCameraModel().right().rectifyImage(data.rightRaw()));
+			{
+				_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
 		{
@@ -3836,6 +3869,111 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
 					(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
 			{
+				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)
+						{
+							// 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
+						{
+							//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)
+							{
+								// 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
+							{
+								//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);
+				}
 				keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
 				t = timer.ticks();
 				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
@@ -3891,6 +4029,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();
@@ -4177,8 +4317,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 
 			t = timer.ticks();
 			UASSERT(words3D.size() == words.size());
-			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D_motion(), t*1000.0f);
-			UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
+			UDEBUG("time keypoints 3D by motion (%d) = %fs", (int)words3D.size(), t);
 		}
 	}
 

guilib/include/rtabmap/gui/MainWindow.h

@@ -327,6 +327,7 @@ private:
 	bool _savedMaximized;
 	QStringList _waypoints;
 	int _waypointsIndex;
+	std::vector<CameraModel> _rectCameraModels;
 
 	QMap<int, Signature> _cachedSignatures;
 	long _cachedMemoryUsage;

guilib/src/CalibrationDialog.cpp

@@ -706,7 +706,19 @@ void CalibrationDialog::calibrate()
 		K = cv::Mat::eye(3,3,CV_64FC1);
 		UINFO("calibrate!");
 		//Find intrinsic and extrinsic camera parameters
-		double rms = cv::calibrateCamera(objectPoints,
+		double rms = 0.0;
+		bool fishEye = ui_->checkBox_fisheye->isChecked();
+		if(fishEye)
+		{
+			rms = cv::fisheye::calibrate(objectPoints,
+				imagePoints_[id],
+				imageSize_[id],
+				K,
+				D,
+				rvecs,
+				tvecs);
+		}
+		rms = cv::calibrateCamera(objectPoints,
 				imagePoints_[id],
 				imageSize_[id],
 				K,
@@ -724,7 +736,14 @@ 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(fishEye)
+			{
+				cv::fisheye::projectPoints( cv::Mat(objectPoints[i]), rvecs[i], tvecs[i], K, D, imagePoints2);
+			}
+			else
+			{
+				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();
@@ -741,6 +760,16 @@ void CalibrationDialog::calibrate()
 		P.at<double>(2,3) = 1;
 		K.copyTo(P.colRange(0,3).rowRange(0,3));
 
+		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);
+		}
+
 		std::cout << "K = " << K << std::endl;
 		std::cout << "D = " << D << std::endl;
 		std::cout << "width = " << imageSize_[id].width << std::endl;
@@ -875,28 +904,68 @@ 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;
+	bool fishEye = left.D_raw().cols == 6;
 	// calibrate extrinsic
+	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);
 #if CV_MAJOR_VERSION < 3
-	double rms = cv::stereoCalibrate(
+		rms = cv::fisheye::stereoCalibrate(
-			objectPoints,
+				objectPoints,
-			stereoImagePoints_[0],
+				stereoImagePoints_[0],
-			stereoImagePoints_[1],
+				stereoImagePoints_[1],
-			left.K_raw(), left.D_raw(),
+				left.K_raw(), D_left,
-			right.K_raw(), right.D_raw(),
+				right.K_raw(), D_right,
-			imageSize, R, T, E, F,
+				imageSize, R, T,
-			cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5),
+				cv::CALIB_FIX_INTRINSIC,
-			cv::CALIB_FIX_INTRINSIC);
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
 #else
-	double rms = cv::stereoCalibrate(
+		rms = cv::fisheye::stereoCalibrate(
-			objectPoints,
+				objectPoints,
-			stereoImagePoints_[0],
+				stereoImagePoints_[0],
-			stereoImagePoints_[1],
+				stereoImagePoints_[1],
-			left.K_raw(), left.D_raw(),
+				left.K_raw(), D_left,
-			right.K_raw(), right.D_raw(),
+				right.K_raw(), D_right,
-			imageSize, R, T, E, F,
+				imageSize, R, T,
-			cv::CALIB_FIX_INTRINSIC,
+				cv::CALIB_FIX_INTRINSIC,
-			cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, 1e-5));
 #endif
+	}
+	else
+	{
+#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;

guilib/src/MainWindow.cpp

@@ -2781,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>);
@@ -2792,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
@@ -6293,6 +6344,7 @@ void MainWindow::clearTheCache()
 	_octomap = new OctoMap(_preferencesDialog->getAllParameters());
 #endif
 	_occupancyGrid->clear();
+	_rectCameraModels.clear();
 }
 
 void MainWindow::openHelp()

guilib/src/PreferencesDialog.cpp

@@ -703,6 +703,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()));
 

guilib/src/ui/calibrationDialog.ui

@@ -17,16 +17,7 @@
    <property name="spacing">
     <number>0</number>
    </property>
-   <property name="leftMargin">
+   <property name="margin">
-    <number>0</number>
-   </property>
-   <property name="topMargin">
-    <number>0</number>
-   </property>
-   <property name="rightMargin">
-    <number>0</number>
-   </property>
-   <property name="bottomMargin">
     <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">
+         <property name="margin">
-          <number>0</number>
-         </property>
-         <property name="topMargin">
-          <number>0</number>
-         </property>
-         <property name="rightMargin">
-          <number>0</number>
-         </property>
-         <property name="bottomMargin">
           <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">

guilib/src/ui/preferencesDialog.ui

@@ -95,7 +95,7 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>681</width>
+            <width>678</width>
             <height>2943</height>
            </rect>
           </property>
@@ -103,16 +103,7 @@
            <property name="spacing">
             <number>0</number>
            </property>
-           <property name="leftMargin">
+           <property name="margin">
-            <number>0</number>
-           </property>
-           <property name="topMargin">
-            <number>0</number>
-           </property>
-           <property name="rightMargin">
-            <number>0</number>
-           </property>
-           <property name="bottomMargin">
             <number>0</number>
            </property>
            <item>
@@ -126,7 +117,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>16</number>
+                <number>7</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -5093,16 +5084,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">
+                       <property name="margin">
-                        <number>0</number>
-                       </property>
-                       <property name="topMargin">
-                        <number>0</number>
-                       </property>
-                       <property name="rightMargin">
-                        <number>0</number>
-                       </property>
-                       <property name="bottomMargin">
                         <number>0</number>
                        </property>
                        <item>
@@ -6290,6 +6272,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 +6378,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 +6437,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 +15281,7 @@ Lower the ratio -&gt; higher the precision.</string>
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="leftMargin">
+                        <property name="margin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -15365,16 +15361,7 @@ Lower the ratio -&gt; higher the precision.</string>
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="leftMargin">
+                        <property name="margin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -15486,16 +15473,7 @@ Lower the ratio -&gt; higher the precision.</string>
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="leftMargin">
+                        <property name="margin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>