CameraRealSense2: Odom extrinsics against another sensor can be calibrated with having to calibrate stereo first. DBViewer: fixed local grid wrongly using global grid parameters.

corelib/include/rtabmap/core/Parameters.h

@@ -830,7 +830,11 @@ public:
     static bool isFeatureParameter(const std::string & param);
     static ParametersMap getDefaultOdometryParameters(bool stereo = false, bool vis = true, bool icp = false);
     static ParametersMap getDefaultParameters(const std::string & group);
-    static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group);
+    /**
+     * If remove=false: keep only parameters of the specified group.
+     * If remove=true: remove parameters of the specified group.
+     */
+    static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group, bool remove = false);
 
     static void readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly = false);
     static void writeINI(const std::string & configFile, const ParametersMap & parameters);

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

@@ -89,7 +89,7 @@ public:
 	void setJsonConfig(const std::string & json);
 	// T265 related parameters
 	void setImagesRectified(bool enabled);
-	void setOdomProvided(bool enabled, bool imageStreamsDisabled=false);
+	void setOdomProvided(bool enabled, bool imageStreamsDisabled=false, bool onlyLeftStream = false);
 
 #ifdef RTABMAP_REALSENSE2
 private:
@@ -116,8 +116,6 @@ private:
 	std::string deviceId_;
 	rs2::syncer syncer_;
 	float depth_scale_meters_;
-	rs2_intrinsics depthIntrinsics_;
-	rs2_intrinsics rgbIntrinsics_;
 	cv::Mat depthBuffer_;
 	cv::Mat rgbBuffer_;
 	CameraModel model_;
@@ -138,6 +136,7 @@ private:
 	bool rectifyImages_;
 	bool odometryProvided_;
 	bool odometryImagesDisabled_;
+	bool odometryOnlyLeftStream_;
 	int cameraWidth_;
 	int cameraHeight_;
 	int cameraFps_;

corelib/src/CameraModel.cpp

@@ -350,7 +350,7 @@ bool CameraModel::load(const std::string & filePath)
 		}
 		catch(const cv::Exception & e)
 		{
-			UERROR("Error reading calibration file \"%s\": %s", filePath.c_str(), e.what());
+			UERROR("Error reading calibration file \"%s\": %s (Make sure the first line of the yaml file is \"%YAML:1.0\")", filePath.c_str(), e.what());
 		}
 	}
 	else

corelib/src/Parameters.cpp

@@ -213,14 +213,15 @@ ParametersMap Parameters::getDefaultParameters(const std::string & groupIn)
 	return parameters;
 }
 
-ParametersMap Parameters::filterParameters(const ParametersMap & parameters, const std::string & groupIn)
+ParametersMap Parameters::filterParameters(const ParametersMap & parameters, const std::string & group, bool remove)
 {
 	ParametersMap output;
 	for(rtabmap::ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 	{
 		UASSERT(uSplit(iter->first, '/').size()  == 2);
 		std::string group = uSplit(iter->first, '/').front();
-		if(group.compare(groupIn) == 0)
+		bool sameGroup = group.compare(group) == 0;
+		if((!remove && sameGroup) || (remove && !sameGroup))
 		{
 			output.insert(*iter);
 		}

corelib/src/camera/CameraRealSense2.cpp

@@ -70,6 +70,7 @@ CameraRealSense2::CameraRealSense2(
 	rectifyImages_(true),
 	odometryProvided_(false),
 	odometryImagesDisabled_(false),
+	odometryOnlyLeftStream_(false),
 	cameraWidth_(640),
 	cameraHeight_(480),
 	cameraFps_(30),
@@ -193,7 +194,7 @@ void CameraRealSense2::pose_callback(rs2::frame frame)
 
 void CameraRealSense2::frame_callback(rs2::frame frame)
 {
-	//UDEBUG("Frame callback! %f", frame.get_timestamp());
+	UDEBUG("Frame callback! %f", frame.get_timestamp());
 	syncer_(frame);
 }
 void CameraRealSense2::multiple_message_callback(rs2::frame frame)
@@ -694,8 +695,6 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 	UDEBUG("");
 
 	model_ = CameraModel();
-	rs2::stream_profile depthStreamProfile;
-	rs2::stream_profile rgbStreamProfile;
 	std::vector<std::vector<rs2::stream_profile> > profilesPerSensor(sensors.size());
 	 for (unsigned int i=0; i<sensors.size(); ++i)
 	 {
@@ -759,8 +758,6 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 								intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy,
 								intrinsic.model,
 								intrinsic.coeffs[0], intrinsic.coeffs[1], intrinsic.coeffs[2], intrinsic.coeffs[3], intrinsic.coeffs[4]);
-						rgbStreamProfile = profile;
-						rgbIntrinsics_ = intrinsic;
 						added = true;
 						if(video_profile.format() == RS2_FORMAT_RGB8 || profilesPerSensor[i].size()==2)
 						{
@@ -773,8 +770,6 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 					{
 						profilesPerSensor[i].push_back(profile);
 						depthBuffer_ = cv::Mat(cv::Size(cameraWidth_, cameraHeight_), video_profile.format() == RS2_FORMAT_Y8?CV_8UC1:CV_16UC1, cv::Scalar(0));
-						depthStreamProfile = profile;
-						depthIntrinsics_ = intrinsic;
 						added = true;
 						if(!ir_ || irDepth_ || profilesPerSensor[i].size()==2)
 						{
@@ -828,20 +823,44 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 				{
 					UASSERT(i<2);
 					profilesPerSensor[i].push_back(profile);
-					auto intrinsic = video_profile.get_intrinsics();
 					if(pi==0)
 					{
 						// LEFT FISHEYE
 						rgbBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
-						rgbStreamProfile = profile;
+						if(odometryOnlyLeftStream_)
-						rgbIntrinsics_ = intrinsic;
+						{
+							auto intrinsic = video_profile.get_intrinsics();
+							UINFO("Model: %dx%d fx=%f fy=%f cx=%f cy=%f dist model=%d coeff=%f %f %f %f",
+									intrinsic.width, intrinsic.height,
+									intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy,
+									intrinsic.model,
+									intrinsic.coeffs[0], intrinsic.coeffs[1], intrinsic.coeffs[2], intrinsic.coeffs[3]);
+							cv::Mat K = cv::Mat::eye(3,3,CV_64FC1);
+							K.at<double>(0,0) = intrinsic.fx;
+							K.at<double>(1,1) = intrinsic.fy;
+							K.at<double>(0,2) = intrinsic.ppx;
+							K.at<double>(1,2) = intrinsic.ppy;
+							UASSERT(intrinsic.model == RS2_DISTORTION_KANNALA_BRANDT4); // we expect fisheye 4 values
+							cv::Mat D = cv::Mat::zeros(1,6,CV_64FC1);
+							D.at<double>(0,0) = intrinsic.coeffs[0];
+							D.at<double>(0,1) = intrinsic.coeffs[1];
+							D.at<double>(0,4) = intrinsic.coeffs[2];
+							D.at<double>(0,5) = intrinsic.coeffs[3];
+							cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
+							P.at<double>(0,0) = intrinsic.fx;
+							P.at<double>(1,1) = intrinsic.fy;
+							P.at<double>(0,2) = intrinsic.ppx;
+							P.at<double>(1,2) = intrinsic.ppy;
+							cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
+							model_ = CameraModel(camera_name, cv::Size(intrinsic.width, intrinsic.height), K, D, R, P, this->getLocalTransform());
+							if(rectifyImages_)
+								model_.initRectificationMap();
+						}
 					}
-					else
+					else if(!odometryOnlyLeftStream_)
 					{
 						// RIGHT FISHEYE
 						depthBuffer_ = cv::Mat(cv::Size(848, 800), CV_8UC1, cv::Scalar(0));
-						depthStreamProfile = profile;
-						depthIntrinsics_ = intrinsic;
 					}
 					added = true;
 				}
@@ -961,7 +980,9 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		 {
 			 serial = cameraName;
 		 }
-		if(!calibrationFolder.empty() && !serial.empty())
+		if(!odometryImagesDisabled_ &&
+		   !odometryOnlyLeftStream_ &&
+		   !calibrationFolder.empty() && !serial.empty())
 		{
 			if(!stereoModel_.load(calibrationFolder, serial, false))
 			{
@@ -1005,7 +1026,10 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 
 			Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
 			this->setLocalTransform(this->getLocalTransform() * opticalTransform.inverse());
-			stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
+			if(odometryOnlyLeftStream_)
+				model_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
+			else
+				stereoModel_.setLocalTransform(this->getLocalTransform()*poseToLeftT);
 			imuLocalTransform_ = this->getLocalTransform()* poseToIMUT;
 
 			if(odometryImagesDisabled_)
@@ -1027,9 +1051,12 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			 UINFO("leftToIMU = %s", leftToIMUT.prettyPrint().c_str());
 			 imuLocalTransform_ = this->getLocalTransform() * leftToIMUT;
 			 UINFO("imu local transform = %s", imuLocalTransform_.prettyPrint().c_str());
-			 stereoModel_.setLocalTransform(this->getLocalTransform());
+			 if(odometryOnlyLeftStream_)
+				 model_.setLocalTransform(this->getLocalTransform());
+			 else
+				 stereoModel_.setLocalTransform(this->getLocalTransform());
 		}
-		if(rectifyImages_ && !stereoModel_.isValidForRectification())
+		if(!odometryImagesDisabled_ && rectifyImages_ && !model_.isValidForRectification() && !stereoModel_.isValidForRectification())
 		{
 			UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
 			return false;
@@ -1192,6 +1219,7 @@ void CameraRealSense2::setDualMode(bool enabled, const Transform & extrinsics)
 	{
 		odometryProvided_ = true;
 		odometryImagesDisabled_ = false;
+		odometryOnlyLeftStream_ = false;
 	}
 #endif
 }
@@ -1210,7 +1238,7 @@ void CameraRealSense2::setImagesRectified(bool enabled)
 #endif
 }
 
-void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled)
+void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled, bool onlyLeftStream)
 {
 #ifdef RTABMAP_REALSENSE2
 	if(dualMode_ && !enabled)
@@ -1220,6 +1248,7 @@ void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled)
 	}
 	odometryProvided_ = enabled;
 	odometryImagesDisabled_ = enabled && imageStreamsDisabled;
+	odometryOnlyLeftStream_ = enabled && !imageStreamsDisabled && onlyLeftStream;
 #endif
 }
 
@@ -1374,27 +1403,48 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 					data = SensorData(bgr, depth, model_, this->getNextSeqID(), stamp);
 				}
 			}
-			else if(is_left_fisheye_arrived && is_right_fisheye_arrived)
+			else if(is_left_fisheye_arrived)
 			{
-				auto from_image_frame = depth_frame.as<rs2::video_frame>();
+				if(odometryOnlyLeftStream_)
-				cv::Mat left,right;
-				if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
 				{
-					left = stereoModel_.left().rectifyImage(cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()));
+					cv::Mat left;
-					right = stereoModel_.right().rectifyImage(cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()));
+					if(rectifyImages_ && model_.isValidForRectification())
-				}
+					{
-				else
+						left = model_.rectifyImage(cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()));
-				{
+					}
-					left = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()).clone();
+					else
-					right = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
+					{
-				}
+						left = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()).clone();
+					}
 
-				if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
+					if(model_.imageHeight() == 0 || model_.imageWidth() == 0)
-				{
+					{
-					stereoModel_.setImageSize(left.size());
+						model_.setImageSize(left.size());
-				}
+					}
 
-				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), stamp);
+					data = SensorData(left, cv::Mat(), model_, this->getNextSeqID(), stamp);
+				}
+				else if(is_right_fisheye_arrived)
+				{
+					cv::Mat left,right;
+					if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
+					{
+						left = stereoModel_.left().rectifyImage(cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()));
+						right = stereoModel_.right().rectifyImage(cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()));
+					}
+					else
+					{
+						left = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data()).clone();
+						right = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
+					}
+
+					if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
+					{
+						stereoModel_.setImageSize(left.size());
+					}
+
+					data = SensorData(left, right, stereoModel_, this->getNextSeqID(), stamp);
+				}
 			}
 			else
 			{
@@ -1471,6 +1521,13 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 				lastImuStamp_ = imuStamp;
 			}
 		}
+		else if(frameset.size()==1 && frameset[0].get_profile().stream_type() == RS2_STREAM_FISHEYE)
+		{
+			UERROR("Missing frames (received %d, needed=%d). For T265 camera, "
+					"either use realsense sdk v2.42.0, or apply "
+					"this patch (https://github.com/IntelRealSense/librealsense/issues/9030#issuecomment-962223017) "
+					"to fix this problem.", (int)frameset.size(), desiredFramesetSize);
+		}
 		else
 		{
 			UERROR("Missing frames (received %d, needed=%d)", (int)frameset.size(), desiredFramesetSize);

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -415,7 +415,7 @@ private:
 	void addParameters(const QGroupBox * box);
 	QList<QGroupBox*> getGroupBoxes();
 	void readSettingsBegin();
-	Camera * createCamera(Src driver, const QString & device, const QString & calibrationPath, bool useRawImages, bool useColor, bool odomOnly); // return camera should be deleted if not null
+	Camera * createCamera(Src driver, const QString & device, const QString & calibrationPath, bool useRawImages, bool useColor, bool odomOnly, bool odomSensorExtrinsicsCalib); // return camera should be deleted if not null
 
 protected:
 	PANEL_FLAGS _obsoletePanels;

guilib/src/DatabaseViewer.cpp

@@ -5025,6 +5025,7 @@ void DatabaseViewer::update(int value,
 								float xMin=0.0f, yMin=0.0f;
 								cv::Mat map8S;
 								ParametersMap parameters = ui_->parameters_toolbox->getParameters();
+								parameters = Parameters::filterParameters(parameters, "GridGlobal", true);
 								float gridCellSize = Parameters::defaultGridCellSize();
 								Parameters::parse(parameters, Parameters::kGridCellSize(), gridCellSize);
 #ifdef RTABMAP_OCTOMAP
@@ -5035,7 +5036,7 @@ void DatabaseViewer::update(int value,
 								else
 #endif
 								{
-									OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
+									OccupancyGrid grid(parameters);
 									grid.setCellSize(gridCellSize);
 									grid.addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second);
 									grid.update(poses);

guilib/src/PreferencesDialog.cpp

@@ -5857,6 +5857,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 		 !_ui->checkBox_stereo_rectify->isChecked()) || 
 		useRawImages, 
 		useColor, 
+		false,
 		false);
 }
 
@@ -5866,7 +5867,8 @@ Camera * PreferencesDialog::createCamera(
 		const QString & calibrationPath,
 		bool useRawImages,
 		bool useColor,
-		bool odomOnly)
+		bool odomOnly,
+		bool odomSensorExtrinsicsCalib)
 {
 	if(odomOnly && !(driver == kSrcStereoRealSense2 || driver == kSrcStereoZed))
 	{
@@ -6014,7 +6016,7 @@ Camera * PreferencesDialog::createCamera(
 			if(driver == kSrcStereoRealSense2)
 			{
 				((CameraRealSense2*)camera)->setImagesRectified(!useRawImages);
-				((CameraRealSense2*)camera)->setOdomProvided(_ui->comboBox_odom_sensor->currentIndex() == 1 || odomOnly, odomOnly);
+				((CameraRealSense2*)camera)->setOdomProvided(_ui->comboBox_odom_sensor->currentIndex() == 1 || odomOnly, odomOnly, odomSensorExtrinsicsCalib);
 			}
 			else
 			{
@@ -6383,7 +6385,7 @@ Camera * PreferencesDialog::createOdomSensor(Transform & extrinsics, double & ti
 		timeOffset = _ui->doubleSpinBox_odom_sensor_time_offset->value()/1000.0;
 		scaleFactor = (float)_ui->doubleSpinBox_odom_sensor_scale_factor->value();
 
-		return createCamera(driver, _ui->lineEdit_odomSourceDevice->text(), _ui->lineEdit_odom_sensor_path_calibration->text(), false, true, true);
+		return createCamera(driver, _ui->lineEdit_odomSourceDevice->text(), _ui->lineEdit_odom_sensor_path_calibration->text(), false, true, true, false);
 	}
 	return 0;
 }
@@ -7060,7 +7062,7 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics()
 						odomDriver,
 						_ui->lineEdit_odomSourceDevice->text(),
 						_ui->lineEdit_odom_sensor_path_calibration->text(),
-						false, true, false); // Odom sensor
+						false, true, false, true); // Odom sensor
 				if(!camera)
 				{
 					return;