From c6055062679e3710445645e07e8ba6c23c1e23ba Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 3 Oct 2026 16:14:24 -0700 Subject: [PATCH] SensorData: ignore invalid CameraModel --- corelib/include/rtabmap/core/SensorData.h | 5 ++++ corelib/src/SensorData.cpp | 28 ++++++++++++++++++-- corelib/test/test_sensordata.cpp | 31 ++++++++++++++++++++++- 3 files changed, 61 insertions(+), 3 deletions(-) diff --git a/corelib/include/rtabmap/core/SensorData.h b/corelib/include/rtabmap/core/SensorData.h index b542ff19..79c2bf08 100644 --- a/corelib/include/rtabmap/core/SensorData.h +++ b/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 diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index d9b30ead..c6fbd7dc 100644 --- a/corelib/src/SensorData.cpp +++ b/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 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 models; - models.push_back(model); + if(keepCameraModel(model, rgb, depth, clearPreviousData)) + { + models.push_back(model); + } setRGBDImage(rgb, depth, depthConfidence, models, clearPreviousData); } void SensorData::setRGBDImage( diff --git a/corelib/test/test_sensordata.cpp b/corelib/test/test_sensordata.cpp index 2532f200..54e54d74 100644 --- a/corelib/test/test_sensordata.cpp +++ b/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"; +}