i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 10:00:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Adding doc and tests (#1492)

Browse Source 
* added doc and tests for util2d.h

* updated cmake-ros ci

* Added util3d.h doc and tests

* util3d_transforms.h: Added doc and tests

* util3d_filtering.h: started doc and test

* util3d_filtering.h: more tests and doc

* Added more doc/tests

* finished util3d_filtering doc and tests

* added test for util2d::depthBleedingFiltering

* Added util3d_registration tests

* Added util3d_features.h doc/tests

* added doc/tests for util3d_correspondences.h

* added doc/gtest for util3d_mapping.h (missing hpp functions)

* finished testing util3d_mapping.hpp

* Added util3d_motion_estimation.h tests (2D->3D done)

* finished util3d_motion_estimation.h tests

* minimal util3d_surface.h

* Added Transform and VisualWord tests

* Added doc for CameraModel and StereoCameraModel

* Added more logs in ros ci

* Passing tests on fical

* improved all devcontainer

* added devcontainer kilted, fixed source setup.bash, removed ldconfig in ros-cmake workflow

* cleanup

* source ros

* Added utilite tests

* Added testing to appveyor, github actions cancellable on re-commit on same branch

* appveyor testing without all targets

* appveyor: specifying ALL_BUILD target

* Fixed Util2dTest.NMSImageBoundsRespected test

* Fixing PCL Indices error on old pcl

* Added VWDictionary tests and doc. Fixed LSH not working (fix from https://github.com/flann-lib/flann/pull/472

* fixing some appveyor CI errors, added test to check dictionary serialization against all type

* Added StereoDense, StereoBM and StereoSGBM doc and tests

* Added Stereo tests

* Added CameraModel and StereoCameraModel tests

* Added doc and test for Statistics

* Added doc/tests for Signature

* Added doc/test for SensorEvent, added doc for SensorCaptureInfo

* Added doc to SensorData

* Added SensorData tests

* Added SensorCapture and SensorCaptureThread doc and tests

* fixed sensordata test

* updated SSC test and doc

* Added doc and tests for BayesFilter class

* Enabled testing on mac, updated windows testing like on linux

* added test_link

* fixed unresolved on windows

* fixed ThreadHandle error on macos ci

* Added GPS and GeodeticCoords tests

* Added tests for compression

* Added Odometry tests (base class only)

* Added DBDriver tests

* Added coverage report

* uniformized test names

* fixing concurancy and coverage ci

* dont built tools, examples and app for coverage build

* fixed report tool rebuilt without qt compilation error

* updated coverage option

* updated coverage config

* added doc CI job

* fixing windows and mac ci errors

* Added DBDriverSqlite3 tests

* Added IMU tests

* Added Graph tests

* fixing flaky macos test

* Added IMUThread and IMUFilter tests

* Added Landmarks tests

* Added LASWriter tests

* fixing seed flaky test

* fixing flaky macos timing tests

* Added LocalGrid tests

* Added LocalGridMaker tests

* fixing ci errors

* Added GlobalMap tests

* Added doc for EnvSensor

* Added Features2D tests

* Added Registration tests

* Added RegistrationVis tests

* Added doc for Rtabmap and Memory classes

* Added Memory and Rtabmap tests

* making some tests less flaky

* lcov 1.14 support

* updated compatible tool arguments

* Added integration tests (RGB-D, Stereo, Lidar2d, Lidar3d)

* More octomap checks

* Refactored how/when python interpretor is created to simplify library usage

* Added python tests

* fixed some flaky tests

* suppressed some third party related warnings

* fixed ceres tests

* more flaky fixes

* Fixing tests without libpointmatcher

* Added RANSAC rejection filter to PCL ICP

* fixing multi platform flakiness

* Added test to detect regression

* Fixing windows pcl link error

* fixed some macos flakiness

* bigger 2D2D registration error on opencv 4.6.0

* flakiness

* fixing flaky tests on windows and mac

* flaky thread test on slow mac VM

* windows slow test

* fixing more ci erros

* fxing temp dir on windows

* Added Optimizer tests and discovered some bugs (fixed)

* fixing flaky tests in mac and windows

* Added Optimizer doc

* Added GTSAM BA, updated Ceres to use g2o ba parameters. Renamed g2o's ba related parameters to Optimizer group and used by both gtsam and ceres.

* fixing build without gtsam

* fixing home dir

* fixing python ci isssues

* Added multicam ba tests

* Added Ceres multicam BA support

* Aligned BundleAdjustment parameters with Optimizer/Strategy to avoid confusion in the code

* Added BA integration test

* Added robust graph optimization integration test

* Added loop3it test

* Added stereo20Hz test

* Added smartfactor gtsam

* Fixed bugged check and warn if python didn't return any descriptors

* Fixing gtsam version build issues

* fixing tilt on windows ci

* loosing ceres integration test for ci

* mac ci flakiness

* updating missing param in gui

* updating test bound for mac

* added appearance-based tests, set min gftt quality to quality level

* testing more stuff

* improving features2d tests

* ci flakiness

* fixing flaky ci

* ci fixes

* flaky fixes

* Added RegistrationIcp tests

* Added icp integration test with real-worl corridor like env

* intermediate nodes

* fixing enum

* Updated test to catch #1714

* Fixed 2d corridor failing on pcl

* flaky pnp test

* flaky brisk test

* Set rtabmap_integration test as long

* updating loop closure test

* flaky ci tests

* TEsting roundtrip g2o/toro save/load

* loosing test bound

* fixed cuda capable checks

* flaky tests

* Debugging test hanging

* more debugging stuff

* updating limit

* windows: disabled cuda on ci to avoid incompatible driver issue. Fixing a bad test mem allocation

* trying fixing cuda hanging issue

* fixing ci flakyness

* flaky tests

* Updated BOW flaky tests by checking min precision/recall instead of recall@100precision. Fixed signature test

* CameraModel::load() test initRectificationMap param

* test dbdriver load dictionary idsOnly

* Memory: test keepLinkedInDb param

* added dummyDictionary tests

* test intermediate nodes count

* Added MarkerDetector tests

* reverted breaking change of UMutex and USemaphore

* Features2d: fixed compiltion warnings with clang about override

* clang warnings

* fixing test build with pcl 1.8

* g2o and gtsam build errors on android

* opencv5 test fixes

* disabled testing for ios and android builds

* normalized endline characters for easier diff

* added LF CRLF rule

* bump 0.23.10. fixing doc version

* Publish rtabmap website doc from ci

* fixing MSCVC build error

* macos icp flaky test

* fixing ceres macos test bound

* ficing more flaky tests

* fixing opencv5 related test errors. Also fixed an actual bug in ENU_WGS84ToGeocentric_WGS84()

* added comment about mrpt change

* removed rosdoc2 (will add it for rtabmap_ros later)

* fixing website style

* updated download links

* locally deployable website with api

* sweep doxygen issues

* improved/revised doxygen main pages

* removed examples empty page

* Updated doxygen style

* more concise doxygen groups

* added api link on main readme

* fixing utilite test error

* fixing CommonFilteringGroundNormalsUp test

* updated precisionRecall test bounds for Freak and brief descriptors

* fixing scale check in ba tests

* disabled tests on windows cuda build (missing dlls amd runner cannot test cuda anyway)

* ceres: missing suitesparse dep in windows ci

* adjusting recall thr for fast/freak

* ficing more flaky tests

* fixing flaky tests

* disabled coverage in ros ci

* Enable integration tests for ros ci jobs

* loosing up some threshold for failing tests

* trigger cache

* fixing test data in ros ci. Updated flaky test for mac

* slaking some test limit

* Fixed rtabmap-detectMoreLoopClosures inverted output value

* loosing up sift recall on mac

* optimizer re-ordered distribution for reproducible results (mac g2o)

* macos dump test crash log

* combining all tests to save time on shared library reload. Also fixed Logs with missing arguments.

* Added ENABLE_FORMAT_ERRORS cmake option

* do test only one time

* fixed all format warnings

* format security android build errors

* less verbose tests

* updated ImuUThread test

* fixed a log

* Fixed libpointmatcher 2d normals eigen issue

* Fixing libpointmatcher conversion issues

* fixing libpointmatcher test on windows ci

* cleanup comments, relax some test thr

* disabled sequoia-intel ci build (too flaky, would need extensive testing directly on that machine)
This commit is contained in:
matlabbe
2026-08-06 13:32:20 -07:00
committed by GitHub
parent bcdb4b4546
commit ee49beaf4f
309 changed files with 67469 additions and 3070 deletions
Download Patch File Download Diff File Expand all files Collapse all files

841
corelib/test/test_cameramodel.cpp Normal file
View File

@@ -0,0 +1,841 @@
#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 (same as CameraModel.cpp)
EXPECT_NEAR(hFOV, fovX * 180.0 / CV_PI, 0.1);
EXPECT_NEAR(vFOV, fovY * 180.0 / CV_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, LoadInitRectificationMaps)
{
// Create a temporary directory for testing
std::string testDir = "test_camera_calibration5";
UDirectory::makeDir(testDir);
// A model valid for rectification, so the maps would be built on load.
std::string cameraName = "rect_camera";
CameraModel original(cameraName, imageSize_, K_, D_, R_, P_);
ASSERT_TRUE(original.isValidForRectification());
ASSERT_TRUE(original.save(testDir));
const std::string filePath = testDir + "/" + cameraName + ".yaml";
// Default: rectification maps are built while loading.
CameraModel withMaps;
EXPECT_TRUE(withMaps.load(filePath));
EXPECT_TRUE(withMaps.isRectificationMapInitialized());
// initRectificationMaps=false: everything is loaded but the maps are not
// built, so the model can be inspected without paying for them.
CameraModel withoutMaps;
EXPECT_TRUE(withoutMaps.load(filePath, false));
EXPECT_FALSE(withoutMaps.isRectificationMapInitialized());
// The calibration itself must be untouched by the flag.
EXPECT_TRUE(withoutMaps.isValidForRectification());
EXPECT_EQ(withoutMaps.name(), withMaps.name());
EXPECT_EQ(withoutMaps.imageSize(), withMaps.imageSize());
EXPECT_DOUBLE_EQ(withoutMaps.fx(), withMaps.fx());
EXPECT_DOUBLE_EQ(withoutMaps.fy(), withMaps.fy());
EXPECT_DOUBLE_EQ(withoutMaps.cx(), withMaps.cx());
EXPECT_DOUBLE_EQ(withoutMaps.cy(), withMaps.cy());
// ... so the maps can still be built afterwards on demand.
EXPECT_TRUE(withoutMaps.initRectificationMap());
EXPECT_TRUE(withoutMaps.isRectificationMapInitialized());
// Same behavior through the directory+name overload.
CameraModel withoutMapsByName;
EXPECT_TRUE(withoutMapsByName.load(testDir, cameraName, false));
EXPECT_FALSE(withoutMapsByName.isRectificationMapInitialized());
EXPECT_TRUE(withoutMapsByName.isValidForRectification());
CameraModel withMapsByName;
EXPECT_TRUE(withMapsByName.load(testDir, cameraName));
EXPECT_TRUE(withMapsByName.isRectificationMapInitialized());
}
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());
}