Added doc for CameraModel and StereoCameraModel

corelib/include/rtabmap/core/CameraModel.h

@@ -35,21 +35,53 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class CameraModel
+ * @brief Represents a pinhole camera model containing intrinsic and extrinsic 
+ *        parameters, used for projection, rectification, and transformation.
+ *
+ * This class encapsulates camera calibration data, including intrinsic parameters (fx, fy, cx, cy),
+ * distortion coefficients, rectification and projection matrices. It provides utility functions
+ * for image rectification, projection from 2D to 3D, and vice versa.
+ * 
+ * This class supports the 4 to 14 parameters Radial Tangential distortion model (also called Plumb Bob or 
+ * Brown-Conrady model) and 4 parameters Fish Eye model (also known as Equidistant model).
+ * 
+ * @see OpenCV's calib3d module for all supported camera models.
+ */
 class RTABMAP_CORE_EXPORT CameraModel
 {
 public:
 	/**
-	 * Optical rotation used to transform image coordinate frame (x->right, y->down, z->forward)
+	 * @brief Returns the default optical rotation to convert image coordinates to robot coordinates.
-	 * to robot coordinate frame (x->forward, y->left, z->up).
+	 * 
+	 * Image frame: x -> right, y -> down, z -> forward  
+	 * Robot frame: x -> forward, y -> left, z -> up
+	 * 
+	 * @return Transform rotation matrix.
 	 */
 	static Transform opticalRotation() {return Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);}
 
 public:
+
+	/// Default constructor.
 	CameraModel();
-	// K is the camera intrinsic 3x3 CV_64FC1
+	
-	// D is the distortion coefficients 1x5 CV_64FC1
+	/**
-	// R is the rectification matrix 3x3 CV_64FC1 (computed from stereo or Identity)
+	 * @brief Constructor using full camera parameters.
-	// P is the projection matrix 3x4 CV_64FC1 (computed from stereo or equal to [K [0 0 1]'])
+	 * 
+	 * @param name           Camera name or ID.
+	 * @param imageSize      Size of the image (width x height).
+	 * @param K              Intrinsic matrix (3x3).
+	 * @param D              Distortion coefficients, 1xN matrix where N is between 4 and 14 parameters: 
+	 *                       k1,k2,p1,p2[,k3[,k4,k5,k6[,s1,s2,s3,s4[,tx,ty]]]]).
+	 *                       To set Fish Eye / Equidistant model, it is implicitly used if we 
+	 *                       provide 6 values like this: [k1,k2,0,0,k3,k4], where you only need to 
+	 *                       fill "k" parameters.
+	 * @param R              Rectification matrix (3x3).
+	 * @param P              Projection matrix (3x4).
+	 * @param localTransform Local transform to apply to the camera frame.
+	 */
 	CameraModel(
 			const std::string & name,
 			const cv::Size & imageSize,
@@ -59,7 +91,17 @@ public:
 			const cv::Mat & P,
 			const Transform & localTransform = opticalRotation());
 
-	// minimal
+	/**
+	 * @brief Minimal constructor using intrinsic parameters. This assumes the images are already rectified.
+	 *
+	 * @param fx             Focal length x.
+	 * @param fy             Focal length y.
+	 * @param cx             Principal point x.
+	 * @param cy             Principal point y.
+	 * @param localTransform Local transform to apply to the camera frame.
+	 * @param Tx             Baseline * fx (optional). Mainly used in case of stereo pair.
+	 * @param imageSize      Image size (optional).
+	 */
 	CameraModel(
 			double fx,
 			double fy,
@@ -68,7 +110,10 @@ public:
 			const Transform & localTransform = opticalRotation(),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
-	// minimal to be saved
+	
+	/**
+	 * @brief Minimal constructor with name for saving.
+	 */
 	CameraModel(
 			const std::string & name,
 			double fx,
@@ -79,13 +124,51 @@ public:
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 
+	/// Destructor.
 	virtual ~CameraModel() {}
 
+	/**
+	 * @brief Initializes the rectification maps used to undistort and rectify images.
+	 *
+	 * This function prepares the `mapX_` and `mapY_` lookup tables used for image rectification.
+	 * It supports both standard radial-tangential distortion and fisheye/equidistant distortion models.
+	 *
+	 * - If the distortion model is **fisheye** (indicated by `D_.cols == 6`), it uses
+	 *   `cv::fisheye::initUndistortRectifyMap()` to create the rectification maps. This requires OpenCV ≥ 2.4.10.
+	 * - Otherwise, it uses the standard `cv::initUndistortRectifyMap()` for plumb bob or rational polynomial models.
+	 *
+	 * @pre The camera model must be valid for rectification:
+	 *   - `imageSize_` must be non-zero.
+	 *   - `D_` must be a 1-row matrix with an accepted number of columns (4, 5, 6, 8, 12, or 14).
+	 *   - `R_` must be a 3x3 rectification matrix.
+	 *   - `P_` must be a 3x4 projection matrix.
+	 *
+	 * @return `true` if the rectification maps were successfully initialized (`mapX_` and `mapY_` are not empty),
+	 *         `false` otherwise.
+	 *
+	 * @see isRectificationMapInitialized(), rectifyImage(), rectifyDepth()
+	 *
+	 * @warning Requires OpenCV 2.4.10 or newer for fisheye support. If the version is older, fisheye rectification will not work.
+	 */
 	bool initRectificationMap();
+
+	/**
+	 * @brief Checks if the rectification map has been initialized.
+	 * @return True if both mapX_ and mapY_ are initialized.
+	 */
 	bool isRectificationMapInitialized() const {return !mapX_.empty() && !mapY_.empty();}
 
+	/**
+	 * @brief Checks if the model is valid for 2D->3D projection.
+	 */
 	bool isValidForProjection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0;}
+	/**
+	 * @brief Checks if the model is valid for 3D->2D reprojection.
+	 */
 	bool isValidForReprojection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0 && imageWidth()>0 && imageHeight()>0;}
+	/**
+	 * @brief Checks if the model has sufficient data for image rectification.
+	 */
 	bool isValidForRectification() const
 	{
 		return imageSize_.width>0 &&
@@ -96,69 +179,408 @@ public:
 			   !P_.empty();
 	}
 
+	/// Sets the camera name, used to set a camera name when saving to a file.
 	void setName(const std::string & name) {name_=name;}
+	/// Returns the camera name.
 	const std::string & name() const {return name_;}
 
+	/// Returns focal length in x.
 	double fx() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(0,0):P_.at<double>(0,0);}
+	/// Returns focal length in y.
 	double fy() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(1,1):P_.at<double>(1,1);}
+	/// Returns principal point x.
 	double cx() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(0,2):P_.at<double>(0,2);}
+	/// Returns principal point y.
 	double cy() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(1,2):P_.at<double>(1,2);}
+	/// Returns the x translation (usually fx * baseline in case of stereo, otherwise would be 0).
 	double Tx() const {return P_.empty()?0.0:P_.at<double>(0,3);}
 
-	cv::Mat K_raw() const {return K_;} //intrinsic camera matrix (before rectification)
+	/// Returns the raw intrinsic matrix (before rectification).
-	cv::Mat D_raw() const {return D_;} //intrinsic distorsion matrix (before rectification)
+	cv::Mat K_raw() const {return K_;}
-	cv::Mat K() const {return !P_.empty()?P_.colRange(0,3):K_;} // if P exists, return rectified version
+	/// Returns the raw distortion coefficients (before rectification).
+	cv::Mat D_raw() const {return D_;} 
+	/// Returns the rectified camera intrinsic matrix if the projection matrix P exists, otherwise returns the raw intrinsic matrix.
+	cv::Mat K() const {return !P_.empty()?P_.colRange(0,3):K_;}
+	/// Returns the rectified distortion coefficients (1x5 filled with zeros) if the projection matrix P exists, otherwise returns the raw distortion coefficients.
 	cv::Mat D() const {return P_.empty()&&!D_.empty()?D_:cv::Mat::zeros(1,5,CV_64FC1);} // if P exists, return rectified version
-	cv::Mat R() const {return R_;} //rectification matrix
+	/// Returns the rectification matrix.
-	cv::Mat P() const {return P_;} //projection matrix
+	cv::Mat R() const {return R_;}
+	/// Returns the projection matrix.
+	cv::Mat P() const {return P_;}
 
+	/// Sets the local transform of the camera (base frame to optical frame).
 	void setLocalTransform(const Transform & transform) {localTransform_ = transform;}
+	/// Returns the local transform (base frame to optical frame).
 	const Transform & localTransform() const {return localTransform_;}
 
+	/**
+	 * @brief Sets the image size of the camera model and updates the principal point if undefined.
+	 *
+	 * This function updates the internal image size (`imageSize_`) with the provided size.
+	 * If the intrinsic matrices (`K_` or `P_`) are present and the principal point coordinates
+	 * (`cx`, `cy`) are zero, they are set to the image center (`width/2 - 0.5`, `height/2 - 0.5`).
+	 *
+	 * This ensures the camera model remains valid and useful even when the calibration file
+	 * has no principal point set or the image size is updated manually.
+	 *
+	 * @param size The new image size. It must be either both dimensions zero (clearing) or both positive.
+	 *
+	 * @pre `size.width > 0 && size.height > 0` or `size.width == 0 && size.height == 0`
+	 * @post Updates the `imageSize_`, and adjusts `cx` and `cy` in `K_` and `P_` if they were initially zero.
+	 *
+	 * @warning If `K_` or `P_` are not initialized (`empty()`), no updates will be applied to them.
+	 *
+	 * @see imageSize(), imageWidth(), imageHeight()
+	 */
 	void setImageSize(const cv::Size & size);
 	const cv::Size & imageSize() const {return imageSize_;}
 	int imageWidth() const {return imageSize_.width;}
 	int imageHeight() const {return imageSize_.height;}
 
-	double fovX() const; // in radians
+	/**
-	double fovY() const; // in radians
+	 * @brief Returns the horizontal field of view (FoV) in radians.
-	double horizontalFOV() const; // in degrees
+	 *
-	double verticalFOV() const;   // in degrees
+	 * The FoV is computed using the pinhole camera model as:
+	 * \f[
+	 * \text{FoV}_x = 2 \cdot \tan^{-1}\left(\frac{\text{image width}}{2 \cdot f_x}\right)
+	 * \f]
+	 *
+	 * @return Horizontal field of view in radians. Returns 0.0 if image width or focal length is invalid.
+	 */
+	double fovX() const;
+
+	/**
+	 * @brief Returns the vertical field of view (FoV) in radians.
+	 *
+	 * The FoV is computed using the pinhole camera model as:
+	 * \f[
+	 * \text{FoV}_y = 2 \cdot \tan^{-1}\left(\frac{\text{image height}}{2 \cdot f_y}\right)
+	 * \f]
+	 *
+	 * @return Vertical field of view in radians. Returns 0.0 if image height or focal length is invalid.
+	 */
+	double fovY() const;
+
+	/**
+	 * @brief Returns the horizontal field of view in degrees.
+	 *
+	 * Converts the result of `fovX()` from radians to degrees.
+	 *
+	 * @return Horizontal field of view in degrees. Returns 0.0 if the result is invalid.
+	 */
+	double horizontalFOV() const;
+	
+	/**
+	 * @brief Returns the vertical field of view in degrees.
+	 *
+	 * Converts the result of `fovY()` from radians to degrees.
+	 *
+	 * @return Vertical field of view in degrees. Returns 0.0 if the result is invalid.
+	 */
+	double verticalFOV() const;
+
+	/// Checks if the distortion model is fisheye (6 coefficients: k1,k2,0,0,k3,k4).
 	bool isFisheye() const {return D_.cols == 6;}
 
+	/**
+	 * @brief Loads the camera model parameters from a YAML calibration file.
+	 *
+	 * This method attempts to read camera intrinsic/extrinsic parameters and image size from a YAML file,
+	 * typically in the ROS calibration format. If the distortion model is "fisheye" or "equidistant", we expect
+	 * 4 coefficients, which are converted to a 6-coefficient format for internal representation.
+	 *
+	 * Fields loaded (if present):  
+	 * - `camera_name`  
+	 * - `image_width` (pixels)
+	 * - `image_height` (pixels)
+	 * - `camera_matrix` (K, 3x3 double matrix)  
+	 * - `distortion_coefficients` (D, 1xN double matrix)  
+	 * - `distortion_model`  (string: name of the model)
+	 * - `rectification_matrix` (R, 3x3 double matrix)  
+	 * - `projection_matrix` (P, 3x4 double matrix)  
+	 * - `local_transform` (camera pose w.r.t robot frame)
+	 *
+	 * On success, the internal matrices and settings of the camera model are updated. If the model
+	 * is valid for rectification, the rectification map is initialized.
+	 *
+	 * @param filePath Absolute or relative path to the YAML file.
+	 * @return True if the file was successfully loaded and parsed, false otherwise.
+	 *
+	 * @warning Logs warnings if any fields are missing. If file does not exist or parsing fails, returns false.
+	 */
 	bool load(const std::string & filePath);
+
+	/**
+	 * @brief Loads the camera model by constructing a file path from a directory and camera name.
+	 *
+	 * This is a convenience wrapper around `load(filePath)` that constructs the file path as:
+	 * `directory + "/" + cameraName + ".yaml"`.
+	 *
+	 * @param directory Path to the folder containing the camera YAML file.
+	 * @param cameraName Base name of the camera file (without extension).
+	 * @return True if loading from the constructed path succeeds, false otherwise.
+	 */
 	bool load(const std::string & directory, const std::string & cameraName);
+
+	/**
+	 * @brief Saves the camera model parameters to a YAML calibration file in ROS format.
+	 *
+	 * The file will include the following fields if they are not empty:
+	 * - `camera_name`
+	 * - `image_width`
+	 * - `image_height`
+	 * - `camera_matrix` (K)
+	 * - `distortion_coefficients` (D)
+	 * - `distortion_model` (auto-detected based on number of distortion coefficients)
+	 * - `rectification_matrix` (R)
+	 * - `projection_matrix` (P)
+	 * - `local_transform` (camera pose w.r.t robot frame)
+	 *
+	 * If the distortion matrix contains 6 coefficients (used for fisheye), it is converted
+	 * to a standard 4-coefficient format for ROS compatibility.
+	 *
+	 * @param directory Path to the folder where the YAML file will be saved.
+	 * @return True if saving was successful, false otherwise.
+	 *
+	 * @note If `name_` is empty, "camera.yaml" is used as the default filename.
+	 * @warning Returns false and logs an error if none of the matrices are set.
+	 */
 	bool save(const std::string & directory) const;
+
+	/**
+	 * @brief Serializes the camera model to a binary format.
+	 *
+	 * The serialization includes the camera intrinsics (`K_`, `D_`), rectification matrix (`R_`), 
+	 * projection matrix (`P_`), image size, and the local transform. The format is compact and suitable 
+	 * for file storage or transmission over a network.
+	 *
+	 * Data layout:
+	 * - Header (11 integers):
+	 *   - [0-2]  RTAB-Map version (major, minor, patch)
+	 *   - [3]    Camera type (0 = mono, 1=stereo)
+	 *   - [4-5]  Image width, height
+	 *   - [6-9]  Element counts for K, D, R, P matrices
+	 *   - [10]   Size of localTransform (0 if null)
+	 * - Data section (in order): raw memory blocks for K, D, R, P (`double` values), followed by `float` values for localTransform
+	 *
+	 * @return A byte vector containing the serialized data. The format is compatible with `deserialize()`.
+	 *
+	 * @note This is a custom binary format, not meant to be human-readable.
+	 * @see deserialize(), StereoCameraModel
+	 */
 	std::vector<unsigned char> serialize() const;
+	/**
+	 * @brief Deserializes a camera model from a byte vector.
+	 *
+	 * This is a convenience wrapper around `deserialize(const unsigned char*, unsigned int)` 
+	 * that takes a `std::vector<unsigned char>` instead of a raw buffer.
+	 *
+	 * @param data Byte vector containing data serialized by `serialize()`.
+	 * @return The number of bytes successfully read and parsed. Returns 0 on failure.
+	 *
+	 * @see serialize(), deserialize(const unsigned char*, unsigned int)
+	 */
 	unsigned int deserialize(const std::vector<unsigned char>& data);
+
+	/**
+	 * @brief Deserializes a camera model from a raw byte buffer.
+	 *
+	 * Reads the camera intrinsics, distortion, rectification, projection matrices, image size, 
+	 * and local transform from a serialized binary format previously created with `serialize()`.
+	 *
+	 * @param data Pointer to the binary data buffer.
+	 * @param dataSize Size of the data buffer in bytes.
+	 * @return The number of bytes successfully read. Returns 0 on error or if the format is invalid.
+	 *
+	 * @warning If the buffer format does not match the expected layout or version, an error is logged 
+	 *          and the camera model remains in a default-initialized state.
+	 *
+	 * @note Assumes little-endian architecture and strict size/type matching. The serialized format 
+	 *       must be created by `CameraModel::serialize()`. Non-mono camera types are not supported. 
+	 *       See `StereoCameraModel` to serialize/deserialize stereo models.
+	 *
+	 * @see serialize()
+	 */
 	unsigned int deserialize(const unsigned char * data, unsigned int dataSize);
 
+	/**
+	 * @brief Returns a new camera model with all intrinsic parameters scaled by a given factor.
+	 *
+	 * This method scales the camera's intrinsic matrix (`K_`) and projection matrix (`P_`), as well as 
+	 * the image size, by the given `scale` factor. The distortion and rectification matrices are left unchanged.
+	 *
+	 * Only valid camera models (i.e., those for which `isValidForProjection()` returns true) are scaled.
+	 * If the model is invalid, a warning is issued and the original model is returned unchanged.
+	 *
+	 * @param scale Scaling factor (> 0). For example, use 0.5 to downscale or 2.0 to upscale.
+	 * @return A scaled copy of the camera model with updated intrinsics and image size.
+	 *
+	 * @warning If the camera model is not valid for projection, the scale operation is ignored.
+	 */
 	CameraModel scaled(double scale) const;
+
+	/**
+	 * @brief Returns a new camera model adjusted for a given region of interest (ROI).
+	 *
+	 * This method shifts the principal point (`cx`, `cy`) in the intrinsic matrix (`K_`) and projection matrix (`P_`)
+	 * by subtracting the ROI’s top-left `(x, y)` offset. The image size is also set to the ROI size.
+	 *
+	 * Only valid camera models (i.e., those for which `isValidForProjection()` returns true) can be adjusted.
+	 * If the model is invalid, a warning is issued and the original model is returned unchanged.
+	 *
+	 * @param roi Region of interest defined as a rectangle (typically a subwindow of the full image).
+	 * @return A new camera model adapted to the ROI with adjusted intrinsics and image size.
+	 *
+	 * @warning If the camera model is not valid for projection, the ROI operation is ignored.
+	 */
 	CameraModel roi(const cv::Rect & roi) const;
 
-	// For depth images, your should use cv::INTER_NEAREST
+	/**
+	 * @brief Rectifies a raw image using the precomputed rectification maps.
+	 *
+	 * This function applies geometric correction (rectification) to an image using the camera model's
+	 * `mapX_` and `mapY_` rectification maps. It is typically used to correct lens distortion in images
+	 * based on the calibration parameters.
+	 *
+	 * @param raw Input raw image (e.g., from camera). Must be a valid `cv::Mat`.
+	 * @param interpolation Interpolation method to use. Typically `cv::INTER_LINEAR` or `cv::INTER_NEAREST`.
+	 *
+	 * @return Rectified image. If the rectification maps are not initialized, the function logs an error
+	 *         and returns a clone of the original image.
+	 *
+	 * @pre `mapX_` and `mapY_` must be initialized using `initRectificationMap()`.
+	 *
+	 * @note Works for color and grayscale images of any valid type.
+	 *
+	 * @see initRectificationMap(), rectifyDepth()
+	 */
 	cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
+
+	/**
+	 * @brief Rectifies a raw depth image using the precomputed rectification maps.
+	 *
+	 * This function applies geometric correction (rectification) to a 16-bit unsigned depth image.
+	 * It performs a pixel-by-pixel bilinear interpolation, only if all neighboring pixels have valid
+	 * (non-zero) depth values, and the variation among them is within 1% of their average.
+	 *
+	 * The method is optimized to avoid introducing noise in regions of high depth variance.
+	 *
+	 * @param raw Input raw depth image (`CV_16UC1`). Must contain 16-bit unsigned depth values.
+	 *
+	 * @return Rectified depth image. If the rectification maps are not initialized or the input
+	 *         image is not of type `CV_16UC1`, the function logs an error and returns a clone of the input.
+	 *
+	 * @pre Input image must be of type `CV_16UC1`. `mapX_` and `mapY_` must be initialized.
+	 *
+	 * @note Inspired by the Kinect2 CPU depth registration implementation from:
+	 *       https://github.com/code-iai/iai_kinect2
+	 *
+	 * @warning Invalid or noisy regions are skipped in interpolation to maintain depth consistency.
+	 *
+	 * @see initRectificationMap(), rectifyImage()
+	 */
 	cv::Mat rectifyDepth(const cv::Mat & raw) const;
 
-	// Project 2D pixel to 3D (in /camera_link frame)
+	/**
+	 * @brief Projects a 2D pixel and depth value into a 3D point in the camera coordinate frame (/camera_link).
+	 *
+	 * This function uses the camera's intrinsic parameters to compute the 3D point corresponding to the given
+	 * 2D image coordinates and depth value.
+	 *
+	 * @param u Horizontal image coordinate (in pixels).
+	 * @param v Vertical image coordinate (in pixels).
+	 * @param depth Depth value at (u, v) in meters.
+	 * @param[out] x Output X coordinate in 3D space.
+	 * @param[out] y Output Y coordinate in 3D space.
+	 * @param[out] z Output Z coordinate in 3D space (equals `depth`).
+	 *
+	 * @note If `depth <= 0`, the output (x, y, z) will be set to `NaN`.
+	 *
+	 * @see reproject()
+	 */
 	void project(float u, float v, float depth, float & x, float & y, float & z) const;
-	// Reproject 3D point (in /camera_link frame) to pixel
+	
+	/**
+	 * @brief Reprojects a 3D point in the camera frame (/camera_link) into 2D image coordinates (floating-point).
+	 *
+	 * This function computes the image plane coordinates for a given 3D point using the camera's
+	 * intrinsic parameters.
+	 *
+	 * @param x X coordinate in camera space.
+	 * @param y Y coordinate in camera space.
+	 * @param z Z coordinate in camera space (must be non-zero).
+	 * @param[out] u Output horizontal image coordinate (float).
+	 * @param[out] v Output vertical image coordinate (float).
+	 *
+	 * @pre `z != 0`
+	 *
+	 * @see project(), reproject(int&, int&)
+	 */
 	void reproject(float x, float y, float z, float & u, float & v) const;
+
+	/**
+	 * @brief Reprojects a 3D point in the camera frame (/camera_link) into 2D image coordinates (rounded to int).
+	 *
+	 * This version of `reproject()` returns integer pixel indices, computed from the 3D position.
+	 *
+	 * @param x X coordinate in camera space.
+	 * @param y Y coordinate in camera space.
+	 * @param z Z coordinate in camera space (must be non-zero).
+	 * @param[out] u Output horizontal image coordinate (integer pixel).
+	 * @param[out] v Output vertical image coordinate (integer pixel).
+	 *
+	 * @pre `z != 0`
+	 *
+	 * @see project(), reproject(float&, float&)
+	 */
 	void reproject(float x, float y, float z, int & u, int & v) const;
+
+	/**
+	 * @brief Checks if a given pixel coordinate lies within the image bounds.
+	 *
+	 * @param u Horizontal image coordinate (in pixels).
+	 * @param v Vertical image coordinate (in pixels).
+	 * @return `true` if the pixel is within the image dimensions, `false` otherwise.
+	 *
+	 * @note Inclusive lower bound, exclusive upper bound: `[0, width)`, `[0, height)`
+	 */
 	bool inFrame(int u, int v) const;
 
 private:
-	std::string name_;
+	std::string name_;         ///< Camera name.
-	cv::Size imageSize_;
+	cv::Size imageSize_;       ///< Image size.
-	cv::Mat K_;
+	cv::Mat K_;                ///< Intrinsic matrix.
-	cv::Mat D_;
+	cv::Mat D_;                ///< Distortion coefficients.
-	cv::Mat R_;
+	cv::Mat R_;                ///< Rectification matrix.
-	cv::Mat P_;
+	cv::Mat P_;                ///< Projection matrix.
-	cv::Mat mapX_;
+	cv::Mat mapX_;             ///< Rectification map X.
-	cv::Mat mapY_;
+	cv::Mat mapY_;             ///< Rectification map Y.
-	Transform localTransform_;
+	Transform localTransform_; ///< Transform from camera to base link.
 };
 
+/**
+ * @brief Stream operator for printing a camera model to an output stream.
+ *
+ * This function outputs the name, image size, and camera matrices (K, D, R, P)
+ * along with the local transformation.
+ *
+ * Example output:
+ * ```
+ * Name: camera1
+ * Size: 640x480
+ * K= [fx,  0, cx;
+ *      0, fy, cy;
+ *      0,  0,  1]
+ * D= [...]
+ * R= [...]
+ * P= [...]
+ * LocalTransform= [...]
+ * ```
+ *
+ * @param os Output stream.
+ * @param model Camera model to print.
+ * @return The modified output stream.
+ */
 RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const CameraModel& model);
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -32,10 +32,63 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class StereoCameraModel
+ * @brief A class representing a calibrated stereo camera system.
+ *
+ * This class encapsulates the calibration data and operations associated with a stereo camera setup,
+ * including intrinsic and extrinsic parameters for both left and right cameras, stereo rectification, 
+ * and methods for computing depth or disparity from stereo images.
+ *
+ * It relies internally on two `CameraModel` instances for the left and right cameras.
+ *
+ * Typical uses include:
+ * - Stereo rectification
+ * - Stereo disparity-to-depth conversion
+ * - Saving and loading stereo camera calibration data
+ * - Projecting or reprojecting points
+ *
+ * @see CameraModel
+ */
 class RTABMAP_CORE_EXPORT StereoCameraModel
 {
 public:
+	/**
+     * @brief Default constructor. Creates an empty stereo model with default suffixes ("left", "right").
+     */
 	StereoCameraModel() : leftSuffix_("left"), rightSuffix_("right") {}
+
+	/**
+	 * @brief Constructs a StereoCameraModel from detailed intrinsic and extrinsic parameters for both cameras.
+	 * 
+	 * Initializes the stereo camera model by specifying the calibration parameters for the left and right cameras,
+	 * along with the stereo extrinsic parameters.
+	 * 
+	 * @param name Name identifier for the stereo camera.
+	 * @param imageSize1 Image size (width, height) of the left camera.
+	 * @param K1 Intrinsic camera matrix (3x3, CV_64FC1) for the left camera.
+	 * @param D1 Distortion coefficients for the left camera.
+	 * @param R1 Rectification matrix (3x3, CV_64FC1) for the left camera.
+	 * @param P1 Projection matrix (3x4, CV_64FC1) for the left camera.
+	 * @param imageSize2 Image size (width, height) of the right camera.
+	 * @param K2 Intrinsic camera matrix (3x3, CV_64FC1) for the right camera.
+	 * @param D2 Distortion coefficients for the right camera.
+	 * @param R2 Rectification matrix (3x3, CV_64FC1) for the right camera.
+	 * @param P2 Projection matrix (3x4, CV_64FC1) for the right camera.
+	 * @param R Rotation matrix (3x3, CV_64FC1) representing the rotation from left to right camera coordinate system.
+	 *          Can be empty if unknown.
+	 * @param T Translation vector (3x1, CV_64FC1) representing the translation from left to right camera coordinate system.
+	 *          Can be empty if unknown.
+	 * @param E Essential matrix (3x3, CV_64FC1) encoding the stereo camera epipolar geometry.
+	 *          Can be empty if unknown.
+	 * @param F Fundamental matrix (3x3, CV_64FC1) encoding the stereo camera epipolar constraints.
+	 *          Can be empty if unknown.
+	 * @param localTransform The local transform associated with the stereo camera model.
+	 * 
+	 * @note All matrices must have correct sizes and types as specified.
+	 *       The rectification and projection matrices (R1, P1, R2, P2) are used to define the stereo rectification parameters.
+	 *       The rotation and translation (R, T) define the relative pose between the cameras.
+	 */
 	StereoCameraModel(
 			const std::string & name,
 			const cv::Size & imageSize1,
@@ -45,7 +98,29 @@ public:
 			const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
 			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
 
-	// if R and T are not null, left and right camera models should be valid to be rectified.
+	/**
+	 * @brief Constructs a StereoCameraModel from two individual camera models and optional stereo extrinsic parameters.
+	 *
+	 * This constructor initializes the stereo camera model by assigning the provided left and right camera models.
+	 * If the stereo extrinsics (`R`, `T`) are provided and valid, stereo rectification will be attempted—provided both
+	 * cameras are valid for rectification and their image dimensions match.
+	 *
+	 * Each camera model will automatically have its name updated using the `name` parameter and default suffixes ("left", "right").
+	 *
+	 * @param name             The base name for the stereo camera model.
+	 * @param leftCameraModel  The camera model representing the left camera.
+	 * @param rightCameraModel The camera model representing the right camera.
+	 * @param R                (Optional) Rotation matrix from left to right camera (3x3, CV_64FC1).
+	 * @param T                (Optional) Translation vector from left to right camera (3x1, CV_64FC1).
+	 * @param E                (Optional) Essential matrix between the two cameras (3x3, CV_64FC1).
+	 * @param F                (Optional) Fundamental matrix between the two cameras (3x3, CV_64FC1).
+	 *
+	 * @throws UException if any of the provided matrices (`R`, `T`, `E`, `F`) are non-empty and not of the expected type/shape.
+	 * @throws UException if `R` and `T` are provided but the camera models are not valid for rectification.
+	 * 
+	 * @note Stereo rectification is only attempted if both `R` and `T` are non-empty, the cameras are valid, and their image sizes match.
+	 * @see updateStereoRectification()
+	 */
 	StereoCameraModel(
 			const std::string & name,
 			const CameraModel & leftCameraModel,
@@ -54,14 +129,39 @@ public:
 			const cv::Mat & T = cv::Mat(),
 			const cv::Mat & E = cv::Mat(),
 			const cv::Mat & F = cv::Mat());
-	// if extrinsics transform is not null, left and right camera models should be valid to be rectified.
+	
+	/**
+	 * @brief Constructs a StereoCameraModel from two camera models and an extrinsic Transform between them.
+	 *
+	 * This constructor sets up a stereo camera model using the given left and right camera models along with
+	 * an optional 3D transform (`extrinsics`) representing the pose of the right camera relative to the left camera.
+	 * 
+	 * If a valid (non-null) transform is provided, the corresponding rotation and translation matrices are extracted
+	 * and stored as the stereo extrinsic parameters. Stereo rectification will be attempted if both camera models
+	 * are valid for rectification and their image sizes match.
+	 *
+	 * Each camera model will be renamed using the provided `name` and default suffixes ("left", "right").
+	 *
+	 * @param name         Base name for the stereo camera model.
+	 * @param leftCameraModel  Camera model for the left camera.
+	 * @param rightCameraModel Camera model for the right camera.
+	 * @param extrinsics   (Optional) Transform from the left camera to the right camera. If null, no extrinsics are used.
+	 *
+	 * @throws UException if `extrinsics` is not null and either camera model is not valid for rectification.
+	 * 
+	 * @note Stereo rectification is performed only when `extrinsics` is valid and both camera models are rectifiable
+	 *       with matching image dimensions.
+	 * @see updateStereoRectification()
+	 */
 	StereoCameraModel(
 			const std::string & name,
 			const CameraModel & leftCameraModel,
 			const CameraModel & rightCameraModel,
 			const Transform & extrinsics);
 
-	//minimal
+	/**
+     * @brief Minimal constructor using focal lengths and baseline only.
+     */
 	StereoCameraModel(
 			double fx,
 			double fy,
@@ -70,7 +170,9 @@ public:
 			double baseline,
 			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			const cv::Size & imageSize = cv::Size(0,0));
-	//minimal to be saved
+	/**
+     * @brief Minimal constructor that also sets a name, required if we want to save it to a file.
+     */
 	StereoCameraModel(
 			const std::string & name,
 			double fx,
@@ -80,66 +182,278 @@ public:
 			double baseline,
 			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
 			const cv::Size & imageSize = cv::Size(0,0));
+
+	/**
+     * @brief Destructor.
+     */
 	virtual ~StereoCameraModel() {}
 
+	/**
+     * @brief Returns true if both left and right models are valid for projection and the baseline is positive.
+     */
 	bool isValidForProjection() const {return left_.isValidForProjection() && right_.isValidForProjection() && baseline() > 0.0;}
+	
+	/**
+     * @brief Returns true if both left and right models are valid for rectification.
+     */
 	bool isValidForRectification() const {return left_.isValidForRectification() && right_.isValidForRectification();}
 
+	/**
+     * @brief Initializes the rectification maps for both cameras.
+     */
 	void initRectificationMap() {left_.initRectificationMap(); right_.initRectificationMap();}
+
+	/**
+     * @brief Returns true if rectification maps are initialized.
+     */
 	bool isRectificationMapInitialized() const {return left_.isRectificationMapInitialized() && right_.isRectificationMapInitialized();}
 
+	/**
+     * @brief Sets the camera name and optional image suffixes for the left and right cameras.
+     */
 	void setName(const std::string & name, const std::string & leftSuffix = "left", const std::string & rightSuffix = "right");
+	
+	/**
+     * @brief Gets the camera name.
+     */
 	const std::string & name() const {return name_;}
 
-	// backward compatibility
+	/**
+     * @brief Sets the image size for both left and right cameras.
+     */
 	void setImageSize(const cv::Size & size) {left_.setImageSize(size); right_.setImageSize(size);}
 
+	/**
+	 * @brief Loads stereo camera calibration data from disk.
+	 *
+	 * This method loads the intrinsic parameters for both the left and right cameras from files in the specified directory,
+	 * using the provided camera name and internal suffixes. If `ignoreStereoTransform` is false, it also attempts to load
+	 * the stereo extrinsic parameters (rotation, translation, essential, and fundamental matrices) from a YAML file.
+	 *
+	 * The stereo extrinsics are expected in the file:
+	 * `directory/cameraName_pose.yaml`, following the ROS calibration format.
+	 *
+	 * @param directory The directory where the calibration files are located.
+	 * @param cameraName The base name of the stereo camera (used to derive filenames).
+	 * @param ignoreStereoTransform If true, skips loading stereo extrinsic parameters.
+	 * @return true if loading is successful, false otherwise.
+	 *
+	 * @see save(), saveStereoTransform()
+	 */
 	bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
+
+	/**
+	 * @brief Saves stereo camera calibration data to disk.
+	 *
+	 * This method saves the intrinsic parameters of both left and right cameras to the specified directory.
+	 * If `ignoreStereoTransform` is false, it also saves the stereo extrinsic parameters (rotation, translation,
+	 * essential, and fundamental matrices) in a ROS-compatible YAML file named `cameraName_pose.yaml`.
+	 *
+	 * @param directory The directory where calibration files should be saved.
+	 * @param ignoreStereoTransform If true, skips saving stereo extrinsic parameters.
+	 * @return true if saving was successful, false otherwise.
+	 *
+	 * @see load(), saveStereoTransform()
+	 */
 	bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
+
+	/**
+	 * @brief Saves stereo extrinsic parameters to a YAML file in ROS format.
+	 *
+	 * This method exports the stereo transform, including rotation, translation, essential, and fundamental matrices,
+	 * into a YAML file named `cameraName_pose.yaml` located in the specified directory.
+	 * The file format is compatible with ROS camera calibration tools.
+	 *
+	 * @param directory The target directory for saving the calibration file.
+	 * @return true if saving was successful, false if required matrices are missing or invalid.
+	 *
+	 * @warning If extrinsics (`R_`, `T_`, `E_`, `F_`) are empty or invalid, nothing will be saved and a warning is printed.
+	 *
+	 * @see load(), save()
+	 */
 	bool saveStereoTransform(const std::string & directory) const;
+
+	/**
+	 * @brief Serializes the stereo camera model into a byte vector.
+	 *
+	 * This method serializes the left and right camera models along with the stereo extrinsic parameters
+	 * (rotation matrix R_, translation vector T_, essential matrix E_, and fundamental matrix F_) into a
+	 * contiguous byte array. The serialization format starts with a fixed-size integer header containing
+	 * version info, stereo type, matrix sizes, and serialized data sizes, followed by the actual matrices and
+	 * serialized camera data.
+	 *
+	 * The serialized data can later be restored using the corresponding `deserialize()` method.
+	 *
+	 * @return A vector of unsigned char containing the serialized stereo camera data.
+	 */
 	std::vector<unsigned char> serialize() const;
+
+	/**
+	 * @brief Deserializes stereo camera model data from a byte vector.
+	 *
+	 * This method wraps the pointer-based `deserialize()` and attempts to restore the stereo camera
+	 * model from the given serialized byte vector.
+	 *
+	 * @param data The vector of bytes containing previously serialized stereo camera model data.
+	 * @return The number of bytes read from the data if successful, 0 otherwise.
+	 *
+	 * @see deserialize(const unsigned char*, unsigned int)
+	 */
 	unsigned int deserialize(const std::vector<unsigned char>& data);
+
+	/**
+	 * @brief Deserializes stereo camera model data from a raw byte array.
+	 *
+	 * This method reconstructs the stereo camera model from the provided serialized data buffer.
+	 * It expects the data format to match the one produced by `serialize()`, including a header with
+	 * version info, matrix sizes, and data sizes, followed by the serialized extrinsic matrices and
+	 * serialized left and right camera data.
+	 *
+	 * The method performs various sanity checks on data sizes and matrix dimensions and will fail if
+	 * the data format or sizes are inconsistent.
+	 *
+	 * @param data Pointer to the raw serialized data buffer.
+	 * @param dataSize Size in bytes of the data buffer.
+	 * @return The number of bytes consumed during deserialization if successful, or 0 on failure.
+	 *
+	 * @warning The stereo camera model is reset to a default empty state before deserialization.
+	 * @warning If the serialized data type is not stereo (type != 1), deserialization will fail.
+	 *
+	 * @see serialize()
+	 */
 	unsigned int deserialize(const unsigned char * data, unsigned int dataSize);
 
+	/**
+     * @brief Returns the stereo baseline in meters.
+     */
 	double baseline() const {return right_.fx()!=0.0 && left_.fx() != 0.0 ? left_.Tx() / left_.fx() - right_.Tx()/right_.fx():0.0;}
 
+	/**
+	 * @brief Computes the depth (Z coordinate) from a given disparity value.
+	 *
+	 * Uses the stereo camera model parameters to convert disparity to depth using the formula:
+	 * \f[
+	 *     \text{depth} = \frac{\text{baseline} \times f_x}{\text{disparity} + (c_{x_{right}} - c_{x_{left}})}
+	 * \f]
+	 * where \( f_x \) is the focal length of the left camera and \( c_x \) are principal points.
+	 *
+	 * @param disparity The disparity value (difference in pixel coordinates between left and right images).
+	 * @return The computed depth in the same unit as the baseline (typically meters).
+	 *         Returns 0 if disparity is zero or if the model is not valid for projection.
+	 *
+	 * @note This function requires the stereo camera to be valid for projection (i.e., calibrated and rectified).
+	 */
 	float computeDepth(float disparity) const;
+
+	/**
+	 * @brief Computes the disparity value from a given depth.
+	 *
+	 * Converts depth back to disparity using the inverse formula:
+	 * \f[
+	 *     \text{disparity} = \frac{\text{baseline} \times f_x}{\text{depth}} - (c_{x_{right}} - c_{x_{left}})
+	 * \f]
+	 *
+	 * @param depth Depth value in the same unit as the baseline (typically meters).
+	 * @return The computed disparity in pixels.
+	 *         Returns 0 if depth is zero or if the model is not valid for projection.
+	 *
+	 * @note This function requires the stereo camera to be valid for projection (i.e., calibrated and rectified).
+	 */
 	float computeDisparity(float depth) const; // m
+
+	/**
+	 * @brief Computes the disparity value from a depth given in unsigned short format (millimeters).
+	 *
+	 * Converts depth expressed as an unsigned short (in millimeters) to disparity.
+	 * The depth is first converted to meters before computing disparity using the formula:
+	 * \f[
+	 *     \text{disparity} = \frac{\text{baseline} \times f_x}{\text{depth (meters)}} - (c_{x_{right}} - c_{x_{left}})
+	 * \f]
+	 *
+	 * @param depth Depth value in millimeters as an unsigned short.
+	 * @return The computed disparity in pixels.
+	 *         Returns 0 if depth is zero or if the model is not valid for projection.
+	 *
+	 * @note This function requires the stereo camera to be valid for projection (i.e., calibrated and rectified).
+	 */
 	float computeDisparity(unsigned short depth) const; // mm
 
-	const cv::Mat & R() const {return R_;} //extrinsic rotation matrix
+	const cv::Mat & R() const {return R_;} ///< Stereo extrinsic rotation matrix.
-	const cv::Mat & T() const {return T_;} //extrinsic translation matrix
+	const cv::Mat & T() const {return T_;} ///< Stereo extrinsic translation vector.
-	const cv::Mat & E() const {return E_;} //extrinsic essential matrix
+	const cv::Mat & E() const {return E_;} ///< Essential matrix.
-	const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix
+	const cv::Mat & F() const {return F_;} ///< Fundamental matrix
 
+	/**
+     * @brief Scales both cameras' calibration by a factor.
+     */
 	void scale(double scale);
+	/**
+     * @brief Applies region-of-interest (ROI) cropping to both cameras.
+     */
 	void roi(const cv::Rect & roi);
 
+	/**
+     * @brief Sets the local transform from left camera to robot base.
+     */
 	void setLocalTransform(const Transform & transform) {left_.setLocalTransform(transform);}
+
+	/**
+     * @brief Gets the local transform from left camera to robot base.
+     */
 	const Transform & localTransform() const {return left_.localTransform();}
+
+	/**
+     * @brief Returns the stereo transform (right camera relative to left).
+     */
 	Transform stereoTransform() const;
 
+	/**
+     * @brief Returns the left camera model.
+     */
 	const CameraModel & left() const {return left_;}
+	/**
+     * @brief Returns the right camera model.
+     */
 	const CameraModel & right() const {return right_;}
 
+	/**
+     * @brief Gets the suffix used for the left camera calibration file.
+     */
 	const std::string & getLeftSuffix() const {return leftSuffix_;}
+	/**
+     * @brief Gets the suffix used for the right camera calibration file.
+     */
 	const std::string & getRightSuffix() const {return rightSuffix_;}
 
 private:
 	void updateStereoRectification();
 
 private:
-	std::string leftSuffix_;
+	std::string leftSuffix_;   ///< Suffix for the left calibration file.
-	std::string rightSuffix_;
+	std::string rightSuffix_;  ///< Suffix for the right calibration file.
-	CameraModel left_;
+	CameraModel left_;         ///< Left camera model.
-	CameraModel right_;
+	CameraModel right_;        ///< Right camera model.
-	std::string name_;
+	std::string name_;         ///< Model name or ID.
-	cv::Mat R_;
+
-	cv::Mat T_;
+	cv::Mat R_; ///< Rotation matrix between cameras.
-	cv::Mat E_;
+	cv::Mat T_; ///< Translation vector between cameras.
-	cv::Mat F_;
+	cv::Mat E_; ///< Essential matrix.
+	cv::Mat F_; ///< Fundamental matrix.
 };
 
+/**
+ * @brief Outputs a textual representation of the StereoCameraModel to the given output stream.
+ *
+ * This operator prints the details of the stereo camera model including:
+ * - The left camera parameters.
+ * - The right camera parameters.
+ * - The stereo extrinsic matrices: Rotation (R), Translation (T), Essential (E), and Fundamental (F).
+ * - The baseline distance between the two cameras.
+ *
+ * @param os The output stream to write to.
+ * @param model The StereoCameraModel instance to output.
+ * @return A reference to the output stream after writing the model information.
+ */
 RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const StereoCameraModel& model);
 
 } // rtabmap