Added CameraModel and StereoCameraModel tests

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -110,8 +110,8 @@ public:
 	 * @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 R                (Optional) Rotation matrix of the left camera relative to the right camera coordinate system (3x3, CV_64FC1).
+	 * @param T                (Optional) Translation vector of the left camera relative to the right camera coordinate system (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).
 	 *
@@ -134,7 +134,7 @@ public:
 	 * @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.
+	 * an optional 3D transform (`extrinsics`) representing the pose of the left camera relative to the right camera coordinate system.
 	 * 
 	 * 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
@@ -145,7 +145,7 @@ public:
 	 * @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.
+	 * @param extrinsics   (Optional) Transform of the left camera relative to the right camera coordinate system. If null, no extrinsics are used.
 	 *
 	 * @throws UException if `extrinsics` is not null and either camera model is not valid for rectification.
 	 * 
@@ -160,8 +160,23 @@ public:
 			const Transform & extrinsics);
 
 	/**
-     * @brief Minimal constructor using focal lengths and baseline only.
-     */
+	 * @brief Minimal constructor using focal lengths and baseline only.
+	 * 
+	 * Creates a simplified stereo camera model using only the essential intrinsic parameters
+	 * and baseline. This constructor assumes the images are already rectified and both cameras
+	 * have the same intrinsic parameters.
+	 * 
+	 * @param fx Focal length in x direction (pixels).
+	 * @param fy Focal length in y direction (pixels).
+	 * @param cx Principal point x coordinate (pixels).
+	 * @param cy Principal point y coordinate (pixels).
+	 * @param baseline Stereo baseline distance in meters.
+	 * @param localTransform Local transform from camera to robot base frame (default: optical rotation).
+	 * @param imageSize Image size (width, height). Optional, can be set later.
+	 * 
+	 * @note This constructor creates a simplified model suitable for rectified stereo pairs.
+	 *       For full calibration with distortion, use the constructors that accept camera matrices.
+	 */
 	StereoCameraModel(
 			double fx,
 			double fy,
@@ -170,9 +185,24 @@ 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 Minimal constructor that also sets a name, required if we want to save it to a file.
-     */
+	 * @brief Minimal constructor that also sets a name, required if we want to save it to a file.
+	 * 
+	 * Same as the minimal constructor but also sets the camera name, which is required
+	 * when saving the calibration to disk.
+	 * 
+	 * @param name Camera name identifier (used for saving calibration files).
+	 * @param fx Focal length in x direction (pixels).
+	 * @param fy Focal length in y direction (pixels).
+	 * @param cx Principal point x coordinate (pixels).
+	 * @param cy Principal point y coordinate (pixels).
+	 * @param baseline Stereo baseline distance in meters.
+	 * @param localTransform Local transform from camera to robot base frame (default: optical rotation).
+	 * @param imageSize Image size (width, height). Optional, can be set later.
+	 * 
+	 * @note Use this constructor when you plan to save the calibration to a file.
+	 */
 	StereoCameraModel(
 			const std::string & name,
 			double fx,
@@ -209,8 +239,17 @@ public:
 	bool isRectificationMapInitialized() const {return left_.isRectificationMapInitialized() && right_.isRectificationMapInitialized();}
 
 	/**
-     * @brief Sets the camera name and optional image suffixes for the left and right cameras.
-     */
+	 * @brief Sets the camera name and optional image suffixes for the left and right cameras.
+	 * 
+	 * Updates the stereo camera model name and the suffixes used for identifying left and right
+	 * camera calibration files. The suffixes are used when loading/saving calibration data from disk.
+	 * 
+	 * @param name Base name for the stereo camera model.
+	 * @param leftSuffix Suffix for the left camera (default: "left"). Used in filenames like "cameraName_left.yaml".
+	 * @param rightSuffix Suffix for the right camera (default: "right"). Used in filenames like "cameraName_right.yaml".
+	 * 
+	 * @note The suffixes are used by load() and save() methods to construct filenames for each camera.
+	 */
 	void setName(const std::string & name, const std::string & leftSuffix = "left", const std::string & rightSuffix = "right");
 	
 	/**
@@ -324,8 +363,16 @@ public:
 	unsigned int deserialize(const unsigned char * data, unsigned int dataSize);
 
 	/**
-     * @brief Returns the stereo baseline in meters.
-     */
+	 * @brief Returns the stereo baseline in meters.
+	 * 
+	 * Computes the baseline distance between the left and right cameras using the projection
+	 * matrices. The baseline is calculated as the difference in x-translation (Tx) normalized
+	 * by the focal length.
+	 * 
+	 * @return The baseline distance in meters. Returns 0.0 if focal lengths are invalid or zero.
+	 * 
+	 * @note The baseline is a physical distance and is essential for depth computation from disparity.
+	 */
 	double baseline() const {return right_.fx()!=0.0 && left_.fx() != 0.0 ? left_.Tx() / left_.fx() - right_.Tx()/right_.fx():0.0;}
 
 	/**
@@ -384,12 +431,31 @@ public:
 	const cv::Mat & F() const {return F_;} ///< Fundamental matrix
 
 	/**
-     * @brief Scales both cameras' calibration by a factor.
-     */
+	 * @brief Scales both cameras' calibration by a factor.
+	 * 
+	 * Scales the intrinsic parameters (focal lengths, principal points) and image sizes
+	 * of both left and right cameras by the given scale factor. This is useful when working
+	 * with downscaled or upscaled images.
+	 * 
+	 * @param scale Scaling factor (> 0). For example, use 0.5 to downscale or 2.0 to upscale.
+	 * 
+	 * @note The baseline is not scaled, as it represents a physical distance between cameras.
+	 * @note Only valid camera models are scaled. Invalid models are left unchanged.
+	 */
 	void scale(double scale);
+	
 	/**
-     * @brief Applies region-of-interest (ROI) cropping to both cameras.
-     */
+	 * @brief Applies region-of-interest (ROI) cropping to both cameras.
+	 * 
+	 * Adjusts both camera models for a region of interest by shifting the principal points
+	 * and updating the image sizes. This is useful when working with cropped or subwindowed images.
+	 * 
+	 * @param roi Region of interest rectangle. The top-left corner defines the offset for principal points.
+	 * 
+	 * @note The principal points (cx, cy) are adjusted by subtracting the ROI's top-left coordinates.
+	 * @note The image size is set to the ROI size.
+	 * @note Only valid camera models are adjusted. Invalid models are left unchanged.
+	 */
 	void roi(const cv::Rect & roi);
 
 	/**
@@ -403,8 +469,29 @@ public:
 	const Transform & localTransform() const {return left_.localTransform();}
 
 	/**
-     * @brief Returns the stereo transform (right camera relative to left).
-     */
+	 * @brief Returns the stereo transform (left camera relative to right camera coordinate system).
+	 * 
+	 * The stereo transform brings points given in the
+	 * first (left) camera's coordinate system to points in the second (right) camera's coordinate
+	 * system. In more technical terms, it performs a change of basis from the
+	 * first camera's coordinate system to the second camera's coordinate system. Due to its duality,
+	 * it is equivalent to the position of the first camera with respect to the second
+	 * camera coordinate system.
+	 * 
+	 * @return Transform from left camera to right camera coordinate system. Returns identity if R_ or T_ are empty.
+	 * 
+	 * @note The transform is constructed from the stereo extrinsic parameters R_ and T_.
+	 * 
+	 * @par Example:
+	 * For a stereo camera with a baseline of 15 cm, where the right camera is positioned to the
+	 * right of the left camera, the x value of the returned Transform would be -0.15 (negative
+	 * because it represents the position of the left camera in the right camera's coordinate system).
+	 * @code
+	 * StereoCameraModel stereo(...);
+	 * Transform transform = stereo.stereoTransform();
+	 * // If baseline is 0.15 m, transform.x() would be approximately -0.15
+	 * @endcode
+	 */
 	Transform stereoTransform() const;
 
 	/**
@@ -426,6 +513,16 @@ public:
 	const std::string & getRightSuffix() const {return rightSuffix_;}
 
 private:
+	/**
+	 * @brief Updates stereo rectification parameters for both cameras.
+	 * 
+	 * This private method computes the rectification and projection matrices for both left and right
+	 * cameras based on the stereo extrinsic parameters (R_, T_). It is called automatically when
+	 * constructing a StereoCameraModel with valid extrinsics.
+	 * 
+	 * @note Requires both R_ and T_ to be non-empty and valid.
+	 * @note Both camera models must be valid for rectification.
+	 */
 	void updateStereoRectification();
 
 private:

corelib/src/StereoCameraModel.cpp

@@ -248,7 +248,7 @@ bool StereoCameraModel::load(const std::string & directory, const std::string &
 			n = fs["camera_name"];
 			if(n.type() != cv::FileNode::NONE)
 			{
-				name_ = (int)n;
+				name_ = n.string();
 			}
 			else
 			{

corelib/test/CMakeLists.txt

@@ -74,4 +74,14 @@ gtest_discover_tests(test_stereo_dense)
 #Stereo.h (tests both BlockMatching and OpticalFlow strategies)
 add_executable(test_stereo test_stereo.cpp)
 target_link_libraries(test_stereo gtest_main rtabmap_core)
-gtest_discover_tests(test_stereo)
+gtest_discover_tests(test_stereo)
+
+#CameraModel.h
+add_executable(test_cameramodel test_cameramodel.cpp)
+target_link_libraries(test_cameramodel gtest_main rtabmap_core)
+gtest_discover_tests(test_cameramodel)
+
+#StereoCameraModel.h
+add_executable(test_stereocameramodel test_stereocameramodel.cpp)
+target_link_libraries(test_stereocameramodel gtest_main rtabmap_core)
+gtest_discover_tests(test_stereocameramodel)

corelib/test/test_cameramodel.cpp

@@ -0,0 +1,792 @@
+#include <gtest/gtest.h>
+#include <opencv2/core.hpp>
+#include <opencv2/imgproc.hpp>
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/Transform.h"
+#include "rtabmap/utilite/UException.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+#include <cmath>
+
+using namespace rtabmap;
+
+class CameraModelTest : public ::testing::Test {
+protected:
+    void SetUp() override {
+        // Create test camera parameters
+        fx_ = 525.0;
+        fy_ = 525.0;
+        cx_ = 320.0;
+        cy_ = 240.0;
+        imageWidth_ = 640;
+        imageHeight_ = 480;
+        imageSize_ = cv::Size(imageWidth_, imageHeight_);
+        
+        // Create intrinsic matrix K
+        K_ = (cv::Mat_<double>(3, 3) <<
+            fx_, 0.0, cx_,
+            0.0, fy_, cy_,
+            0.0, 0.0, 1.0);
+        
+        // Create distortion coefficients (4 parameters: k1, k2, p1, p2)
+        D_ = (cv::Mat_<double>(1, 4) << -0.1, 0.05, 0.001, -0.001);
+        
+        // Create rectification matrix (identity for simplicity)
+        R_ = cv::Mat::eye(3, 3, CV_64FC1);
+        
+        // Create projection matrix P
+        P_ = (cv::Mat_<double>(3, 4) <<
+            fx_, 0.0, cx_, 0.0,
+            0.0, fy_, cy_, 0.0,
+            0.0, 0.0, 1.0, 0.0);
+    }
+
+    void TearDown() override {
+    }
+
+    double fx_, fy_, cx_, cy_;
+    int imageWidth_, imageHeight_;
+    cv::Size imageSize_;
+    cv::Mat K_, D_, R_, P_;
+};
+
+// Constructor Tests
+
+TEST_F(CameraModelTest, DefaultConstructor)
+{
+    CameraModel model;
+    EXPECT_FALSE(model.isValidForProjection());
+    EXPECT_FALSE(model.isValidForReprojection());
+    EXPECT_FALSE(model.isValidForRectification());
+    EXPECT_EQ(model.fx(), 0.0);
+    EXPECT_EQ(model.fy(), 0.0);
+    EXPECT_EQ(model.cx(), 0.0);
+    EXPECT_EQ(model.cy(), 0.0);
+}
+
+TEST_F(CameraModelTest, MinimalConstructor)
+{
+    CameraModel model(fx_, fy_, cx_, cy_);
+    EXPECT_TRUE(model.isValidForProjection());
+    EXPECT_FALSE(model.isValidForReprojection()); // No image size
+    EXPECT_DOUBLE_EQ(model.fx(), fx_);
+    EXPECT_DOUBLE_EQ(model.fy(), fy_);
+    EXPECT_DOUBLE_EQ(model.cx(), cx_);
+    EXPECT_DOUBLE_EQ(model.cy(), cy_);
+}
+
+TEST_F(CameraModelTest, MinimalConstructorWithImageSize)
+{
+    CameraModel model(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
+    EXPECT_TRUE(model.isValidForProjection());
+    EXPECT_TRUE(model.isValidForReprojection());
+    EXPECT_EQ(model.imageWidth(), imageWidth_);
+    EXPECT_EQ(model.imageHeight(), imageHeight_);
+}
+
+TEST_F(CameraModelTest, MinimalConstructorWithName)
+{
+    std::string name = "test_camera";
+    CameraModel model(name, fx_, fy_, cx_, cy_);
+    EXPECT_EQ(model.name(), name);
+    EXPECT_TRUE(model.isValidForProjection());
+}
+
+TEST_F(CameraModelTest, FullConstructor)
+{
+    std::string name = "test_camera";
+    CameraModel model(name, imageSize_, K_, D_, R_, P_);
+    EXPECT_EQ(model.name(), name);
+    EXPECT_TRUE(model.isValidForProjection());
+    EXPECT_TRUE(model.isValidForReprojection());
+    EXPECT_TRUE(model.isValidForRectification());
+    EXPECT_DOUBLE_EQ(model.fx(), fx_);
+    EXPECT_DOUBLE_EQ(model.fy(), fy_);
+    EXPECT_DOUBLE_EQ(model.cx(), cx_);
+    EXPECT_DOUBLE_EQ(model.cy(), cy_);
+}
+
+// Getter Tests
+
+TEST_F(CameraModelTest, Getters)
+{
+    CameraModel model(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
+    
+    EXPECT_DOUBLE_EQ(model.fx(), fx_);
+    EXPECT_DOUBLE_EQ(model.fy(), fy_);
+    EXPECT_DOUBLE_EQ(model.cx(), cx_);
+    EXPECT_DOUBLE_EQ(model.cy(), cy_);
+    EXPECT_EQ(model.Tx(), 0.0);
+    EXPECT_EQ(model.imageSize(), imageSize_);
+    EXPECT_EQ(model.imageWidth(), imageWidth_);
+    EXPECT_EQ(model.imageHeight(), imageHeight_);
+}
+
+TEST_F(CameraModelTest, MatrixGetters)
+{
+    CameraModel model("test", imageSize_, K_, D_, R_, P_);
+    
+    cv::Mat K = model.K();
+    EXPECT_FALSE(K.empty());
+    EXPECT_DOUBLE_EQ(K.at<double>(0, 0), fx_);
+    
+    cv::Mat D = model.D();
+    EXPECT_FALSE(D.empty());
+    
+    cv::Mat R = model.R();
+    EXPECT_FALSE(R.empty());
+    
+    cv::Mat P = model.P();
+    EXPECT_FALSE(P.empty());
+}
+
+// Validation Tests
+
+TEST_F(CameraModelTest, IsValidForProjection)
+{
+    CameraModel invalid;
+    EXPECT_FALSE(invalid.isValidForProjection());
+    
+    CameraModel valid(fx_, fy_, cx_, cy_);
+    EXPECT_TRUE(valid.isValidForProjection());
+}
+
+TEST_F(CameraModelTest, IsValidForReprojection)
+{
+    CameraModel noSize(fx_, fy_, cx_, cy_);
+    EXPECT_FALSE(noSize.isValidForReprojection());
+    
+    CameraModel withSize(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
+    EXPECT_TRUE(withSize.isValidForReprojection());
+}
+
+TEST_F(CameraModelTest, IsValidForRectification)
+{
+    CameraModel minimal(fx_, fy_, cx_, cy_);
+    EXPECT_FALSE(minimal.isValidForRectification());
+    
+    CameraModel full("test", imageSize_, K_, D_, R_, P_);
+    EXPECT_TRUE(full.isValidForRectification());
+}
+
+// Rectification Tests
+
+TEST_F(CameraModelTest, InitRectificationMap)
+{
+    CameraModel model("test", imageSize_, K_, D_, R_, P_);
+    
+    EXPECT_FALSE(model.isRectificationMapInitialized());
+    bool result = model.initRectificationMap();
+    EXPECT_TRUE(result);
+    EXPECT_TRUE(model.isRectificationMapInitialized());
+}
+
+TEST_F(CameraModelTest, InitRectificationMapInvalid)
+{
+    CameraModel model(fx_, fy_, cx_, cy_); // No distortion, no rectification matrices
+    EXPECT_THROW(model.initRectificationMap(), UException);
+}
+
+TEST_F(CameraModelTest, RectifyImage)
+{
+    CameraModel model("test", imageSize_, K_, D_, R_, P_);
+    model.initRectificationMap();
+    
+    // Create a test image
+    cv::Mat testImage = cv::Mat::zeros(imageHeight_, imageWidth_, CV_8UC1);
+    cv::circle(testImage, cv::Point(imageWidth_/2, imageHeight_/2), 50, cv::Scalar(255), -1);
+    
+    cv::Mat rectified = model.rectifyImage(testImage);
+    EXPECT_FALSE(rectified.empty());
+    EXPECT_EQ(rectified.rows, imageHeight_);
+    EXPECT_EQ(rectified.cols, imageWidth_);
+}
+
+TEST_F(CameraModelTest, RectifyImageWithoutMap)
+{
+    CameraModel model(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
+    
+    cv::Mat testImage = cv::Mat::zeros(imageHeight_, imageWidth_, CV_8UC1);
+    cv::Mat rectified = model.rectifyImage(testImage);
+    
+    // Should return a clone of the original if maps are not initialized
+    EXPECT_FALSE(rectified.empty());
+    EXPECT_EQ(rectified.rows, imageHeight_);
+    EXPECT_EQ(rectified.cols, imageWidth_);
+}
+
+TEST_F(CameraModelTest, RectifyDepth)
+{
+    CameraModel model("test", imageSize_, K_, D_, R_, P_);
+    model.initRectificationMap();
+    
+    // Create a test depth image
+    cv::Mat depthImage = cv::Mat::zeros(imageHeight_, imageWidth_, CV_16UC1);
+    depthImage.at<unsigned short>(imageHeight_/2, imageWidth_/2) = 1000; // 1 meter in mm
+    
+    cv::Mat rectified = model.rectifyDepth(depthImage);
+    EXPECT_FALSE(rectified.empty());
+    EXPECT_EQ(rectified.rows, imageHeight_);
+    EXPECT_EQ(rectified.cols, imageWidth_);
+    EXPECT_EQ(rectified.type(), CV_16UC1);
+}
+
+// Projection/Reprojection Tests
+
+TEST_F(CameraModelTest, Project)
+{
+    CameraModel model(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
+    
+    float u = cx_;
+    float v = cy_;
+    float depth = 1.0f; // 1 meter
+    
+    float x, y, z;
+    model.project(u, v, depth, x, y, z);
+    
+    // At principal point with depth 1.0, x and y should be approximately 0
+    EXPECT_NEAR(x, 0.0f, 0.01f);
+    EXPECT_NEAR(y, 0.0f, 0.01f);
+    EXPECT_FLOAT_EQ(z, depth);
+}
+
+TEST_F(CameraModelTest, ProjectInvalidDepth)
+{
+    CameraModel model(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
+    
+    float u = cx_;
+    float v = cy_;
+    float depth = 0.0f; // Invalid depth
+    
+    float x, y, z;
+    model.project(u, v, depth, x, y, z);
+    
+    // Should return NaN for invalid depth
+    EXPECT_TRUE(std::isnan(x));
+    EXPECT_TRUE(std::isnan(y));
+    EXPECT_TRUE(std::isnan(z));
+}
+
+TEST_F(CameraModelTest, ReprojectFloat)
+{
+    CameraModel model(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
+    
+    float x = 0.0f;
+    float y = 0.0f;
+    float z = 1.0f; // 1 meter
+    
+    float u, v;
+    model.reproject(x, y, z, u, v);
+    
+    // At origin with z=1.0, should project to principal point
+    EXPECT_NEAR(u, cx_, 0.01f);
+    EXPECT_NEAR(v, cy_, 0.01f);
+}
+
+TEST_F(CameraModelTest, ReprojectInt)
+{
+    CameraModel model(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
+    
+    float x = 0.0f;
+    float y = 0.0f;
+    float z = 1.0f;
+    
+    int u, v;
+    model.reproject(x, y, z, u, v);
+    
+    EXPECT_NEAR(u, static_cast<int>(cx_), 1);
+    EXPECT_NEAR(v, static_cast<int>(cy_), 1);
+}
+
+// Field of View Tests
+
+TEST_F(CameraModelTest, FieldOfView)
+{
+    CameraModel model(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
+    
+    double fovX = model.fovX();
+    double fovY = model.fovY();
+    double hFOV = model.horizontalFOV();
+    double vFOV = model.verticalFOV();
+    
+    EXPECT_GT(fovX, 0.0);
+    EXPECT_GT(fovY, 0.0);
+    EXPECT_GT(hFOV, 0.0);
+    EXPECT_GT(vFOV, 0.0);
+    
+    // Horizontal FOV should be larger than vertical for typical cameras
+    EXPECT_GT(fovX, fovY);
+    
+    // Degrees should be approximately radians * 180 / PI
+    EXPECT_NEAR(hFOV, fovX * 180.0 / M_PI, 0.1);
+    EXPECT_NEAR(vFOV, fovY * 180.0 / M_PI, 0.1);
+}
+
+// InFrame Tests
+
+TEST_F(CameraModelTest, InFrame)
+{
+    CameraModel model(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
+    
+    EXPECT_TRUE(model.inFrame(0, 0));
+    EXPECT_TRUE(model.inFrame(imageWidth_ - 1, imageHeight_ - 1));
+    EXPECT_FALSE(model.inFrame(-1, 0));
+    EXPECT_FALSE(model.inFrame(0, -1));
+    EXPECT_FALSE(model.inFrame(imageWidth_, 0));
+    EXPECT_FALSE(model.inFrame(0, imageHeight_));
+}
+
+// Scaling and ROI Tests
+
+TEST_F(CameraModelTest, Scaled)
+{
+    CameraModel model(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
+    
+    double scale = 0.5;
+    CameraModel scaled = model.scaled(scale);
+    
+    EXPECT_NEAR(scaled.fx(), fx_ * scale, 0.01);
+    EXPECT_NEAR(scaled.fy(), fy_ * scale, 0.01);
+    EXPECT_NEAR(scaled.cx(), cx_ * scale, 0.01);
+    EXPECT_NEAR(scaled.cy(), cy_ * scale, 0.01);
+    EXPECT_EQ(scaled.imageWidth(), static_cast<int>(imageWidth_ * scale));
+    EXPECT_EQ(scaled.imageHeight(), static_cast<int>(imageHeight_ * scale));
+}
+
+TEST_F(CameraModelTest, ROI)
+{
+    CameraModel model(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
+    
+    cv::Rect roi(100, 100, 200, 200);
+    CameraModel roiModel = model.roi(roi);
+    
+    EXPECT_NEAR(roiModel.cx(), cx_ - roi.x, 0.01);
+    EXPECT_NEAR(roiModel.cy(), cy_ - roi.y, 0.01);
+    EXPECT_EQ(roiModel.imageWidth(), roi.width);
+    EXPECT_EQ(roiModel.imageHeight(), roi.height);
+}
+
+// Serialization Tests
+
+TEST_F(CameraModelTest, SerializeDeserialize)
+{
+    CameraModel original("test_camera", imageSize_, K_, D_, R_, P_);
+    original.setName("original");
+    
+    std::vector<unsigned char> data = original.serialize();
+    EXPECT_FALSE(data.empty());
+    
+    CameraModel restored;
+    unsigned int bytesRead = restored.deserialize(data);
+    EXPECT_GT(bytesRead, 0u);
+    
+    EXPECT_DOUBLE_EQ(restored.fx(), original.fx());
+    EXPECT_DOUBLE_EQ(restored.fy(), original.fy());
+    EXPECT_DOUBLE_EQ(restored.cx(), original.cx());
+    EXPECT_DOUBLE_EQ(restored.cy(), original.cy());
+    EXPECT_EQ(restored.imageSize(), original.imageSize());
+}
+
+// Name and Transform Tests
+
+TEST_F(CameraModelTest, SetName)
+{
+    CameraModel model;
+    std::string name = "my_camera";
+    model.setName(name);
+    EXPECT_EQ(model.name(), name);
+}
+
+TEST_F(CameraModelTest, LocalTransform)
+{
+    Transform transform = Transform(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0);
+    CameraModel model(fx_, fy_, cx_, cy_, transform);
+    
+    EXPECT_FALSE(model.localTransform().isNull());
+    model.setLocalTransform(CameraModel::opticalRotation());
+    EXPECT_FALSE(model.localTransform().isNull());
+}
+
+// Fisheye Tests
+
+TEST_F(CameraModelTest, IsFisheye)
+{
+    // Standard distortion (4 parameters)
+    CameraModel standard("test", imageSize_, K_, D_, R_, P_);
+    EXPECT_FALSE(standard.isFisheye());
+    
+    // Fisheye distortion (6 parameters: k1, k2, 0, 0, k3, k4)
+    cv::Mat D_fisheye = (cv::Mat_<double>(1, 6) << 0.1, 0.05, 0.0, 0.0, 0.01, 0.005);
+    CameraModel fisheye("test", imageSize_, K_, D_fisheye, R_, P_);
+    EXPECT_TRUE(fisheye.isFisheye());
+}
+
+// SetImageSize Tests
+
+TEST_F(CameraModelTest, SetImageSize)
+{
+    CameraModel model(fx_, fy_, 0.0, 0.0); // cx, cy = 0
+    
+    cv::Size newSize(320, 240);
+    model.setImageSize(newSize);
+    
+    EXPECT_EQ(model.imageSize(), newSize);
+    // Principal point should be set to center
+    EXPECT_NEAR(model.cx(), newSize.width / 2.0 - 0.5, 0.01);
+    EXPECT_NEAR(model.cy(), newSize.height / 2.0 - 0.5, 0.01);
+}
+
+// Tx (Baseline) Tests
+
+TEST_F(CameraModelTest, Tx)
+{
+    double Tx = fx_ * 0.12; // Baseline * fx (e.g., 12cm baseline)
+    CameraModel model(fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), Tx, imageSize_);
+    
+    EXPECT_NEAR(model.Tx(), Tx, 0.01);
+}
+
+// Save/Load Tests
+
+TEST_F(CameraModelTest, SaveLoadRoundTrip)
+{
+    // Create a temporary directory for testing
+    std::string testDir = "test_camera_calibration";
+    UDirectory::makeDir(testDir);
+    
+    // Create original camera model with all parameters
+    std::string cameraName = "test_camera";
+    Transform localTransform = Transform(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0);
+    CameraModel original(cameraName, imageSize_, K_, D_, R_, P_, localTransform);
+    
+    // Save the model
+    bool saveResult = original.save(testDir);
+    EXPECT_TRUE(saveResult);
+    
+    // Verify file was created
+    std::string expectedFile = testDir + "/" + cameraName + ".yaml";
+    EXPECT_TRUE(UFile::exists(expectedFile));
+    
+    // Load the model back
+    CameraModel loaded;
+    bool loadResult = loaded.load(testDir, cameraName);
+    EXPECT_TRUE(loadResult);
+    
+    // Verify all parameters match
+    
+    // Basic parameters
+    EXPECT_EQ(loaded.name(), original.name());
+    EXPECT_EQ(loaded.imageSize(), original.imageSize());
+    EXPECT_EQ(loaded.imageWidth(), original.imageWidth());
+    EXPECT_EQ(loaded.imageHeight(), original.imageHeight());
+    
+    // Intrinsic parameters
+    EXPECT_DOUBLE_EQ(loaded.fx(), original.fx());
+    EXPECT_DOUBLE_EQ(loaded.fy(), original.fy());
+    EXPECT_DOUBLE_EQ(loaded.cx(), original.cx());
+    EXPECT_DOUBLE_EQ(loaded.cy(), original.cy());
+    EXPECT_DOUBLE_EQ(loaded.Tx(), original.Tx());
+    
+    // Raw intrinsic matrix K
+    cv::Mat K_raw_loaded = loaded.K_raw();
+    cv::Mat K_raw_original = original.K_raw();
+    if(!K_raw_loaded.empty() && !K_raw_original.empty())
+    {
+        EXPECT_EQ(K_raw_loaded.rows, K_raw_original.rows);
+        EXPECT_EQ(K_raw_loaded.cols, K_raw_original.cols);
+        for(int i = 0; i < K_raw_loaded.rows; ++i)
+        {
+            for(int j = 0; j < K_raw_loaded.cols; ++j)
+            {
+                EXPECT_NEAR(K_raw_loaded.at<double>(i, j), K_raw_original.at<double>(i, j), 0.001);
+            }
+        }
+    }
+    
+    // Intrinsic matrix K (may be rectified)
+    cv::Mat K_loaded = loaded.K();
+    cv::Mat K_original = original.K();
+    EXPECT_EQ(K_loaded.rows, K_original.rows);
+    EXPECT_EQ(K_loaded.cols, K_original.cols);
+    for(int i = 0; i < K_loaded.rows; ++i)
+    {
+        for(int j = 0; j < K_loaded.cols; ++j)
+        {
+            EXPECT_NEAR(K_loaded.at<double>(i, j), K_original.at<double>(i, j), 0.001);
+        }
+    }
+    
+    // Raw distortion coefficients
+    cv::Mat D_raw_loaded = loaded.D_raw();
+    cv::Mat D_raw_original = original.D_raw();
+    if(!D_raw_loaded.empty() && !D_raw_original.empty())
+    {
+        EXPECT_EQ(D_raw_loaded.rows, D_raw_original.rows);
+        EXPECT_EQ(D_raw_loaded.cols, D_raw_original.cols);
+        for(int i = 0; i < D_raw_loaded.cols; ++i)
+        {
+            EXPECT_NEAR(D_raw_loaded.at<double>(0, i), D_raw_original.at<double>(0, i), 0.001);
+        }
+    }
+    
+    // Distortion coefficients (may be rectified)
+    cv::Mat D_loaded = loaded.D();
+    cv::Mat D_original = original.D();
+    if(!D_loaded.empty() && !D_original.empty())
+    {
+        EXPECT_EQ(D_loaded.rows, D_original.rows);
+        EXPECT_EQ(D_loaded.cols, D_original.cols);
+        for(int i = 0; i < D_loaded.cols; ++i)
+        {
+            EXPECT_NEAR(D_loaded.at<double>(0, i), D_original.at<double>(0, i), 0.001);
+        }
+    }
+    
+    // Rectification matrix R
+    cv::Mat R_loaded = loaded.R();
+    cv::Mat R_original = original.R();
+    if(!R_loaded.empty() && !R_original.empty())
+    {
+        EXPECT_EQ(R_loaded.rows, R_original.rows);
+        EXPECT_EQ(R_loaded.cols, R_original.cols);
+        for(int i = 0; i < R_loaded.rows; ++i)
+        {
+            for(int j = 0; j < R_loaded.cols; ++j)
+            {
+                EXPECT_NEAR(R_loaded.at<double>(i, j), R_original.at<double>(i, j), 0.001);
+            }
+        }
+    }
+    
+    // Projection matrix P
+    cv::Mat P_loaded = loaded.P();
+    cv::Mat P_original = original.P();
+    if(!P_loaded.empty() && !P_original.empty())
+    {
+        EXPECT_EQ(P_loaded.rows, P_original.rows);
+        EXPECT_EQ(P_loaded.cols, P_original.cols);
+        for(int i = 0; i < P_loaded.rows; ++i)
+        {
+            for(int j = 0; j < P_loaded.cols; ++j)
+            {
+                EXPECT_NEAR(P_loaded.at<double>(i, j), P_original.at<double>(i, j), 0.001);
+            }
+        }
+    }
+    
+    // Local transform
+    Transform localTransform_loaded = loaded.localTransform();
+    Transform localTransform_original = original.localTransform();
+    if(!localTransform_loaded.isNull() && !localTransform_original.isNull())
+    {
+        // Compare transform matrices element by element
+        for(int i = 0; i < 3; ++i)
+        {
+            for(int j = 0; j < 4; ++j)
+            {
+                EXPECT_NEAR(localTransform_loaded.data()[i*4+j], localTransform_original.data()[i*4+j], 0.001);
+            }
+        }
+    }
+    
+    // Fisheye detection
+    EXPECT_EQ(loaded.isFisheye(), original.isFisheye());
+    
+    // Validation states
+    EXPECT_EQ(loaded.isValidForProjection(), original.isValidForProjection());
+    EXPECT_EQ(loaded.isValidForReprojection(), original.isValidForReprojection());
+    EXPECT_EQ(loaded.isValidForRectification(), original.isValidForRectification());
+    
+    // Field of view (if image size is set)
+    if(loaded.imageWidth() > 0 && loaded.imageHeight() > 0 && original.imageWidth() > 0 && original.imageHeight() > 0)
+    {
+        EXPECT_NEAR(loaded.fovX(), original.fovX(), 0.001);
+        EXPECT_NEAR(loaded.fovY(), original.fovY(), 0.001);
+        EXPECT_NEAR(loaded.horizontalFOV(), original.horizontalFOV(), 0.001);
+        EXPECT_NEAR(loaded.verticalFOV(), original.verticalFOV(), 0.001);
+    }
+}
+
+TEST_F(CameraModelTest, SaveLoadRoundTripFullPath)
+{
+    // Create a temporary directory for testing
+    std::string testDir = "test_camera_calibration2";
+    UDirectory::makeDir(testDir);
+    
+    // Create original camera model
+    std::string cameraName = "test_camera2";
+    CameraModel original(cameraName, imageSize_, K_, D_, R_, P_);
+    
+    // Save the model
+    bool saveResult = original.save(testDir);
+    EXPECT_TRUE(saveResult);
+    
+    // Load using full file path
+    std::string filePath = testDir + "/" + cameraName + ".yaml";
+    CameraModel loaded;
+    bool loadResult = loaded.load(filePath);
+    EXPECT_TRUE(loadResult);
+    
+    // Verify all parameters match
+    EXPECT_EQ(loaded.name(), original.name());
+    EXPECT_EQ(loaded.imageSize(), original.imageSize());
+    EXPECT_DOUBLE_EQ(loaded.fx(), original.fx());
+    EXPECT_DOUBLE_EQ(loaded.fy(), original.fy());
+    EXPECT_DOUBLE_EQ(loaded.cx(), original.cx());
+    EXPECT_DOUBLE_EQ(loaded.cy(), original.cy());
+    EXPECT_DOUBLE_EQ(loaded.Tx(), original.Tx());
+    
+    // Verify matrices
+    cv::Mat K_loaded = loaded.K();
+    cv::Mat K_original = original.K();
+    if(!K_loaded.empty() && !K_original.empty())
+    {
+        EXPECT_EQ(K_loaded.rows, K_original.rows);
+        EXPECT_EQ(K_loaded.cols, K_original.cols);
+        for(int i = 0; i < K_loaded.rows; ++i)
+        {
+            for(int j = 0; j < K_loaded.cols; ++j)
+            {
+                EXPECT_NEAR(K_loaded.at<double>(i, j), K_original.at<double>(i, j), 0.001);
+            }
+        }
+    }
+    
+    cv::Mat D_loaded = loaded.D();
+    cv::Mat D_original = original.D();
+    if(!D_loaded.empty() && !D_original.empty())
+    {
+        EXPECT_EQ(D_loaded.cols, D_original.cols);
+        for(int i = 0; i < D_loaded.cols; ++i)
+        {
+            EXPECT_NEAR(D_loaded.at<double>(0, i), D_original.at<double>(0, i), 0.001);
+        }
+    }
+    
+    // Verify validation states
+    EXPECT_EQ(loaded.isValidForProjection(), original.isValidForProjection());
+    EXPECT_EQ(loaded.isValidForReprojection(), original.isValidForReprojection());
+    EXPECT_EQ(loaded.isValidForRectification(), original.isValidForRectification());
+}
+
+TEST_F(CameraModelTest, SaveLoadRoundTripMinimal)
+{
+    // Create a temporary directory for testing
+    std::string testDir = "test_camera_calibration3";
+    UDirectory::makeDir(testDir);
+    
+    // Create minimal camera model (no distortion, no rectification)
+    std::string cameraName = "minimal_camera";
+    CameraModel original(cameraName, fx_, fy_, cx_, cy_, CameraModel::opticalRotation(), 0.0, imageSize_);
+    
+    // Save the model
+    bool saveResult = original.save(testDir);
+    EXPECT_TRUE(saveResult);
+    
+    // Load the model back
+    CameraModel loaded;
+    bool loadResult = loaded.load(testDir, cameraName);
+    EXPECT_TRUE(loadResult);
+    
+    // Verify all parameters match
+    EXPECT_EQ(loaded.name(), original.name());
+    EXPECT_EQ(loaded.imageSize(), original.imageSize());
+    EXPECT_EQ(loaded.imageWidth(), original.imageWidth());
+    EXPECT_EQ(loaded.imageHeight(), original.imageHeight());
+    EXPECT_DOUBLE_EQ(loaded.fx(), original.fx());
+    EXPECT_DOUBLE_EQ(loaded.fy(), original.fy());
+    EXPECT_DOUBLE_EQ(loaded.cx(), original.cx());
+    EXPECT_DOUBLE_EQ(loaded.cy(), original.cy());
+    EXPECT_DOUBLE_EQ(loaded.Tx(), original.Tx());
+    
+    // Verify matrices
+    cv::Mat K_loaded = loaded.K();
+    cv::Mat K_original = original.K();
+    if(!K_loaded.empty() && !K_original.empty())
+    {
+        EXPECT_EQ(K_loaded.rows, K_original.rows);
+        EXPECT_EQ(K_loaded.cols, K_original.cols);
+        for(int i = 0; i < K_loaded.rows; ++i)
+        {
+            for(int j = 0; j < K_loaded.cols; ++j)
+            {
+                EXPECT_NEAR(K_loaded.at<double>(i, j), K_original.at<double>(i, j), 0.001);
+            }
+        }
+    }
+    
+    // Verify validation states
+    EXPECT_EQ(loaded.isValidForProjection(), original.isValidForProjection());
+    EXPECT_EQ(loaded.isValidForReprojection(), original.isValidForReprojection());
+}
+
+TEST_F(CameraModelTest, SaveLoadRoundTripFisheye)
+{
+    // Create a temporary directory for testing
+    std::string testDir = "test_camera_calibration4";
+    UDirectory::makeDir(testDir);
+    
+    // Create fisheye camera model (6 distortion parameters: k1, k2, 0, 0, k3, k4)
+    // Format: [k1, k2, p1, p2, k3, k4] where p1=p2=0 for fisheye
+    double k1 = 0.1;
+    double k2 = 0.05;
+    double k3 = 0.01;
+    double k4 = 0.005;
+    cv::Mat D_fisheye = (cv::Mat_<double>(1, 6) << k1, k2, 0.0, 0.0, k3, k4);
+    std::string cameraName = "fisheye_camera";
+    CameraModel original(cameraName, imageSize_, K_, D_fisheye, R_, P_);
+    
+    EXPECT_TRUE(original.isFisheye());
+    EXPECT_EQ(original.D_raw().cols, 6);
+    
+    // Verify original distortion coefficients
+    cv::Mat D_original = original.D_raw();
+    EXPECT_DOUBLE_EQ(D_original.at<double>(0, 0), k1);
+    EXPECT_DOUBLE_EQ(D_original.at<double>(0, 1), k2);
+    EXPECT_DOUBLE_EQ(D_original.at<double>(0, 2), 0.0); // p1
+    EXPECT_DOUBLE_EQ(D_original.at<double>(0, 3), 0.0); // p2
+    EXPECT_DOUBLE_EQ(D_original.at<double>(0, 4), k3);
+    EXPECT_DOUBLE_EQ(D_original.at<double>(0, 5), k4);
+    
+    // Save the model (converts 6 params to 4 params for ROS compatibility)
+    bool saveResult = original.save(testDir);
+    EXPECT_TRUE(saveResult);
+    
+    // Load the model back (converts 4 params back to 6 params)
+    CameraModel loaded;
+    bool loadResult = loaded.load(testDir, cameraName);
+    EXPECT_TRUE(loadResult);
+    
+    // Verify basic parameters match
+    EXPECT_EQ(loaded.name(), original.name());
+    EXPECT_DOUBLE_EQ(loaded.fx(), original.fx());
+    EXPECT_DOUBLE_EQ(loaded.fy(), original.fy());
+    EXPECT_DOUBLE_EQ(loaded.cx(), original.cx());
+    EXPECT_DOUBLE_EQ(loaded.cy(), original.cy());
+    EXPECT_EQ(loaded.imageSize(), original.imageSize());
+    
+    // Verify fisheye model is preserved after save/load conversion
+    EXPECT_TRUE(loaded.isFisheye());
+    EXPECT_EQ(loaded.D_raw().cols, 6);
+    
+    // Verify distortion coefficients are correctly converted back
+    // Save converts: [k1, k2, 0, 0, k3, k4] -> [k1, k2, k3, k4]
+    // Load converts: [k1, k2, k3, k4] -> [k1, k2, 0, 0, k3, k4]
+    cv::Mat D_loaded = loaded.D_raw();
+    EXPECT_DOUBLE_EQ(D_loaded.at<double>(0, 0), k1); // k1 preserved
+    EXPECT_DOUBLE_EQ(D_loaded.at<double>(0, 1), k2); // k2 preserved
+    EXPECT_DOUBLE_EQ(D_loaded.at<double>(0, 2), 0.0); // p1 should be 0
+    EXPECT_DOUBLE_EQ(D_loaded.at<double>(0, 3), 0.0); // p2 should be 0
+    EXPECT_DOUBLE_EQ(D_loaded.at<double>(0, 4), k3); // k3 preserved
+    EXPECT_DOUBLE_EQ(D_loaded.at<double>(0, 5), k4); // k4 preserved
+    
+    // Verify the model is still valid for rectification
+    EXPECT_TRUE(loaded.isValidForRectification());
+    EXPECT_EQ(loaded.isValidForProjection(), original.isValidForProjection());
+    
+    // Verify rectification maps can be initialized
+    bool mapInitResult = loaded.initRectificationMap();
+    EXPECT_TRUE(mapInitResult);
+    EXPECT_TRUE(loaded.isRectificationMapInitialized());
+}
+

corelib/test/test_stereocameramodel.cpp

@@ -0,0 +1,673 @@
+#include <gtest/gtest.h>
+#include <opencv2/core.hpp>
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/Transform.h"
+#include "rtabmap/utilite/UException.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+#include <cmath>
+
+using namespace rtabmap;
+
+class StereoCameraModelTest : public ::testing::Test {
+protected:
+    void SetUp() override {
+        // Create test camera parameters
+        fx_ = 525.0;
+        fy_ = 525.0;
+        cx_ = 320.0;
+        cy_ = 240.0;
+        baseline_ = 0.12; // 12 cm baseline
+        imageWidth_ = 640;
+        imageHeight_ = 480;
+        imageSize_ = cv::Size(imageWidth_, imageHeight_);
+        
+        // Create intrinsic matrix K
+        K_ = (cv::Mat_<double>(3, 3) <<
+            fx_, 0.0, cx_,
+            0.0, fy_, cy_,
+            0.0, 0.0, 1.0);
+        
+        // Create distortion coefficients
+        D_ = (cv::Mat_<double>(1, 4) << -0.1, 0.05, 0.001, -0.001);
+        
+        // Create rectification matrix
+        R_ = cv::Mat::eye(3, 3, CV_64FC1);
+        
+        // Create projection matrix P (with Tx = baseline * fx for left camera)
+        double Tx = baseline_ * fx_;
+        P_left_ = (cv::Mat_<double>(3, 4) <<
+            fx_, 0.0, cx_, Tx,
+            0.0, fy_, cy_, 0.0,
+            0.0, 0.0, 1.0, 0.0);
+        
+        // Right camera projection matrix (Tx = 0 typically)
+        P_right_ = (cv::Mat_<double>(3, 4) <<
+            fx_, 0.0, cx_, 0.0,
+            0.0, fy_, cy_, 0.0,
+            0.0, 0.0, 1.0, 0.0);
+        
+        // Create stereo extrinsic parameters
+        // R and T represent the left camera relative to the right camera coordinate system
+        // For parallel cameras with baseline along x-axis, T is negative baseline
+        // Translation of left camera relative to right camera coordinate system
+        T_ = (cv::Mat_<double>(3, 1) << -baseline_, 0.0, 0.0);
+        
+        // Rotation matrix of left camera relative to right camera coordinate system
+        // (identity for parallel cameras)
+        R_stereo_ = cv::Mat::eye(3, 3, CV_64FC1);
+    }
+
+    void TearDown() override {
+    }
+
+    double fx_, fy_, cx_, cy_, baseline_;
+    int imageWidth_, imageHeight_;
+    cv::Size imageSize_;
+    cv::Mat K_, D_, R_, P_left_, P_right_, R_stereo_, T_;
+};
+
+// Constructor Tests
+
+TEST_F(StereoCameraModelTest, DefaultConstructor)
+{
+    StereoCameraModel model;
+    EXPECT_FALSE(model.isValidForProjection());
+    EXPECT_FALSE(model.isValidForRectification());
+    EXPECT_EQ(model.baseline(), 0.0);
+}
+
+TEST_F(StereoCameraModelTest, MinimalConstructor)
+{
+    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
+    EXPECT_TRUE(model.isValidForProjection());
+    EXPECT_NEAR(model.baseline(), baseline_, 0.001);
+    EXPECT_DOUBLE_EQ(model.left().fx(), fx_);
+    EXPECT_DOUBLE_EQ(model.right().fx(), fx_);
+}
+
+TEST_F(StereoCameraModelTest, MinimalConstructorWithName)
+{
+    std::string name = "stereo_camera";
+    StereoCameraModel model(name, fx_, fy_, cx_, cy_, baseline_);
+    EXPECT_EQ(model.name(), name);
+    EXPECT_TRUE(model.isValidForProjection());
+    EXPECT_NEAR(model.baseline(), baseline_, 0.001);
+}
+
+TEST_F(StereoCameraModelTest, ConstructorFromCameraModels)
+{
+    CameraModel left("left", imageSize_, K_, D_, R_, P_left_);
+    CameraModel right("right", imageSize_, K_, D_, R_, P_right_);
+    
+    StereoCameraModel model("stereo", left, right, R_stereo_, T_);
+    
+    EXPECT_EQ(model.name(), "stereo");
+    EXPECT_TRUE(model.isValidForProjection());
+    EXPECT_NEAR(model.baseline(), baseline_, 0.01);
+    EXPECT_EQ(model.left().name(), "stereo_left");
+    EXPECT_EQ(model.right().name(), "stereo_right");
+}
+
+TEST_F(StereoCameraModelTest, ConstructorFromCameraModelsWithTransform)
+{
+    CameraModel left("left", imageSize_, K_, D_, R_, P_left_);
+    CameraModel right("right", imageSize_, K_, D_, R_, P_right_);
+    
+    // Transform represents left camera relative to right camera coordinate system
+    // For baseline along x-axis, x should be negative
+    Transform extrinsics = Transform(-baseline_, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
+    StereoCameraModel model("stereo", left, right, extrinsics);
+    
+    EXPECT_TRUE(model.isValidForProjection());
+    EXPECT_NEAR(model.baseline(), baseline_, 0.01);
+    
+    // Verify stereo transform matches (left camera relative to right camera coordinate system)
+    Transform stereoTransform = model.stereoTransform();
+    EXPECT_NEAR(stereoTransform.x(), -baseline_, 0.001);
+}
+
+TEST_F(StereoCameraModelTest, FullConstructor)
+{
+    std::string name = "stereo_camera";
+    StereoCameraModel model(
+        name,
+        imageSize_, K_, D_, R_, P_left_,
+        imageSize_, K_, D_, R_, P_right_,
+        R_stereo_, T_, cv::Mat(), cv::Mat()
+    );
+    
+    EXPECT_EQ(model.name(), name);
+    EXPECT_TRUE(model.isValidForProjection());
+    EXPECT_TRUE(model.isValidForRectification());
+    EXPECT_NEAR(model.baseline(), baseline_, 0.01);
+}
+
+// Validation Tests
+
+TEST_F(StereoCameraModelTest, IsValidForProjection)
+{
+    StereoCameraModel invalid;
+    EXPECT_FALSE(invalid.isValidForProjection());
+    
+    StereoCameraModel valid(fx_, fy_, cx_, cy_, baseline_);
+    EXPECT_TRUE(valid.isValidForProjection());
+}
+
+TEST_F(StereoCameraModelTest, IsValidForRectification)
+{
+    StereoCameraModel minimal(fx_, fy_, cx_, cy_, baseline_);
+    EXPECT_FALSE(minimal.isValidForRectification());
+    
+    CameraModel left("left", imageSize_, K_, D_, R_, P_left_);
+    CameraModel right("right", imageSize_, K_, D_, R_, P_right_);
+    StereoCameraModel full("stereo", left, right, R_stereo_, T_);
+    EXPECT_TRUE(full.isValidForRectification());
+}
+
+// Rectification Tests
+
+TEST_F(StereoCameraModelTest, InitRectificationMap)
+{
+    CameraModel left("left", imageSize_, K_, D_, R_, P_left_);
+    CameraModel right("right", imageSize_, K_, D_, R_, P_right_);
+    StereoCameraModel model("stereo", left, right, R_stereo_, T_);
+    
+    EXPECT_FALSE(model.isRectificationMapInitialized());
+    model.initRectificationMap();
+    EXPECT_TRUE(model.isRectificationMapInitialized());
+}
+
+// Depth/Disparity Conversion Tests
+
+TEST_F(StereoCameraModelTest, ComputeDepth)
+{
+    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
+    
+    // Test with known disparity
+    // disparity = baseline * fx / depth
+    float depth = 1.0f; // 1 meter
+    float expectedDisparity = static_cast<float>(baseline_ * fx_ / depth);
+    
+    float computedDepth = model.computeDepth(expectedDisparity);
+    EXPECT_NEAR(computedDepth, depth, 0.01f);
+}
+
+TEST_F(StereoCameraModelTest, ComputeDepthZeroDisparity)
+{
+    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
+    float depth = model.computeDepth(0.0f);
+    EXPECT_EQ(depth, 0.0f);
+}
+
+TEST_F(StereoCameraModelTest, ComputeDisparityFromDepth)
+{
+    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
+    
+    float depth = 1.0f; // 1 meter
+    float disparity = model.computeDisparity(depth);
+    
+    // Verify round-trip
+    float computedDepth = model.computeDepth(disparity);
+    EXPECT_NEAR(computedDepth, depth, 0.01f);
+}
+
+TEST_F(StereoCameraModelTest, ComputeDisparityFromDepthMM)
+{
+    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
+    
+    unsigned short depthMM = 1000; // 1 meter in millimeters
+    float disparity = model.computeDisparity(depthMM);
+    
+    // Should be same as computing from meters
+    float disparityFromMeters = model.computeDisparity(1.0f);
+    EXPECT_NEAR(disparity, disparityFromMeters, 0.1f);
+}
+
+TEST_F(StereoCameraModelTest, ComputeDisparityZeroDepth)
+{
+    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
+    float disparity = model.computeDisparity(0.0f);
+    EXPECT_EQ(disparity, 0.0f);
+    
+    unsigned short depthMM = 0;
+    float disparityMM = model.computeDisparity(depthMM);
+    EXPECT_EQ(disparityMM, 0.0f);
+}
+
+// Getter Tests
+
+TEST_F(StereoCameraModelTest, Baseline)
+{
+    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
+    EXPECT_NEAR(model.baseline(), baseline_, 0.001);
+}
+
+TEST_F(StereoCameraModelTest, LeftRightModels)
+{
+    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
+    
+    const CameraModel& left = model.left();
+    const CameraModel& right = model.right();
+    
+    EXPECT_DOUBLE_EQ(left.fx(), fx_);
+    EXPECT_DOUBLE_EQ(right.fx(), fx_);
+    EXPECT_DOUBLE_EQ(left.fy(), fy_);
+    EXPECT_DOUBLE_EQ(right.fy(), fy_);
+}
+
+TEST_F(StereoCameraModelTest, ExtrinsicMatrices)
+{
+    CameraModel left("left", imageSize_, K_, D_, R_, P_left_);
+    CameraModel right("right", imageSize_, K_, D_, R_, P_right_);
+    StereoCameraModel model("stereo", left, right, R_stereo_, T_);
+    
+    // R and T represent left camera relative to right camera coordinate system
+    const cv::Mat& R = model.R();
+    const cv::Mat& T = model.T();
+    
+    EXPECT_FALSE(R.empty());
+    EXPECT_FALSE(T.empty());
+    EXPECT_EQ(R.rows, 3);
+    EXPECT_EQ(R.cols, 3);
+    EXPECT_EQ(T.rows, 3);
+    EXPECT_EQ(T.cols, 1);
+    
+    // Verify T matches expected value (negative baseline for left relative to right)
+    EXPECT_NEAR(T.at<double>(0, 0), -baseline_, 0.001);
+    EXPECT_NEAR(T.at<double>(1, 0), 0.0, 0.001);
+    EXPECT_NEAR(T.at<double>(2, 0), 0.0, 0.001);
+}
+
+// Name and Suffix Tests
+
+TEST_F(StereoCameraModelTest, SetName)
+{
+    StereoCameraModel model;
+    std::string name = "my_stereo";
+    model.setName(name);
+    EXPECT_EQ(model.name(), name);
+}
+
+TEST_F(StereoCameraModelTest, SetNameWithSuffixes)
+{
+    StereoCameraModel model;
+    model.setName("stereo", "cam1", "cam2");
+    EXPECT_EQ(model.name(), "stereo");
+    EXPECT_EQ(model.getLeftSuffix(), "cam1");
+    EXPECT_EQ(model.getRightSuffix(), "cam2");
+}
+
+TEST_F(StereoCameraModelTest, GetSuffixes)
+{
+    StereoCameraModel model;
+    EXPECT_EQ(model.getLeftSuffix(), "left");
+    EXPECT_EQ(model.getRightSuffix(), "right");
+}
+
+// Transform Tests
+
+TEST_F(StereoCameraModelTest, LocalTransform)
+{
+    Transform transform = Transform(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0);
+    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_, transform);
+    
+    EXPECT_FALSE(model.localTransform().isNull());
+    model.setLocalTransform(Transform());
+    EXPECT_TRUE(model.localTransform().isNull());
+}
+
+TEST_F(StereoCameraModelTest, StereoTransform)
+{
+    CameraModel left("left", imageSize_, K_, D_, R_, P_left_);
+    CameraModel right("right", imageSize_, K_, D_, R_, P_right_);
+    StereoCameraModel model("stereo", left, right, R_stereo_, T_);
+    
+    Transform stereoTransform = model.stereoTransform();
+    EXPECT_FALSE(stereoTransform.isNull());
+    
+    // Stereo transform represents left camera relative to right camera coordinate system
+    // For a baseline of 0.12 m, the x value should be -0.12 (negative)
+    EXPECT_NEAR(stereoTransform.x(), -baseline_, 0.001);
+}
+
+TEST_F(StereoCameraModelTest, StereoTransformBaselineExample)
+{
+    // Test the specific example from documentation: 15 cm baseline -> -0.15 x value
+    double testBaseline = 0.15; // 15 cm
+    
+    CameraModel left("left", imageSize_, K_, D_, R_, P_left_);
+    CameraModel right("right", imageSize_, K_, D_, R_, P_right_);
+    
+    // Create T with negative baseline (left camera relative to right camera coordinate system)
+    cv::Mat T_test = (cv::Mat_<double>(3, 1) << -testBaseline, 0.0, 0.0);
+    cv::Mat R_test = cv::Mat::eye(3, 3, CV_64FC1);
+    
+    StereoCameraModel model("stereo", left, right, R_test, T_test);
+    
+    Transform stereoTransform = model.stereoTransform();
+    EXPECT_FALSE(stereoTransform.isNull());
+    
+    // Verify the x value is -0.15 as documented
+    EXPECT_NEAR(stereoTransform.x(), -0.15, 0.001);
+    
+    // Verify baseline matches
+    EXPECT_NEAR(model.baseline(), testBaseline, 0.001);
+}
+
+// Scaling and ROI Tests
+
+TEST_F(StereoCameraModelTest, Scale)
+{
+    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
+    model.setImageSize(imageSize_);
+    
+    double scale = 0.5;
+    model.scale(scale);
+    
+    EXPECT_NEAR(model.left().fx(), fx_ * scale, 0.01);
+    EXPECT_NEAR(model.right().fx(), fx_ * scale, 0.01);
+    EXPECT_EQ(model.left().imageWidth(), static_cast<int>(imageWidth_ * scale));
+    // Baseline should not be scaled (it's a physical distance)
+    EXPECT_NEAR(model.baseline(), baseline_, 0.001);
+}
+
+TEST_F(StereoCameraModelTest, ROI)
+{
+    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
+    model.setImageSize(imageSize_);
+    
+    cv::Rect roi(100, 100, 200, 200);
+    model.roi(roi);
+    
+    EXPECT_EQ(model.left().imageWidth(), roi.width);
+    EXPECT_EQ(model.left().imageHeight(), roi.height);
+    EXPECT_EQ(model.right().imageWidth(), roi.width);
+    EXPECT_EQ(model.right().imageHeight(), roi.height);
+}
+
+// SetImageSize Tests
+
+TEST_F(StereoCameraModelTest, SetImageSize)
+{
+    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
+    
+    cv::Size newSize(320, 240);
+    model.setImageSize(newSize);
+    
+    EXPECT_EQ(model.left().imageSize(), newSize);
+    EXPECT_EQ(model.right().imageSize(), newSize);
+}
+
+// Serialization Tests
+
+TEST_F(StereoCameraModelTest, SerializeDeserialize)
+{
+    StereoCameraModel original(fx_, fy_, cx_, cy_, baseline_);
+    original.setName("test_stereo");
+    original.setImageSize(imageSize_);
+    
+    std::vector<unsigned char> data = original.serialize();
+    EXPECT_FALSE(data.empty());
+    
+    StereoCameraModel restored;
+    unsigned int bytesRead = restored.deserialize(data);
+    EXPECT_GT(bytesRead, 0u);
+    
+    EXPECT_NEAR(restored.baseline(), original.baseline(), 0.001);
+    EXPECT_DOUBLE_EQ(restored.left().fx(), original.left().fx());
+    EXPECT_DOUBLE_EQ(restored.right().fx(), original.right().fx());
+}
+
+TEST_F(StereoCameraModelTest, SerializeDeserializeFromPointer)
+{
+    StereoCameraModel original(fx_, fy_, cx_, cy_, baseline_);
+    original.setName("test_stereo");
+    
+    std::vector<unsigned char> data = original.serialize();
+    
+    StereoCameraModel restored;
+    unsigned int bytesRead = restored.deserialize(data.data(), data.size());
+    EXPECT_GT(bytesRead, 0u);
+    EXPECT_EQ(bytesRead, data.size());
+    
+    EXPECT_NEAR(restored.baseline(), original.baseline(), 0.001);
+}
+
+// Round-trip Depth/Disparity Tests
+
+TEST_F(StereoCameraModelTest, DepthDisparityRoundTrip)
+{
+    StereoCameraModel model(fx_, fy_, cx_, cy_, baseline_);
+    
+    // Test multiple depths
+    float testDepths[] = {0.5f, 1.0f, 2.0f, 5.0f, 10.0f};
+    
+    for(float depth : testDepths)
+    {
+        float disparity = model.computeDisparity(depth);
+        float computedDepth = model.computeDepth(disparity);
+        EXPECT_NEAR(computedDepth, depth, 0.01f) << "Depth: " << depth;
+    }
+}
+
+// Edge Cases
+
+TEST_F(StereoCameraModelTest, InvalidBaseline)
+{
+    StereoCameraModel model(fx_, fy_, cx_, cy_, 0.0); // Zero baseline
+    EXPECT_FALSE(model.isValidForProjection());
+    EXPECT_EQ(model.baseline(), 0.0);
+}
+
+TEST_F(StereoCameraModelTest, NegativeBaseline)
+{
+    // Negative baseline should still compute, but may not be physically meaningful
+    StereoCameraModel model(fx_, fy_, cx_, cy_, -0.12);
+    EXPECT_DOUBLE_EQ(model.baseline(),-0.12);
+}
+
+// Save/Load Tests
+
+TEST_F(StereoCameraModelTest, SaveLoadRoundTrip)
+{
+    // Create a temporary directory for testing
+    std::string testDir = "test_stereo_calibration";
+    UDirectory::makeDir(testDir);
+    
+    // Create original stereo camera model with full parameters
+    std::string cameraName = "test_stereo";
+    CameraModel left("left", imageSize_, K_, D_, R_, P_left_);
+    CameraModel right("right", imageSize_, K_, D_, R_, P_right_);
+    StereoCameraModel original(cameraName, left, right, R_stereo_, T_);
+    
+    // Save the model (with stereo transform)
+    bool saveResult = original.save(testDir, false);
+    EXPECT_TRUE(saveResult);
+    
+    // Verify files were created
+    std::string leftFile = testDir + "/" + cameraName + "_left.yaml";
+    std::string rightFile = testDir + "/" + cameraName + "_right.yaml";
+    std::string poseFile = testDir + "/" + cameraName + "_pose.yaml";
+    EXPECT_TRUE(UFile::exists(leftFile));
+    EXPECT_TRUE(UFile::exists(rightFile));
+    EXPECT_TRUE(UFile::exists(poseFile));
+    
+    // Load the model back
+    StereoCameraModel loaded;
+    bool loadResult = loaded.load(testDir, cameraName, false);
+    EXPECT_TRUE(loadResult);
+    
+    // Verify all parameters match
+    EXPECT_STREQ(loaded.name().c_str(), original.name().c_str());
+    EXPECT_NEAR(loaded.baseline(), original.baseline(), 0.001);
+    
+    // Verify left camera parameters
+    EXPECT_DOUBLE_EQ(loaded.left().fx(), original.left().fx());
+    EXPECT_DOUBLE_EQ(loaded.left().fy(), original.left().fy());
+    EXPECT_DOUBLE_EQ(loaded.left().cx(), original.left().cx());
+    EXPECT_DOUBLE_EQ(loaded.left().cy(), original.left().cy());
+    EXPECT_DOUBLE_EQ(loaded.left().Tx(), original.left().Tx());
+    EXPECT_EQ(loaded.left().imageSize(), original.left().imageSize());
+    
+    // Verify right camera parameters
+    EXPECT_DOUBLE_EQ(loaded.right().fx(), original.right().fx());
+    EXPECT_DOUBLE_EQ(loaded.right().fy(), original.right().fy());
+    EXPECT_DOUBLE_EQ(loaded.right().cx(), original.right().cx());
+    EXPECT_DOUBLE_EQ(loaded.right().cy(), original.right().cy());
+    EXPECT_DOUBLE_EQ(loaded.right().Tx(), original.right().Tx());
+    EXPECT_EQ(loaded.right().imageSize(), original.right().imageSize());
+    
+    // Verify stereo extrinsic matrices
+    cv::Mat R_loaded = loaded.R();
+    cv::Mat R_original = original.R();
+    if(!R_loaded.empty() && !R_original.empty())
+    {
+        EXPECT_EQ(R_loaded.rows, R_original.rows);
+        EXPECT_EQ(R_loaded.cols, R_original.cols);
+        for(int i = 0; i < R_loaded.rows; ++i)
+        {
+            for(int j = 0; j < R_loaded.cols; ++j)
+            {
+                EXPECT_NEAR(R_loaded.at<double>(i, j), R_original.at<double>(i, j), 0.001);
+            }
+        }
+    }
+    
+    cv::Mat T_loaded = loaded.T();
+    cv::Mat T_original = original.T();
+    if(!T_loaded.empty() && !T_original.empty())
+    {
+        EXPECT_EQ(T_loaded.rows, T_original.rows);
+        EXPECT_EQ(T_loaded.cols, T_original.cols);
+        for(int i = 0; i < T_loaded.rows; ++i)
+        {
+            for(int j = 0; j < T_loaded.cols; ++j)
+            {
+                EXPECT_NEAR(T_loaded.at<double>(i, j), T_original.at<double>(i, j), 0.001);
+            }
+        }
+    }
+    
+    // Verify stereo transform (left camera relative to right camera coordinate system)
+    Transform stereoTransform_loaded = loaded.stereoTransform();
+    Transform stereoTransform_original = original.stereoTransform();
+    if(!stereoTransform_loaded.isNull() && !stereoTransform_original.isNull())
+    {
+        // Compare transform matrices element by element
+        for(int i = 0; i < 3; ++i)
+        {
+            for(int j = 0; j < 4; ++j)
+            {
+                EXPECT_NEAR(stereoTransform_loaded.data()[i*4+j], stereoTransform_original.data()[i*4+j], 0.001);
+            }
+        }
+        
+        // Verify x value is negative baseline (left camera relative to right camera coordinate system)
+        EXPECT_NEAR(stereoTransform_loaded.x(), -original.baseline(), 0.001);
+        EXPECT_NEAR(stereoTransform_original.x(), -original.baseline(), 0.001);
+    }
+    
+    // Verify local transform
+    Transform localTransform_loaded = loaded.localTransform();
+    Transform localTransform_original = original.localTransform();
+    if(!localTransform_loaded.isNull() && !localTransform_original.isNull())
+    {
+        for(int i = 0; i < 3; ++i)
+        {
+            for(int j = 0; j < 4; ++j)
+            {
+                EXPECT_NEAR(localTransform_loaded.data()[i*4+j], localTransform_original.data()[i*4+j], 0.001);
+            }
+        }
+    }
+    
+    // Verify validation states
+    EXPECT_EQ(loaded.isValidForProjection(), original.isValidForProjection());
+    EXPECT_EQ(loaded.isValidForRectification(), original.isValidForRectification());
+}
+
+TEST_F(StereoCameraModelTest, SaveLoadRoundTripIgnoreTransform)
+{
+    // Create a temporary directory for testing
+    std::string testDir = "test_stereo_calibration2";
+    UDirectory::makeDir(testDir);
+    
+    // Create original stereo camera model
+    std::string cameraName = "test_stereo2";
+    StereoCameraModel original(cameraName, fx_, fy_, cx_, cy_, baseline_);
+    original.setImageSize(imageSize_);
+    
+    // Save the model (without stereo transform)
+    bool saveResult = original.save(testDir, true);
+    EXPECT_TRUE(saveResult);
+    
+    // Verify camera files were created (but not pose file)
+    std::string leftFile = testDir + "/" + cameraName + "_left.yaml";
+    std::string rightFile = testDir + "/" + cameraName + "_right.yaml";
+    EXPECT_TRUE(UFile::exists(leftFile));
+    EXPECT_TRUE(UFile::exists(rightFile));
+    
+    // Load the model back (ignoring stereo transform)
+    StereoCameraModel loaded;
+    bool loadResult = loaded.load(testDir, cameraName, true);
+    EXPECT_TRUE(loadResult);
+    
+    // Verify parameters match
+    EXPECT_EQ(loaded.name(), original.name());
+    EXPECT_NEAR(loaded.baseline(), original.baseline(), 0.001);
+    EXPECT_DOUBLE_EQ(loaded.left().fx(), original.left().fx());
+    EXPECT_DOUBLE_EQ(loaded.right().fx(), original.right().fx());
+}
+
+TEST_F(StereoCameraModelTest, SaveLoadRoundTripMinimal)
+{
+    // Create a temporary directory for testing
+    std::string testDir = "test_stereo_calibration3";
+    UDirectory::makeDir(testDir);
+    
+    // Create minimal stereo camera model
+    std::string cameraName = "minimal_stereo";
+    StereoCameraModel original(cameraName, fx_, fy_, cx_, cy_, baseline_);
+    original.setImageSize(imageSize_);
+    
+    // Save the model
+    bool saveResult = original.save(testDir);
+    EXPECT_TRUE(saveResult);
+    
+    // Load the model back
+    StereoCameraModel loaded;
+    bool loadResult = loaded.load(testDir, cameraName);
+    EXPECT_TRUE(loadResult);
+    
+    // Verify parameters match
+    EXPECT_EQ(loaded.name(), original.name());
+    EXPECT_NEAR(loaded.baseline(), original.baseline(), 0.001);
+    EXPECT_DOUBLE_EQ(loaded.left().fx(), original.left().fx());
+    EXPECT_DOUBLE_EQ(loaded.right().fx(), original.right().fx());
+    EXPECT_EQ(loaded.left().imageSize(), original.left().imageSize());
+    EXPECT_EQ(loaded.right().imageSize(), original.right().imageSize());
+}
+
+TEST_F(StereoCameraModelTest, SaveStereoTransform)
+{
+    // Create a temporary directory for testing
+    std::string testDir = "test_stereo_calibration4";
+    UDirectory::makeDir(testDir);
+    
+    // Create stereo camera model with extrinsics
+    std::string cameraName = "stereo_with_extrinsics";
+    CameraModel left("left", imageSize_, K_, D_, R_, P_left_);
+    CameraModel right("right", imageSize_, K_, D_, R_, P_right_);
+    StereoCameraModel model(cameraName, left, right, R_stereo_, T_);
+    
+    // Save stereo transform separately
+    bool saveResult = model.saveStereoTransform(testDir);
+    EXPECT_TRUE(saveResult);
+    
+    // Verify pose file was created
+    std::string poseFile = testDir + "/" + cameraName + "_pose.yaml";
+    EXPECT_TRUE(UFile::exists(poseFile));
+}
+