Odometry: when "Rtabmap/ImagesAlreadyRectified" is false and odometry…

… strategy doesn't support raw images, added rectification for RGB-D data (only rgb rectified though) for convenience.
introlab · Jun 1, 2021 · 630cb6a · 630cb6a
1 parent 12c4628
commit 630cb6a
Show file tree

Hide file tree

Showing 4 changed files with 73 additions and 5 deletions.
diff --git a/corelib/include/rtabmap/core/CameraModel.h b/corelib/include/rtabmap/core/CameraModel.h
@@ -81,7 +81,7 @@ class RTABMAP_EXP CameraModel
 
 	virtual ~CameraModel() {}
 
-	void initRectificationMap();
+	bool initRectificationMap();
 	bool isRectificationMapInitialized() const {return !mapX_.empty() && !mapY_.empty();}
 
 	bool isValidForProjection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0;}

diff --git a/corelib/include/rtabmap/core/Odometry.h b/corelib/include/rtabmap/core/Odometry.h
@@ -121,6 +121,7 @@ class RTABMAP_EXP Odometry
 	std::vector<ParticleFilter *> particleFilters_;
 	cv::KalmanFilter kalmanFilter_;
 	StereoCameraModel stereoModel_;
+	std::vector<CameraModel> models_;
 	std::map<double, Transform> imus_;
 
 protected:

diff --git a/corelib/src/CameraModel.cpp b/corelib/src/CameraModel.cpp
@@ -152,7 +152,7 @@ CameraModel::CameraModel(
 	K_.at<double>(1,2) = cy;
 }
 
-void CameraModel::initRectificationMap()
+bool CameraModel::initRectificationMap()
 {
 	UASSERT(imageSize_.height > 0 && imageSize_.width > 0);
 	UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 6 || D_.cols == 8));
@@ -182,6 +182,7 @@ void CameraModel::initRectificationMap()
 		// RadialTangential
 		cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
 	}
+	return isRectificationMapInitialized();
 }
 
 void CameraModel::setImageSize(const cv::Size & size)

diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp
@@ -338,8 +338,10 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 				}
 				else
 				{
-					UERROR("Odometry approach chosen cannot process raw images (not rectified images) and we cannot rectify them. "
-							"Make sure images are rectified, and set %s parameter back to true.",
+					UERROR("Odometry approach chosen cannot process raw images (not rectified images) "
+							"and we cannot rectify them as the rectification map failed to initialize (valid calibration?). "
+							"Make sure images are rectified and set %s parameter back to true, or make sure "
+							"calibration is valid for rectification.",
 							Parameters::kRtabmapImagesAlreadyRectified().c_str());
 				}
 			}
@@ -352,10 +354,74 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 						false);
 			}
 		}
+		else if(!data.cameraModels().empty())
+		{
+			bool valid = true;
+			if(data.cameraModels().size() != models_.size())
+			{
+				models_.clear();
+				valid = false;
+			}
+			else
+			{
+				for(size_t i=0; i<data.cameraModels().size() && valid; ++i)
+				{
+					valid = models_[i].isRectificationMapInitialized() &&
+							models_[i].imageSize() == data.cameraModels()[i].imageSize();
+				}
+			}
+
+			if(!valid)
+			{
+				models_ = data.cameraModels();
+				valid = true;
+				for(size_t i=0; i<models_.size() && valid; ++i)
+				{
+					valid = models_[i].initRectificationMap();
+				}
+				if(valid)
+				{
+					UWARN("%s parameter is set to false but the selected odometry approach cannot "
+							"process raw images. We will rectify them for convenience (only "
+							"rgb is rectified, we assume depth image is already rectified!).",
+							Parameters::kRtabmapImagesAlreadyRectified().c_str());
+				}
+				else
+				{
+					UERROR("Odometry approach chosen cannot process raw images (not rectified images) "
+							"and we cannot rectify them as the rectification map failed to initialize (valid calibration?). "
+							"Make sure images are rectified and set %s parameter back to true, or "
+							"make sure calibration is valid for rectification",
+							Parameters::kRtabmapImagesAlreadyRectified().c_str());
+					models_.clear();
+				}
+			}
+			if(valid)
+			{
+				// Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
+				if(models_.size()==1)
+				{
+					data.setRGBDImage(models_[0].rectifyImage(data.imageRaw()), data.depthRaw(), models_, false);
+				}
+				else
+				{
+					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();
+					for(size_t i=0; i<models_.size() && valid; ++i)
+					{
+						cv::Mat rectifiedImage = models_[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)));
+					}
+					data.setRGBDImage(rectifiedImages, data.depthRaw(), models_, false);
+				}
+			}
+		}
 		else
 		{
 			UERROR("Odometry approach chosen cannot process raw images (not rectified images). Make sure images "
-					"are rectified, and set %s parameter back to true.",
+					"are rectified, and set %s parameter back to true, or make sure that calibration is valid "
+					"for rectification so we can rectifiy them for convenience",
 					Parameters::kRtabmapImagesAlreadyRectified().c_str());
 		}
 	}