Added Odometry tests (base class only)

corelib/include/rtabmap/core/Odometry.h

@@ -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,
+		kTypeUndef = -1,    /**< Undefined / invalid type. */
-		kTypeF2M = 0,
+		kTypeF2M = 0,       /**< Frame-to-map (default). */
-		kTypeF2F = 1,
+		kTypeF2F = 1,       /**< Frame-to-frame. */
-		kTypeFovis = 2,
+		kTypeFovis = 2,     /**< FOVIS stereo visual odometry. */
-		kTypeViso2 = 3,
+		kTypeViso2 = 3,     /**< libviso2. */
-		kTypeDVO = 4,
+		kTypeDVO = 4,       /**< Dense visual odometry. */
-		kTypeORBSLAM = 5,
+		kTypeORBSLAM = 5,   /**< ORB-SLAM 2/3. */
-		kTypeOkvis = 6,
+		kTypeOkvis = 6,     /**< OKVIS. */
-		kTypeLOAM = 7,
+		kTypeLOAM = 7,      /**< LOAM lidar odometry. */
-		kTypeMSCKF = 8,
+		kTypeMSCKF = 8,     /**< MSCKF visual-inertial. */
-		kTypeVINSFusion = 9,
+		kTypeVINSFusion = 9,/**< VINS-Fusion. */
-		kTypeOpenVINS = 10,
+		kTypeOpenVINS = 10, /**< OpenVINS. */
-		kTypeFLOAM = 11,
+		kTypeFLOAM = 11,    /**< FLOAM lidar odometry. */
-		kTypeOpen3D = 12,
+		kTypeOpen3D = 12,   /**< Open3D RGB-D odometry. */
-		kTypeCuVSLAM = 13,
+		kTypeCuVSLAM = 13,  /**< cuVSLAM. */
-		kTypeLIOSAM = 14
+		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

@@ -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

@@ -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());
+}