From 75ea820017b840c15790b32c3714363b3f80c148 Mon Sep 17 00:00:00 2001 From: Oscar Mendez Date: Tue, 5 Dec 2023 15:26:34 +0000 Subject: [PATCH] Vision constraints (#349) * FIrst commit for pose estimation from fixed 3D landmarks. Includes a pinhole camera model (no distortion), along with a fixed version and unit tests. Also includes a fixed landmark constraint which optimizes for pose and optionally calibration, along with unit tests. * Updating camera variables, tests and constraints to allow initialisation from a set of 3D points to support multi-marker configurations. * Fixes from PR review. * Adding validations for pts3d_ in cost functor. --- fuse_constraints/CMakeLists.txt | 25 + .../fixed_3d_landmark_constraint.h | 229 ++++++ .../fixed_3d_landmark_cost_functor.h | 195 +++++ .../src/fixed_3d_landmark_constraint.cpp | 117 +++ .../test_fixed_3d_landmark_constraint.cpp | 681 ++++++++++++++++++ fuse_variables/CMakeLists.txt | 50 ++ .../include/fuse_variables/pinhole_camera.h | 184 +++++ .../fuse_variables/pinhole_camera_fixed.h | 108 +++ fuse_variables/src/pinhole_camera.cpp | 87 +++ fuse_variables/src/pinhole_camera_fixed.cpp | 63 ++ fuse_variables/test/test_pinhole_camera.cpp | 462 ++++++++++++ .../test/test_pinhole_camera_fixed.cpp | 478 ++++++++++++ 12 files changed, 2679 insertions(+) create mode 100644 fuse_constraints/include/fuse_constraints/fixed_3d_landmark_constraint.h create mode 100644 fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h create mode 100644 fuse_constraints/src/fixed_3d_landmark_constraint.cpp create mode 100644 fuse_constraints/test/test_fixed_3d_landmark_constraint.cpp create mode 100644 fuse_variables/include/fuse_variables/pinhole_camera.h create mode 100644 fuse_variables/include/fuse_variables/pinhole_camera_fixed.h 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/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 2f8f83de6..c5886f3d4 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -44,6 +44,7 @@ add_library(${PROJECT_NAME} src/absolute_orientation_3d_stamped_euler_constraint.cpp src/absolute_pose_2d_stamped_constraint.cpp src/absolute_pose_3d_stamped_constraint.cpp + src/fixed_3d_landmark_constraint.cpp src/marginal_constraint.cpp src/marginal_cost_function.cpp src/marginalize_variables.cpp @@ -241,6 +242,30 @@ if(CATKIN_ENABLE_TESTING) CXX_STANDARD_REQUIRED YES ) + # Fixed 3D Landmark Constraint Tests + catkin_add_gtest(test_fixed_3d_landmark_constraint + test/test_fixed_3d_landmark_constraint.cpp + ) + add_dependencies(test_fixed_3d_landmark_constraint + ${catkin_EXPORTED_TARGETS} + ) + target_include_directories(test_fixed_3d_landmark_constraint + PRIVATE + include + ${catkin_INCLUDE_DIRS} + ${CERES_INCLUDE_DIRS} + ) + target_link_libraries(test_fixed_3d_landmark_constraint + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${CERES_LIBRARIES} + ) + set_target_properties(test_fixed_3d_landmark_constraint + PROPERTIES + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED YES + ) + # Absolute Pose 3D Stamped Constraint Tests catkin_add_gtest(test_absolute_pose_3d_stamped_constraint test/test_absolute_pose_3d_stamped_constraint.cpp diff --git a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_constraint.h b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_constraint.h new file mode 100644 index 000000000..032638c2d --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_constraint.h @@ -0,0 +1,229 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created on Mon Nov 12 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_CONSTRAINTS_FIXED_3D_LANDMARK_CONSTRAINT_H +#define FUSE_CONSTRAINTS_FIXED_3D_LANDMARK_CONSTRAINT_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace fuse_constraints +{ + +/** + * @brief A constraint that represents an observation of a 3D landmark (ARTag or Similar) + * + * A landmark is represented as a 3D pose (3D position and a 3D orientation). This class takes + * the ground truth location of the 3D landmark and applies a reprojection-error based constraint + * an constraint on the position, orientation and calibration of the camera that observed the landmark. + * + * In most cases, the camera calibration should be held fixed as a single landmark does not present enough + * points to accurate constrain the pose AND the calibraton. + * + */ +class Fixed3DLandmarkConstraint : public fuse_core::Constraint +{ +public: + FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(Fixed3DLandmarkConstraint); + + /** + * @brief Default constructor + */ + Fixed3DLandmarkConstraint() = default; + + /** + * @brief Create a constraint using a known 3D fiducial marker + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position The variable representing the position components of the landmark pose + * @param[in] orientation The variable representing the orientation components of the marker pose + * @param[in] calibraton The intrinsic calibration parameters of the camera (4x1 vector: cx, cy, fx, fy). + * NOTE: Best practice is to fix this variable unless we have several observations + * with the same camera + * @param[in] pts3d Matrix of 3D points in marker coordiate frame. + * @param[in] observations The 2D (pixel) observations of each marker's corners. + * @param[in] mean The measured/prior pose of the marker as a vector (7x1 vector: x, y, z, qw, qx, qy, qz) + * @param[in] covariance The measurement/prior marker pose covariance (6x6 matrix: x, y, z, qx, qy, qz) + */ + Fixed3DLandmarkConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_variables::PinholeCamera& calibraton, + const fuse_core::MatrixXd& pts3d, + const fuse_core::MatrixXd& observations, const fuse_core::Vector7d& mean, + const fuse_core::Matrix6d& covariance); + + /** + * @brief Create a constraint using a known 3D fiducial marker. Convenience constructor for the special case of + * an ARTag with 4 corners. + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position The variable representing the position components of the marker pose + * @param[in] orientation The variable representing the orientation components of the marker pose + * @param[in] calibraton The intrinsic calibration parameters of the camera (4x1 vector: cx, cy, fx, fy). + * NOTE: Best practice is to fix this variable unless we have several observations + * with the same camera + * @param[in] marker_size The size of the marker, in meters. Assumed to be square. + * @param[in] observations The 2D (pixel) observations of each marker's corners. + * @param[in] mean The measured/prior pose of the marker as a vector (7x1 vector: x, y, z, qw, qx, qy, qz) + * @param[in] covariance The measurement/prior marker pose covariance (6x6 matrix: x, y, z, qx, qy, qz) + */ + Fixed3DLandmarkConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_variables::PinholeCamera& calibraton, + const double& marker_size, + const fuse_core::MatrixXd& observations, const fuse_core::Vector7d& mean, + const fuse_core::Matrix6d& covariance); + + /** + * @brief Destructor + */ + virtual ~Fixed3DLandmarkConstraint() = default; + + /** + * @brief Read-only access to the measured/prior vector of mean values. + * + * Order is (x, y, z, qw, qx, qy, qz) + */ + const fuse_core::Vector7d& mean() const + { + return mean_; + } + + /** + * @brief Read-only access to the square root information matrix. + * + * Order is (x, y, z, qx, qy, qz) + */ + const fuse_core::Matrix6d& sqrtInformation() const + { + return sqrt_information_; + } + + /** + * @brief Compute the measurement covariance matrix. + * + * Order is (x, y, z, qx, qy, qz) + */ + fuse_core::Matrix6d covariance() const + { + return (sqrt_information_.transpose() * sqrt_information_).inverse(); + } + + /** + * @brief Read-only access to the observation Matrix (Nx2). + * + * Order is (x, y) + */ + const fuse_core::MatrixXd& pts3d() const + { + return pts3d_; + } + + /** + * @brief Read-only access to the observation Matrix (Nx2). + * + * Order is (x, y) + */ + const fuse_core::MatrixXd& observations() const + { + return observations_; + } + + /** + * @brief Print a human-readable description of the constraint to the provided stream. + * + * @param[out] stream The stream to write to. Defaults to stdout. + */ + void print(std::ostream& stream = std::cout) const override; + + /** + * @brief Construct an instance of this constraint's cost function + * + * The function caller will own the new cost function instance. It is the responsibility of the caller to delete + * the cost function object when it is no longer needed. If the pointer is provided to a Ceres::Problem object, the + * Ceres::Problem object will takes ownership of the pointer and delete it during destruction. + * + * @return A base pointer to an instance of a derived CostFunction. + */ + ceres::CostFunction* costFunction() const override; + +protected: + fuse_core::MatrixXd pts3d_; //!< The 3D points in marker Coordinate frame + fuse_core::MatrixXd observations_; //!< The 2D observations (in pixel space) of the marker at postion mean_ + fuse_core::Vector7d mean_; //!< The measured/prior mean vector for this variable + fuse_core::Matrix6d sqrt_information_; //!< The square root information matrix + +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); + archive& pts3d_; + archive& observations_; + archive& mean_; + archive& sqrt_information_; + } +}; + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_KEY(fuse_constraints::Fixed3DLandmarkConstraint); + +#endif // FUSE_CONSTRAINTS_FIXED_3D_LANDMARK_CONSTRAINT_H diff --git a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h new file mode 100644 index 000000000..caa76becf --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h @@ -0,0 +1,195 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created on Mon Nov 12 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_CONSTRAINTS_FIXED_3D_LANDMARK_COST_FUNCTOR_H +#define FUSE_CONSTRAINTS_FIXED_3D_LANDMARK_COST_FUNCTOR_H + +#include +#include +#include +#include + +#include + +namespace fuse_constraints +{ + +/** + * @brief Create a prior cost function on the marker position, minimising reprojection error. + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost function that + * applies a prior constraint on the 3D position, orientation and calibration variables at once. + * + * The cost function is of the form: + * + * cost(x) = || A * (K * [R_q | p] * [R_{b(3:6)} | b(0:2))] * X - x) || + * + * where, the matrix A and the vector b are fixed, p is the camera position variable, and q is the camera orientation + * variable, K is the calibration matrix created from the calibration variable, X is the set of marker 3D points, + * R_b(0:3) is the Rotation matrix from the fixed landmark orentation (b(3:6)), b(0:2) is the fixed landmark position + * and x is the 2D ovservations. + * + * Note that the covariance submatrix for the quaternion is 3x3, representing errors in the orientation local + * parameterization tangent space. In case the user is interested in implementing a cost function of the form + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the square root + * information matrix (the inverse of the covariance). + */ +class Fixed3DLandmarkCostFunctor +{ +public: + FUSE_MAKE_ALIGNED_OPERATOR_NEW(); + + /** + * @brief Construct a cost function instance + * + * @param[in] A The residual weighting matrix, most likely derived from the square root information + * matrix in order (x, y, z, qx, qy, qz) + * @param[in] b The 3D pose measurement or prior in order (x, y, z, qw, qx, qy, qz) + * + * @param[in] obs The 2D projection of marker points (Nx2 in order x, y). + * + * @param[in] marker_size The size of the marker (in meters). + **/ + Fixed3DLandmarkCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector7d& b, const fuse_core::MatrixXd& obs, + const fuse_core::Vector1d& marker_size); + + /** + * @brief Construct a cost function instance + * + * @param[in] A The residual weighting matrix, most likely derived from the square root information + * matrix in order (x, y, z, qx, qy, qz) + * @param[in] b The 3D pose measurement or prior in order (x, y, z, qw, qx, qy, qz) + * + * @param[in] obs The 2D projection of marker points (Nx2 in order x, y). + * + * @param[in] pts3d The 3D points in marker coordinate frame (Nx3 in order x, y, z). + **/ + Fixed3DLandmarkCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector7d& b, const fuse_core::MatrixXd& obs, + const fuse_core::MatrixXd& pts3d); + + /** + * @brief Evaluate the cost function. Used by the Ceres optimization engine. + */ + template + bool operator()(const T* const position, const T* const orientation, const T* const calibration, T* residual) const; + +private: + fuse_core::MatrixXd A_; + fuse_core::Vector7d b_; + fuse_core::MatrixXd obs_; + fuse_core::MatrixXd pts3d_; + + NormalPriorOrientation3DCostFunctor orientation_functor_; +}; + +Fixed3DLandmarkCostFunctor::Fixed3DLandmarkCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector7d& b, + const fuse_core::MatrixXd& obs, const fuse_core::MatrixXd& pts3d) + : A_(A) + , b_(b) + , obs_(obs) + , pts3d_(pts3d.transpose()) // Transpose from Nx3 to 3xN to make math easier. + , orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Delta will not be scaled +{ + assert(pts3d_.rows() == 3); // Check if we have 3xN + + // Create Marker Transform Matrix (Tm) + double qm[4] = { b_(3), b_(4), b_(5), b_(6) }; + double rm[9]; + ceres::QuaternionToRotation(qm, rm); + + Eigen::Matrix Tm; + Tm << rm[0], rm[1], rm[2], b_(0), // NOLINT + rm[3], rm[4], rm[5], b_(1), // NOLINT + rm[6], rm[7], rm[8], b_(2), // NOLINT + 0.0, 0.0, 0.0, 1.0; // NOLINT + + // TODO(omendez): Can we do this directly on quaternions? + // ceres::QuaternionProduct(q, position, difference); + // ceres::QuaternionRotatePoint() + + // Transform points to marker pose and store for later use. + pts3d_ = Tm * pts3d_.colwise().homogeneous(); +} + +template +bool Fixed3DLandmarkCostFunctor::operator()(const T* const position, const T* const orientation, + const T* const calibration, T* residual) const +{ + // Create Calibration Matrix K + Eigen::Matrix K; + K << calibration[0], T(0.0), calibration[2], T(0.0), // NOLINT + T(0.0), calibration[1], calibration[3], T(0.0), // NOLINT + T(0.0), T(0.0), T(1.0), T(0.0), // NOLINT + T(0.0), T(0.0), T(0.0), T(1.0); // NOLINT + + // Create Camera Translation Matrix from Params. + T q[4] = { orientation[0], orientation[1], orientation[2], orientation[3] }; + T r[9]; + ceres::QuaternionToRotation(q, r); + + Eigen::Matrix Ta; + Ta << r[0], r[1], r[2], position[0], // NOLINT + r[3], r[4], r[5], position[1], // NOLINT + r[6], r[7], r[8], position[2], // NOLINT + T(0.0), T(0.0), T(0.0), T(1.0); // NOLINT + + // Transform 3D Points to Marker Location + Eigen::Matrix xp; + + // Project to camera at Ta + xp = (K * Ta * pts3d_.cast()); + xp.transposeInPlace(); + xp = xp.array().colwise() / xp.col(2).array(); + + auto d = (obs_.cast() - xp.block(0, 0, xp.rows(), 2)); + + for (uint i = 0; i < 4; i++) + { + residual[i * 2] = d.row(i)[0]; + residual[i * 2 + 1] = d.row(i)[1]; + } + + // TODO(omendez): how do we apply covariance weigthing to point losses from a pose uncertainty? + + return true; +} + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS_FIXED_3D_LANDMARK_COST_FUNCTOR_H diff --git a/fuse_constraints/src/fixed_3d_landmark_constraint.cpp b/fuse_constraints/src/fixed_3d_landmark_constraint.cpp new file mode 100644 index 000000000..0250c62aa --- /dev/null +++ b/fuse_constraints/src/fixed_3d_landmark_constraint.cpp @@ -0,0 +1,117 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created on Mon Nov 12 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_constraints +{ + +Fixed3DLandmarkConstraint::Fixed3DLandmarkConstraint( + const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, const fuse_variables::PinholeCamera& calibration, + const fuse_core::MatrixXd& pts3d, const fuse_core::MatrixXd& observations, const fuse_core::Vector7d& mean, + const fuse_core::Matrix6d& covariance) + : fuse_core::Constraint(source, { position.uuid(), orientation.uuid(), calibration.uuid() }) + , pts3d_(pts3d) + , observations_(observations) + , mean_(mean) + , sqrt_information_(covariance.inverse().llt().matrixU()) +{ + assert(pts3d_.cols() == 3); + assert(observations_.cols() == 2); + assert(pts3d_.rows() == observations_.rows()); +} + +Fixed3DLandmarkConstraint::Fixed3DLandmarkConstraint( + const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, const fuse_variables::PinholeCamera& calibration, + const double& marker_size, const fuse_core::MatrixXd& observations, const fuse_core::Vector7d& mean, + const fuse_core::Matrix6d& covariance) + : fuse_core::Constraint(source, { position.uuid(), orientation.uuid(), calibration.uuid() }) + , pts3d_(4, 3) + , observations_(observations) + , mean_(mean) + , sqrt_information_(covariance.inverse().llt().matrixU()) +{ + // Define 3D Homogeneous 3D Points at origin, assume z-up + pts3d_ << -1.0, -1.0, 0.0, // NOLINT + -1.0, 1.0, 0.0, // NOLINT + 1.0, -1.0, 0.0, // NOLINT + 1.0, 1.0, 0.0; // NOLINT + pts3d_ *= marker_size; // Scalar Multiplication + + assert(pts3d_.cols() == 3); + assert(observations_.cols() == 2); + assert(pts3d_.rows() == observations_.rows()); +} + +void Fixed3DLandmarkConstraint::print(std::ostream& stream) const +{ + stream << type() << "\n" + << " source: " << source() << "\n" + << " uuid: " << uuid() << "\n" + << " position variable: " << variables().at(0) << "\n" + << " orientation variable: " << variables().at(1) << "\n" + << " mean: " << mean().transpose() << "\n" + << " sqrt_info: " << sqrtInformation() << "\n" + << " observations: " << observations() << "\n"; + + if (loss()) + { + stream << " loss: "; + loss()->print(stream); + } +} + +ceres::CostFunction* Fixed3DLandmarkConstraint::costFunction() const +{ + return new ceres::AutoDiffCostFunction( + new Fixed3DLandmarkCostFunctor(sqrt_information_, mean_, observations_, pts3d_)); +} + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::Fixed3DLandmarkConstraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::Fixed3DLandmarkConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/test/test_fixed_3d_landmark_constraint.cpp b/fuse_constraints/test/test_fixed_3d_landmark_constraint.cpp new file mode 100644 index 000000000..229d16d05 --- /dev/null +++ b/fuse_constraints/test/test_fixed_3d_landmark_constraint.cpp @@ -0,0 +1,681 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created on Fri Nov 17 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 +#include + +#include +#include +#include +#include + +#include +#include + +using fuse_constraints::Fixed3DLandmarkConstraint; +using fuse_variables::Orientation3DStamped; +using fuse_variables::PinholeCameraFixed; +using fuse_variables::Position3DStamped; + +TEST(Fixed3DLandmarkConstraint, Constructor) +{ + // Construct a constraint just to make sure it compiles. + Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); + PinholeCameraFixed calibration_variable(0); + + double marker_size = 1.0; + + fuse_core::Vector7d mean; + mean << 1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the required precision) + fuse_core::Matrix6d cov; + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT + + // 2D observations (arbitrary) + Eigen::Matrix obs; + obs << 320, 240, 320, 240, 320, 240, 320, 240; + + EXPECT_NO_THROW(Fixed3DLandmarkConstraint constraint("test", position_variable, orientation_variable, + calibration_variable, marker_size, obs, mean, cov)); +} + +TEST(Fixed3DLandmarkConstraint, Covariance) +{ + // Verify the covariance <--> sqrt information conversions are correct + Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo")); + Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo")); + PinholeCameraFixed calibration_variable(0); + + double marker_size = 1.0; + + fuse_core::Vector7d mean; + mean << 1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0; + + // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the required precision) + fuse_core::Matrix6d cov; + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT + + // 2D observations (arbitrary) + Eigen::Matrix obs; + obs << 320, 240, 320, 240, 320, 240, 320, 240; + + Fixed3DLandmarkConstraint constraint("test", position_variable, orientation_variable, calibration_variable, + marker_size, obs, mean, cov); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix6d expected_sqrt_info; + expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, + -3.19509486240291, // NOLINT + 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT + 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + fuse_core::Matrix6d expected_cov = cov; + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-9); +} + +TEST(Fixed3DLandmarkConstraint, Optimization) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are generated. + // Create the variables + auto position_variable = Position3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + position_variable->x() = 1.5; + position_variable->y() = -3.0; + position_variable->z() = 10.0; + + auto orientation_variable = Orientation3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->w() = 0.952; + orientation_variable->x() = 0.038; + orientation_variable->y() = -0.189; + orientation_variable->z() = 0.239; + + auto calibration_variable = PinholeCameraFixed::make_shared(0); + calibration_variable->fx() = 638.34478759765620; + calibration_variable->fy() = 643.10717773437500; + calibration_variable->cx() = 310.29060457226840; + calibration_variable->cy() = 237.80861559081677; + + // Define Marker Size + double marker_size = 1.0; + + // Create an fixed position for the marker. + fuse_core::Vector7d mean; + mean << 0, 0, 10, 0.9238795, 0, -0.3826834, 0; + + // And observations... + Eigen::Matrix obs; + obs << 261.71822455, 168.60442225, // NOLINT + 261.71822455, 307.01280893, // NOLINT + 352.44745875, 177.74503448, // NOLINT + 352.44745875, 297.87219670; // NOLINT + + // Define Pose Covariance + fuse_core::Matrix6d cov; + cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, // NOLINT + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, // NOLINT + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, // NOLINT + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, // NOLINT + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, // NOLINT + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; // NOLINT + + auto constraint = Fixed3DLandmarkConstraint::make_shared("test", *position_variable, *orientation_variable, + *calibration_variable, marker_size, obs, mean, cov); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock(position_variable->data(), position_variable->size(), + position_variable->localParameterization()); + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), + orientation_variable->localParameterization()); + problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), + calibration_variable->localParameterization()); + + std::vector parameter_blocks; + parameter_blocks.push_back(position_variable->data()); + parameter_blocks.push_back(orientation_variable->data()); + parameter_blocks.push_back(calibration_variable->data()); + + if (calibration_variable->holdConstant()) + { + problem.SetParameterBlockConstant(calibration_variable->data()); + } + + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(0.0, position_variable->x(), 1.0e-5); + EXPECT_NEAR(0.0, position_variable->y(), 1.0e-5); + EXPECT_NEAR(0.0, position_variable->z(), 1.0e-5); + + EXPECT_NEAR(1.0, orientation_variable->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); + + EXPECT_NEAR(638.34478759765620, calibration_variable->fx(), 1.0e-3); + EXPECT_NEAR(643.10717773437500, calibration_variable->fy(), 1.0e-3); + EXPECT_NEAR(310.29060457226840, calibration_variable->cx(), 1.0e-3); + EXPECT_NEAR(237.80861559081677, calibration_variable->cy(), 1.0e-3); + + // // Compute the covariance + // std::vector > covariance_blocks; + // covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); + // covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + // covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); + + // ceres::Covariance::Options cov_options; + // ceres::Covariance covariance(cov_options); + // covariance.Compute(covariance_blocks, &problem); + // fuse_core::MatrixXd cov_pos_pos(position_variable->size(), position_variable->size()); + // covariance.GetCovarianceBlock(position_variable->data(), position_variable->data(), cov_pos_pos.data()); + + // fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), orientation_variable->localSize()); + // covariance.GetCovarianceBlockInTangentSpace( + // orientation_variable->data(), orientation_variable->data(), cov_or_or.data()); + + // fuse_core::MatrixXd cov_pos_or(position_variable->localSize(), orientation_variable->localSize()); + // covariance.GetCovarianceBlockInTangentSpace( + // position_variable->data(), orientation_variable->data(), cov_pos_or.data()); + + // // Assemble the full covariance from the covariance blocks + // fuse_core::Matrix6d actual_covariance; + // actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // // Define the expected covariance + // fuse_core::Matrix6d expected_covariance; + // expected_covariance << + // 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, + // 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, + // 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, + // 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, + // 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, + // 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; + + // EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-5); +} + +TEST(Fixed3DLandmarkConstraint, OptimizationScaledMarker) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are generated. + // Create the variables + auto position_variable = Position3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + position_variable->x() = 1.5; + position_variable->y() = -3.0; + position_variable->z() = 10.0; + + auto orientation_variable = Orientation3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->w() = 0.952; + orientation_variable->x() = 0.038; + orientation_variable->y() = -0.189; + orientation_variable->z() = 0.239; + + auto calibration_variable = PinholeCameraFixed::make_shared(0); + calibration_variable->fx() = 638.34478759765620; + calibration_variable->fy() = 643.10717773437500; + calibration_variable->cx() = 310.29060457226840; + calibration_variable->cy() = 237.80861559081677; + + // Define Marker Size + double marker_size = 0.25; + + // Create an fixed position for the marker. + fuse_core::Vector7d mean; + mean << 0, 0, 10, 0.9238795, 0, -0.3826834, 0; + + // And observations... + Eigen::Matrix obs; + obs << 298.8030834636202, 221.44160554498040, // NOLINT + 298.8030834636202, 254.17562563665314, // NOLINT + 321.3790354517349, 222.01021505859384, // NOLINT + 321.3790354517349, 253.60701612303970; // NOLINT + + // Define Pose Covariance + fuse_core::Matrix6d cov; + cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, // NOLINT + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, // NOLINT + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, // NOLINT + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, // NOLINT + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, // NOLINT + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; // NOLINT + + auto constraint = Fixed3DLandmarkConstraint::make_shared("test", *position_variable, *orientation_variable, + *calibration_variable, marker_size, obs, mean, cov); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock(position_variable->data(), position_variable->size(), + position_variable->localParameterization()); + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), + orientation_variable->localParameterization()); + problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), + calibration_variable->localParameterization()); + + std::vector parameter_blocks; + parameter_blocks.push_back(position_variable->data()); + parameter_blocks.push_back(orientation_variable->data()); + parameter_blocks.push_back(calibration_variable->data()); + + if (calibration_variable->holdConstant()) + { + problem.SetParameterBlockConstant(calibration_variable->data()); + } + + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(0.0, position_variable->x(), 1.0e-5); + EXPECT_NEAR(0.0, position_variable->y(), 1.0e-5); + EXPECT_NEAR(0.0, position_variable->z(), 1.0e-5); + + EXPECT_NEAR(1.0, orientation_variable->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); + + EXPECT_NEAR(638.34478759765620, calibration_variable->fx(), 1.0e-3); + EXPECT_NEAR(643.10717773437500, calibration_variable->fy(), 1.0e-3); + EXPECT_NEAR(310.29060457226840, calibration_variable->cx(), 1.0e-3); + EXPECT_NEAR(237.80861559081677, calibration_variable->cy(), 1.0e-3); +} + +TEST(Fixed3DLandmarkConstraint, OptimizationPoints) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are generated. + // Create the variables + auto position_variable = Position3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + position_variable->x() = 1.5; + position_variable->y() = -3.0; + position_variable->z() = 10.0; + + auto orientation_variable = Orientation3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->w() = 0.952; + orientation_variable->x() = 0.038; + orientation_variable->y() = -0.189; + orientation_variable->z() = 0.239; + + auto calibration_variable = PinholeCameraFixed::make_shared(0); + calibration_variable->fx() = 638.34478759765620; + calibration_variable->fy() = 643.10717773437500; + calibration_variable->cx() = 310.29060457226840; + calibration_variable->cy() = 237.80861559081677; + + // Create an fixed position for the marker. + fuse_core::Vector7d mean; + mean << 0, 0, 10, 0.9238795, 0, -0.3826834, 0; + + // Create the 3D points + Eigen::Matrix pts3d; + pts3d << -1.50, -1.5, 0.0, // NOLINT + -1.50, 1.5, 0.0, // NOLINT + 1.50, -1.5, 0.0, // NOLINT + 1.50, 1.5, 0.0, // NOLINT + 3.24, -1.5, 0.0, // NOLINT + 3.24, 1.5, 0.0, // NOLINT + 1.74, -1.5, 0.0, // NOLINT + 1.74, 1.5, 0.0; // NOLINT + + // And observations... + Eigen::Matrix obs; + obs << 234.55045762290558, 129.89675764784400, // NOLINT + 234.55045762290558, 345.72047353378950, // NOLINT + 371.50457352525080, 150.59313756704000, // NOLINT + 371.50457352525080, 325.02409361459354, // NOLINT + 429.27697088295537, 159.32364907215157, // NOLINT + 429.27697088295537, 316.29358210948200, // NOLINT + 380.22578098989925, 151.91107841060833, // NOLINT + 380.22578098989925, 323.70615277102520; // NOLINT + + // Define Pose Covariance + fuse_core::Matrix6d cov; + cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, // NOLINT + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, // NOLINT + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, // NOLINT + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, // NOLINT + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, // NOLINT + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; // NOLINT + + auto constraint = Fixed3DLandmarkConstraint::make_shared("test", *position_variable, *orientation_variable, + *calibration_variable, pts3d, obs, mean, cov); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock(position_variable->data(), position_variable->size(), + position_variable->localParameterization()); + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), + orientation_variable->localParameterization()); + problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), + calibration_variable->localParameterization()); + + std::vector parameter_blocks; + parameter_blocks.push_back(position_variable->data()); + parameter_blocks.push_back(orientation_variable->data()); + parameter_blocks.push_back(calibration_variable->data()); + + if (calibration_variable->holdConstant()) + { + problem.SetParameterBlockConstant(calibration_variable->data()); + } + + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(0.0, position_variable->x(), 1.0e-5); + EXPECT_NEAR(0.0, position_variable->y(), 1.0e-5); + EXPECT_NEAR(0.0, position_variable->z(), 1.0e-5); + + EXPECT_NEAR(1.0, orientation_variable->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); + + EXPECT_NEAR(638.34478759765620, calibration_variable->fx(), 1.0e-3); + EXPECT_NEAR(643.10717773437500, calibration_variable->fy(), 1.0e-3); + EXPECT_NEAR(310.29060457226840, calibration_variable->cx(), 1.0e-3); + EXPECT_NEAR(237.80861559081677, calibration_variable->cy(), 1.0e-3); +} + +TEST(Fixed3DLandmarkConstraint, MultiViewOptimization) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are generated. + // Create the variables + + auto calibration_variable = PinholeCameraFixed::make_shared(0); + calibration_variable->fx() = 638.34478759765620; + calibration_variable->fy() = 643.10717773437500; + calibration_variable->cx() = 310.29060457226840; + calibration_variable->cy() = 237.80861559081677; + + // Define Marker Size + double marker_size = 1.0; + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + + // Create N Constraints + uint N = 5; + std::vector > obs(5); + + obs[0] << 124.33479102109460, 30.196035559968568, // NOLINT + 124.33479102109460, 168.60442224720070, // NOLINT + 233.20987206846505, 57.617872261057050, // NOLINT + 233.20987206846505, 177.74503448089686; // NOLINT + + obs[1] << 193.02650778349710, 99.400228903584630, // NOLINT + 193.02650778349710, 237.80861559081677, // NOLINT + 292.82866541025334, 117.68145337097695, // NOLINT + 292.82866541025334, 237.80861559081677; // NOLINT + + obs[2] << 373.42853736463960, 168.46718090279768, // NOLINT + 373.42853736463960, 307.15005027883580, // NOLINT + 466.82773058187524, 176.09986812497370, // NOLINT + 466.82773058187524, 299.51736305665980; // NOLINT + + obs[3] << 442.25647916056020, 237.80861559081674, // NOLINT + 442.25647916056020, 376.49148496685490, // NOLINT + 528.07950735845330, 237.80861559081677, // NOLINT + 528.07950735845330, 361.22611052250290; // NOLINT + + obs[4] << 511.08442095648064, 307.15005027883580, // NOLINT + 511.08442095648064, 445.83291965487400, // NOLINT + 589.33128413503120, 299.51736305665980, // NOLINT + 589.33128413503120, 422.93485798834600; // NOLINT + + std::vector position_vars(N); + std::vector orientation_vars(N); + + for (uint i = 0; i < N; i++) + { + position_vars[i] = Position3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + position_vars[i]->x() = 0.0; + position_vars[i]->y() = 0.0; + position_vars[i]->z() = 0.0; + + orientation_vars[i] = Orientation3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_vars[i]->w() = 0.952; + orientation_vars[i]->x() = 0.038; + orientation_vars[i]->y() = -0.189; + orientation_vars[i]->z() = 0.239; + + problem.AddParameterBlock(position_vars[i]->data(), position_vars[i]->size(), + position_vars[i]->localParameterization()); + problem.AddParameterBlock(orientation_vars[i]->data(), orientation_vars[i]->size(), + orientation_vars[i]->localParameterization()); + problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), + calibration_variable->localParameterization()); + + std::vector parameter_blocks; + parameter_blocks.push_back(position_vars[i]->data()); + parameter_blocks.push_back(orientation_vars[i]->data()); + parameter_blocks.push_back(calibration_variable->data()); + + // Create an fixed position for the marker. + fuse_core::Vector7d mean; + mean << 0, 0, 10, 0.9238795, 0, -0.3826834, 0; + + fuse_core::Matrix6d cov; + cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, // NOLINT + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, // NOLINT + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, // NOLINT + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, // NOLINT + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, // NOLINT + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; // NOLINT + + auto constraint = Fixed3DLandmarkConstraint::make_shared("test", *position_vars[i], *orientation_vars[i], + *calibration_variable, marker_size, obs[i], mean, cov); + + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); + } + + if (calibration_variable->holdConstant()) + { + problem.SetParameterBlockConstant(calibration_variable->data()); + } + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(-2.0, position_vars[0]->x(), 1.0e-5); + EXPECT_NEAR(-2.0, position_vars[0]->y(), 1.0e-5); + EXPECT_NEAR(0.0, position_vars[0]->z(), 1.0e-5); + + EXPECT_NEAR(1.0, orientation_vars[0]->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[0]->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[0]->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[0]->z(), 1.0e-3); + + EXPECT_NEAR(-1.0, position_vars[1]->x(), 1.0e-5); + EXPECT_NEAR(-1.0, position_vars[1]->y(), 1.0e-5); + EXPECT_NEAR(0.0, position_vars[1]->z(), 1.0e-5); + + EXPECT_NEAR(1.0, orientation_vars[1]->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[1]->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[1]->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[1]->z(), 1.0e-3); + + EXPECT_NEAR(0.0, position_vars[2]->x(), 1.0e-5); + EXPECT_NEAR(0.0, position_vars[2]->y(), 1.0e-5); + EXPECT_NEAR(0.0, position_vars[2]->z(), 1.0e-5); + + EXPECT_NEAR(0.9961947, orientation_vars[2]->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[2]->x(), 1.0e-3); + EXPECT_NEAR(0.0871557, orientation_vars[2]->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[2]->z(), 1.0e-3); + + EXPECT_NEAR(1.0, position_vars[3]->x(), 1.0e-5); + EXPECT_NEAR(1.0, position_vars[3]->y(), 1.0e-5); + EXPECT_NEAR(0.0, position_vars[3]->z(), 1.0e-5); + + EXPECT_NEAR(0.9961947, orientation_vars[3]->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[3]->x(), 1.0e-3); + EXPECT_NEAR(0.0871557, orientation_vars[3]->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[3]->z(), 1.0e-3); + + EXPECT_NEAR(2.0, position_vars[4]->x(), 1.0e-5); + EXPECT_NEAR(2.0, position_vars[4]->y(), 1.0e-5); + EXPECT_NEAR(0.0, position_vars[4]->z(), 1.0e-5); + + EXPECT_NEAR(0.9961947, orientation_vars[4]->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[4]->x(), 1.0e-3); + EXPECT_NEAR(0.0871557, orientation_vars[4]->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[4]->z(), 1.0e-3); + + EXPECT_NEAR(638.34478759765620, calibration_variable->fx(), 1.0e-3); + EXPECT_NEAR(643.10717773437500, calibration_variable->fy(), 1.0e-3); + EXPECT_NEAR(310.29060457226840, calibration_variable->cx(), 1.0e-3); + EXPECT_NEAR(237.80861559081677, calibration_variable->cy(), 1.0e-3); +} + +TEST(Fixed3DLandmarkConstraint, Serialization) +{ + // Construct a constraint + Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); + + PinholeCameraFixed calibration_variable(0); + calibration_variable.fx() = 638.34478759765620; + calibration_variable.fy() = 643.10717773437500; + calibration_variable.cx() = 310.29060457226840; + calibration_variable.cy() = 237.80861559081677; + + double marker_size = 1.0; + + fuse_core::Vector7d mean; + mean << 1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the required precision) + fuse_core::Matrix6d cov; + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; + + // And observations... + Eigen::Matrix obs; + obs << 261.71822455, 168.60442225, 261.71822455, 307.01280893, 352.44745875, 177.74503448, 352.44745875, 297.87219670; + + Fixed3DLandmarkConstraint expected("test", position_variable, orientation_variable, calibration_variable, marker_size, + obs, mean, cov); + + // Serialize the constraint into an archive + std::stringstream stream; + { + fuse_core::TextOutputArchive archive(stream); + expected.serialize(archive); + } + + // Deserialize a new constraint from that same stream + Fixed3DLandmarkConstraint actual; + { + fuse_core::TextInputArchive archive(stream); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.uuid(), actual.uuid()); + EXPECT_EQ(expected.variables(), actual.variables()); + EXPECT_MATRIX_EQ(expected.mean(), actual.mean()); + EXPECT_MATRIX_EQ(expected.pts3d(), actual.pts3d()); + EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); + EXPECT_MATRIX_EQ(expected.observations(), actual.observations()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/fuse_variables/CMakeLists.txt b/fuse_variables/CMakeLists.txt index ca25103e7..e09f5a868 100644 --- a/fuse_variables/CMakeLists.txt +++ b/fuse_variables/CMakeLists.txt @@ -34,6 +34,8 @@ add_library(${PROJECT_NAME} src/acceleration_angular_3d_stamped.cpp src/acceleration_linear_2d_stamped.cpp src/acceleration_linear_3d_stamped.cpp + src/pinhole_camera.cpp + src/pinhole_camera_fixed.cpp src/orientation_2d_stamped.cpp src/orientation_3d_stamped.cpp src/point_2d_fixed_landmark.cpp @@ -196,6 +198,54 @@ if(CATKIN_ENABLE_TESTING) CXX_STANDARD_REQUIRED YES ) + # Camera Intrinsics tests + catkin_add_gtest(test_pinhole_camera + test/test_pinhole_camera.cpp + ) + add_dependencies(test_pinhole_camera + ${catkin_EXPORTED_TARGETS} + ) + target_include_directories(test_pinhole_camera + PRIVATE + include + ${catkin_INCLUDE_DIRS} + ${CERES_INCLUDE_DIRS} + ) + target_link_libraries(test_pinhole_camera + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${CERES_LIBRARIES} + ) + set_target_properties(test_pinhole_camera + PROPERTIES + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED YES + ) + + # Camera Intrinsics tests + catkin_add_gtest(test_pinhole_camera_fixed + test/test_pinhole_camera_fixed.cpp + ) + add_dependencies(test_pinhole_camera_fixed + ${catkin_EXPORTED_TARGETS} + ) + target_include_directories(test_pinhole_camera_fixed + PRIVATE + include + ${catkin_INCLUDE_DIRS} + ${CERES_INCLUDE_DIRS} + ) + target_link_libraries(test_pinhole_camera_fixed + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${CERES_LIBRARIES} + ) + set_target_properties(test_pinhole_camera_fixed + PROPERTIES + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED YES + ) + # Fixed Size Variable Tests catkin_add_gtest(test_fixed_size_variable test/test_fixed_size_variable.cpp diff --git a/fuse_variables/include/fuse_variables/pinhole_camera.h b/fuse_variables/include/fuse_variables/pinhole_camera.h new file mode 100644 index 000000000..c967cdff1 --- /dev/null +++ b/fuse_variables/include/fuse_variables/pinhole_camera.h @@ -0,0 +1,184 @@ +/* + * 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 + +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; + +protected: + /** + * @brief Construct a point 3D variable given a landmarks id + * + * @param[in] landmark_id The id associated to a landmark + */ + PinholeCamera(const fuse_core::UUID& uuid, const uint64_t& landmark_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.h b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.h new file mode 100644 index 000000000..abc0c388a --- /dev/null +++ b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.h @@ -0,0 +1,108 @@ +/* + * 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 + +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; } + +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 000000000..ec665c7af --- /dev/null +++ b/fuse_variables/src/pinhole_camera.cpp @@ -0,0 +1,87 @@ +/* + * 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 000000000..472663388 --- /dev/null +++ b/fuse_variables/src/pinhole_camera_fixed.cpp @@ -0,0 +1,63 @@ +/* + * 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/test_pinhole_camera.cpp b/fuse_variables/test/test_pinhole_camera.cpp new file mode 100644 index 000000000..5a0569ef3 --- /dev/null +++ b/fuse_variables/test/test_pinhole_camera.cpp @@ -0,0 +1,462 @@ +/* + * 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 + +using fuse_variables::PinholeCamera; + +TEST(PinholeCamera, Type) +{ + PinholeCamera 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 variable1(0); + PinholeCamera variable2(0); + EXPECT_EQ(variable1.uuid(), variable2.uuid()); + } + + // Verify two positions with the different landmark ids produce different uuids + { + PinholeCamera variable1(0); + PinholeCamera variable2(1); + EXPECT_NE(variable1.uuid(), variable2.uuid()); + } +} + +struct CostFunctor +{ + CostFunctor() + { + } + + 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(), 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 k, T* residual) const + { + // Create Matrix + Eigen::Matrix K(3, 3); + K << k[0], T(0.0), k[2], // NOLINT + T(0.0), k[1], k[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[i * 2] = T(d.row(i)[0]); + residual[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(), 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 +{ + double X[8][3]; // 3D Points (2x2 Cube at 0,0,10) + double x[8][2]; // 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 + T xp[8][2]; + 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[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(), 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; + 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(), 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 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 000000000..834b3ff2b --- /dev/null +++ b/fuse_variables/test/test_pinhole_camera_fixed.cpp @@ -0,0 +1,478 @@ +/* + * 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 + +using fuse_variables::PinholeCameraFixed; + +TEST(PinholeCameraFixed, Type) +{ + PinholeCameraFixed 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 variable1(0); + PinholeCameraFixed variable2(0); + EXPECT_EQ(variable1.uuid(), variable2.uuid()); + } + + // Verify two positions with the different landmark ids produce different uuids + { + PinholeCameraFixed variable1(0); + PinholeCameraFixed variable2(1); + EXPECT_NE(variable1.uuid(), variable2.uuid()); + } +} + +struct CostFunctor +{ + CostFunctor() + { + } + + 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(), 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 k, T* residual) const + { + // Create Matrix + Eigen::Matrix K(3, 3); + K << k[0], T(0.0), k[2], // NOLINT + T(0.0), k[1], k[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[i * 2] = T(d.row(i)[0]); + residual[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(), 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 +{ + double X[8][3]; // 3D Points (2x2 Cube at 0,0,10) + double x[8][2]; // 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 + T xp[8][2]; + 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[i * 2] = xerr; + residual[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(), 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; + 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(), 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 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(); +}