Added Landmarks tests

corelib/include/rtabmap/core/Landmark.h

@@ -36,65 +36,99 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-class Landmark
+/**
+ * @class Landmark
+ * @brief Optimized pose of a visual landmark (e.g. ArUco/AprilTag) in the map.
+ *
+ * Stores a positive landmark @ref id(), optional physical @ref size() (m), world
+ * @ref pose(), and a 6×6 pose covariance matrix (x, y, z, roll, pitch, yaw).
+ *
+ * Used in @ref Landmarks maps on @ref SensorData and in memory during SLAM.
+ * Graph constraints to landmarks use @ref Link::kLandmark with a **negative**
+ * landmark id on the link (`to` is typically `-id()`).
+ *
+ * Covariance diagonal entries must be finite and strictly positive. Use a large
+ * value (e.g. `9999`) on angular axes when orientation uncertainty is unknown.
+ *
+ * @see SensorData::setLandmarks()
+ * @see Signature::addLandmark()
+ */
+class RTABMAP_CORE_EXPORT Landmark
 {
 public:
+	/** @brief Default-constructs an invalid landmark (`id == 0`). */
 	Landmark() :
 		id_(0),
-        size_(0.0f)
+		size_(0.0f)
 	{}
-    Landmark(const int & id, const float & size, const Transform & pose, const cv::Mat & covariance) :
+
-        id_(id),
+	/**
-        size_(size),
+	 * @brief Constructs a landmark with size, pose, and covariance.
-        pose_(pose),
+	 * @param id Positive landmark identifier.
-        covariance_(covariance)
+	 * @param size Physical size of the marker (m); `0` if unknown.
-    {
+	 * @param pose Landmark pose in the base frame (typically the robot frame).
-        UASSERT(id_>0);
+	 * @param covariance 6×6 `CV_64FC1` covariance (diagonal entries must be > 0).
-        UASSERT(!pose_.isNull());
+	 */
-        UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
+	Landmark(const int & id, const float & size, const Transform & pose, const cv::Mat & covariance) :
-        UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(0,0)).c_str());
+		id_(id),
-        UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(1,1)).c_str());
+		size_(size),
-        UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(2,2)).c_str());
+		pose_(pose),
-        UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(3,3)).c_str());
+		covariance_(covariance)
-        UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
+	{
-        UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
+		UASSERT(id_>0);
-    }
+		UASSERT(!pose_.isNull());
-    // Use constructor with size=0 instead.
+		UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
-    RTABMAP_DEPRECATED Landmark(const int & id, const Transform & pose, const cv::Mat & covariance);
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(0,0)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(1,1)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(2,2)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(3,3)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
+		UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
+	}
+
+	/**
+	 * @brief Deprecated; use the constructor with explicit @p size (`0` if unknown).
+	 */
+	RTABMAP_DEPRECATED Landmark(const int & id, const Transform & pose, const cv::Mat & covariance);
 
 	virtual ~Landmark() {}
 
+	/** @return Positive landmark id (map key in @ref Landmarks). */
 	const int & id() const {return id_;}
-    const float & size() const {return size_;}
+	/** @return Marker size in metres (`0` if unknown). */
+	const float & size() const {return size_;}
+	/** @return Landmark pose in the map frame. */
 	const Transform & pose() const {return pose_;}
+	/** @return 6×6 pose covariance (`CV_64FC1`). */
 	const cv::Mat & covariance() const {return covariance_;}
 
 private:
 	int id_;
-    float size_;
+	float size_;
 	Transform pose_;
 	cv::Mat covariance_;
 };
 
+/** @brief Map of landmark id → @ref Landmark (typically positive keys). */
 typedef std::map<int, Landmark> Landmarks;
 
 inline Landmark::Landmark(const int & id, const Transform & pose, const cv::Mat & covariance) :
-    id_(id),
+	id_(id),
-    size_(0.0f),
+	size_(0.0f),
-    pose_(pose),
+	pose_(pose),
-    covariance_(covariance)
+	covariance_(covariance)
 {
-    UASSERT(id_>0);
+	UASSERT(id_>0);
-    UASSERT(!pose_.isNull());
+	UASSERT(!pose_.isNull());
-    UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
+	UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
-    UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(0,0)).c_str());
+	UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(0,0)).c_str());
-    UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(1,1)).c_str());
+	UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(1,1)).c_str());
-    UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(2,2)).c_str());
+	UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(2,2)).c_str());
-    UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(3,3)).c_str());
+	UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(3,3)).c_str());
-    UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
+	UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
-    UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
+	UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
 }
 
-}
+} // namespace rtabmap
 
 #endif /* CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_ */

corelib/test/CMakeLists.txt

@@ -121,6 +121,11 @@ add_executable(test_imuthread test_imuthread.cpp)
 target_link_libraries(test_imuthread gtest_main rtabmap_core)
 add_test(NAME test_imuthread COMMAND test_imuthread)
 
+#Landmark.h
+add_executable(test_landmark test_landmark.cpp)
+target_link_libraries(test_landmark gtest_main rtabmap_core)
+add_test(NAME test_landmark COMMAND test_landmark)
+
 #Graph.h
 add_executable(test_graph test_graph.cpp)
 target_link_libraries(test_graph gtest_main rtabmap_core)

corelib/test/test_landmark.cpp

@@ -0,0 +1,95 @@
+#include <gtest/gtest.h>
+#include <rtabmap/core/Landmark.h>
+#include <opencv2/core.hpp>
+
+using namespace rtabmap;
+
+namespace {
+
+static cv::Mat covarianceDiagonal(
+		double x,
+		double y,
+		double z,
+		double roll,
+		double pitch,
+		double yaw)
+{
+	cv::Mat cov = cv::Mat::zeros(6, 6, CV_64FC1);
+	cov.at<double>(0, 0) = x;
+	cov.at<double>(1, 1) = y;
+	cov.at<double>(2, 2) = z;
+	cov.at<double>(3, 3) = roll;
+	cov.at<double>(4, 4) = pitch;
+	cov.at<double>(5, 5) = yaw;
+	return cov;
+}
+
+} // namespace
+
+TEST(LandmarkTest, DefaultConstructor)
+{
+	const Landmark landmark;
+	EXPECT_EQ(landmark.id(), 0);
+	EXPECT_FLOAT_EQ(landmark.size(), 0.0f);
+	EXPECT_TRUE(landmark.pose().isNull());
+	EXPECT_TRUE(landmark.covariance().empty());
+}
+
+TEST(LandmarkTest, ConstructorStoresFields)
+{
+	const cv::Mat covariance = covarianceDiagonal(0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
+	const Transform pose(1.0f, 2.0f, 3.0f, 0.f, 0.f, 0.5f);
+	const Landmark landmark(10, 0.25f, pose, covariance);
+
+	EXPECT_EQ(landmark.id(), 10);
+	EXPECT_FLOAT_EQ(landmark.size(), 0.25f);
+	EXPECT_FLOAT_EQ(landmark.pose().x(), 1.0f);
+	EXPECT_FLOAT_EQ(landmark.pose().y(), 2.0f);
+	EXPECT_FLOAT_EQ(landmark.pose().z(), 3.0f);
+	EXPECT_FLOAT_EQ(landmark.pose().theta(), 0.5f);
+	EXPECT_EQ(landmark.covariance().rows, 6);
+	EXPECT_EQ(landmark.covariance().cols, 6);
+	EXPECT_EQ(landmark.covariance().type(), CV_64FC1);
+	EXPECT_DOUBLE_EQ(landmark.covariance().at<double>(0, 0), 0.1);
+	EXPECT_DOUBLE_EQ(landmark.covariance().at<double>(5, 5), 0.6);
+}
+
+TEST(LandmarkTest, DeprecatedConstructorUsesZeroSize)
+{
+	const cv::Mat covariance = covarianceDiagonal(1, 1, 1, 9999, 9999, 9999);
+	const Transform pose(0.5f, 0.0f, 0.0f, 0.f, 0.f, 0.f);
+	const Landmark landmark(5, pose, covariance);
+
+	EXPECT_EQ(landmark.id(), 5);
+	EXPECT_FLOAT_EQ(landmark.size(), 0.0f);
+	EXPECT_FLOAT_EQ(landmark.pose().x(), 0.5f);
+}
+
+TEST(LandmarkTest, AngularCovarianceCanUseLargeValueForUnknown)
+{
+	const cv::Mat covariance = covarianceDiagonal(0.01, 0.01, 0.01, 9999, 9999, 9999);
+	const Landmark landmark(
+			1,
+			0.0f,
+			Transform(0, 0, 0, 0, 0, 0),
+			covariance);
+
+	EXPECT_DOUBLE_EQ(landmark.covariance().at<double>(3, 3), 9999.0);
+	EXPECT_DOUBLE_EQ(landmark.covariance().at<double>(5, 5), 9999.0);
+}
+
+TEST(LandmarkTest, LandmarksMapKeyedById)
+{
+	Landmarks landmarks;
+	landmarks.insert(std::make_pair(
+			1,
+			Landmark(1, 0.1f, Transform(1, 0, 0, 0, 0, 0), covarianceDiagonal(1, 1, 1, 1, 1, 1))));
+	landmarks.insert(std::make_pair(
+			2,
+			Landmark(2, 0.2f, Transform(2, 0, 0, 0, 0, 0), covarianceDiagonal(2, 2, 2, 2, 2, 2))));
+
+	ASSERT_EQ(landmarks.size(), 2u);
+	EXPECT_EQ(landmarks.at(1).id(), 1);
+	EXPECT_FLOAT_EQ(landmarks.at(2).size(), 0.2f);
+	EXPECT_DOUBLE_EQ(landmarks.at(2).covariance().at<double>(0, 0), 2.0);
+}