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 Odometry tests (base class only)

Browse Source
This commit is contained in:
matlabbe
2026-05-16 17:55:03 -07:00
parent a0d891bff0
commit 551c9e8a0f
3 changed files with 381 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Odometry.h
+80 -23
View File
@@ -39,55 +39,115 @@ namespace rtabmap {
class OdometryInfo;
class ParticleFilter;
/**
* @class Odometry
* @brief Abstract base class for visual, lidar and visual-inertial odometry backends.
*
* Odometry estimates the incremental motion between consecutive @ref SensorData frames.
* Concrete implementations override @ref computeTransform(); the public @ref process()
* pipeline handles IMU caching, optional motion guesses, filtering (Kalman or particle),
* image decimation, deskewing and pose integration.
*
* Use @ref create() to instantiate a backend from @ref Parameters::kOdomStrategy().
*
* @see OdometryThread
* @see OdometryInfo
*/
class RTABMAP_CORE_EXPORT Odometry
{
public:
/** @brief Odometry backend selected by @ref Parameters::kOdomStrategy(). */
enum Type {
kTypeUndef = -1,
kTypeF2M = 0,
kTypeF2F = 1,
kTypeFovis = 2,
kTypeViso2 = 3,
kTypeDVO = 4,
kTypeORBSLAM = 5,
kTypeOkvis = 6,
kTypeLOAM = 7,
kTypeMSCKF = 8,
kTypeVINSFusion = 9,
kTypeOpenVINS = 10,
kTypeFLOAM = 11,
kTypeOpen3D = 12,
kTypeCuVSLAM = 13,
kTypeLIOSAM = 14
kTypeUndef = -1, /**< Undefined / invalid type. */
kTypeF2M = 0, /**< Frame-to-map (default). */
kTypeF2F = 1, /**< Frame-to-frame. */
kTypeFovis = 2, /**< FOVIS stereo visual odometry. */
kTypeViso2 = 3, /**< libviso2. */
kTypeDVO = 4, /**< Dense visual odometry. */
kTypeORBSLAM = 5, /**< ORB-SLAM 2/3. */
kTypeOkvis = 6, /**< OKVIS. */
kTypeLOAM = 7, /**< LOAM lidar odometry. */
kTypeMSCKF = 8, /**< MSCKF visual-inertial. */
kTypeVINSFusion = 9,/**< VINS-Fusion. */
kTypeOpenVINS = 10, /**< OpenVINS. */
kTypeFLOAM = 11, /**< FLOAM lidar odometry. */
kTypeOpen3D = 12, /**< Open3D RGB-D odometry. */
kTypeCuVSLAM = 13, /**< cuVSLAM. */
kTypeLIOSAM = 14 /**< LIO-SAM. */
};
public:
/**
* @brief Creates an odometry instance from @ref Parameters::kOdomStrategy() in @p parameters.
* @param parameters RTAB-Map parameters (odometry strategy and related options).
* @return New odometry object (caller owns the pointer). Falls back to @ref kTypeF2M if the type is unknown.
*/
static Odometry * create(const ParametersMap & parameters = ParametersMap());
/**
* @brief Creates an odometry instance of a given @p type.
* @param type In/out odometry type; updated to @ref kTypeF2M if @p type is unknown.
* @param parameters RTAB-Map parameters passed to the concrete backend.
*/
static Odometry * create(Type & type, const ParametersMap & parameters = ParametersMap());
public:
virtual ~Odometry();
/**
* @brief Processes a sensor frame and updates the integrated pose.
* @param data Input sensor data (must have @c id() >= 0). May be modified in place (decompression, deskewing).
* @param info Optional output statistics and debug data.
* @return Updated integrated pose (@ref getPose()) after the frame is processed,
* or a null transform if odometry is lost. The incremental transform is
* available in @ref OdometryInfo::transform when @p info is provided.
*/
Transform process(SensorData & data, OdometryInfo * info = 0);
/**
* @brief Processes a sensor frame with an external motion guess.
* @param data Input sensor data.
* @param guess Optional prior on the incremental transform (used by the backend when supported).
* @param info Optional output statistics and debug data.
*/
Transform process(SensorData & data, const Transform & guess, OdometryInfo * info = 0);
/**
* @brief Resets internal state and sets the initial pose.
* @param initialPose Starting pose (must not be null). Z/roll/pitch may be cleared if @ref Parameters::kRegForce3DoF() is enabled.
*/
virtual void reset(const Transform & initialPose = Transform::getIdentity());
/** @return Concrete odometry backend type. */
virtual Odometry::Type getType() = 0;
/** @return True if the backend can process unrectified camera images. */
virtual bool canProcessRawImages() const {return false;}
/** @return True if the backend processes IMU asynchronously outside @ref process(). */
virtual bool canProcessAsyncIMU() const {return false;}
//getters
/** @return Current integrated odometry pose. */
const Transform & getPose() const {return _pose;}
/** @return True if @ref OdometryInfo debug/statistics fields are filled in @ref process(). */
bool isInfoDataFilled() const {return _fillInfoData;}
// Use getVelocityGuess() instead.
/** @deprecated Use @ref getVelocityGuess() instead. */
RTABMAP_DEPRECATED const Transform & previousVelocityTransform() const;
/** @return Last estimated velocity used for motion guessing (may be null). */
const Transform & getVelocityGuess() const {return velocityGuess_;}
/** @return Timestamp of the previously processed frame. */
double previousStamp() const {return previousStamp_;}
/** @return Number of frames processed since the last @ref reset(). */
unsigned int framesProcessed() const {return framesProcessed_;}
/** @return True if input images are already rectified (see @ref Parameters::kRtabmapImagesAlreadyRectified()). */
bool imagesAlreadyRectified() const {return _imagesAlreadyRectified;}
protected:
/** @return IMU orientations cached from recent frames (stamp → transform). */
const std::map<double, Transform> & imus() const {return imus_;}
/** @brief Constructs the base odometry state from RTAB-Map parameters. */
Odometry(const rtabmap::ParametersMap & parameters);
private:
/**
* @brief Computes the incremental transform for one frame (implemented by subclasses).
* @param data Sensor data for this frame (may already be decimated or deskewed).
* @param guess Motion prior from the base class or the caller.
* @param info Optional debug/statistics output.
* @return Incremental transform, or null if tracking failed.
*/
virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
void initKalmanFilter(const Transform & initialPose = Transform::getIdentity(), float vx=0.0f, float vy=0.0f, float vz=0.0f, float vroll=0.0f, float vpitch=0.0f, float vyaw=0.0f);
@@ -129,9 +189,6 @@ private:
std::vector<StereoCameraModel> stereoModels_;
std::vector<CameraModel> models_;
std::map<double, Transform> imus_;
protected:
Odometry(const rtabmap::ParametersMap & parameters);
};
} /* namespace rtabmap */
corelib/test/CMakeLists.txt
+5
View File
@@ -116,6 +116,11 @@ add_executable(test_compression test_compression.cpp)
target_link_libraries(test_compression gtest_main rtabmap_core)
gtest_discover_tests(test_compression)
#Odometry.h
add_executable(test_odometry test_odometry.cpp)
target_link_libraries(test_odometry gtest_main rtabmap_core)
gtest_discover_tests(test_odometry)
#Signature.h
add_executable(test_signature test_signature.cpp)
target_link_libraries(test_signature gtest_main rtabmap_core)
corelib/test/test_odometry.cpp
+181
View File
@@ -0,0 +1,181 @@
#include <gtest/gtest.h>
#include <rtabmap/core/Odometry.h>
#include <rtabmap/core/OdometryInfo.h>
#include <rtabmap/core/CameraModel.h>
#include <rtabmap/core/Parameters.h>
#include <opencv2/core.hpp>
using namespace rtabmap;
namespace {
class MockOdometry : public Odometry
{
public:
explicit MockOdometry(const ParametersMap & parameters = ParametersMap())
: Odometry(parameters)
{
}
Type getType() override
{
return kTypeUndef;
}
void setNextTransform(const Transform & transform)
{
nextTransform_ = transform;
}
const Transform & lastGuess() const
{
return lastGuess_;
}
protected:
Transform computeTransform(SensorData &, const Transform & guess, OdometryInfo *) override
{
lastGuess_ = guess;
return nextTransform_;
}
private:
Transform nextTransform_;
Transform lastGuess_;
};
SensorData makeImageData(int id, double stamp)
{
const cv::Mat image = cv::Mat::zeros(32, 32, CV_8UC1);
const CameraModel model(100.0, 100.0, 16.0, 16.0);
SensorData data(image, model, id);
data.setStamp(stamp);
return data;
}
ParametersMap baseTestParameters()
{
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kOdomGuessMotion(), "false"));
parameters.insert(ParametersPair(Parameters::kOdomFilteringStrategy(), "0"));
parameters.insert(ParametersPair(Parameters::kOdomFillInfoData(), "true"));
parameters.insert(ParametersPair(Parameters::kRtabmapImagesAlreadyRectified(), "true"));
return parameters;
}
void expectTransformNear(const Transform & a, const Transform & b, float tol = 1e-4f)
{
EXPECT_NEAR(a.x(), b.x(), tol);
EXPECT_NEAR(a.y(), b.y(), tol);
EXPECT_NEAR(a.z(), b.z(), tol);
}
} // namespace
TEST(OdometryTest, CreateUsesF2MByDefault)
{
ParametersMap parameters;
parameters.insert(ParametersPair(Parameters::kOdomStrategy(), "0"));
Odometry * odometry = Odometry::create(parameters);
ASSERT_NE(odometry, nullptr);
EXPECT_EQ(odometry->getType(), Odometry::kTypeF2M);
delete odometry;
}
TEST(OdometryTest, CreateUnknownTypeFallsBackToF2M)
{
Odometry::Type type = static_cast<Odometry::Type>(999);
Odometry * odometry = Odometry::create(type);
ASSERT_NE(odometry, nullptr);
EXPECT_EQ(type, Odometry::kTypeF2M);
EXPECT_EQ(odometry->getType(), Odometry::kTypeF2M);
delete odometry;
}
TEST(OdometryTest, ResetSetsInitialPoseAndCounters)
{
MockOdometry odometry(baseTestParameters());
const Transform initialPose(1.0f, 2.0f, 3.0f, 0.0f, 0.0f, 0.0f);
odometry.reset(initialPose);
EXPECT_EQ(odometry.framesProcessed(), 0u);
EXPECT_DOUBLE_EQ(odometry.previousStamp(), 0.0);
expectTransformNear(odometry.getPose(), initialPose);
EXPECT_TRUE(odometry.getVelocityGuess().isNull());
}
TEST(OdometryTest, ProcessAccumulatesPose)
{
MockOdometry odometry(baseTestParameters());
odometry.reset(Transform::getIdentity());
const Transform step(0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
odometry.setNextTransform(step);
SensorData frame0 = makeImageData(0, 0.0);
const Transform pose0 = odometry.process(frame0);
expectTransformNear(pose0, step);
expectTransformNear(odometry.getPose(), pose0);
EXPECT_EQ(odometry.framesProcessed(), 1u);
SensorData frame1 = makeImageData(1, 1.0);
const Transform pose1 = odometry.process(frame1);
const Transform expectedPose1(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
expectTransformNear(pose1, expectedPose1);
expectTransformNear(odometry.getPose(), expectedPose1);
EXPECT_EQ(odometry.framesProcessed(), 2u);
}
TEST(OdometryTest, ProcessWithExternalGuess)
{
MockOdometry odometry(baseTestParameters());
odometry.reset(Transform::getIdentity());
odometry.setNextTransform(Transform(0.1f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f));
SensorData data = makeImageData(0, 0.0);
const Transform guess(0.2f, 0.3f, 0.0f, 0.0f, 0.0f, 0.0f);
odometry.process(data, guess);
expectTransformNear(odometry.lastGuess(), guess);
}
TEST(OdometryTest, ProcessFillsOdometryInfo)
{
MockOdometry odometry(baseTestParameters());
odometry.reset(Transform::getIdentity());
odometry.setNextTransform(Transform(0.2f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f));
SensorData data = makeImageData(0, 1.0);
OdometryInfo info;
const Transform pose = odometry.process(data, &info);
EXPECT_FALSE(pose.isNull());
EXPECT_FALSE(info.lost);
EXPECT_DOUBLE_EQ(info.stamp, 1.0);
expectTransformNear(info.transform, Transform(0.2f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f));
expectTransformNear(pose, Transform(0.2f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f));
EXPECT_TRUE(odometry.isInfoDataFilled());
}
TEST(OdometryTest, ProcessLostReturnsNullTransform)
{
MockOdometry odometry(baseTestParameters());
odometry.reset(Transform::getIdentity());
odometry.setNextTransform(Transform());
SensorData data = makeImageData(0, 0.0);
OdometryInfo info;
const Transform t = odometry.process(data, &info);
EXPECT_TRUE(t.isNull());
EXPECT_TRUE(info.lost);
EXPECT_EQ(odometry.framesProcessed(), 0u);
}
TEST(OdometryTest, DefaultCapabilityFlags)
{
MockOdometry odometry(baseTestParameters());
EXPECT_FALSE(odometry.canProcessRawImages());
EXPECT_FALSE(odometry.canProcessAsyncIMU());
}