i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-03 16:47:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added Landmarks tests

Browse Source
This commit is contained in:
matlabbe
2026-05-17 14:24:40 -07:00
parent 7fdf3a13f2
commit 057d385404
3 changed files with 170 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Landmark.h
+70 -36
View File
@@ -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),
pose_(pose),
covariance_(covariance)
{
UASSERT(id_>0);
UASSERT(!pose_.isNull());
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>(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());
}
// Use constructor with size=0 instead.
RTABMAP_DEPRECATED Landmark(const int & id, const Transform & pose, const cv::Mat & covariance);
/**
* @brief Constructs a landmark with size, pose, and covariance.
* @param id Positive landmark identifier.
* @param size Physical size of the marker (m); `0` if unknown.
* @param pose Landmark pose in the base frame (typically the robot frame).
* @param covariance 6×6 `CV_64FC1` covariance (diagonal entries must be > 0).
*/
Landmark(const int & id, const float & size, const Transform & pose, const cv::Mat & covariance) :
id_(id),
size_(size),
pose_(pose),
covariance_(covariance)
{
UASSERT(id_>0);
UASSERT(!pose_.isNull());
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>(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),
size_(0.0f),
pose_(pose),
covariance_(covariance)
id_(id),
size_(0.0f),
pose_(pose),
covariance_(covariance)
{
UASSERT(id_>0);
UASSERT(!pose_.isNull());
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>(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());
UASSERT(id_>0);
UASSERT(!pose_.isNull());
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>(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());
}
}
} // namespace rtabmap
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_ */
corelib/test/CMakeLists.txt
+5
View File
@@ -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
+95
View File
@@ -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);
}