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 test_link

Browse Source
This commit is contained in:
matlabbe
2026-05-16 16:57:14 -07:00
parent 6a7f90557f
commit 7e06eb7fe4
3 changed files with 279 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Link.h
+77 -15
View File
@@ -35,35 +35,71 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
/**
* @class Link
* @brief Directed constraint between two nodes in RTAB-Map's pose graph.
*
* A link connects signature @ref from() to signature @ref to() with a relative
* @ref Transform and an information matrix (inverse covariance) used by graph
* optimization. Links are stored on @ref Signature objects and persisted in the
* database; they define odometry chains, loop closures, landmarks, and priors.
*
* The transform is expressed from the @p from node frame to the @p to node frame
* (i.e. pose of @p to relative to @p from), unless @ref type() indicates a
* special semantics (e.g. @ref kPosePrior, @ref kLandmark).
*
* @see Signature
* @see Memory::addLink()
*/
class RTABMAP_CORE_EXPORT Link
{
public:
/**
* @brief Link category and filter sentinels.
*
* Values @ref kSelfRefLink, @ref kAllWithLandmarks, @ref kAllWithoutLandmarks and
* @ref kUndef are also used as query filters when retrieving links from memory or
* the database (they are not stored as link types on signatures).
*/
enum Type {
kNeighbor,
kGlobalClosure,
kLocalSpaceClosure,
kLocalTimeClosure,
kUserClosure,
kVirtualClosure,
kNeighborMerged,
kPosePrior, // Absolute pose in /world frame, From == To
kLandmark, // Transform /base_link -­­> /landmark, "From" is node observing the landmark "To" (landmark is negative id)
kGravity, // Orientation of the base frame accordingly to gravity (From == To)
kNeighbor, /**< Sequential odometry link between consecutive nodes. */
kGlobalClosure, /**< Global loop closure added by global loop closure detection (i.e., using bags-of-words to find loop closures with other nodes in WM without using current estimated pose). */
kLocalSpaceClosure, /**< Local loop closure added by proximity detection by space (i.e. using current estimated pose to find loop closures with nearby nodes in WM). */
kLocalTimeClosure, /**< Local loop closure added by proximity detection by time (i.e. between nodes in STM). */
kUserClosure, /**< User-defined loop closure constraint. */
kVirtualClosure, /**< Virtual link added to keep the path linked to local map. */
kNeighborMerged, /**< Merged neighbor link after graph reduction. */
kPosePrior, /**< Absolute pose prior in the world frame (@p from == @p to). */
kLandmark, /**< Observation of a landmark: @p from is the observer node, @p to is a negative landmark id. */
kGravity, /**< Gravity direction constraint on the base frame (@p from == @p to). */
kEnd,
kSelfRefLink = 97, // Include kPosePrior and kGravity (all links where From=To)
kAllWithLandmarks = 98,
kAllWithoutLandmarks = 99,
kUndef = 99};
kSelfRefLink = 97, /**< Filter: links where @p from == @p to (e.g. @ref kPosePrior, @ref kGravity). */
kAllWithLandmarks = 98, /**< Filter: all link types including @ref kLandmark. */
kAllWithoutLandmarks = 99, /**< Filter: all link types except @ref kLandmark. */
kUndef = 99}; /**< Undefined type or invalid link. */
/** @return Human-readable name for @p type (e.g. "Neighbor", "GlobalClosure"). */
static std::string typeName(Type type);
/** @brief Default constructor; creates an invalid link (@ref kUndef). */
Link();
/**
* @brief Constructs a link between two nodes.
* @param from Source signature id.
* @param to Target signature id (negative for landmarks when @p type is @ref kLandmark).
* @param type Link category.
* @param transform Relative transform from @p from to @p to.
* @param infMatrix 6x6 information matrix (inverse covariance)
* @param userData Optional payload; compressed automatically if not already @c CV_8UC1.
*/
Link(int from,
int to,
Type type,
const Transform & transform,
const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1), // information matrix: inverse of covariance matrix
const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1),
const cv::Mat & userData = cv::Mat());
/** @return True if ids, transform, and type are valid for use in the graph. */
bool isValid() const {return from_ != 0 && to_ != 0 && !transform_.isNull() && type_!=kUndef;}
int from() const {return from_;}
@@ -72,21 +108,47 @@ public:
Type type() const {return type_;}
std::string typeName() const {return typeName(type_);}
const cv::Mat & infMatrix() const {return infMatrix_;}
/**
* @brief Rotation variance derived from the information matrix diagonal (roll, pitch, yaw).
* @param minimum If true, returns the largest diagonal entry (most uncertain axis);
* if false, returns the smallest non-zero entry.
*/
double rotVariance(bool minimum = true) const;
/**
* @brief Translation variance derived from the information matrix diagonal (x, y, z).
* @param minimum If true, returns the largest diagonal entry; if false, the smallest non-zero entry.
*/
double transVariance(bool minimum = true) const;
void setFrom(int from) {from_ = from;}
void setTo(int to) {to_ = to;}
void setTransform(const Transform & transform) {transform_ = transform;}
void setType(Type type) {type_ = type;}
/** @brief Sets the 6x6 information matrix (@c CV_64FC1); diagonal entries must be positive and finite. */
void setInfMatrix(const cv::Mat & infMatrix);
const cv::Mat & userDataRaw() const {return _userDataRaw;}
const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
/** @brief Decompresses user data into @ref userDataRaw() if compressed data is stored. */
void uncompressUserData();
/** @return Uncompressed user data without modifying internal storage. */
cv::Mat uncompressUserDataConst() const;
/**
* @brief Chains this link (from → to) with @p link (to → link.to).
* @param link Second link; must satisfy @c this->to() == link.from().
* @param outputType Type of the merged link.
* @return Single link from @ref from() to @p link.to() with transform
* \(T_{ac} = T_{ab} T_{bc}\) (or null if either input transform is null).
* Information matrix handling depends on @p outputType:
* - @ref kNeighborMerged: \(\Omega_{ac} = (\Omega_{ab}^{-1} + \Omega_{bc}^{-1})^{-1}\)
* (covariances add when both legs are diagonal and independent).
* - Other types: keeps the full information matrix of @p link unless
* \(\Omega_{ab}(0,0) < \Omega_{bc}(0,0)\) (i.e. the smaller x information entry).
*/
Link merge(const Link & link, Type outputType) const;
/** @return Link with swapped endpoints and inverted transform. */
Link inverse() const;
private:
corelib/test/CMakeLists.txt
+5
View File
@@ -96,6 +96,11 @@ add_executable(test_bayesfilter test_bayesfilter.cpp)
target_link_libraries(test_bayesfilter gtest_main rtabmap_core)
gtest_discover_tests(test_bayesfilter)
#Link.h
add_executable(test_link test_link.cpp)
target_link_libraries(test_link gtest_main rtabmap_core)
gtest_discover_tests(test_link)
#Signature.h
add_executable(test_signature test_signature.cpp)
target_link_libraries(test_signature gtest_main rtabmap_core)
corelib/test/test_link.cpp
+197
View File
@@ -0,0 +1,197 @@
#include <gtest/gtest.h>
#include <rtabmap/core/Link.h>
#include <rtabmap/core/Transform.h>
#include <cmath>
using namespace rtabmap;
static cv::Mat infMatrixDiagonal(
double x,
double y,
double z,
double roll,
double pitch,
double yaw)
{
cv::Mat inf = cv::Mat::zeros(6, 6, CV_64FC1);
inf.at<double>(0, 0) = x;
inf.at<double>(1, 1) = y;
inf.at<double>(2, 2) = z;
inf.at<double>(3, 3) = roll;
inf.at<double>(4, 4) = pitch;
inf.at<double>(5, 5) = yaw;
return inf;
}
TEST(LinkTest, DefaultConstructorIsInvalid)
{
Link link;
EXPECT_FALSE(link.isValid());
EXPECT_EQ(link.from(), 0);
EXPECT_EQ(link.to(), 0);
EXPECT_EQ(link.type(), Link::kUndef);
EXPECT_TRUE(link.transform().isNull());
}
TEST(LinkTest, ValidNeighborLink)
{
Link link(1, 2, Link::kNeighbor, Transform::getIdentity());
EXPECT_TRUE(link.isValid());
EXPECT_EQ(link.from(), 1);
EXPECT_EQ(link.to(), 2);
EXPECT_EQ(link.type(), Link::kNeighbor);
EXPECT_EQ(link.typeName(), "Neighbor");
EXPECT_TRUE(link.transform().isIdentity());
EXPECT_EQ(link.infMatrix().rows, 6);
EXPECT_EQ(link.infMatrix().cols, 6);
}
TEST(LinkTest, InvalidWhenTransformNull)
{
Link link(1, 2, Link::kNeighbor, Transform());
EXPECT_FALSE(link.isValid());
}
TEST(LinkTest, TypeNameStatic)
{
for(int type = Link::kNeighbor; type < Link::kEnd; ++type)
{
const std::string name = Link::typeName(static_cast<Link::Type>(type));
EXPECT_NE(name, "Undefined") << "type=" << type;
EXPECT_FALSE(name.empty()) << "type=" << type;
}
EXPECT_EQ(Link::typeName(Link::kNeighbor), "Neighbor");
EXPECT_EQ(Link::typeName(Link::kGlobalClosure), "GlobalClosure");
EXPECT_EQ(Link::typeName(Link::kLocalSpaceClosure), "LocalSpaceClosure");
EXPECT_EQ(Link::typeName(Link::kLocalTimeClosure), "LocalTimeClosure");
EXPECT_EQ(Link::typeName(Link::kUserClosure), "UserClosure");
EXPECT_EQ(Link::typeName(Link::kVirtualClosure), "VirtualClosure");
EXPECT_EQ(Link::typeName(Link::kNeighborMerged), "NeighborMerged");
EXPECT_EQ(Link::typeName(Link::kPosePrior), "PosePrior");
EXPECT_EQ(Link::typeName(Link::kLandmark), "Landmark");
EXPECT_EQ(Link::typeName(Link::kGravity), "Gravity");
EXPECT_EQ(Link::typeName(Link::kEnd), "Undefined");
EXPECT_EQ(Link::typeName(Link::kUndef), "Undefined");
}
TEST(LinkTest, VariancesFromInformationMatrix)
{
const cv::Mat inf = infMatrixDiagonal(4.0, 4.0, 4.0, 9.0, 9.0, 9.0);
Link link(1, 2, Link::kNeighbor, Transform::getIdentity(), inf);
EXPECT_NEAR(link.transVariance(true), 0.25, 1e-6); // max diag -> min variance
EXPECT_NEAR(link.transVariance(false), 0.25, 1e-6);
EXPECT_NEAR(link.rotVariance(true), 1.0 / 9.0, 1e-6);
EXPECT_NEAR(link.rotVariance(false), 1.0 / 9.0, 1e-6);
}
TEST(LinkTest, VariancesMinimumUsesLargestDiagonal)
{
const cv::Mat inf = infMatrixDiagonal(1.0, 4.0, 16.0, 1.0, 4.0, 16.0);
Link link(1, 2, Link::kNeighbor, Transform::getIdentity(), inf);
EXPECT_NEAR(link.transVariance(true), 1.0 / 16.0, 1e-6); // 1 / max(1,4,16)
EXPECT_NEAR(link.transVariance(false), 1.0, 1e-6); // 1 / min(1,4,16)
}
TEST(LinkTest, InverseSwapsEndpointsAndTransform)
{
const Transform t(1.0f, 2.0f, 3.0f, 0.0f, 0.0f, 0.0f);
Link link(5, 10, Link::kGlobalClosure, t);
const Link inv = link.inverse();
EXPECT_EQ(inv.from(), 10);
EXPECT_EQ(inv.to(), 5);
EXPECT_EQ(inv.type(), Link::kGlobalClosure);
const Transform expectedInv = t.inverse();
EXPECT_NEAR(inv.transform().x(), expectedInv.x(), 1e-4f);
EXPECT_NEAR(inv.transform().y(), expectedInv.y(), 1e-4f);
EXPECT_NEAR(inv.transform().z(), expectedInv.z(), 1e-4f);
const Transform roundTrip = t * inv.transform();
EXPECT_NEAR(roundTrip.x(), 0.0f, 1e-4f);
EXPECT_NEAR(roundTrip.y(), 0.0f, 1e-4f);
EXPECT_NEAR(roundTrip.z(), 0.0f, 1e-4f);
}
TEST(LinkTest, MergeNeighborMergedComposesTransformAndInformation)
{
const Link ab(1, 2, Link::kNeighbor, Transform(1.0f, 0.0f, 0.0f, 0, 0, 0));
const Link bc(2, 3, Link::kNeighbor, Transform(0.0f, 1.0f, 0.0f, 0, 0, 0));
const Link ac = ab.merge(bc, Link::kNeighborMerged);
EXPECT_EQ(ac.from(), 1);
EXPECT_EQ(ac.to(), 3);
EXPECT_EQ(ac.type(), Link::kNeighborMerged);
EXPECT_NEAR(ac.transform().x(), 1.0f, 1e-4f);
EXPECT_NEAR(ac.transform().y(), 1.0f, 1e-4f);
EXPECT_NEAR(ac.transform().z(), 0.0f, 1e-4f);
EXPECT_NEAR(ac.transVariance(true), 2.0, 1e-4f);
}
TEST(LinkTest, MergeGlobalClosureKeepsLinkWithLowerInformationOnX)
{
// Non-NeighborMerged merge keeps @p link's matrix unless inf_ab(0,0) < inf_bc(0,0).
const cv::Mat infHigh = infMatrixDiagonal(10.0, 10.0, 10.0, 10.0, 10.0, 10.0);
const cv::Mat infLow = infMatrixDiagonal(2.0, 2.0, 2.0, 2.0, 2.0, 2.0);
const Link ab(1, 2, Link::kNeighbor, Transform::getIdentity(), infHigh);
const Link bc(2, 3, Link::kNeighbor, Transform::getIdentity(), infLow);
const Link ac = ab.merge(bc, Link::kGlobalClosure);
EXPECT_EQ(ac.type(), Link::kGlobalClosure);
EXPECT_NEAR(ac.infMatrix().at<double>(0, 0), 2.0, 1e-6);
EXPECT_NEAR(ac.transVariance(true), 0.5, 1e-6);
}
TEST(LinkTest, UserDataCompressesAndRoundTrips)
{
const cv::Mat userData = (cv::Mat_<float>(2, 2) << 1.f, 2.f, 3.f, 4.f);
Link link(1, 2, Link::kNeighbor, Transform::getIdentity(), cv::Mat::eye(6, 6, CV_64FC1), userData);
EXPECT_FALSE(link.userDataCompressed().empty());
EXPECT_EQ(link.userDataCompressed().type(), CV_8UC1);
const cv::Mat restored = link.uncompressUserDataConst();
ASSERT_EQ(restored.rows, userData.rows);
ASSERT_EQ(restored.cols, userData.cols);
ASSERT_EQ(restored.type(), userData.type());
for(int r = 0; r < restored.rows; ++r)
{
for(int c = 0; c < restored.cols; ++c)
{
EXPECT_NEAR(restored.at<float>(r, c), userData.at<float>(r, c), 1e-5f);
}
}
link.uncompressUserData();
EXPECT_FALSE(link.userDataRaw().empty());
}
TEST(LinkTest, UserDataAlreadyCompressedIsKeptAsIs)
{
const cv::Mat compressed(10, 1, CV_8UC1, cv::Scalar(42));
Link link(1, 2, Link::kNeighbor, Transform::getIdentity(), cv::Mat::eye(6, 6, CV_64FC1), compressed);
EXPECT_EQ(link.userDataCompressed().data, compressed.data);
EXPECT_TRUE(link.userDataRaw().empty());
}
TEST(LinkTest, SetInfMatrixUpdatesVariances)
{
Link link(1, 2, Link::kNeighbor, Transform::getIdentity());
link.setInfMatrix(infMatrixDiagonal(2.0, 2.0, 2.0, 2.0, 2.0, 2.0));
EXPECT_NEAR(link.transVariance(true), 0.5, 1e-6);
}
TEST(LinkTest, LandmarkLink)
{
Link link(5, -10, Link::kLandmark, Transform(1.0f, 0.0f, 0.0f, 0, 0, 0));
EXPECT_TRUE(link.isValid());
EXPECT_EQ(link.to(), -10);
EXPECT_EQ(link.typeName(), "Landmark");
}