SensorData: ignore invalid CameraModel

corelib/include/rtabmap/core/SensorData.h

@@ -598,6 +598,8 @@ public:
 	/**
 	 * Set image data. Detect automatically if raw or compressed.
 	 * A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 * An invalid @p model (not CameraModel::isValidForProjection()) without any image is
+	 * a placeholder (e.g., scan-only data): it is not added, so cameraModels() is empty.
 	 * @param clearPreviousData, clear previous raw and compressed images before setting the new ones.
 	 */
 	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const CameraModel & model, bool clearPreviousData = true);
@@ -1025,6 +1027,9 @@ public:
 #endif
 
 private:
+	/// Whether setRGBDImage() keeps @p model: not an invalid model without any image.
+	bool keepCameraModel(const CameraModel & model, const cv::Mat & rgb, const cv::Mat & depth, bool clearPreviousData) const;
+
 	int _id; ///< Unique sensor data ID (0 if invalid)
 	double _stamp; ///< Timestamp in seconds
 

corelib/src/SensorData.cpp

@@ -313,6 +313,24 @@ SensorData::~SensorData()
 {
 }
 
+bool SensorData::keepCameraModel(
+		const CameraModel & model,
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		bool clearPreviousData) const
+{
+	// An invalid model without any image is only a placeholder (e.g., scan-only data
+	// created with CameraModel()): it is not kept, so that cameraModels() is empty when
+	// there is no camera. An invalid model with an image is kept: images can be used
+	// without calibration, and they are split per camera model.
+	return model.isValidForProjection() ||
+			!rgb.empty() ||
+			!depth.empty() ||
+			(!clearPreviousData && (
+					!_imageRaw.empty() || !_imageCompressed.empty() ||
+					!_depthOrRightRaw.empty() || !_depthOrRightCompressed.empty()));
+}
+
 void SensorData::setRGBDImage(
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
@@ -320,7 +338,10 @@ void SensorData::setRGBDImage(
 		bool clearPreviousData)
 {
 	std::vector<CameraModel> models;
-	models.push_back(model);
+	if(keepCameraModel(model, rgb, depth, clearPreviousData))
+	{
+		models.push_back(model);
+	}
 	setRGBDImage(rgb, depth, models, clearPreviousData);
 }
 void SensorData::setRGBDImage(
@@ -331,7 +352,10 @@ void SensorData::setRGBDImage(
 	bool clearPreviousData)
 {
 	std::vector<CameraModel> models;
-	models.push_back(model);
+	if(keepCameraModel(model, rgb, depth, clearPreviousData))
+	{
+		models.push_back(model);
+	}
 	setRGBDImage(rgb, depth, depthConfidence, models, clearPreviousData);
 }
 void SensorData::setRGBDImage(

corelib/test/test_sensordata.cpp

@@ -199,7 +199,7 @@ TEST(SensorDataTest, IsValidWithCameraModel)
 {
     SensorData data;
     data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel());
-    EXPECT_FALSE(data.cameraModels().empty());
+    EXPECT_TRUE(data.cameraModels().empty()); // invalid without image: placeholder not kept
     EXPECT_FALSE(data.isValid()); // not valid for projection
 
     data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(525.0, 525.0, 320.0, 240.0));
@@ -986,3 +986,32 @@ TEST(SensorDataTest, DifferentDepthTypes)
     EXPECT_EQ(data.depthOrRightRaw().type(), CV_32FC1);
 }
 
+
+// An invalid CameraModel without any image is a placeholder (e.g., lidar odometry
+// creating scan-only data with CameraModel()): it is not kept. With an image, it is kept,
+// as images can be used without calibration.
+TEST(SensorDataTest, InvalidCameraModelIsKeptOnlyWithImages)
+{
+	const LaserScan scan(cv::Mat(1, 3, CV_32FC2, cv::Scalar(1.0f, 0.0f)), 0, 10.0f, LaserScan::kXY);
+	const SensorData scanOnly(scan, cv::Mat(), cv::Mat(), CameraModel(), 1, 1.0);
+	EXPECT_TRUE(scanOnly.cameraModels().empty());
+	EXPECT_TRUE(scanOnly.isValid());
+
+	const cv::Mat image(4, 6, CV_8UC1, cv::Scalar(1));
+	const SensorData uncalibrated(image, CameraModel(), 1, 1.0);
+	EXPECT_EQ(uncalibrated.cameraModels().size(), 1u);
+
+	const SensorData compressedOnly(compressImage2(image, ".png"), CameraModel(), 1, 1.0);
+	EXPECT_EQ(compressedOnly.cameraModels().size(), 1u);
+
+	const CameraModel valid(10.0, 10.0, 3.0, 2.0);
+	const SensorData calibratedNoImage(scan, cv::Mat(), cv::Mat(), valid, 1, 1.0);
+	EXPECT_EQ(calibratedNoImage.cameraModels().size(), 1u) << "valid models are always kept";
+
+	// Keeping the images already there: they still need their model
+	SensorData data(image, CameraModel(), 1, 1.0);
+	data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(), false);
+	EXPECT_EQ(data.cameraModels().size(), 1u);
+	data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(), true);
+	EXPECT_TRUE(data.cameraModels().empty()) << "images cleared, nothing left to describe";
+}