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
+195 -138
View File
@@ -1,138 +1,195 @@
/* /*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. All rights reserved.
Redistribution and use in source and binary forms, with or without Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met: modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright * Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer. notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright * Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution. documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the * Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission. derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#ifndef ODOMETRY_H_ #ifndef ODOMETRY_H_
#define ODOMETRY_H_ #define ODOMETRY_H_
#include <rtabmap/core/rtabmap_core_export.h> #include <rtabmap/core/rtabmap_core_export.h>
#include <rtabmap/core/Transform.h> #include <rtabmap/core/Transform.h>
#include <rtabmap/core/SensorData.h> #include <rtabmap/core/SensorData.h>
#include <rtabmap/core/Parameters.h> #include <rtabmap/core/Parameters.h>
namespace rtabmap { namespace rtabmap {
class OdometryInfo; class OdometryInfo;
class ParticleFilter; class ParticleFilter;
class RTABMAP_CORE_EXPORT Odometry /**
{ * @class Odometry
public: * @brief Abstract base class for visual, lidar and visual-inertial odometry backends.
enum Type { *
kTypeUndef = -1, * Odometry estimates the incremental motion between consecutive @ref SensorData frames.
kTypeF2M = 0, * Concrete implementations override @ref computeTransform(); the public @ref process()
kTypeF2F = 1, * pipeline handles IMU caching, optional motion guesses, filtering (Kalman or particle),
kTypeFovis = 2, * image decimation, deskewing and pose integration.
kTypeViso2 = 3, *
kTypeDVO = 4, * Use @ref create() to instantiate a backend from @ref Parameters::kOdomStrategy().
kTypeORBSLAM = 5, *
kTypeOkvis = 6, * @see OdometryThread
kTypeLOAM = 7, * @see OdometryInfo
kTypeMSCKF = 8, */
kTypeVINSFusion = 9, class RTABMAP_CORE_EXPORT Odometry
kTypeOpenVINS = 10, {
kTypeFLOAM = 11, public:
kTypeOpen3D = 12, /** @brief Odometry backend selected by @ref Parameters::kOdomStrategy(). */
kTypeCuVSLAM = 13, enum Type {
kTypeLIOSAM = 14 kTypeUndef = -1, /**< Undefined / invalid type. */
}; kTypeF2M = 0, /**< Frame-to-map (default). */
kTypeF2F = 1, /**< Frame-to-frame. */
public: kTypeFovis = 2, /**< FOVIS stereo visual odometry. */
static Odometry * create(const ParametersMap & parameters = ParametersMap()); kTypeViso2 = 3, /**< libviso2. */
static Odometry * create(Type & type, const ParametersMap & parameters = ParametersMap()); kTypeDVO = 4, /**< Dense visual odometry. */
kTypeORBSLAM = 5, /**< ORB-SLAM 2/3. */
public: kTypeOkvis = 6, /**< OKVIS. */
virtual ~Odometry(); kTypeLOAM = 7, /**< LOAM lidar odometry. */
Transform process(SensorData & data, OdometryInfo * info = 0); kTypeMSCKF = 8, /**< MSCKF visual-inertial. */
Transform process(SensorData & data, const Transform & guess, OdometryInfo * info = 0); kTypeVINSFusion = 9,/**< VINS-Fusion. */
virtual void reset(const Transform & initialPose = Transform::getIdentity()); kTypeOpenVINS = 10, /**< OpenVINS. */
virtual Odometry::Type getType() = 0; kTypeFLOAM = 11, /**< FLOAM lidar odometry. */
virtual bool canProcessRawImages() const {return false;} kTypeOpen3D = 12, /**< Open3D RGB-D odometry. */
virtual bool canProcessAsyncIMU() const {return false;} kTypeCuVSLAM = 13, /**< cuVSLAM. */
kTypeLIOSAM = 14 /**< LIO-SAM. */
//getters };
const Transform & getPose() const {return _pose;}
bool isInfoDataFilled() const {return _fillInfoData;} /**
// Use getVelocityGuess() instead. * @brief Creates an odometry instance from @ref Parameters::kOdomStrategy() in @p parameters.
RTABMAP_DEPRECATED const Transform & previousVelocityTransform() const; * @param parameters RTAB-Map parameters (odometry strategy and related options).
const Transform & getVelocityGuess() const {return velocityGuess_;} * @return New odometry object (caller owns the pointer). Falls back to @ref kTypeF2M if the type is unknown.
double previousStamp() const {return previousStamp_;} */
unsigned int framesProcessed() const {return framesProcessed_;} static Odometry * create(const ParametersMap & parameters = ParametersMap());
bool imagesAlreadyRectified() const {return _imagesAlreadyRectified;} /**
* @brief Creates an odometry instance of a given @p type.
protected: * @param type In/out odometry type; updated to @ref kTypeF2M if @p type is unknown.
const std::map<double, Transform> & imus() const {return imus_;} * @param parameters RTAB-Map parameters passed to the concrete backend.
*/
private: static Odometry * create(Type & type, const ParametersMap & parameters = ParametersMap());
virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
virtual ~Odometry();
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); /**
void predictKalmanFilter(float dt, float * vx=0, float * vy=0, float * vz=0, float * vroll=0, float * vpitch=0, float * vyaw=0); * @brief Processes a sensor frame and updates the integrated pose.
void updateKalmanFilter(float & vx, float & vy, float & vz, float & vroll, float & vpitch, float & vyaw); * @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.
private: * @return Updated integrated pose (@ref getPose()) after the frame is processed,
int _resetCountdown; * or a null transform if odometry is lost. The incremental transform is
bool _force3DoF; * available in @ref OdometryInfo::transform when @p info is provided.
bool _holonomic; */
bool guessFromMotion_; Transform process(SensorData & data, OdometryInfo * info = 0);
float guessSmoothingDelay_; /**
int _filteringStrategy; * @brief Processes a sensor frame with an external motion guess.
int _particleSize; * @param data Input sensor data.
float _particleNoiseT; * @param guess Optional prior on the incremental transform (used by the backend when supported).
float _particleLambdaT; * @param info Optional output statistics and debug data.
float _particleNoiseR; */
float _particleLambdaR; Transform process(SensorData & data, const Transform & guess, OdometryInfo * info = 0);
bool _fillInfoData; /**
float _kalmanProcessNoise; * @brief Resets internal state and sets the initial pose.
float _kalmanMeasurementNoise; * @param initialPose Starting pose (must not be null). Z/roll/pitch may be cleared if @ref Parameters::kRegForce3DoF() is enabled.
unsigned int _imageDecimation; */
bool _alignWithGround; virtual void reset(const Transform & initialPose = Transform::getIdentity());
bool _publishRAMUsage; /** @return Concrete odometry backend type. */
bool _imagesAlreadyRectified; virtual Odometry::Type getType() = 0;
bool _deskewing; /** @return True if the backend can process unrectified camera images. */
Transform _pose; virtual bool canProcessRawImages() const {return false;}
int _resetCurrentCount; /** @return True if the backend processes IMU asynchronously outside @ref process(). */
double previousStamp_; virtual bool canProcessAsyncIMU() const {return false;}
std::list<std::pair<std::vector<float>, double> > previousVelocities_;
Transform velocityGuess_; /** @return Current integrated odometry pose. */
Transform imuLastTransform_; const Transform & getPose() const {return _pose;}
Transform previousGroundTruthPose_; /** @return True if @ref OdometryInfo debug/statistics fields are filled in @ref process(). */
float distanceTravelled_; bool isInfoDataFilled() const {return _fillInfoData;}
unsigned int framesProcessed_; /** @deprecated Use @ref getVelocityGuess() instead. */
RTABMAP_DEPRECATED const Transform & previousVelocityTransform() const;
std::vector<ParticleFilter *> particleFilters_; /** @return Last estimated velocity used for motion guessing (may be null). */
cv::KalmanFilter kalmanFilter_; const Transform & getVelocityGuess() const {return velocityGuess_;}
std::vector<StereoCameraModel> stereoModels_; /** @return Timestamp of the previously processed frame. */
std::vector<CameraModel> models_; double previousStamp() const {return previousStamp_;}
std::map<double, Transform> imus_; /** @return Number of frames processed since the last @ref reset(). */
unsigned int framesProcessed() const {return framesProcessed_;}
protected: /** @return True if input images are already rectified (see @ref Parameters::kRtabmapImagesAlreadyRectified()). */
Odometry(const rtabmap::ParametersMap & parameters); bool imagesAlreadyRectified() const {return _imagesAlreadyRectified;}
};
protected:
} /* namespace rtabmap */ /** @return IMU orientations cached from recent frames (stamp → transform). */
#endif /* ODOMETRY_H_ */ 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);
void predictKalmanFilter(float dt, float * vx=0, float * vy=0, float * vz=0, float * vroll=0, float * vpitch=0, float * vyaw=0);
void updateKalmanFilter(float & vx, float & vy, float & vz, float & vroll, float & vpitch, float & vyaw);
private:
int _resetCountdown;
bool _force3DoF;
bool _holonomic;
bool guessFromMotion_;
float guessSmoothingDelay_;
int _filteringStrategy;
int _particleSize;
float _particleNoiseT;
float _particleLambdaT;
float _particleNoiseR;
float _particleLambdaR;
bool _fillInfoData;
float _kalmanProcessNoise;
float _kalmanMeasurementNoise;
unsigned int _imageDecimation;
bool _alignWithGround;
bool _publishRAMUsage;
bool _imagesAlreadyRectified;
bool _deskewing;
Transform _pose;
int _resetCurrentCount;
double previousStamp_;
std::list<std::pair<std::vector<float>, double> > previousVelocities_;
Transform velocityGuess_;
Transform imuLastTransform_;
Transform previousGroundTruthPose_;
float distanceTravelled_;
unsigned int framesProcessed_;
std::vector<ParticleFilter *> particleFilters_;
cv::KalmanFilter kalmanFilter_;
std::vector<StereoCameraModel> stereoModels_;
std::vector<CameraModel> models_;
std::map<double, Transform> imus_;
};
} /* namespace rtabmap */
#endif /* ODOMETRY_H_ */
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) target_link_libraries(test_compression gtest_main rtabmap_core)
gtest_discover_tests(test_compression) 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 #Signature.h
add_executable(test_signature test_signature.cpp) add_executable(test_signature test_signature.cpp)
target_link_libraries(test_signature gtest_main rtabmap_core) 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());
}