From 190ff993b4e3d9c67b73d575da6ced1d6f84e5a9 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Mon, 7 Oct 2024 13:43:23 -0600 Subject: [PATCH] Port pinhole camera from ROS1 (#4) * add upstream pinhole-camera-related files * fix pinhole camera --------- Co-authored-by: Bilal Gill --- .clang-tidy | 4 + fuse_variables/CMakeLists.txt | 2 + .../include/fuse_variables/pinhole_camera.hpp | 224 ++++++++ .../fuse_variables/pinhole_camera_fixed.hpp | 124 +++++ fuse_variables/src/pinhole_camera.cpp | 86 ++++ fuse_variables/src/pinhole_camera_fixed.cpp | 62 +++ fuse_variables/test/CMakeLists.txt | 2 + fuse_variables/test/test_pinhole_camera.cpp | 461 +++++++++++++++++ .../test/test_pinhole_camera_fixed.cpp | 477 ++++++++++++++++++ 9 files changed, 1442 insertions(+) create mode 100644 fuse_variables/include/fuse_variables/pinhole_camera.hpp create mode 100644 fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp create mode 100644 fuse_variables/src/pinhole_camera.cpp create mode 100644 fuse_variables/src/pinhole_camera_fixed.cpp create mode 100644 fuse_variables/test/test_pinhole_camera.cpp create mode 100644 fuse_variables/test/test_pinhole_camera_fixed.cpp diff --git a/.clang-tidy b/.clang-tidy index 5c44be68..19ae154e 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -13,8 +13,12 @@ Checks: -*, -bugprone-exception-escape, -cppcoreguidelines-avoid-magic-numbers, -cppcoreguidelines-non-private-member-variables-in-classes, + -cppcoreguidelines-pro-bounds-constant-array-index, + -cppcoreguidelines-pro-bounds-pointer-arithmetic, -google-readability-casting, + -google-default-arguments, -misc-include-cleaner, + -misc-non-private-member-variables-in-classes, -modernize-use-trailing-return-type, -modernize-avoid-bind, -readability-identifier-length, diff --git a/fuse_variables/CMakeLists.txt b/fuse_variables/CMakeLists.txt index 845de66a..e74127af 100644 --- a/fuse_variables/CMakeLists.txt +++ b/fuse_variables/CMakeLists.txt @@ -30,6 +30,8 @@ add_library( src/acceleration_linear_3d_stamped.cpp src/orientation_2d_stamped.cpp src/orientation_3d_stamped.cpp + src/pinhole_camera.cpp + src/pinhole_camera_fixed.cpp src/point_2d_fixed_landmark.cpp src/point_2d_landmark.cpp src/point_3d_fixed_landmark.cpp diff --git a/fuse_variables/include/fuse_variables/pinhole_camera.hpp b/fuse_variables/include/fuse_variables/pinhole_camera.hpp new file mode 100644 index 00000000..c74f32e3 --- /dev/null +++ b/fuse_variables/include/fuse_variables/pinhole_camera.hpp @@ -0,0 +1,224 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created: 11.13.2023 + * + * Copyright (c) 2023, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_VARIABLES_PINHOLE_CAMERA_H +#define FUSE_VARIABLES_PINHOLE_CAMERA_H + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace fuse_variables +{ +/** + * @brief Variable representing intrinsic parameters of a camera. + * + * The UUID of this class is constant after + * construction and dependent on a user input database id. As such, the database id cannot be altered after + * construction. + */ +class PinholeCamera : public FixedSizeVariable<4> +{ +public: + FUSE_VARIABLE_DEFINITIONS(PinholeCamera); + + /** + * @brief Can be used to directly index variables in the data array + */ + enum : size_t + { + FX = 0, + FY = 1, + CX = 2, + CY = 3 + }; + + /** + * @brief Default constructor + */ + PinholeCamera() = default; + + /** + * @brief Construct a pinhole camera variable given a camera id + * + * @param[in] camera_id The id associated to a camera + */ + explicit PinholeCamera(const uint64_t& camera_id); + + /** + * @brief Construct a pinhole camera variable given a camera id and intrinsic parameters + * + * @param[in] camera_id The id associated to a camera + */ + explicit PinholeCamera(const fuse_core::UUID& uuid, const uint64_t& camera_id, const double& fx, const double& fy, + const double& cx, const double& cy); + + /** + * @brief Read-write access to the cx parameter. + */ + double& cx() + { + return data_[CX]; + } + + /** + * @brief Read-only access to the cx parameter. + */ + const double& cx() const + { + return data_[CX]; + } + + /** + * @brief Read-write access to the cy parameter. + */ + double& cy() + { + return data_[CY]; + } + + /** + * @brief Read-only access to the cy parameter. + */ + const double& cy() const + { + return data_[CY]; + } + + /** + * @brief Read-write access to the fx parameter. + */ + double& fx() + { + return data_[FX]; + } + + /** + * @brief Read-only access to the fx parameter. + */ + const double& fx() const + { + return data_[FX]; + } + + /** + * @brief Read-write access to the fy parameter. + */ + double& fy() + { + return data_[FY]; + } + + /** + * @brief Read-only access to the fy parameter. + */ + const double& fy() const + { + return data_[FY]; + } + + /** + * @brief Read-only access to the id + */ + const uint64_t& id() const + { + return id_; + } + + /** + * @brief Print a human-readable description of the variable to the provided + * stream. + * + * @param[out] stream The stream to write to. Defaults to stdout. + */ + void print(std::ostream& stream = std::cout) const override; + +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold* manifold() const override + { + return nullptr; + } +#endif + +protected: + /** + * @brief Construct a point 3D variable given a camera id + * + * @param[in] camera_id The id associated to a camera + */ + PinholeCamera(const fuse_core::UUID& uuid, const uint64_t& camera_id); + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + uint64_t id_{ 0 }; + + /** + * @brief The Boost Serialize method that serializes all of the data members + * in to/out of the archive + * + * @param[in/out] archive - The archive object that holds the serialized class + * members + * @param[in] version - The version of the archive being read/written. + * Generally unused. + */ + template + void serialize(Archive& archive, const unsigned int /* version */) + { + archive& boost::serialization::base_object>(*this); + archive& id_; + } +}; + +} // namespace fuse_variables + +BOOST_CLASS_EXPORT_KEY(fuse_variables::PinholeCamera); + +#endif // FUSE_VARIABLES_PINHOLE_CAMERA_H diff --git a/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp new file mode 100644 index 00000000..bebfed26 --- /dev/null +++ b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.hpp @@ -0,0 +1,124 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created: 11.13.2023 + * + * Copyright (c) 2023, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_VARIABLES_PINHOLE_CAMERA_FIXED_H +#define FUSE_VARIABLES_PINHOLE_CAMERA_FIXED_H + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace fuse_variables +{ +/** + * @brief Variable representing a pinhole camera that exists across time. + * + */ +class PinholeCameraFixed : public PinholeCamera +{ +public: + FUSE_VARIABLE_DEFINITIONS(PinholeCameraFixed); + + /** + * @brief Default constructor + */ + PinholeCameraFixed() = default; + + /** + * @brief Construct a point 2D variable given a landmarks id + * + * @param[in] camera_id The id associated to a camera + */ + explicit PinholeCameraFixed(const uint64_t& camera_id); + + /** + * @brief Construct a pinhole camera variable given a camera id and intrinsic parameters + * + * @param[in] camera_id The id associated to a camera + */ + explicit PinholeCameraFixed(const uint64_t& camera_id, const double& fx, const double& fy, const double& cx, + const double& cy); + /** + * @brief Specifies if the value of the variable should not be changed during optimization + */ + bool holdConstant() const override + { + return true; + } + +#if CERES_SUPPORTS_MANIFOLDS + /** + * @brief Create a null Ceres manifold + * + * Overriding the manifold() method prevents additional processing with the ManifoldAdapter + */ + fuse_core::Manifold* manifold() const override + { + return nullptr; + } +#endif + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @brief The Boost Serialize method that serializes all of the data members + * in to/out of the archive + * + * @param[in/out] archive - The archive object that holds the serialized class + * members + * @param[in] version - The version of the archive being read/written. + * Generally unused. + */ + template + void serialize(Archive& archive, const unsigned int /* version */) + { + archive& boost::serialization::base_object(*this); + } +}; + +} // namespace fuse_variables + +BOOST_CLASS_EXPORT_KEY(fuse_variables::PinholeCameraFixed); + +#endif // FUSE_VARIABLES_PINHOLE_CAMERA_FIXED_H diff --git a/fuse_variables/src/pinhole_camera.cpp b/fuse_variables/src/pinhole_camera.cpp new file mode 100644 index 00000000..68583c0b --- /dev/null +++ b/fuse_variables/src/pinhole_camera.cpp @@ -0,0 +1,86 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created: 11.13.2023 + * + * Copyright (c) 2023, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include + +#include +#include +#include +#include + +#include + +#include + +namespace fuse_variables +{ +PinholeCamera::PinholeCamera(const fuse_core::UUID& uuid, const uint64_t& camera_id) + : FixedSizeVariable(uuid), id_(camera_id) +{ +} + +PinholeCamera::PinholeCamera(const uint64_t& camera_id) + : PinholeCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) +{ +} + +PinholeCamera::PinholeCamera(const fuse_core::UUID& /*uuid*/, const uint64_t& camera_id, const double& fx, + const double& fy, const double& cx, const double& cy) + : PinholeCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) +{ + data_[FX] = fx; + data_[FY] = fy; + data_[CX] = cx; + data_[CY] = cy; +} + +void PinholeCamera::print(std::ostream& stream) const +{ + stream << type() << ":\n" + << " uuid: " << uuid() << "\n" + << " size: " << size() << "\n" + << " landmark id: " << id() << "\n" + << " data:\n" + << " - cx: " << cx() << "\n" + << " - cy: " << cy() << "\n" + << " - fx: " << fx() << "\n" + << " - fy: " << fy() << "\n"; +} + +} // namespace fuse_variables + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::PinholeCamera); +PLUGINLIB_EXPORT_CLASS(fuse_variables::PinholeCamera, fuse_core::Variable); diff --git a/fuse_variables/src/pinhole_camera_fixed.cpp b/fuse_variables/src/pinhole_camera_fixed.cpp new file mode 100644 index 00000000..29c0f79a --- /dev/null +++ b/fuse_variables/src/pinhole_camera_fixed.cpp @@ -0,0 +1,62 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created: 11.13.2023 + * + * Copyright (c) 2023, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include + +#include +#include +#include +#include + +#include + +namespace fuse_variables +{ +PinholeCameraFixed::PinholeCameraFixed(const uint64_t& camera_id) + : PinholeCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) +{ +} + +PinholeCameraFixed::PinholeCameraFixed(const uint64_t& camera_id, const double& fx, const double& fy, const double& cx, + const double& cy) + : PinholeCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id, fx, fy, cx, cy) +{ +} + +} // namespace fuse_variables + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::PinholeCameraFixed); +PLUGINLIB_EXPORT_CLASS(fuse_variables::PinholeCameraFixed, fuse_core::Variable); diff --git a/fuse_variables/test/CMakeLists.txt b/fuse_variables/test/CMakeLists.txt index 7bdde53c..7d534c05 100644 --- a/fuse_variables/test/CMakeLists.txt +++ b/fuse_variables/test/CMakeLists.txt @@ -8,6 +8,8 @@ set(TEST_TARGETS test_fixed_size_variable test_orientation_2d_stamped test_orientation_3d_stamped + test_pinhole_camera_fixed + test_pinhole_camera test_point_2d_fixed_landmark test_point_2d_landmark test_point_3d_fixed_landmark diff --git a/fuse_variables/test/test_pinhole_camera.cpp b/fuse_variables/test/test_pinhole_camera.cpp new file mode 100644 index 00000000..f7a9165e --- /dev/null +++ b/fuse_variables/test/test_pinhole_camera.cpp @@ -0,0 +1,461 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created: 11.13.2023 + * + * Copyright (c) 2021, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +using fuse_variables::PinholeCamera; + +TEST(PinholeCamera, Type) +{ + PinholeCamera const variable(0); + EXPECT_EQ("fuse_variables::PinholeCamera", variable.type()); +} + +TEST(PinholeCamera, UUID) +{ + // Verify two positions with the same landmark ids produce the same uuids + { + PinholeCamera const variable1(0); + PinholeCamera const variable2(0); + EXPECT_EQ(variable1.uuid(), variable2.uuid()); + } + + // Verify two positions with the different landmark ids produce different uuids + { + PinholeCamera const variable1(0); + PinholeCamera const variable2(1); + EXPECT_NE(variable1.uuid(), variable2.uuid()); + } +} + +struct CostFunctor +{ + CostFunctor() = default; + + template + bool operator()(const T* const k, T* residual) const + { + residual[0] = k[0] - T(1.2); + residual[1] = k[1] + T(0.8); + residual[2] = k[2] - T(0.51); + residual[3] = k[3] - T(0.49); + return true; + } +}; + +TEST(PinholeCamera, Optimization) +{ + // Create a Point3DLandmark + PinholeCamera k(0); + k.fx() = 4.1; + k.fy() = 3.5; + k.cx() = 5; + k.cy() = 2.49; + + // Create a simple a constraint + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(k.data(), static_cast(k.size())); + std::vector parameter_blocks; + parameter_blocks.push_back(k.data()); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_SCHUR; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(1.2, k.fx(), 1.0e-5); + EXPECT_NEAR(-0.8, k.fy(), 1.0e-5); + EXPECT_NEAR(0.51, k.cx(), 1.0e-5); + EXPECT_NEAR(0.49, k.cy(), 1.0e-5); +} + +struct FuseProjectionCostFunctor +{ + Eigen::Matrix X; + Eigen::Matrix x; + + FuseProjectionCostFunctor() + { + // Define 3D Points + X << (-1.0), (-1.0), (9.0), // NOLINT + (-1.0), (-1.0), (11.0), // NOLINT + (-1.0), (1.0), (9.0), // NOLINT + (-1.0), (1.0), (11.0), // NOLINT + (1.0), (-1.0), (9.0), // NOLINT + (1.0), (-1.0), (11.0), // NOLINT + (1.0), (1.0), (9.0), // NOLINT + (1.0), (1.0), (11.0); // NOLINT + + // Define 2D Points + x << (239.36340595030663), (166.35226250921954), (1.0), // NOLINT + (252.25926024520876), (179.34432670587358), (1.0), // NOLINT + (239.36340595030663), (309.26496867241400), (1.0), // NOLINT + (252.25926024520876), (296.27290447575996), (1.0), // NOLINT + (381.21780319423020), (166.35226250921954), (1.0), // NOLINT + (368.32194889932810), (179.34432670587358), (1.0), // NOLINT + (381.21780319423020), (309.26496867241400), (1.0), // NOLINT + (368.32194889932810), (296.27290447575996), (1.0); // NOLINT + } + + /* FuseProjectionCostFunctor + * Projects a set of 3D poitns X to 2D points x using the equation: + * x = KX + * and optiimzes K using a reprojection error as residual + * + */ + + template + bool operator()(const T* const points, T* residual) const + { + // Create Matrix + Eigen::Matrix k(3, 3); + k << points[0], T(0.0), points[2], // NOLINT + T(0.0), points[1], points[3], // NOLINT + T(0.0), T(0.0), T(1.0); // NOLINT + + Eigen::Matrix xp(8, 3); + xp = (k * X.cast().transpose()); + + // TODO(omendez): There is probably a better way to do this using hnormalized on operation above. + xp.transposeInPlace(); + xp = xp.array().colwise() / X.cast().col(2).array(); + + Eigen::Matrix d = x.cast() - xp; + + for (uint i = 0; i < 8; i++) + { + residual[static_cast(i * 2)] = T(d.row(i)[0]); + residual[static_cast(i * 2) + 1] = T(d.row(i)[1]); + } + + return true; + } +}; + +TEST(PinholeCamera, FuseProjectionOptimization) +{ + // Create a Camera to Optimize + PinholeCamera k(0); + k.fx() = 640.0; // fx == w + k.fy() = 640.0; // fy == w + k.cx() = 320.0; // cx == w/2 + k.cy() = 240.0; // cy == h/2 + + // Expected Intrinsics + PinholeCamera expected(0); + expected.fx() = 638.34478759765620; + expected.fy() = 643.10717773437500; + expected.cx() = 310.29060457226840; + expected.cy() = 237.80861559081677; + + // Create a simple a constraint + // (NOTE: Separating X and Y residuals is significantly better than joint distance. + // Therefore, 16 residuals for 8 points.) + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction(new FuseProjectionCostFunctor()); + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(k.data(), static_cast(k.size())); + std::vector parameter_blocks; + parameter_blocks.push_back(k.data()); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_SCHUR; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(expected.fx(), k.fx(), 0.1); // 0.1 Pixel Error + EXPECT_NEAR(expected.fy(), k.fy(), 0.1); + EXPECT_NEAR(expected.cx(), k.cx(), 0.1); + EXPECT_NEAR(expected.cy(), k.cy(), 0.1); +} + +struct ProjectionCostFunctor +{ + std::array, 8> X; // 3D Points (2x2 Cube at 0,0,10) + std::array, 8> x; // 2D Points (Projection of Cube w/GT intrinsics) + ProjectionCostFunctor() + : // Define 3D Points (2x2 Cube at 0,0,10) + X // NOLINT + { + -1.0, -1.0, 9.0, // NOLINT + -1.0, -1.0, 11.0, // NOLINT + -1.0, 1.0, 9.0, // NOLINT + -1.0, 1.0, 11.0, // NOLINT + 1.0, -1.0, 9.0, // NOLINT + 1.0, -1.0, 11.0, // NOLINT + 1.0, 1.0, 9.0, // NOLINT + 1.0, 1.0, 11.0 // NOLINT + } + , + // Define 2D Points + x // NOLINT + { + 239.36340595030663, 166.35226250921954, // NOLINT + 252.25926024520876, 179.34432670587358, // NOLINT + 239.36340595030663, 309.26496867241400, // NOLINT + 252.25926024520876, 296.27290447575996, // NOLINT + 381.21780319423020, 166.35226250921954, // NOLINT + 368.32194889932810, 179.34432670587358, // NOLINT + 381.21780319423020, 309.26496867241400, // NOLINT + 368.32194889932810, 296.27290447575996 // NOLINT + } + { + } + + /* ProjectionCostFunctor + * Projects a set of 3D points X to 2D points x using the equation: + * x = (x*f_x + z * c_x)/z + * y = (y*f_y + z * c_y)/z + * and optiimzes f_x, f_y, c_x, c_y using a reprojection error as residual. + * (NOTE: Separating X and Y residuals is significantly better than joint distance. + * + */ + + template + bool operator()(const T* const k, T* residual) const + { + // Do Projection Manually + std::array, 8> xp{}; + for (uint i = 0; i < 8; i++) + { + xp[i][0] = (T(X[i][0]) * k[0] + T(X[i][2]) * k[2]) / T(X[i][2]); // x = (x*f_x + z * c_x)/z + xp[i][1] = (T(X[i][1]) * k[1] + T(X[i][2]) * k[3]) / T(X[i][2]); // y = (y*f_y + z * c_y)/z + + T xerr = xp[i][0] - T(x[i][0]); + T yerr = xp[i][1] - T(x[i][1]); + residual[static_cast(i * 2)] = xerr; + residual[i * 2 + 1] = yerr; + } + return true; + } +}; + +TEST(PinholeCamera, ProjectionOptimization) +{ + // Create a Camera to Optimize + PinholeCamera k(0); + k.fx() = 640.0; // fx == w + k.fy() = 640.0; // fy == w + k.cx() = 320.0; // cx == w/2 + k.cy() = 240.0; // cy == h/2 + + PinholeCamera expected(0); + expected.fx() = 638.34478759765620; + expected.fy() = 643.10717773437500; + expected.cx() = 310.29060457226840; + expected.cy() = 237.80861559081677; + + // Create a simple a constraint + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction(new ProjectionCostFunctor()); + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(k.data(), static_cast(k.size())); + std::vector parameter_blocks; + parameter_blocks.push_back(k.data()); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); + + // Run the solver + ceres::Solver::Options const options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(expected.fx(), k.fx(), 0.1); // 0.1 Pixel Error + EXPECT_NEAR(expected.fy(), k.fy(), 0.1); + EXPECT_NEAR(expected.cx(), k.cx(), 0.1); + EXPECT_NEAR(expected.cy(), k.cy(), 0.1); +} + +struct PerPointProjectionCostFunctor +{ + double pt3d_x; + double pt3d_y; + double pt3d_z; + double observed_x; + double observed_y; + + PerPointProjectionCostFunctor(double pt3d_x, double pt3d_y, double pt3d_z, double observed_x, double observed_y) + : pt3d_x(pt3d_x), pt3d_y(pt3d_y), pt3d_z(pt3d_z), observed_x(observed_x), observed_y(observed_y) + { + } + + /* PerPointProjectionCostFunctor + * + * Projects a 3D point XYZ to 2D xy points using the equation: + * x = (X*f_x + Z * c_x)/Z + * y = (Y*f_y + Z * c_y)/Z + * and optiimzes f_x, f_y, c_x, c_y using a reprojection error as residual. + * (NOTE: Separating X and Y residuals is significantly better than joint distance. + * + */ + + template + bool operator()(const T* const k, T* residual) const + { + // Do Projection Manually + T xp = (T(pt3d_x) * k[0] + T(pt3d_z) * k[2]) / T(pt3d_z); // x = (x*f_x + z * c_x)/z + T yp = (T(pt3d_y) * k[1] + T(pt3d_z) * k[3]) / T(pt3d_z); // y = (y*f_y + z * c_y)/z + + residual[0] = T(xp - T(observed_x)); + residual[1] = T(yp - T(observed_y)); + + return true; + } +}; + +TEST(PinholeCamera, PerPointProjectionCostFunctor) +{ + // Create a Camera to Optimize + PinholeCamera k(0); + k.fx() = 645.0; // fx == w + k.fy() = 635.0; // fy == w + k.cx() = 325.0; // cx == w/2 + k.cy() = 245.0; // cy == h/2 + + PinholeCamera expected(0); + expected.fx() = 638.34478759765620; + expected.fy() = 643.10717773437500; + expected.cx() = 310.29060457226840; + expected.cy() = 237.80861559081677; + + double X[8][3] = { -1.0, -1.0, 9.0, // NOLINT + -1.0, -1.0, 11.0, // NOLINT + -1.0, 1.0, 9.0, // NOLINT + -1.0, 1.0, 11.0, // NOLINT + 1.0, -1.0, 9.0, // NOLINT + 1.0, -1.0, 11.0, // NOLINT + 1.0, 1.0, 9.0, // NOLINT + 1.0, 1.0, 11.0 }; // NOLINT + + // Define 2D Points + double pts2d[8][2] = { 239.36340595030663, 166.35226250921954, // NOLINT + 252.25926024520876, 179.34432670587358, // NOLINT + 239.36340595030663, 309.26496867241400, // NOLINT + 252.25926024520876, 296.27290447575996, // NOLINT + 381.21780319423020, 166.35226250921954, // NOLINT + 368.32194889932810, 179.34432670587358, // NOLINT + 381.21780319423020, 309.26496867241400, // NOLINT + 368.32194889932810, 296.27290447575996 }; // NOLINT + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(k.data(), static_cast(k.size())); + + std::vector camera_parameter_blocks; + camera_parameter_blocks.push_back(k.data()); + + for (uint i = 0; i < 8; i++) + { + // Create a simple a constraint + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction( + new PerPointProjectionCostFunctor(X[i][0], X[i][1], X[i][2], pts2d[i][0], pts2d[i][1])); + problem.AddResidualBlock(cost_function, nullptr, k.data()); + } + + // Run the solver + ceres::Solver::Options const options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(expected.fx(), k.fx(), 0.1); // 0.1 Pixel Error + EXPECT_NEAR(expected.fy(), k.fy(), 0.1); + EXPECT_NEAR(expected.cx(), k.cx(), 0.1); + EXPECT_NEAR(expected.cy(), k.cy(), 0.1); +} + +TEST(Point3DLandmark, Serialization) +{ + // Create a Point3DLandmark + PinholeCamera expected(0); + expected.fx() = 640; + expected.fy() = 480; + expected.cx() = 320; + expected.cy() = 240; + + // Serialize the variable into an archive + std::stringstream stream; + { + fuse_core::TextOutputArchive archive(stream); + expected.serialize(archive); + } + + // Deserialize a new variable from that same stream + PinholeCamera actual; + { + fuse_core::TextInputArchive archive(stream); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.id(), actual.id()); + EXPECT_EQ(expected.uuid(), actual.uuid()); + EXPECT_EQ(expected.fx(), actual.fx()); + EXPECT_EQ(expected.fy(), actual.fy()); + EXPECT_EQ(expected.cx(), actual.cx()); + EXPECT_EQ(expected.cy(), actual.cy()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/fuse_variables/test/test_pinhole_camera_fixed.cpp b/fuse_variables/test/test_pinhole_camera_fixed.cpp new file mode 100644 index 00000000..a364dc81 --- /dev/null +++ b/fuse_variables/test/test_pinhole_camera_fixed.cpp @@ -0,0 +1,477 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created: 11.13.2023 + * + * Copyright (c) 2021, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +using fuse_variables::PinholeCameraFixed; + +TEST(PinholeCameraFixed, Type) +{ + PinholeCameraFixed const variable(0); + EXPECT_EQ("fuse_variables::PinholeCameraFixed", variable.type()); +} + +TEST(PinholeCameraFixed, UUID) +{ + // Verify two positions with the same landmark ids produce the same uuids + { + PinholeCameraFixed const variable1(0); + PinholeCameraFixed const variable2(0); + EXPECT_EQ(variable1.uuid(), variable2.uuid()); + } + + // Verify two positions with the different landmark ids produce different uuids + { + PinholeCameraFixed const variable1(0); + PinholeCameraFixed const variable2(1); + EXPECT_NE(variable1.uuid(), variable2.uuid()); + } +} + +struct CostFunctor +{ + CostFunctor() = default; + + template + bool operator()(const T* const k, T* residual) const + { + residual[0] = k[0] - T(1.2); + residual[1] = k[1] + T(0.8); + residual[2] = k[2] - T(0.51); + residual[3] = k[3] - T(0.49); + return true; + } +}; + +TEST(PinholeCameraFixed, Optimization) +{ + // Create a Point3DLandmark + PinholeCameraFixed k(0); + k.fx() = 4.10; + k.fy() = 3.50; + k.cx() = 5.00; + k.cy() = 2.49; + + // Create a simple a constraint + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(k.data(), static_cast(k.size())); + std::vector parameter_blocks; + parameter_blocks.push_back(k.data()); + + if (k.holdConstant()) + { + problem.SetParameterBlockConstant(k.data()); + } + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_SCHUR; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(4.10, k.fx(), 1.0e-5); + EXPECT_NEAR(3.50, k.fy(), 1.0e-5); + EXPECT_NEAR(5.00, k.cx(), 1.0e-5); + EXPECT_NEAR(2.49, k.cy(), 1.0e-5); +} + +struct FuseProjectionCostFunctor +{ + Eigen::Matrix X; + Eigen::Matrix x; + + FuseProjectionCostFunctor() + { + // Define 3D Points + X << (-1.0), (-1.0), (9.0), // NOLINT + (-1.0), (-1.0), (11.0), // NOLINT + (-1.0), (1.0), (9.0), // NOLINT + (-1.0), (1.0), (11.0), // NOLINT + (1.0), (-1.0), (9.0), // NOLINT + (1.0), (-1.0), (11.0), // NOLINT + (1.0), (1.0), (9.0), // NOLINT + (1.0), (1.0), (11.0); // NOLINT + + // Define 2D Points + x << (239.36340595030663), (166.35226250921954), (1.0), // NOLINT + (252.25926024520876), (179.34432670587358), (1.0), // NOLINT + (239.36340595030663), (309.26496867241400), (1.0), // NOLINT + (252.25926024520876), (296.27290447575996), (1.0), // NOLINT + (381.21780319423020), (166.35226250921954), (1.0), // NOLINT + (368.32194889932810), (179.34432670587358), (1.0), // NOLINT + (381.21780319423020), (309.26496867241400), (1.0), // NOLINT + (368.32194889932810), (296.27290447575996), (1.0); // NOLINT + } + + /* FuseProjectionCostFunctor + * Projects a set of 3D poitns X to 2D points x using the equation: + * x = KX + * and optiimzes K using a reprojection error as residual + * + */ + + template + bool operator()(const T* const points, T* residual) const + { + // Create Matrix + Eigen::Matrix k(3, 3); + k << points[0], T(0.0), points[2], // NOLINT + T(0.0), points[1], points[3], // NOLINT + T(0.0), T(0.0), T(1.0); // NOLINT + + Eigen::Matrix xp(8, 3); + xp = (k * X.cast().transpose()); + + // TODO(omendez): There is probably a better way to do this using hnormalized on operation above. + xp.transposeInPlace(); + xp = xp.array().colwise() / X.cast().col(2).array(); + + Eigen::Matrix d = x.cast() - xp; + + for (uint i = 0; i < 8; i++) + { + residual[static_cast(i * 2)] = T(d.row(i)[0]); + residual[static_cast(i * 2) + 1] = T(d.row(i)[1]); + } + + return true; + } +}; + +TEST(PinholeCameraFixed, FuseProjectionOptimization) +{ + // Create a Camera to Optimize + PinholeCameraFixed k(0); + k.fx() = 640.0; // fx == w + k.fy() = 640.0; // fy == w + k.cx() = 320.0; // cx == w/2 + k.cy() = 240.0; // cy == h/2 + + // Expected Intrinsics + PinholeCameraFixed expected(0); + expected.fx() = 640.0; + expected.fy() = 640.0; + expected.cx() = 320.0; + expected.cy() = 240.0; + + // Create a simple a constraint + // (NOTE: Separating X and Y residuals is significantly better than joint distance. + // Therefore, 16 residuals for 8 points.) + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction(new FuseProjectionCostFunctor()); + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(k.data(), static_cast(k.size())); + std::vector parameter_blocks; + parameter_blocks.push_back(k.data()); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); + + if (k.holdConstant()) + { + problem.SetParameterBlockConstant(k.data()); + } + + // Run the solver + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_SCHUR; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(expected.fx(), k.fx(), 0.1); // 0.1 Pixel Error + EXPECT_NEAR(expected.fy(), k.fy(), 0.1); + EXPECT_NEAR(expected.cx(), k.cx(), 0.1); + EXPECT_NEAR(expected.cy(), k.cy(), 0.1); +} + +struct ProjectionCostFunctor +{ + std::array, 8> X; // 3D Points (2x2 Cube at 0,0,10) + std::array, 8> x; // 2D Points (Projection of Cube w/GT intrinsics) + ProjectionCostFunctor() + : // Define 3D Points (2x2 Cube at 0,0,10) + X // NOLINT + { + -1.0, -1.0, 9.0, // NOLINT + -1.0, -1.0, 11.0, // NOLINT + -1.0, 1.0, 9.0, // NOLINT + -1.0, 1.0, 11.0, // NOLINT + 1.0, -1.0, 9.0, // NOLINT + 1.0, -1.0, 11.0, // NOLINT + 1.0, 1.0, 9.0, // NOLINT + 1.0, 1.0, 11.0 // NOLINT + } + , + // Define 2D Points + x // NOLINT + { + 239.36340595030663, 166.35226250921954, // NOLINT + 252.25926024520876, 179.34432670587358, // NOLINT + 239.36340595030663, 309.26496867241400, // NOLINT + 252.25926024520876, 296.27290447575996, // NOLINT + 381.21780319423020, 166.35226250921954, // NOLINT + 368.32194889932810, 179.34432670587358, // NOLINT + 381.21780319423020, 309.26496867241400, // NOLINT + 368.32194889932810, 296.27290447575996 // NOLINT + } + { + } + + /* ProjectionCostFunctor + * Projects a set of 3D points X to 2D points x using the equation: + * x = (x*f_x + z * c_x)/z + * y = (y*f_y + z * c_y)/z + * and optiimzes f_x, f_y, c_x, c_y using a reprojection error as residual. + * (NOTE: Separating X and Y residuals is significantly better than joint distance. + * + */ + + template + bool operator()(const T* const k, T* residual) const + { + // Do Projection Manually + std::array, 8> xp{}; + for (uint i = 0; i < 8; i++) + { + xp[i][0] = (T(X[i][0]) * k[0] + T(X[i][2]) * k[2]) / T(X[i][2]); // x = (x*f_x + z * c_x)/z + xp[i][1] = (T(X[i][1]) * k[1] + T(X[i][2]) * k[3]) / T(X[i][2]); // y = (y*f_y + z * c_y)/z + + T xerr = xp[i][0] - T(x[i][0]); + T yerr = xp[i][1] - T(x[i][1]); + residual[static_cast(i * 2)] = xerr; + residual[static_cast(i * 2) + 1] = yerr; + } + return true; + } +}; + +TEST(PinholeCameraFixed, ProjectionOptimization) +{ + // Create a Camera to Optimize + PinholeCameraFixed k(0, 640, 640, 320, 240); + + PinholeCameraFixed expected(0); + expected.fx() = 640.0; + expected.fy() = 640.0; + expected.cx() = 320.0; + expected.cy() = 240.0; + + // Create a simple a constraint + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction(new ProjectionCostFunctor()); + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(k.data(), static_cast(k.size())); + std::vector parameter_blocks; + parameter_blocks.push_back(k.data()); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); + + if (k.holdConstant()) + { + problem.SetParameterBlockConstant(k.data()); + } + + // Run the solver + ceres::Solver::Options const options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(expected.fx(), k.fx(), 1e-5); // 0.1 Pixel Error + EXPECT_NEAR(expected.fy(), k.fy(), 1e-5); + EXPECT_NEAR(expected.cx(), k.cx(), 1e-5); + EXPECT_NEAR(expected.cy(), k.cy(), 1e-5); +} + +struct PerPointProjectionCostFunctor +{ + double pt3d_x; + double pt3d_y; + double pt3d_z; + double observed_x; + double observed_y; + + PerPointProjectionCostFunctor(double pt3d_x, double pt3d_y, double pt3d_z, double observed_x, double observed_y) + : pt3d_x(pt3d_x), pt3d_y(pt3d_y), pt3d_z(pt3d_z), observed_x(observed_x), observed_y(observed_y) + { + } + + /* PerPointProjectionCostFunctor + * + * Projects a 3D point XYZ to 2D xy points using the equation: + * x = (X*f_x + Z * c_x)/Z + * y = (Y*f_y + Z * c_y)/Z + * and optiimzes f_x, f_y, c_x, c_y using a reprojection error as residual. + * (NOTE: Separating X and Y residuals is significantly better than joint distance. + * + */ + + template + bool operator()(const T* const k, T* residual) const + { + // Do Projection Manually + T xp = (T(pt3d_x) * k[0] + T(pt3d_z) * k[2]) / T(pt3d_z); // x = (x*f_x + z * c_x)/z + T yp = (T(pt3d_y) * k[1] + T(pt3d_z) * k[3]) / T(pt3d_z); // y = (y*f_y + z * c_y)/z + + residual[0] = T(xp - T(observed_x)); + residual[1] = T(yp - T(observed_y)); + + return true; + } +}; + +TEST(PinholeCameraFixed, PerPointProjectionCostFunctor) +{ + // Create a Camera to Optimize + PinholeCameraFixed k(0); + k.fx() = 645.0; // fx == w + k.fy() = 635.0; // fy == w + k.cx() = 325.0; // cx == w/2 + k.cy() = 245.0; // cy == h/2 + + PinholeCameraFixed expected(0); + expected.fx() = 645.0; + expected.fy() = 635.0; + expected.cx() = 325.0; + expected.cy() = 245.0; + + double X[8][3] = { -1.0, -1.0, 9.0, // NOLINT + -1.0, -1.0, 11.0, // NOLINT + -1.0, 1.0, 9.0, // NOLINT + -1.0, 1.0, 11.0, // NOLINT + 1.0, -1.0, 9.0, // NOLINT + 1.0, -1.0, 11.0, // NOLINT + 1.0, 1.0, 9.0, // NOLINT + 1.0, 1.0, 11.0 }; // NOLINT + + // Define 2D Points + double pts2d[8][2] = { 239.36340595030663, 166.35226250921954, // NOLINT + 252.25926024520876, 179.34432670587358, // NOLINT + 239.36340595030663, 309.26496867241400, // NOLINT + 252.25926024520876, 296.27290447575996, // NOLINT + 381.21780319423020, 166.35226250921954, // NOLINT + 368.32194889932810, 179.34432670587358, // NOLINT + 381.21780319423020, 309.26496867241400, // NOLINT + 368.32194889932810, 296.27290447575996 }; // NOLINT + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(k.data(), static_cast(k.size())); + + std::vector camera_parameter_blocks; + camera_parameter_blocks.push_back(k.data()); + + if (k.holdConstant()) + { + problem.SetParameterBlockConstant(k.data()); + } + + for (uint i = 0; i < 8; i++) + { + // Create a simple a constraint + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction( + new PerPointProjectionCostFunctor(X[i][0], X[i][1], X[i][2], pts2d[i][0], pts2d[i][1])); + problem.AddResidualBlock(cost_function, nullptr, k.data()); + } + + // Run the solver + ceres::Solver::Options const options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(expected.fx(), k.fx(), 0.1); // 0.1 Pixel Error + EXPECT_NEAR(expected.fy(), k.fy(), 0.1); + EXPECT_NEAR(expected.cx(), k.cx(), 0.1); + EXPECT_NEAR(expected.cy(), k.cy(), 0.1); +} + +TEST(PinholeCameraFixed, Serialization) +{ + // Create a Point3DLandmark + PinholeCameraFixed expected(0); + expected.fx() = 640; + expected.fy() = 480; + expected.cx() = 320; + expected.cy() = 240; + + // Serialize the variable into an archive + std::stringstream stream; + { + fuse_core::TextOutputArchive archive(stream); + expected.serialize(archive); + } + + // Deserialize a new variable from that same stream + PinholeCameraFixed actual; + { + fuse_core::TextInputArchive archive(stream); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.id(), actual.id()); + EXPECT_EQ(expected.uuid(), actual.uuid()); + EXPECT_EQ(expected.fx(), actual.fx()); + EXPECT_EQ(expected.fy(), actual.fy()); + EXPECT_EQ(expected.cx(), actual.cx()); + EXPECT_EQ(expected.cy(), actual.cy()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}