Added IMUThread and IMUFilter tests

corelib/include/rtabmap/core/IMUFilter.h

@@ -34,47 +34,109 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class IMUFilter
+ * @brief Fuses gyroscope and accelerometer samples into an orientation quaternion.
+ *
+ * Implementations integrate angular rates and correct drift using the measured
+ * gravity vector. The public @ref update() API takes timestamps and computes the
+ * time step between consecutive calls.
+ *
+ * Factory methods @ref create() return heap-allocated instances; the caller owns
+ * the pointer (e.g. @ref SensorCaptureThread, @ref IMUThread, @ref Camera).
+ *
+ * Output orientation from @ref getOrientation() is a unit quaternion
+ * `(qx, qy, qz, qw)` in the same convention as @ref IMU (body frame).
+ *
+ * Filter tuning parameters are read from @ref ParametersMap (ImuFilter/... keys);
+ * see @ref ComplementaryFilter and @ref MadgwickFilter (when RTAB-Map is built
+ * with Madgwick support).
+ *
+ * @see IMU
+ * @see SensorCaptureThread::enableIMUFiltering()
+ */
 class RTABMAP_CORE_EXPORT IMUFilter
 {
 public:
+	/**
+	 * @brief Orientation fusion algorithm.
+	 */
 	enum Type {
-		kMadgwick=0,
-		kComplementaryFilter=1};
-public:
+		kMadgwick = 0,              /**< Madgwick AHRS (attitude and heading reference system). RTAB-Map must be built with Madgwick support. */
+		kComplementaryFilter = 1};  /**< Complementary filter (always available). */
+
+	/**
+	 * @brief Creates a filter using type parsed from @p parameters.
+	 * @param parameters Optional ImuFilter/... tuning parameters.
+	 * @return New filter instance (caller owns the pointer).
+	 */
 	static IMUFilter * create(const ParametersMap & parameters = ParametersMap());
+
+	/**
+	 * @brief Creates a filter of the given @p type.
+	 * @param type Fusion algorithm; falls back to @ref kComplementaryFilter if
+	 *             @ref kMadgwick is requested but not compiled in.
+	 * @param parameters Optional ImuFilter/... tuning parameters.
+	 * @return New filter instance (caller owns the pointer).
+	 */
 	static IMUFilter * create(IMUFilter::Type type, const ParametersMap & parameters = ParametersMap());
 
-public:
-	virtual void parseParameters(const ParametersMap & parameters) {}
-	virtual ~IMUFilter(){}
+	virtual ~IMUFilter() {}
 
+	/**
+	 * @brief Re-reads filter parameters from @p parameters.
+	 * @param parameters ImuFilter/... keys (implementation-specific).
+	 */
+	virtual void parseParameters(const ParametersMap & parameters) {}
+
+	/**
+	 * @brief Integrates one IMU sample and updates the internal orientation estimate.
+	 * @param gx Gyroscope x angular rate (rad/s).
+	 * @param gy Gyroscope y angular rate (rad/s).
+	 * @param gz Gyroscope z angular rate (rad/s).
+	 * @param ax Accelerometer x (m/s²; magnitude ~9.81 when stationary).
+	 * @param ay Accelerometer y (m/s²).
+	 * @param az Accelerometer z (m/s²).
+	 * @param stamp Sample time (seconds); used with the previous stamp to compute `dt`.
+	 */
 	void update(
 			double gx, double gy, double gz,
 			double ax, double ay, double az,
 			double stamp);
 
+	/** @return Active fusion algorithm type. */
 	virtual IMUFilter::Type type() const = 0;
+
+	/**
+	 * @brief Current orientation estimate.
+	 * @param qx Quaternion x
+	 * @param qy Quaternion y
+	 * @param qz Quaternion z
+	 * @param qw Quaternion w (scalar)
+	 */
 	virtual void getOrientation(double & qx, double & qy, double & qz, double & qw) const = 0;
+
+	/**
+	 * @brief Resets internal state to the given orientation.
+	 * @param qx Quaternion x (default 0)
+	 * @param qy Quaternion y (default 0)
+	 * @param qz Quaternion z (default 0)
+	 * @param qw Quaternion w (default 1, identity)
+	 */
 	virtual void reset(double qx = 0.0, double qy = 0.0, double qz = 0.0, double qw = 1.0) = 0;
 
 protected:
 	IMUFilter(const ParametersMap & parameters = ParametersMap()) : previousStamp_(0) {}
 
 private:
-	// Update from accelerometer and gyroscope data.
-	// [gx, gy, gz]: Angular veloctiy, in rad / s.
-	// [ax, ay, az]: Normalized gravity vector.
-	// dt: time delta, in seconds.
 	virtual void updateImpl(
 			double gx, double gy, double gz,
 			double ax, double ay, double az,
 			double dt) = 0;
 
-private:
 	double previousStamp_;
 };
 
-}
-
+} // namespace rtabmap
 
 #endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_ */

corelib/include/rtabmap/core/IMUThread.h

@@ -37,26 +37,63 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <fstream>
 
-namespace rtabmap
-{
+namespace rtabmap {
 
 class IMUFilter;
 
 /**
- * Class IMUThread
- *
+ * @class IMUThread
+ * @brief Background thread that replays IMU measurements from a CSV file.
+ *
+ * Reads gyroscope and accelerometer samples from a comma-separated file (EuRoC-style
+ * or epoch timestamps), optionally fuses them with @ref IMUFilter, and posts
+ * @ref IMUEvent on each step.
+ *
+ * CSV format:
+ * - First line: header (skipped).
+ * - Following lines: `stamp,gx,gy,gz,ax,ay,az` (stamp in seconds with a decimal
+ *   point, or EuRoC nanoseconds without one).
+ *
+ * Playback rate is limited by @ref setRate() when `rate > 0`; otherwise samples are
+ * read as fast as possible (or spaced using inter-sample timestamps after the first
+ * pair). When the file ends, the thread posts an empty @ref IMUEvent and stops.
+ *
+ * @see IMUEvent
+ * @see IMUFilter
+ * @see SensorCaptureThread::enableIMUFiltering()
  */
 class RTABMAP_CORE_EXPORT IMUThread :
 	public UThread,
 	public UEventsSender
 {
 public:
+	/**
+	 * @brief Constructs the replay thread.
+	 * @param rate Target playback rate in Hz (`0` = no rate cap until timestamps apply).
+	 * @param localTransform IMU frame relative to the robot base (stored in each @ref IMU).
+	 */
 	IMUThread(int rate, const Transform & localTransform);
 	virtual ~IMUThread();
 
+	/**
+	 * @brief Opens and validates an IMU CSV file.
+	 * @param path Path to the CSV file.
+	 * @return False if the file is missing or contains no data rows.
+	 */
 	bool init(const std::string & path);
+
+	/** @brief Sets the target playback rate in Hz. */
 	void setRate(int rate);
-	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
+
+	/**
+	 * @brief Enables orientation fusion on replayed samples.
+	 * @param filteringStrategy @ref IMUFilter::Type index (`0` = Madgwick, `1` = complementary).
+	 * @param parameters Optional ImuFilter/... tuning parameters.
+	 * @param baseFrameConversion If true, rotate IMU vectors into the base frame before filtering.
+	 */
+	void enableIMUFiltering(int filteringStrategy = 1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
+
+	/** @brief Disables fusion and deletes the internal @ref IMUFilter. */
 	void disableIMUFiltering();
 
 private: