Added SensorCapture and SensorCaptureThread doc and tests

corelib/include/rtabmap/core/SensorCapture.h

@@ -43,50 +43,231 @@ namespace rtabmap
 {
 
 /**
- * Class Camera
- *
+ * @class SensorCapture
+ * @brief Abstract base class for sensor data capture (cameras, lidars, etc.)
+ * 
+ * SensorCapture provides a unified interface for capturing sensor data from various
+ * sensor types including cameras (RGB-D, stereo, mono) and lidars. It handles frame
+ * rate control, local transform management, and provides a common API for sensor
+ * initialization and data capture.
+ * 
+ * The class implements the Template Method pattern:
+ * - `takeData()` handles frame rate control and timing, then calls the pure virtual
+ *   `captureData()` method implemented by derived classes
+ * - Derived classes must implement `captureData()` to perform the actual sensor capture
+ * 
+ * Key features:
+ * - **Frame rate control**: Automatic throttling to maintain target frame rate
+ * - **Local transform**: Transform from robot base frame to sensor frame
+ * - **Capture timing**: Tracks capture time and provides it via SensorCaptureInfo
+ * - **Sequence IDs**: Automatic sequence number generation for captured data
+ * 
+ * Derived classes include:
+ * - **Camera**: Base class for all camera types (RGB-D, stereo, mono, file readers, etc.)
+ * - **Lidar**: Base class for lidar sensors
+ * 
+ * @note This is an abstract class. Use concrete implementations like CameraRGBD,
+ *       CameraStereo, LidarVLP16, etc., or create custom derived classes.
+ * 
+ * @see Camera
+ * @see Lidar
+ * @see SensorData
+ * @see SensorCaptureInfo
+ * @see SensorCaptureThread
  */
 class RTABMAP_CORE_EXPORT SensorCapture
 {
 public:
+	/**
+	 * @brief Virtual destructor
+	 */
 	virtual ~SensorCapture();
+	
+	/**
+	 * @brief Captures sensor data with frame rate control
+	 * 
+	 * This method handles frame rate throttling and timing, then calls the
+	 * pure virtual `captureData()` method to perform the actual capture.
+	 * 
+	 * The method:
+	 * - Enforces the target frame rate by sleeping if necessary
+	 * - Measures capture time and stores it in SensorCaptureInfo
+	 * - Assigns sequence IDs to captured data
+	 * - Warns if the target frame rate cannot be reached
+	 * 
+	 * @param info Optional pointer to SensorCaptureInfo to fill with capture metadata
+	 *             (ID, timestamp, capture time). If null, no info is filled.
+	 * @return SensorData containing the captured sensor data
+	 * 
+	 * @note If frame rate is 0, data is captured as fast as possible without throttling.
+	 * @note The returned SensorData should have rectified images if calibration was loaded.
+	 * 
+	 * @see captureData()
+	 */
 	SensorData takeData(SensorCaptureInfo * info = 0);
 
+	/**
+	 * @brief Initializes the sensor
+	 * 
+	 * Pure virtual method that must be implemented by derived classes to initialize
+	 * the sensor hardware or data source. This typically involves:
+	 * - Opening device connections or file streams
+	 * - Loading camera calibration parameters
+	 * - Configuring sensor settings
+	 * 
+	 * @param calibrationFolder Directory path where calibration files are located
+	 *                          (default: current directory ".")
+	 * @param cameraName Base name of the camera for loading calibration files
+	 *                   (default: empty string)
+	 * @return True if initialization was successful, false otherwise
+	 * 
+	 * @note Must be called before calling takeData() or captureData()
+	 */
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
+	
+	/**
+	 * @brief Returns the sensor's serial number or unique identifier
+	 * 
+	 * Pure virtual method that must be implemented by derived classes to return
+	 * a unique identifier for the sensor (e.g., device serial number, file path,
+	 * or other identifier).
+	 * 
+	 * @return String identifier for the sensor
+	 */
 	virtual std::string getSerial() const = 0;
+	
+	/**
+	 * @brief Checks if the sensor provides odometry poses
+	 * 
+	 * Some sensors (e.g., visual-inertial cameras) can provide pose estimates
+	 * directly. This method indicates whether the sensor supports pose queries.
+	 * 
+	 * @return True if the sensor provides odometry poses, false otherwise
+	 * 
+	 * @note Default implementation returns false. Derived classes should override
+	 *       if they support pose estimation.
+	 * 
+	 * @see getPose()
+	 */
 	virtual bool odomProvided() const { return false; }
+	
+	/**
+	 * @brief Gets the sensor's pose estimate at a specific timestamp
+	 * 
+	 * Queries the sensor for its pose estimate at the given timestamp. This is
+	 * typically used for sensors that provide visual-inertial odometry or other
+	 * pose estimation capabilities.
+	 * 
+	 * @param stamp Timestamp in seconds for which to query the pose
+	 * @param[out] pose Output transform representing the sensor pose
+	 * @param[out] covariance Output covariance matrix (6x6) representing twist uncertainty
+	 * @param maxWaitTime Maximum time in seconds to wait for pose data (default: 0.06)
+	 * @return True if pose was successfully retrieved, false otherwise
+	 * 
+	 * @note Default implementation returns false. Derived classes should override
+	 *       if they support pose estimation.
+	 * 
+	 * @see odomProvided()
+	 */
 	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06) { return false; }
 
-	//getters
+	// Getters
+	
+	/**
+	 * @brief Returns the target frame rate
+	 * @return Frame rate in Hz (0 = unlimited, capture as fast as possible)
+	 */
 	float getFrameRate() const {return _frameRate;}
+	
+	/**
+	 * @brief Returns the local transform from base frame to sensor frame
+	 * @return Const reference to the local transform
+	 */
 	const Transform & getLocalTransform() const {return _localTransform;}
 
-	//setters
+	// Setters
+	
+	/**
+	 * @brief Sets the target frame rate
+	 * 
+	 * Controls how often `takeData()` will capture data. The method will throttle
+	 * captures to maintain the target rate.
+	 * 
+	 * @param frameRate Target frame rate in Hz (0 = unlimited, capture as fast as possible)
+	 * 
+	 * @note Setting frame rate to 0 disables throttling and captures as fast as possible.
+	 */
 	void setFrameRate(float frameRate) {_frameRate = frameRate;}
+	
+	/**
+	 * @brief Sets the local transform from base frame to sensor frame
+	 * 
+	 * The local transform represents the pose of the sensor relative to the robot's
+	 * base frame. This is used to transform sensor data into the robot's coordinate system.
+	 * 
+	 * @param localTransform Transform from base frame to sensor frame
+	 */
 	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
 
+	/**
+	 * @brief Resets the frame rate timer
+	 * 
+	 * Resets the internal timer used for frame rate control. This is useful when
+	 * starting a new capture session or after a pause.
+	 */
 	void resetTimer();
+	
 protected:
 	/**
-	 * Constructor
-	 *
-	 * @param frameRate the frame rate (Hz), 0 for fast as the sensor can
-	 * @param localTransform the transform from base frame to sensor frame
+	 * @brief Protected constructor
+	 * 
+	 * Creates a SensorCapture instance with the specified frame rate and local transform.
+	 * This constructor is protected because SensorCapture is an abstract base class
+	 * and should not be instantiated directly.
+	 * 
+	 * @param frameRate Target frame rate in Hz (0 = unlimited, capture as fast as possible)
+	 * @param localTransform Transform from base frame to sensor frame (default: identity)
 	 */
 	SensorCapture(float frameRate = 0, const Transform & localTransform = Transform::getIdentity());
 
 	/**
-	 * returned rgb and depth images should be already rectified if calibration was loaded
+	 * @brief Pure virtual method to capture sensor data
+	 * 
+	 * This method must be implemented by derived classes to perform the actual
+	 * sensor data capture. It is called by `takeData()` after frame rate throttling.
+	 * 
+	 * The returned SensorData should:
+	 * - Have rectified images if calibration was loaded during `init()`
+	 * - Include proper timestamps
+	 * - Contain valid sensor data (images, depth, laser scans, etc.)
+	 * 
+	 * @param info Optional pointer to SensorCaptureInfo to fill with capture metadata.
+	 *             The base class will fill ID, timestamp, and capture time, but derived
+	 *             classes can add additional information.
+	 * @return SensorData containing the captured sensor data
+	 * 
+	 * @note If capture fails, return an empty SensorData (id=0, stamp=0.0).
+	 * @note RGB and depth images should be already rectified if calibration was loaded.
+	 * 
+	 * @see takeData()
 	 */
 	virtual SensorData captureData(SensorCaptureInfo * info = 0) = 0;
 
+	/**
+	 * @brief Gets the next sequence ID
+	 * 
+	 * Returns and increments the internal sequence counter. This is used to assign
+	 * unique sequence numbers to captured data.
+	 * 
+	 * @return Next sequence ID (starts at 1, increments with each call)
+	 */
 	int getNextSeqID() {return ++_seq;}
 
 private:
-	float _frameRate;
-	Transform _localTransform;
-	UTimer * _frameRateTimer;
-	int _seq;
+	float _frameRate; ///< Target frame rate in Hz (0 = unlimited)
+	Transform _localTransform; ///< Transform from base frame to sensor frame
+	UTimer * _frameRateTimer; ///< Timer for frame rate control
+	int _seq; ///< Sequence counter for captured data
 };
 
 

corelib/include/rtabmap/core/SensorCaptureThread.h

@@ -52,23 +52,86 @@ class IMUFilter;
 class Feature2D;
 
 /**
- * Class CameraThread
- *
+ * @class SensorCaptureThread
+ * @brief Thread-based sensor data capture and event posting for RTAB-Map
+ * 
+ * SensorCaptureThread is a multi-threaded class that continuously captures sensor
+ * data from cameras and/or lidars in a background thread. It processes the data,
+ * optionally applies filtering and transformations, and posts SensorEvent events
+ * to registered handlers through RTAB-Map's event system.
+ * 
+ * The class supports various sensor configurations:
+ * - **Camera-only**: Single or multiple cameras for RGB-D or stereo imaging
+ * - **Lidar-only**: Single lidar for 3D point cloud capture
+ * - **Camera + Lidar**: Combined visual and range sensing
+ * - **With odometry**: Optional odometry sensor for pose estimation and deskewing
+ * 
+ * Key features:
+ * - **Automatic data synchronization**: Synchronizes camera and lidar data by timestamp
+ * - **Deskewing**: Corrects lidar scans using odometry poses
+ * - **Image processing**: Supports mirroring, decimation, histogram equalization, stereo-to-depth conversion
+ * - **Depth filtering**: Optional bilateral filtering for depth images
+ * - **IMU filtering**: Optional IMU data filtering and fusion
+ * - **Feature detection**: Optional automatic feature detection on captured images
+ * - **Frame rate control**: Configurable capture rate
+ * - **Event-driven architecture**: Posts SensorEvent events for downstream processing
+ * 
+ * The class inherits from UThread (for threading) and UEventsSender (for event posting).
+ * 
+ * @note Ownership of Camera, Lidar, and SensorCapture pointers is transferred to this class.
+ * @note The thread must be started with start() and stopped with kill() or join().
+ * @warning **Thread Safety**: All configuration parameters (setters) should be called
+ *          **before** starting the thread with start(). Changing configuration after the
+ *          thread is started is not thread-safe and may lead to race conditions or
+ *          undefined behavior. If you need to change parameters at runtime, stop the
+ *          thread first, modify the configuration, then restart it.
+ * 
+ * @see Camera
+ * @see Lidar
+ * @see SensorCapture
+ * @see SensorEvent
+ * @see SensorData
+ * @see UThread
+ * @see UEventsSender
  */
 class RTABMAP_CORE_EXPORT SensorCaptureThread :
 	public UThread,
 	public UEventsSender
 {
 public:
-	// ownership transferred
+	/**
+	 * @brief Constructor for camera-only capture
+	 * 
+	 * Creates a SensorCaptureThread that captures images from a single camera.
+	 * The camera can be RGB-D, stereo, or mono.
+	 * 
+	 * @param camera Pointer to the camera to capture from (ownership transferred)
+	 * @param parameters Optional parameters map for configuration
+	 * 
+	 * @note The camera pointer is owned by this class and will be deleted on destruction.
+	 */
 	SensorCaptureThread(
 			Camera * camera,
 			const ParametersMap & parameters = ParametersMap());
+	
 	/**
-	 * @param camera the camera to take images from
-	 * @param odomSensor an odometry sensor to get a pose (can be again the camera)
-	 * @param odomAsGt set odometry sensor pose as ground truth instead of odometry
-	 * @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame  (without optical rotation)
+	 * @brief Constructor for camera with odometry sensor
+	 * 
+	 * Creates a SensorCaptureThread that captures images from a camera and uses
+	 * an odometry sensor for pose estimation. The odometry sensor can be the same
+	 * as the camera or a different sensor.
+	 * 
+	 * @param camera Pointer to the camera to capture images from (ownership transferred)
+	 * @param odomSensor Pointer to the odometry sensor for pose estimation (can be the same as camera)
+	 * @param extrinsics Static transform from odometry sensor's left lens frame to camera's left lens frame
+	 *                   (without optical rotation applied)
+	 * @param poseTimeOffset Time offset in seconds to add to data timestamp when querying pose (default: 0.0)
+	 * @param poseScaleFactor Scale factor to apply to pose translation (default: 1.0, no scaling)
+	 * @param poseWaitTime Maximum time in seconds to wait for pose data (default: 0.1)
+	 * @param parameters Optional parameters map for configuration
+	 * 
+	 * @note If odomAsGt is set to true via setOdomAsGroundTruth(), the odometry pose
+	 *       will be used as ground truth instead of odometry.
 	 */
 	SensorCaptureThread(
 			Camera * camera,
@@ -78,23 +141,55 @@ public:
 			float poseScaleFactor = 1.0f,
 			double poseWaitTime = 0.1,
 			const ParametersMap & parameters = ParametersMap());
+	
 	/**
-	 * @param lidar the lidar to take scans from
+	 * @brief Constructor for lidar-only capture
+	 * 
+	 * Creates a SensorCaptureThread that captures 3D point clouds from a lidar.
+	 * 
+	 * @param lidar Pointer to the lidar to capture scans from (ownership transferred)
+	 * @param parameters Optional parameters map for configuration
+	 * 
+	 * @note The lidar pointer is owned by this class and will be deleted on destruction.
 	 */
 	SensorCaptureThread(
 			Lidar * lidar,
 			const ParametersMap & parameters = ParametersMap());
+	
 	/**
-	 * @param lidar the lidar to take scans from
-	 * @param camera the camera to take images from. If the camera is providing a pose, it can be used for deskewing
+	 * @brief Constructor for lidar with camera
+	 * 
+	 * Creates a SensorCaptureThread that captures both lidar scans and camera images.
+	 * If the camera provides pose information, it can be used for deskewing lidar scans.
+	 * 
+	 * @param lidar Pointer to the lidar to capture scans from (ownership transferred)
+	 * @param camera Pointer to the camera to capture images from (ownership transferred).
+	 *               If the camera provides pose, it can be used for deskewing.
+	 * @param parameters Optional parameters map for configuration
 	 */
 	SensorCaptureThread(
 			Lidar * lidar,
 			Camera * camera,
 			const ParametersMap & parameters = ParametersMap());
+	
 	/**
-	 * @param lidar the lidar to take scans from
-	 * @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the lidar)
+	 * @brief Constructor for lidar with odometry sensor
+	 * 
+	 * Creates a SensorCaptureThread that captures lidar scans and uses an odometry
+	 * sensor for pose estimation and deskewing. The odometry sensor can be the same
+	 * as the lidar or a different sensor.
+	 * 
+	 * @param lidar Pointer to the lidar to capture scans from (ownership transferred)
+	 * @param odomSensor Pointer to the odometry sensor for pose estimation and deskewing
+	 *                   (can be the same as lidar)
+	 * @param poseTimeOffset Time offset in seconds to add to data timestamp when querying pose (default: 0.0)
+	 * @param poseScaleFactor Scale factor to apply to pose translation (default: 1.0, no scaling)
+	 * @param poseWaitTime Maximum time in seconds to wait for pose data (default: 0.1)
+	 * @param parameters Optional parameters map for configuration
+	 * 
+	 * @note Deskewing is enabled by default when an odometry sensor is provided.
+	 *       Use setScanParameters() to configure deskewing behavior.
+	 * @note The lidar's local transform should be set as the extrinsics between odometry sensor's frame and lidar's frame.
 	 */
 	SensorCaptureThread(
 			Lidar * lidar,
@@ -103,11 +198,28 @@ public:
 			float poseScaleFactor = 1.0f,
 			double poseWaitTime = 0.1,
 			const ParametersMap & parameters = ParametersMap());
+	
 	/**
-	 * @param lidar the lidar to take scans from
-	 * @param camera the camera to take images from
-	 * @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the camera or lidar)
-	 * @param extrinsics the static transform between odometry frame to camera frame (without optical rotation)
+	 * @brief Constructor for lidar with camera and odometry sensor
+	 * 
+	 * Creates a SensorCaptureThread that captures both lidar scans and camera images,
+	 * and uses an odometry sensor for pose estimation and deskewing. This is the most
+	 * comprehensive configuration, supporting multi-modal sensing with pose correction.
+	 * 
+	 * @param lidar Pointer to the lidar to capture scans from (ownership transferred)
+	 * @param camera Pointer to the camera to capture images from (ownership transferred)
+	 * @param odomSensor Pointer to the odometry sensor for pose estimation and deskewing
+	 *                   (can be the same as camera or lidar)
+	 * @param extrinsics Static transform from odometry frame to camera frame
+	 *                   (without optical rotation applied)
+	 * @param poseTimeOffset Time offset in seconds to add to data timestamp when querying pose (default: 0.0)
+	 * @param poseScaleFactor Scale factor to apply to pose translation (default: 1.0, no scaling)
+	 * @param poseWaitTime Maximum time in seconds to wait for pose data (default: 0.1)
+	 * @param parameters Optional parameters map for configuration
+	 * 
+	 * @note The camera and lidar data are synchronized by timestamp, with the camera
+	 *       frame being at least as recent as the lidar frame.
+	 * @note The lidar's local transform should be set as the extrinsics between odometry sensor's frame and lidar's frame.
 	 */
 	SensorCaptureThread(
 			Lidar * lidar,
@@ -118,26 +230,182 @@ public:
 			float poseScaleFactor = 1.0f,
 			double poseWaitTime = 0.1,
 			const ParametersMap & parameters = ParametersMap());
+	
+	/**
+	 * @brief Virtual destructor
+	 * 
+	 * Stops the capture thread if running and deletes owned sensor pointers.
+	 */
 	virtual ~SensorCaptureThread();
 
+	/**
+	 * @brief Enables or disables image mirroring (horizontal flip)
+	 * @param enabled True to enable mirroring, false to disable
+	 */
 	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
+	
+	/**
+	 * @brief Enables or disables stereo exposure compensation
+	 * 
+	 * When enabled, adjusts exposure between left and right stereo cameras
+	 * to improve matching quality.
+	 * 
+	 * @param enabled True to enable exposure compensation, false to disable
+	 */
 	void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
+	
+	/**
+	 * @brief Sets whether to capture color images only (skip depth)
+	 * @param colorOnly True to capture color only, false to capture both color and depth
+	 */
 	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
+	
+	/**
+	 * @brief Sets image decimation factor
+	 * 
+	 * Decimation reduces image resolution by skipping pixels. A decimation of N
+	 * means every Nth pixel is kept (e.g., 2 = half resolution, 4 = quarter resolution).
+	 * 
+	 * @param decimation Decimation factor (1 = no decimation, 2 = half resolution, etc.)
+	 */
 	void setImageDecimation(int decimation) {_imageDecimation = decimation;}
+	
+	/**
+	 * @brief Sets histogram equalization method
+	 * 
+	 * Controls the type of histogram equalization applied to captured images.
+	 * Histogram equalization improves image contrast by redistributing pixel intensities.
+	 * 
+	 * @param histogramMethod Histogram equalization method:
+	 *                        - 0 = None (disabled)
+	 *                        - 1 = Standard histogram equalization (cv::equalizeHist)
+	 *                        - 2 = CLAHE - Contrast Limited Adaptive Histogram Equalization (cv::createCLAHE)
+	 * 
+	 * @note For color images, histogram equalization is applied only to the luminance channel (Y in YCrCb color space).
+	 * @note For stereo images, histogram equalization is applied to both left and right images.
+	 */
 	void setHistogramMethod(int histogramMethod) {_histogramMethod = histogramMethod;}
+	
+	/**
+	 * @brief Enables or disables stereo-to-depth conversion
+	 * 
+	 * When enabled, converts stereo images to depth images using dense stereo matching.
+	 * 
+	 * @param enabled True to enable stereo-to-depth conversion, false to disable
+	 */
 	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
+	
+	/**
+	 * @brief Sets the target frame rate for capture
+	 * 
+	 * Controls how often the capture loop runs. A frame rate of 0 means capture
+	 * as fast as possible.
+	 * 
+	 * @param frameRate Target frame rate in Hz (0 = unlimited)
+	 */
 	void setFrameRate(float frameRate);
+	
+	/**
+	 * @deprecated Use setFrameRate() instead
+	 * @brief Sets the target image capture rate (deprecated)
+	 */
 	RTABMAP_DEPRECATED void setImageRate(float frameRate) {setFrameRate(frameRate);}
+	
+	/**
+	 * @brief Sets the depth distortion model file path
+	 * 
+	 * Loads a discrete depth distortion model from the specified file path.
+	 * This is used to correct systematic depth errors at longer ranges using
+	 * the CLAMS (Calibration, Localization, And Mapping System) approach.
+	 * 
+	 * The distortion model is typically created using RTAB-Map's depth calibration
+	 * tool, which uses visual odometry and 3D mapping to generate ground truth
+	 * depth for calibration.
+	 * 
+	 * @param path Path to the distortion model file
+	 * 
+	 * @see https://github.com/introlab/rtabmap/wiki/Depth-Calibration
+	 *      for detailed instructions on how to create and use depth distortion models
+	 */
 	void setDistortionModel(const std::string & path);
+	
+	/**
+	 * @brief Sets whether odometry poses should be treated as ground truth
+	 * 
+	 * When enabled, odometry poses from the odometry sensor are stored as ground
+	 * truth poses instead of odometry poses in the SensorData.
+	 * 
+	 * @param enabled True to use odometry as ground truth, false to use as odometry
+	 */
 	void setOdomAsGroundTruth(bool enabled) {_odomAsGt = enabled;}
+	
+	/**
+	 * @brief Enables bilateral filtering for depth images
+	 * 
+	 * Bilateral filtering reduces noise in depth images while preserving edges.
+	 * 
+	 * @param sigmaS Spatial standard deviation (pixels)
+	 * @param sigmaR Range standard deviation (depth units)
+	 */
 	void enableBilateralFiltering(float sigmaS, float sigmaR);
+	
+	/**
+	 * @brief Disables bilateral filtering
+	 */
 	void disableBilateralFiltering() {_bilateralFiltering = false;}
+	
+	/**
+	 * @brief Enables IMU data filtering
+	 * 
+	 * Enables filtering and fusion of IMU data from the sensor. The filtering
+	 * strategy determines how IMU data is processed to estimate orientation.
+	 * 
+	 * @param filteringStrategy Filtering strategy:
+	 *                          - 0 = Madgwick filter (requires RTAB-Map built with RTABMAP_MADGWICK option)
+	 *                          - 1 = Complementary filter (default)
+	 * @param parameters Optional parameters map for IMU filter configuration
+	 * @param baseFrameConversion If true, converts IMU data to base frame
+	 * 
+	 * @note If Madgwick filter is requested but RTAB-Map is not built with the option enabled,
+	 *       the Complementary filter will be used instead.
+	 * 
+	 * @see IMUFilter
+	 */
 	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
+	
+	/**
+	 * @brief Disables IMU data filtering
+	 */
 	void disableIMUFiltering();
+	
+	/**
+	 * @brief Enables automatic feature detection on captured images
+	 * 
+	 * When enabled, automatically detects and extracts visual features (keypoints,
+	 * descriptors) from captured images.
+	 * 
+	 * @param parameters Optional parameters map for feature detector configuration.
+	 *                   Parameters from the "Vis/" group (e.g., Vis/FeatureType, Vis/MaxFeatures)
+	 *                   are automatically converted to "Kp/" parameters internally.
+	 *                   If both Vis/ and Kp/ parameters are provided, Vis/ parameters take precedence.
+	 * 
+	 * @note The function converts Vis/ parameters to Kp/ parameters before creating the Feature2D detector.
+	 */
 	void enableFeatureDetection(const ParametersMap & parameters = ParametersMap());
+	
+	/**
+	 * @brief Disables automatic feature detection
+	 */
 	void disableFeatureDetection();
 
-	// Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.
+	/**
+	 * @deprecated Use the new version with groundNormalsUp parameter instead
+	 * @brief Sets lidar scan processing parameters (deprecated)
+	 * 
+	 * Use the new version with groundNormalsUp parameter:
+	 * - groundNormalsUp=0.8 for forceGroundNormalsUp=True
+	 * - groundNormalsUp=0.0 for forceGroundNormalsUp=False
+	 */
 	RTABMAP_DEPRECATED void setScanParameters(
 			bool fromDepth,
 			int downsampleStep, // decimation of the depth image in case the scan is from depth image
@@ -148,6 +416,27 @@ public:
 			float normalsRadius,
 			bool forceGroundNormalsUp,
 			bool deskewing);
+	
+	/**
+	 * @brief Sets lidar scan processing parameters
+	 * 
+	 * Configures how lidar scans are processed, including conversion from depth images,
+	 * filtering, normal estimation, and deskewing.
+	 * 
+	 * @param fromDepth If true, generate scans from depth images instead of raw lidar data
+	 * @param downsampleStep Downsampling step:
+	 *                       - If fromDepth is true: Image decimation step for depth images
+	 *                         (1 = no decimation, 2 = every other pixel in both dimensions, etc.)
+	 *                       - If fromDepth is false: Point skipping step for laser scan data
+	 *                         (1 = no skipping, 2 = every other point, etc.)
+	 * @param rangeMin Minimum range in meters (points closer are filtered out, 0 = no minimum)
+	 * @param rangeMax Maximum range in meters (points farther are filtered out, 0 = no maximum)
+	 * @param voxelSize Voxel size in meters for downsampling (0 = no voxelization)
+	 * @param normalsK Number of neighbors for K-nearest neighbors normal estimation (0 = no normals)
+	 * @param normalsRadius Radius in meters for radius-based normal estimation (0 = use K-nearest)
+	 * @param groundNormalsUp Threshold for forcing ground normals upward (0.0-1.0, 0.8 = strong, 0.0 = disabled)
+	 * @param deskewing If true, apply deskewing using odometry poses
+	 */
 	void setScanParameters(
 			bool fromDepth,
 			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
@@ -159,58 +448,120 @@ public:
 			float groundNormalsUp = 0.0f,
 			bool deskewing = false);
 
+	/**
+	 * @brief Post-processing hook called before posting SensorEvent
+	 * 
+	 * This method is called after all processing is complete but before the
+	 * SensorEvent is posted. Derived classes can override this to perform
+	 * additional processing or modifications to the data.
+	 * 
+	 * @param data Pointer to the processed sensor data (can be modified)
+	 * @param info Pointer to the sensor capture info (can be modified, may be null)
+	 */
 	void postUpdate(SensorData * data, SensorCaptureInfo * info = 0) const;
 
-	//getters
+	// Getters
+	
+	/**
+	 * @brief Checks if the capture thread is paused
+	 * @return True if paused (not running), false if running
+	 */
 	bool isPaused() const {return !this->isRunning();}
+	
+	/**
+	 * @brief Checks if the capture thread is actively capturing
+	 * @return True if capturing (running), false if not running
+	 */
 	bool isCapturing() const {return this->isRunning();}
+	
+	/**
+	 * @brief Checks if odometry is provided by the sensors
+	 * @return True if an odometry sensor is configured, false otherwise
+	 */
 	bool odomProvided() const;
 
-	Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
-	SensorCapture * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted
-	Lidar * lidar() {return _lidar;} // return null if not set, valid until CameraThread is deleted
+	/**
+	 * @brief Returns the camera pointer
+	 * @return Pointer to the camera (null if not set). Valid until SensorCaptureThread is deleted.
+	 */
+	Camera * camera() {return _camera;}
+	
+	/**
+	 * @brief Returns the odometry sensor pointer
+	 * @return Pointer to the odometry sensor (null if not set). Valid until SensorCaptureThread is deleted.
+	 */
+	SensorCapture * odomSensor() {return _odomSensor;}
+	
+	/**
+	 * @brief Returns the lidar pointer
+	 * @return Pointer to the lidar (null if not set). Valid until SensorCaptureThread is deleted.
+	 */
+	Lidar * lidar() {return _lidar;}
 
 private:
+	/**
+	 * @brief Called once when the thread starts (before mainLoop)
+	 * 
+	 * Initializes resources needed for the capture loop.
+	 */
 	virtual void mainLoopBegin();
+	
+	/**
+	 * @brief Main capture loop executed in the thread
+	 * 
+	 * Continuously captures sensor data, processes it, and posts SensorEvent events.
+	 * This method runs until the thread is killed.
+	 */
 	virtual void mainLoop();
+	
+	/**
+	 * @brief Called when the thread is being killed
+	 * 
+	 * Performs cleanup and ensures proper shutdown of sensors.
+	 */
 	virtual void mainLoopKill();
 
 private:
-	Camera * _camera;
-	SensorCapture * _odomSensor;
-	Lidar * _lidar;
-	Transform _extrinsicsOdomToCamera;
-	bool _odomAsGt;
-	double _poseTimeOffset;
-	float _poseScaleFactor;
-	double _poseWaitTime;
-	bool _mirroring;
-	bool _stereoExposureCompensation;
-	bool _colorOnly;
-	int _imageDecimation;
-	int _histogramMethod;
-	bool _stereoToDepth;
-	bool _scanDeskewing;
-	bool _scanFromDepth;
-	int _scanDownsampleStep;
-	float _scanRangeMin;
-	float _scanRangeMax;
-	float _scanVoxelSize;
-	int _scanNormalsK;
-	float _scanNormalsRadius;
-	float _scanForceGroundNormalsUp;
-	StereoDense * _stereoDense;
-	clams::DiscreteDepthDistortionModel * _distortionModel;
-	bool _bilateralFiltering;
-	float _bilateralSigmaS;
-	float _bilateralSigmaR;
-	IMUFilter * _imuFilter;
-	bool _imuBaseFrameConversion;
-	Feature2D * _featureDetector;
-	bool _depthAsMask;
+	Camera * _camera; ///< Camera for image capture (owned, null if not used)
+	SensorCapture * _odomSensor; ///< Odometry sensor for pose estimation (owned, null if not used)
+	Lidar * _lidar; ///< Lidar for scan capture (owned, null if not used)
+	Transform _extrinsicsOdomToCamera; ///< Static transform from odometry frame to camera frame
+	bool _odomAsGt; ///< If true, use odometry poses as ground truth instead of odometry
+	double _poseTimeOffset; ///< Time offset in seconds when querying poses
+	float _poseScaleFactor; ///< Scale factor to apply to pose translation
+	double _poseWaitTime; ///< Maximum time to wait for pose data
+	bool _mirroring; ///< Enable horizontal image mirroring
+	bool _stereoExposureCompensation; ///< Enable stereo exposure compensation
+	bool _colorOnly; ///< Capture color images only (skip depth)
+	int _imageDecimation; ///< Image decimation factor (1 = no decimation)
+	int _histogramMethod; ///< Histogram equalization method
+	bool _stereoToDepth; ///< Convert stereo images to depth using dense matching
+	bool _scanDeskewing; ///< Enable lidar scan deskewing using odometry
+	bool _scanFromDepth; ///< Generate scans from depth images instead of raw lidar
+	int _scanDownsampleStep; ///< Decimation step for depth-to-scan conversion
+	float _scanRangeMin; ///< Minimum scan range in meters (0 = no minimum)
+	float _scanRangeMax; ///< Maximum scan range in meters (0 = no maximum)
+	float _scanVoxelSize; ///< Voxel size for scan downsampling (0 = no voxelization)
+	int _scanNormalsK; ///< K-nearest neighbors for normal estimation (0 = disabled)
+	float _scanNormalsRadius; ///< Radius for normal estimation (0 = use K-nearest)
+	float _scanForceGroundNormalsUp; ///< Threshold for forcing ground normals upward (0.0-1.0)
+	StereoDense * _stereoDense; ///< Dense stereo matcher for stereo-to-depth conversion (owned)
+	clams::DiscreteDepthDistortionModel * _distortionModel; ///< Depth distortion correction model (owned)
+	bool _bilateralFiltering; ///< Enable bilateral filtering for depth images
+	float _bilateralSigmaS; ///< Spatial standard deviation for bilateral filtering
+	float _bilateralSigmaR; ///< Range standard deviation for bilateral filtering
+	IMUFilter * _imuFilter; ///< IMU data filter (owned, null if disabled)
+	bool _imuBaseFrameConversion; ///< Convert IMU data to base frame
+	Feature2D * _featureDetector; ///< Feature detector for automatic feature extraction (owned, null if disabled)
+	bool _depthAsMask; ///< Use depth as mask for feature detection
 };
 
-//backward compatibility
+/**
+ * @deprecated Use SensorCaptureThread instead
+ * @brief Backward compatibility typedef
+ * 
+ * CameraThread is deprecated. Use SensorCaptureThread instead.
+ */
 RTABMAP_DEPRECATED typedef SensorCaptureThread CameraThread;
 
 } // namespace rtabmap

corelib/test/CMakeLists.txt

@@ -105,3 +105,13 @@ gtest_discover_tests(test_sensorevent)
 add_executable(test_sensordata test_sensordata.cpp)
 target_link_libraries(test_sensordata gtest_main rtabmap_core)
 gtest_discover_tests(test_sensordata)
+
+#SensorCapture.h
+add_executable(test_sensorcapture test_sensorcapture.cpp)
+target_link_libraries(test_sensorcapture gtest_main rtabmap_core)
+gtest_discover_tests(test_sensorcapture)
+
+#SensorCaptureThread.h
+add_executable(test_sensorcapturethread test_sensorcapturethread.cpp)
+target_link_libraries(test_sensorcapturethread gtest_main rtabmap_core)
+gtest_discover_tests(test_sensorcapturethread)

corelib/test/test_sensorcapture.cpp

@@ -0,0 +1,447 @@
+#include <gtest/gtest.h>
+#include <rtabmap/core/SensorCapture.h>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/SensorCaptureInfo.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <opencv2/core.hpp>
+
+using namespace rtabmap;
+
+// Mock SensorCapture implementation for testing
+class MockSensorCapture : public SensorCapture
+{
+public:
+	MockSensorCapture(float frameRate = 0, const Transform & localTransform = Transform::getIdentity()) :
+		SensorCapture(frameRate, localTransform),
+		initCalled_(false),
+		initResult_(true),
+		serial_("MOCK_SENSOR_001"),
+		odomProvided_(false),
+		captureCount_(0)
+	{
+	}
+
+	virtual ~MockSensorCapture() {}
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "")
+	{
+		initCalled_ = true;
+		calibrationFolder_ = calibrationFolder;
+		cameraName_ = cameraName;
+		return initResult_;
+	}
+
+	virtual std::string getSerial() const
+	{
+		return serial_;
+	}
+
+	virtual bool odomProvided() const
+	{
+		return odomProvided_;
+	}
+
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06)
+	{
+		if (odomProvided_)
+		{
+			pose = Transform(1, 0, 0, 0, 0, 0, 1);
+			covariance = cv::Mat::eye(6, 6, CV_64FC1);
+			return true;
+		}
+		return false;
+	}
+
+	// Test helpers
+	void setInitResult(bool result) { initResult_ = result; }
+	void setSerial(const std::string & serial) { serial_ = serial; }
+	void setOdomProvided(bool provided) { odomProvided_ = true; }
+	
+	bool wasInitCalled() const { return initCalled_; }
+	std::string getCalibrationFolder() const { return calibrationFolder_; }
+	std::string getCameraName() const { return cameraName_; }
+	int getCaptureCount() const { return captureCount_; }
+
+protected:
+	virtual SensorData captureData(SensorCaptureInfo * info = 0)
+	{
+		++captureCount_;
+		
+		cv::Mat image = cv::Mat::ones(480, 640, CV_8UC3) * 128;
+		SensorData data(image, getNextSeqID(), UTimer::now());
+		return data;
+	}
+
+private:
+	bool initCalled_;
+	bool initResult_;
+	std::string serial_;
+	bool odomProvided_;
+	std::string calibrationFolder_;
+	std::string cameraName_;
+	int captureCount_;
+};
+
+// Constructor Tests
+
+TEST(SensorCaptureTest, DefaultConstructor)
+{
+	MockSensorCapture sensor;
+	
+	EXPECT_EQ(sensor.getFrameRate(), 0.0f);
+	EXPECT_TRUE(sensor.getLocalTransform().isIdentity());
+	EXPECT_FALSE(sensor.wasInitCalled());
+	EXPECT_EQ(sensor.getSerial(), "MOCK_SENSOR_001");
+	EXPECT_FALSE(sensor.odomProvided());
+}
+
+TEST(SensorCaptureTest, ConstructorWithFrameRate)
+{
+	float frameRate = 30.0f;
+	MockSensorCapture sensor(frameRate);
+	
+	EXPECT_FLOAT_EQ(sensor.getFrameRate(), frameRate);
+	EXPECT_TRUE(sensor.getLocalTransform().isIdentity());
+}
+
+TEST(SensorCaptureTest, ConstructorWithLocalTransform)
+{
+	Transform localTransform(1, 0, 0, 0, 0, 0, 1);
+	MockSensorCapture sensor(0, localTransform);
+	
+	EXPECT_EQ(sensor.getLocalTransform(), localTransform);
+}
+
+TEST(SensorCaptureTest, ConstructorWithFrameRateAndTransform)
+{
+	float frameRate = 15.0f;
+	Transform localTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
+	MockSensorCapture sensor(frameRate, localTransform);
+	
+	EXPECT_FLOAT_EQ(sensor.getFrameRate(), frameRate);
+	EXPECT_EQ(sensor.getLocalTransform(), localTransform);
+}
+
+// Init Tests
+
+TEST(SensorCaptureTest, InitDefault)
+{
+	MockSensorCapture sensor;
+	
+	EXPECT_TRUE(sensor.init());
+	EXPECT_TRUE(sensor.wasInitCalled());
+	EXPECT_EQ(sensor.getCalibrationFolder(), ".");
+	EXPECT_EQ(sensor.getCameraName(), "");
+}
+
+TEST(SensorCaptureTest, InitWithCalibrationFolder)
+{
+	MockSensorCapture sensor;
+	std::string folder = "/path/to/calibration";
+	
+	EXPECT_TRUE(sensor.init(folder));
+	EXPECT_EQ(sensor.getCalibrationFolder(), folder);
+	EXPECT_EQ(sensor.getCameraName(), "");
+}
+
+TEST(SensorCaptureTest, InitWithCalibrationFolderAndCameraName)
+{
+	MockSensorCapture sensor;
+	std::string folder = "/path/to/calibration";
+	std::string cameraName = "camera1";
+	
+	EXPECT_TRUE(sensor.init(folder, cameraName));
+	EXPECT_EQ(sensor.getCalibrationFolder(), folder);
+	EXPECT_EQ(sensor.getCameraName(), cameraName);
+}
+
+TEST(SensorCaptureTest, InitFailure)
+{
+	MockSensorCapture sensor;
+	sensor.setInitResult(false);
+	
+	EXPECT_FALSE(sensor.init());
+	EXPECT_TRUE(sensor.wasInitCalled());
+}
+
+// Serial Tests
+
+TEST(SensorCaptureTest, GetSerial)
+{
+	MockSensorCapture sensor;
+	
+	EXPECT_EQ(sensor.getSerial(), "MOCK_SENSOR_001");
+	
+	sensor.setSerial("CUSTOM_SERIAL_123");
+	EXPECT_EQ(sensor.getSerial(), "CUSTOM_SERIAL_123");
+}
+
+// Frame Rate Tests
+
+TEST(SensorCaptureTest, SetGetFrameRate)
+{
+	MockSensorCapture sensor;
+	
+	EXPECT_EQ(sensor.getFrameRate(), 0.0f);
+	
+	sensor.setFrameRate(30.0f);
+	EXPECT_FLOAT_EQ(sensor.getFrameRate(), 30.0f);
+	
+	sensor.setFrameRate(0.0f);
+	EXPECT_FLOAT_EQ(sensor.getFrameRate(), 0.0f);
+	
+	sensor.setFrameRate(60.0f);
+	EXPECT_FLOAT_EQ(sensor.getFrameRate(), 60.0f);
+}
+
+// Local Transform Tests
+
+TEST(SensorCaptureTest, SetGetLocalTransform)
+{
+	MockSensorCapture sensor;
+	
+	EXPECT_TRUE(sensor.getLocalTransform().isIdentity());
+	
+	Transform transform(1, 0, 0, 0, 0, 0, 1);
+	sensor.setLocalTransform(transform);
+	EXPECT_EQ(sensor.getLocalTransform(), transform);
+	
+	Transform transform2(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
+	sensor.setLocalTransform(transform2);
+	EXPECT_EQ(sensor.getLocalTransform(), transform2);
+}
+
+// Reset Timer Tests
+
+TEST(SensorCaptureTest, ResetTimer)
+{
+	MockSensorCapture sensor;
+	
+	// Reset should not throw
+	EXPECT_NO_THROW(sensor.resetTimer());
+	
+	// After reset, timer should be ready for frame rate control
+	sensor.setFrameRate(10.0f);
+	sensor.resetTimer();
+	
+	// First capture should be fast (no throttling needed)
+	SensorData data = sensor.takeData();
+	EXPECT_TRUE(data.isValid());
+}
+
+// TakeData Tests
+
+TEST(SensorCaptureTest, TakeDataBasic)
+{
+	MockSensorCapture sensor;
+	
+	SensorData data = sensor.takeData();
+	
+	EXPECT_TRUE(data.isValid());
+	EXPECT_GT(data.id(), 0);
+	EXPECT_GT(data.stamp(), 0.0);
+	EXPECT_FALSE(data.imageRaw().empty());
+	EXPECT_EQ(sensor.getCaptureCount(), 1);
+}
+
+TEST(SensorCaptureTest, TakeDataWithInfo)
+{
+	MockSensorCapture sensor;
+	SensorCaptureInfo info;
+	
+	SensorData data = sensor.takeData(&info);
+	
+	EXPECT_TRUE(data.isValid());
+	EXPECT_EQ(info.id, data.id());
+	EXPECT_DOUBLE_EQ(info.stamp, data.stamp());
+	EXPECT_GT(info.timeCapture, 0.0);
+	EXPECT_LE(info.timeCapture, 0.1); // Should be very fast for mock
+}
+
+TEST(SensorCaptureTest, TakeDataSequenceIDs)
+{
+	MockSensorCapture sensor;
+	
+	SensorData data1 = sensor.takeData();
+	SensorData data2 = sensor.takeData();
+	SensorData data3 = sensor.takeData();
+	
+	EXPECT_EQ(data1.id() + 1, data2.id());
+	EXPECT_EQ(data2.id() + 1, data3.id());
+	EXPECT_EQ(sensor.getCaptureCount(), 3);
+}
+
+TEST(SensorCaptureTest, TakeDataNoFrameRateThrottling)
+{
+	MockSensorCapture sensor;
+	sensor.setFrameRate(0.0f); // Unlimited
+	
+	UTimer timer;
+	SensorData data1 = sensor.takeData();
+	SensorData data2 = sensor.takeData();
+	double elapsed = timer.ticks();
+	
+	// Without throttling, captures should be very fast
+	EXPECT_LT(elapsed, 0.1);
+	EXPECT_TRUE(data1.isValid());
+	EXPECT_TRUE(data2.isValid());
+}
+
+TEST(SensorCaptureTest, TakeDataWithFrameRateThrottling)
+{
+	MockSensorCapture sensor;
+	sensor.setFrameRate(10.0f); // 10 Hz = 100ms per frame
+	sensor.resetTimer();
+	
+	UTimer timer;
+	SensorData data1 = sensor.takeData();
+	SensorData data2 = sensor.takeData();
+	double elapsed = timer.ticks();
+	
+	// With 10 Hz throttling, two captures should take at least ~100ms
+	EXPECT_GE(elapsed, 0.2); // Should be at least 200 ms for two frames
+	EXPECT_LE(elapsed, 0.21); // Slight margin
+	EXPECT_TRUE(data1.isValid());
+	EXPECT_TRUE(data2.isValid());
+}
+
+// OdomProvided Tests
+
+TEST(SensorCaptureTest, OdomProvidedDefault)
+{
+	MockSensorCapture sensor;
+	
+	EXPECT_FALSE(sensor.odomProvided());
+}
+
+TEST(SensorCaptureTest, OdomProvidedEnabled)
+{
+	MockSensorCapture sensor;
+	sensor.setOdomProvided(true);
+	
+	EXPECT_TRUE(sensor.odomProvided());
+}
+
+// GetPose Tests
+
+TEST(SensorCaptureTest, GetPoseDefault)
+{
+	MockSensorCapture sensor;
+	Transform pose;
+	cv::Mat covariance;
+	
+	EXPECT_FALSE(sensor.getPose(0.0, pose, covariance));
+}
+
+TEST(SensorCaptureTest, GetPoseWithOdom)
+{
+	MockSensorCapture sensor;
+	sensor.setOdomProvided(true);
+	
+	Transform pose;
+	cv::Mat covariance;
+	
+	EXPECT_TRUE(sensor.getPose(0.0, pose, covariance));
+	EXPECT_FALSE(pose.isNull());
+	EXPECT_FALSE(covariance.empty());
+	EXPECT_EQ(covariance.rows, 6);
+	EXPECT_EQ(covariance.cols, 6);
+}
+
+// Comprehensive Usage Test
+
+TEST(SensorCaptureTest, ComprehensiveUsage)
+{
+	// Create sensor with frame rate and transform
+	float frameRate = 20.0f;
+	Transform localTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
+	MockSensorCapture sensor(frameRate, localTransform);
+	
+	// Initialize
+	EXPECT_TRUE(sensor.init("/calib", "test_camera"));
+	EXPECT_EQ(sensor.getCalibrationFolder(), "/calib");
+	EXPECT_EQ(sensor.getCameraName(), "test_camera");
+	
+	// Verify settings
+	EXPECT_FLOAT_EQ(sensor.getFrameRate(), frameRate);
+	EXPECT_EQ(sensor.getLocalTransform(), localTransform);
+	EXPECT_EQ(sensor.getSerial(), "MOCK_SENSOR_001");
+	
+	// Change settings
+	sensor.setFrameRate(30.0f);
+	Transform newTransform = Transform::getIdentity();
+	sensor.setLocalTransform(newTransform);
+	sensor.setSerial("NEW_SERIAL");
+	
+	EXPECT_FLOAT_EQ(sensor.getFrameRate(), 30.0f);
+	EXPECT_EQ(sensor.getLocalTransform(), newTransform);
+	EXPECT_EQ(sensor.getSerial(), "NEW_SERIAL");
+	
+	// Capture data
+	sensor.resetTimer();
+	SensorCaptureInfo info;
+	SensorData data = sensor.takeData(&info);
+	
+	EXPECT_TRUE(data.isValid());
+	EXPECT_EQ(info.id, data.id());
+	EXPECT_DOUBLE_EQ(info.stamp, data.stamp());
+	EXPECT_GT(info.timeCapture, 0.0);
+	EXPECT_EQ(sensor.getCaptureCount(), 1);
+}
+
+// Edge Cases
+
+TEST(SensorCaptureTest, TakeDataMultipleCalls)
+{
+	MockSensorCapture sensor;
+	
+	for (int i = 0; i < 10; ++i)
+	{
+		SensorData data = sensor.takeData();
+		EXPECT_TRUE(data.isValid());
+	}
+	
+	EXPECT_EQ(sensor.getCaptureCount(), 10);
+}
+
+TEST(SensorCaptureTest, TakeDataWithNullInfo)
+{
+	MockSensorCapture sensor;
+	
+	// Should not crash with null info
+	SensorData data = sensor.takeData(nullptr);
+	EXPECT_TRUE(data.isValid());
+}
+
+TEST(SensorCaptureTest, HighFrameRate)
+{
+	MockSensorCapture sensor;
+	sensor.setFrameRate(1000.0f); // Very high frame rate
+	sensor.resetTimer();
+	
+	// Should handle high frame rate gracefully
+	UTimer timer;
+	SensorData data = sensor.takeData();
+	double elapsed = timer.ticks();
+
+	EXPECT_TRUE(data.isValid());
+	EXPECT_GE(elapsed, 0.001);
+	EXPECT_LT(elapsed, 0.01);
+}
+
+TEST(SensorCaptureTest, VeryLowFrameRate)
+{
+	MockSensorCapture sensor;
+	sensor.setFrameRate(2.0f); // Low frame rate (500 ms seconds per frame)
+	sensor.resetTimer();
+	
+	UTimer timer;
+	SensorData data = sensor.takeData();
+	double elapsed = timer.ticks();
+	
+	EXPECT_TRUE(data.isValid());
+	EXPECT_GE(elapsed, 0.5);
+	EXPECT_LT(elapsed, 0.51);
+}
+

corelib/test/test_sensorcapturethread.cpp

@@ -0,0 +1,572 @@
+#include <gtest/gtest.h>
+#include <rtabmap/core/SensorCaptureThread.h>
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/Lidar.h>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/SensorCaptureInfo.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/LaserScan.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UThread.h>
+#include <opencv2/core.hpp>
+
+using namespace rtabmap;
+
+// Mock Camera implementation for testing
+class MockCamera : public Camera
+{
+public:
+	MockCamera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity()) :
+		Camera(imageRate, localTransform),
+		initCalled_(false),
+		initResult_(true),
+		serial_("MOCK_CAMERA_001"),
+		calibrated_(false),
+		odomProvided_(false),
+		captureCount_(0)
+	{
+	}
+
+	virtual ~MockCamera() {}
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "")
+	{
+		initCalled_ = true;
+		calibrationFolder_ = calibrationFolder;
+		cameraName_ = cameraName;
+		return initResult_;
+	}
+
+	virtual std::string getSerial() const
+	{
+		return serial_;
+	}
+
+	virtual bool isCalibrated() const
+	{
+		return calibrated_;
+	}
+
+	virtual bool odomProvided() const
+	{
+		return odomProvided_;
+	}
+
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06)
+	{
+		if (odomProvided_)
+		{
+			pose = Transform(1, 0, 0, 0, 0, 0, 1);
+			covariance = cv::Mat::eye(6, 6, CV_64FC1);
+			return true;
+		}
+		return false;
+	}
+
+	// Test helpers
+	void setInitResult(bool result) { initResult_ = result; }
+	void setSerial(const std::string & serial) { serial_ = serial; }
+	void setCalibrated(bool calibrated) { calibrated_ = calibrated; }
+	void setOdomProvided(bool provided) { odomProvided_ = provided; }
+	
+	bool wasInitCalled() const { return initCalled_; }
+	std::string getCalibrationFolder() const { return calibrationFolder_; }
+	std::string getCameraName() const { return cameraName_; }
+	int getCaptureCount() const { return captureCount_; }
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0)
+	{
+		++captureCount_;
+		
+		cv::Mat image = cv::Mat::ones(480, 640, CV_8UC3) * 128;
+		SensorData data(image, getNextSeqID(), UTimer::now());
+		return data;
+	}
+
+private:
+	bool initCalled_;
+	bool initResult_;
+	std::string serial_;
+	bool calibrated_;
+	bool odomProvided_;
+	std::string calibrationFolder_;
+	std::string cameraName_;
+	int captureCount_;
+};
+
+// Mock Lidar implementation for testing
+class MockLidar : public Lidar
+{
+public:
+	MockLidar(float lidarRate = 0, const Transform & localTransform = Transform::getIdentity()) :
+		Lidar(lidarRate, localTransform),
+		initCalled_(false),
+		initResult_(true),
+		serial_("MOCK_LIDAR_001"),
+		captureCount_(0)
+	{
+	}
+
+	virtual ~MockLidar() {}
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "")
+	{
+		initCalled_ = true;
+		calibrationFolder_ = calibrationFolder;
+		cameraName_ = cameraName;
+		return initResult_;
+	}
+
+	virtual std::string getSerial() const
+	{
+		return serial_;
+	}
+
+	// Test helpers
+	void setInitResult(bool result) { initResult_ = result; }
+	void setSerial(const std::string & serial) { serial_ = serial; }
+	
+	bool wasInitCalled() const { return initCalled_; }
+	std::string getCalibrationFolder() const { return calibrationFolder_; }
+	std::string getCameraName() const { return cameraName_; }
+	int getCaptureCount() const { return captureCount_; }
+
+protected:
+	virtual SensorData captureData(SensorCaptureInfo * info = 0)
+	{
+		++captureCount_;
+		
+		// Create a simple laser scan
+		LaserScan scan = LaserScan::backwardCompatibility(cv::Mat(1, 100, CV_32FC2));
+		SensorData data;
+		data.setLaserScan(scan);
+		data.setStamp(UTimer::now());
+		return data;
+	}
+
+private:
+	bool initCalled_;
+	bool initResult_;
+	std::string serial_;
+	std::string calibrationFolder_;
+	std::string cameraName_;
+	int captureCount_;
+};
+
+// Constructor Tests
+
+TEST(SensorCaptureThreadTest, ConstructorCameraOnly)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	EXPECT_FALSE(thread.isCapturing());
+	EXPECT_TRUE(thread.isPaused());
+	EXPECT_EQ(thread.camera(), camera);
+	EXPECT_EQ(thread.lidar(), nullptr);
+	EXPECT_EQ(thread.odomSensor(), nullptr);
+	EXPECT_FALSE(thread.odomProvided());
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, ConstructorLidarOnly)
+{
+	MockLidar * lidar = new MockLidar();
+	SensorCaptureThread thread(lidar);
+	
+	EXPECT_FALSE(thread.isCapturing());
+	EXPECT_TRUE(thread.isPaused());
+	EXPECT_EQ(thread.lidar(), lidar);
+	EXPECT_EQ(thread.camera(), nullptr);
+	EXPECT_EQ(thread.odomSensor(), nullptr);
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, ConstructorLidarWithCamera)
+{
+	MockLidar * lidar = new MockLidar();
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(lidar, camera);
+	
+	EXPECT_FALSE(thread.isCapturing());
+	EXPECT_EQ(thread.lidar(), lidar);
+	EXPECT_EQ(thread.camera(), camera);
+	EXPECT_EQ(thread.odomSensor(), nullptr);
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, ConstructorCameraWithOdomSensor)
+{
+	MockCamera * camera = new MockCamera();
+	MockCamera * odomSensor = new MockCamera();
+	Transform extrinsics = Transform::getIdentity();
+	
+	SensorCaptureThread thread(camera, odomSensor, extrinsics, 0.0, 1.0f, 0.1);
+	
+	EXPECT_FALSE(thread.isCapturing());
+	EXPECT_EQ(thread.camera(), camera);
+	EXPECT_EQ(thread.odomSensor(), odomSensor);
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, ConstructorLidarWithOdomSensor)
+{
+	MockLidar * lidar = new MockLidar();
+	MockCamera * odomSensor = new MockCamera();
+	
+	SensorCaptureThread thread(lidar, odomSensor, 0.0, 1.0f, 0.1);
+	
+	EXPECT_FALSE(thread.isCapturing());
+	EXPECT_EQ(thread.lidar(), lidar);
+	EXPECT_EQ(thread.odomSensor(), odomSensor);
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, ConstructorLidarCameraOdom)
+{
+	MockLidar * lidar = new MockLidar();
+	MockCamera * camera = new MockCamera();
+	MockCamera * odomSensor = new MockCamera();
+	Transform extrinsics = Transform::getIdentity();
+	
+	SensorCaptureThread thread(lidar, camera, odomSensor, extrinsics, 0.0, 1.0f, 0.1);
+	
+	EXPECT_FALSE(thread.isCapturing());
+	EXPECT_EQ(thread.lidar(), lidar);
+	EXPECT_EQ(thread.camera(), camera);
+	EXPECT_EQ(thread.odomSensor(), odomSensor);
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+// Configuration Tests
+
+TEST(SensorCaptureThreadTest, SetGetMirroring)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	thread.setMirroringEnabled(true);
+	// No getter, so we can't verify directly, but it shouldn't crash
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, SetGetStereoExposureCompensation)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	thread.setStereoExposureCompensation(true);
+	// No getter, so we can't verify directly, but it shouldn't crash
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, SetGetColorOnly)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	thread.setColorOnly(true);
+	// No getter, so we can't verify directly, but it shouldn't crash
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, SetGetImageDecimation)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	thread.setImageDecimation(2);
+	// No getter, so we can't verify directly, but it shouldn't crash
+	
+	thread.setImageDecimation(4);
+	thread.setImageDecimation(1);
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, SetGetHistogramMethod)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	thread.setHistogramMethod(0); // None
+	thread.setHistogramMethod(1); // EqualizeHist
+	thread.setHistogramMethod(2); // CLAHE
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, SetGetStereoToDepth)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	thread.setStereoToDepth(true);
+	thread.setStereoToDepth(false);
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, SetGetFrameRate)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	thread.setFrameRate(30.0f);
+	thread.setFrameRate(0.0f);
+	thread.setFrameRate(60.0f);
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, SetOdomAsGroundTruth)
+{
+	MockCamera * camera = new MockCamera();
+	MockCamera * odomSensor = new MockCamera();
+	Transform extrinsics = Transform::getIdentity();
+	
+	SensorCaptureThread thread(camera, odomSensor, extrinsics);
+	
+	thread.setOdomAsGroundTruth(true);
+	thread.setOdomAsGroundTruth(false);
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, EnableDisableBilateralFiltering)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	thread.enableBilateralFiltering(5.0f, 50.0f);
+	thread.disableBilateralFiltering();
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, EnableDisableIMUFiltering)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	ParametersMap params;
+	thread.enableIMUFiltering(1, params, false); // Complementary filter
+	thread.disableIMUFiltering();
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, EnableDisableFeatureDetection)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	ParametersMap params;
+	thread.enableFeatureDetection(params);
+	thread.disableFeatureDetection();
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, SetScanParameters)
+{
+	MockLidar * lidar = new MockLidar();
+	SensorCaptureThread thread(lidar);
+	
+	thread.setScanParameters(
+		false, // fromDepth
+		1,     // downsampleStep
+		0.0f,  // rangeMin
+		10.0f, // rangeMax
+		0.05f, // voxelSize
+		10,    // normalsK
+		0.1f,  // normalsRadius
+		0.8f,  // groundNormalsUp
+		true   // deskewing
+	);
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, SetScanParametersFromDepth)
+{
+	MockLidar * lidar = new MockLidar();
+	SensorCaptureThread thread(lidar);
+	
+	thread.setScanParameters(
+		true,  // fromDepth
+		2,     // downsampleStep (image decimation)
+		0.0f,  // rangeMin
+		0.0f,  // rangeMax
+		0.0f,  // voxelSize
+		0,     // normalsK
+		0.0f,  // normalsRadius
+		0.0f,  // groundNormalsUp
+		false  // deskewing
+	);
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+// Thread State Tests
+
+TEST(SensorCaptureThreadTest, InitialState)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	EXPECT_FALSE(thread.isCapturing());
+	EXPECT_TRUE(thread.isPaused());
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, StartStopThread)
+{
+	MockCamera * camera = new MockCamera();
+	camera->init(); // Initialize camera before starting thread
+	SensorCaptureThread thread(camera);
+	
+	// Start thread
+	thread.start();
+	
+	// Give it a moment to start
+	uSleep(50);
+	
+	EXPECT_TRUE(thread.isCapturing());
+	EXPECT_FALSE(thread.isPaused());
+	
+	// Stop thread
+	thread.kill();
+	thread.join();
+	
+	EXPECT_FALSE(thread.isCapturing());
+	EXPECT_TRUE(thread.isPaused());
+}
+
+TEST(SensorCaptureThreadTest, OdomProvided)
+{
+	MockCamera * camera = new MockCamera();
+	MockCamera * odomSensor = new MockCamera();
+	odomSensor->setOdomProvided(true);
+	Transform extrinsics = Transform::getIdentity();
+	
+	SensorCaptureThread thread(camera, odomSensor, extrinsics);
+	
+	EXPECT_TRUE(thread.odomProvided());
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, OdomNotProvided)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	EXPECT_FALSE(thread.odomProvided());
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+// Comprehensive Usage Test
+
+TEST(SensorCaptureThreadTest, ComprehensiveConfiguration)
+{
+	MockCamera * camera = new MockCamera();
+	ParametersMap params;
+	SensorCaptureThread thread(camera, params);
+	
+	// Configure all settings
+	thread.setMirroringEnabled(true);
+	thread.setStereoExposureCompensation(false);
+	thread.setColorOnly(false);
+	thread.setImageDecimation(2);
+	thread.setHistogramMethod(2); // CLAHE
+	thread.setStereoToDepth(false);
+	thread.setFrameRate(30.0f);
+	thread.setOdomAsGroundTruth(false);
+	thread.enableBilateralFiltering(5.0f, 50.0f);
+	
+	// Verify thread state
+	EXPECT_FALSE(thread.isCapturing());
+	EXPECT_TRUE(thread.isPaused());
+	EXPECT_EQ(thread.camera(), camera);
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+// Edge Cases
+
+TEST(SensorCaptureThreadTest, MultipleConfigurationChanges)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	// Change settings multiple times
+	for (int i = 0; i < 10; ++i)
+	{
+		thread.setImageDecimation(i % 4 + 1);
+		thread.setHistogramMethod(i % 3);
+		thread.setFrameRate((float)(i * 10));
+	}
+	
+	// Should not crash
+	EXPECT_FALSE(thread.isCapturing());
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, ZeroFrameRate)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	thread.setFrameRate(0.0f); // Unlimited
+	
+	// Should handle zero frame rate gracefully
+	EXPECT_FALSE(thread.isCapturing());
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, VeryHighFrameRate)
+{
+	MockCamera * camera = new MockCamera();
+	SensorCaptureThread thread(camera);
+	
+	thread.setFrameRate(1000.0f); // Very high frame rate
+	
+	// Should handle high frame rate gracefully
+	EXPECT_FALSE(thread.isCapturing());
+	
+	// Thread was never started, destructor will handle cleanup
+}
+
+TEST(SensorCaptureThreadTest, ScanParametersEdgeCases)
+{
+	MockLidar * lidar = new MockLidar();
+	SensorCaptureThread thread(lidar);
+	
+	// Test with all zeros (disabled features)
+	thread.setScanParameters(false, 1, 0.0f, 0.0f, 0.0f, 0, 0.0f, 0.0f, false);
+	
+	// Test with maximum values
+	thread.setScanParameters(false, 10, 100.0f, 1000.0f, 1.0f, 100, 5.0f, 1.0f, true);
+	
+	// Should not crash
+	EXPECT_FALSE(thread.isCapturing());
+	
+	// Thread was never started, destructor will handle cleanup
+}
+