From c93d0bc48aa265fbaa6b35aa0a9a2509eb09477a Mon Sep 17 00:00:00 2001 From: Aria Kang Date: Wed, 22 Jan 2025 12:59:34 -0800 Subject: [PATCH] {Feature} C++ - Core - Calibration - unit test for cast in CameraProjection (#180) Summary: Unit test for cast function in CameraProjectionTemplated. Reviewed By: YLouWashU Differential Revision: D68177892 --- .../camera_projections/CMakeLists.txt | 4 + .../camera_projections/test/CMakeLists.txt | 25 ++++ .../test/CameraProjectionTest.cpp | 118 ++++++++++++++++++ 3 files changed, 147 insertions(+) create mode 100644 core/calibration/camera_projections/test/CMakeLists.txt create mode 100644 core/calibration/camera_projections/test/CameraProjectionTest.cpp diff --git a/core/calibration/camera_projections/CMakeLists.txt b/core/calibration/camera_projections/CMakeLists.txt index 1afbe784a..70b0f60c8 100644 --- a/core/calibration/camera_projections/CMakeLists.txt +++ b/core/calibration/camera_projections/CMakeLists.txt @@ -12,6 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +if(BUILD_UNIT_TEST) + add_subdirectory(test) +endif() + add_library(common INTERFACE) target_sources(common INTERFACE Common.h IgnoreJetInfinitesimal.h) target_link_libraries(common INTERFACE Eigen3::Eigen Sophus::Sophus) diff --git a/core/calibration/camera_projections/test/CMakeLists.txt b/core/calibration/camera_projections/test/CMakeLists.txt new file mode 100644 index 000000000..2d2b186d0 --- /dev/null +++ b/core/calibration/camera_projections/test/CMakeLists.txt @@ -0,0 +1,25 @@ +# Copyright (c) Meta Platforms, Inc. and affiliates. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +find_package(GTest) +add_executable(camera_projection_test CameraProjectionTest.cpp) +target_link_libraries(camera_projection_test + PUBLIC + camera_projection + GTest::Main +) + +gtest_discover_tests(camera_projection_test) +add_test(NAME camera_projection_test WORKING_DIRECTORY ${CMAKE_RUNTIME_OUTPUT_DIRECTORY} + COMMAND $) diff --git a/core/calibration/camera_projections/test/CameraProjectionTest.cpp b/core/calibration/camera_projections/test/CameraProjectionTest.cpp new file mode 100644 index 000000000..9d72742e0 --- /dev/null +++ b/core/calibration/camera_projections/test/CameraProjectionTest.cpp @@ -0,0 +1,118 @@ +/* + * Copyright (c) Meta Platforms, Inc. and affiliates. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#define DEFAULT_LOG_CHANNEL "CameraProjectionTest" +using namespace projectaria::tools::calibration; +static const std::unordered_map + kTestProjectionParams = { + {CameraProjection::ModelType::Linear, + Eigen::VectorXd::Map( + std::array{ + {556.9648709428538, 555.9836002606175, 319.5000821079618, 239.500905501914}} + .data(), + 4)}, + {CameraProjection::ModelType::KannalaBrandtK3, + Eigen::VectorXd::Map( + std::array{{556.9648709428538, + 555.9836002606175, + 319.5000821079618, + 239.500905501914, + 0.03981131761869532, + -0.03980438501559722, + 0.1244291105385938, + -0.2780730001905606}} + .data(), + 8)}, + {CameraProjection::ModelType::Fisheye624, + Eigen::VectorXd::Map( + std::array{{242.0503165415735, + 319.8053191211108, + 240.9679064324467, + -0.02689165420917155, + 0.1019919163316408, + -0.07180862612716554, + 0.01245898233008245, + 0.001242086400994195, + -0.0004149914187376608, + 0.0005499786009926172, + 0.0005819661606400815, + -0.001232007804086031, + -0.0003868334860997226, + -0.0005963449586486949, + 1.071922498768078e-05}} + .data(), + 15)}, + {CameraProjection::ModelType::Spherical, + Eigen::VectorXd::Map(std::array{{800.0, 800.0, 640.0, 360.0}}.data(), 4)}}; +static const Eigen::Matrix kPtInCameraFloat(1.0f, 1.0f, 5.0f); +static const Eigen::Matrix kPtInCameraDouble(1.0, 1.0, 5.0); +static const Eigen::Matrix kCameraPixelFloat(340.0f, 240.0f); +static const Eigen::Matrix kCameraPixelDouble(340.0, 240.0); +static constexpr double kDesiredPrecision = 1e-5; + +void compareEigenVector( + const Eigen::VectorXf& vectorInFloat, + const Eigen::VectorXd& vectorInDouble, + const std::string& testNamePrefix) { + double differenceNorm = (vectorInFloat.cast() - vectorInDouble).norm(); + bool are_close = differenceNorm <= + kDesiredPrecision * std::min(vectorInFloat.cast().norm(), vectorInDouble.norm()); + EXPECT_TRUE(are_close); + if (!are_close) { + fmt::print( + "{}: differenceNorm / minNorm: {:.5f}\n", + testNamePrefix, + differenceNorm / std::min(vectorInFloat.cast().norm(), vectorInDouble.norm())); + } +} + +void testCastForSinglePair( + const CameraProjectionTemplated& cameraProjectionDouble, + const CameraProjectionTemplated& cameraProjectionFloat, + const std::string& testNamePrefix) { + compareEigenVector( + cameraProjectionFloat.projectionParams(), + cameraProjectionDouble.projectionParams(), + testNamePrefix + "projectionParams"); + + compareEigenVector( + cameraProjectionFloat.project(kPtInCameraFloat), + cameraProjectionDouble.project(kPtInCameraDouble), + testNamePrefix + "project"); + + compareEigenVector( + cameraProjectionFloat.unproject(kCameraPixelFloat), + cameraProjectionDouble.unproject(kCameraPixelDouble), + testNamePrefix + "unproject"); +} + +TEST(CameraProjectionTest, FloatDoubleParamsComparison) { + for (const auto& [modelType, projectionParamsDouble] : kTestProjectionParams) { + CameraProjectionTemplated cameraProjectionDouble(modelType, projectionParamsDouble); + CameraProjectionTemplated cameraProjectionFloat = cameraProjectionDouble.cast(); + testCastForSinglePair(cameraProjectionDouble, cameraProjectionFloat, "DtoF, "); + + CameraProjectionTemplated cameraProjectionFloat2( + static_cast::ModelType>(modelType), + projectionParamsDouble.cast()); + CameraProjectionTemplated cameraProjectionDouble2 = + cameraProjectionFloat2.cast(); + testCastForSinglePair(cameraProjectionDouble2, cameraProjectionFloat2, "FtoD, "); + } +}