SensorData's isValid() method checks whether camera models are valid (#1769)

* SensorData's isValid() method checks whether camera models are valid

* Added tests and doc

---------

Co-authored-by: matlabbe <[email protected]>

corelib/include/rtabmap/core/SensorData.h

@@ -470,21 +470,33 @@ public:
 	 * @brief Checks if the sensor data is valid
 	 * 
 	 * Returns true if the sensor data contains at least one of:
-	 * - Valid ID (> 0) or non-zero stamp
+	 * - Valid ID (> 0)
 	 * - Images (raw or compressed)
 	 * - Depth/right images (raw or compressed)
 	 * - Depth confidence (raw or compressed)
 	 * - Laser scan (raw or compressed)
-	 * - Camera models (mono or stereo)
+	 * - Camera models (mono or stereo), at least one valid for projection
+	 *   (see CameraModel::isValidForProjection() and StereoCameraModel::isValidForProjection())
 	 * - User data (raw or compressed)
 	 * - Keypoints and descriptors
+	 * - Occupancy grid cells (ground, obstacles or empty)
 	 * - IMU data
 	 *
+	 * @note The stamp is not considered: a SensorData with only a stamp set is not valid
+	 *       (e.g., an empty message converted from ROS still has its header stamp
+	 *       and a camera model created from an empty camera info).
+	 *
 	 * @return True if the sensor data contains any valid information, false otherwise
 	 */
 	bool isValid() const {
+		bool hasCameraModel = false;
+		for (size_t i=0; i < _cameraModels.size() && !hasCameraModel; ++i)
+			hasCameraModel = _cameraModels[i].isValidForProjection();
+		if (!hasCameraModel)
+			for (size_t i=0; i < _stereoCameraModels.size() && !hasCameraModel; ++i)
+				hasCameraModel = _stereoCameraModels[i].isValidForProjection();
+
 		return !(_id == 0 &&
-			_stamp == 0.0 &&
 			_imageRaw.empty() &&
 			_imageCompressed.empty() &&
 			_depthOrRightRaw.empty() &&
@@ -493,8 +505,7 @@ public:
 			_depthConfidenceCompressed.empty() &&
 			_laserScanRaw.isEmpty() &&
 			_laserScanCompressed.isEmpty() &&
-			_cameraModels.empty() &&
+			!hasCameraModel &&
-			_stereoCameraModels.empty() &&
 			_userDataRaw.empty() &&
 			_userDataCompressed.empty() &&
 			_keypoints.size() == 0 &&

corelib/test/test_sensordata.cpp

@@ -187,10 +187,43 @@ TEST(SensorDataTest, IsValidWithId)
 
 TEST(SensorDataTest, IsValidWithStamp)
 {
+    // A stamp alone doesn't make the data valid
     SensorData data;
     EXPECT_FALSE(data.isValid());
 
     data.setStamp(12345.0);
+    EXPECT_FALSE(data.isValid());
+}
+
+TEST(SensorDataTest, IsValidWithCameraModel)
+{
+    SensorData data;
+    data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel());
+    EXPECT_FALSE(data.cameraModels().empty());
+    EXPECT_FALSE(data.isValid()); // not valid for projection
+
+    data.setRGBDImage(cv::Mat(), cv::Mat(), CameraModel(525.0, 525.0, 320.0, 240.0));
+    EXPECT_TRUE(data.isValid());
+
+    // At least one valid model among multiple cameras
+    std::vector<CameraModel> models;
+    models.push_back(CameraModel());
+    models.push_back(CameraModel(525.0, 525.0, 320.0, 240.0));
+    data.setRGBDImage(cv::Mat(), cv::Mat(), models);
+    EXPECT_TRUE(data.isValid());
+}
+
+TEST(SensorDataTest, IsValidWithStereoCameraModel)
+{
+    SensorData data;
+    data.setStereoImage(cv::Mat(), cv::Mat(), StereoCameraModel());
+    EXPECT_FALSE(data.stereoCameraModels().empty());
+    EXPECT_FALSE(data.isValid()); // not valid for projection
+
+    data.setStereoImage(cv::Mat(), cv::Mat(), StereoCameraModel(525.0, 525.0, 320.0, 240.0, 0.0));
+    EXPECT_FALSE(data.isValid()); // null baseline
+
+    data.setStereoImage(cv::Mat(), cv::Mat(), StereoCameraModel(525.0, 525.0, 320.0, 240.0, 0.12));
     EXPECT_TRUE(data.isValid());
 }