From 4e087a9ffdbd01a6fa7c9cf502556a657361a693 Mon Sep 17 00:00:00 2001 From: Tom Moore Date: Wed, 22 Aug 2018 14:43:50 +0100 Subject: [PATCH] RST-1239 Adding 3D relative pose constraint (#27) * Adding 3D relative pose constraint * Updating expected covariance matrices for anything involving rotations --- fuse_constraints/CMakeLists.txt | 13 + .../normal_delta_pose_3d_cost_functor.h | 179 ++++++++++ ...normal_prior_orientation_3d_cost_functor.h | 10 +- .../relative_pose_3d_stamped_constraint.h | 137 ++++++++ .../relative_pose_3d_stamped_constraint.cpp | 79 +++++ ...lute_orientation_3d_stamped_constraint.cpp | 11 +- ...st_absolute_pose_3d_stamped_constraint.cpp | 10 +- ...st_relative_pose_3d_stamped_constraint.cpp | 315 ++++++++++++++++++ 8 files changed, 746 insertions(+), 8 deletions(-) create mode 100644 fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h create mode 100644 fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h create mode 100644 fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp create mode 100644 fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 7caf187c6..d24a72b1d 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -49,6 +49,7 @@ add_library(${PROJECT_NAME} src/normal_delta_orientation_2d.cpp src/normal_prior_orientation_2d.cpp src/relative_pose_2d_stamped_constraint.cpp + src/relative_pose_3d_stamped_constraint.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} @@ -171,4 +172,16 @@ if(CATKIN_ENABLE_TESTING) ${PROJECT_NAME} ${catkin_LIBRARIES} ) + + # Relative Pose 3D Stamped Constraint Tests + catkin_add_gtest(test_relative_pose_3d_stamped_constraint + test/test_relative_pose_3d_stamped_constraint.cpp + ) + add_dependencies(test_relative_pose_3d_stamped_constraint + ${catkin_EXPORTED_TARGETS} + ) + target_link_libraries(test_relative_pose_3d_stamped_constraint + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ) endif() diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h new file mode 100644 index 000000000..7addc3e1a --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h @@ -0,0 +1,179 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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_NORMAL_DELTA_POSE_3D_COST_FUNCTOR_H +#define FUSE_CONSTRAINTS_NORMAL_DELTA_POSE_3D_COST_FUNCTOR_H + +#include +#include + +#include + +namespace fuse_constraints +{ + +/** + * @brief Implements a cost function that models a difference between 3D pose variables. + * + * A single pose involves two variables: a 3D position and a 3D orientation. This cost function computes the difference + * using standard 3D transformation math: + * + * delta = q1^-1 * [position2 - position1] + * [ q2 ] + * + * where q1 and q2 are the orientations of the two poses, given as quaternions. Once the delta is computed, the + * difference between the computed delta and the expected delta is given as follows: + * + * || [ delta(0) - b(0) ] ||^2 + * cost(x) = || [ delta(1) - b(1) ] || + * ||A * [ delta(2) - b(2) ] || + * || [ (delta(3:6) * b(3:6)^-1)(1) ] || + * || [ (delta(3:6) * b(3:6)^-1)(2) ] || + * || [ (delta(3:6) * b(3:6)^-1)(3) ] || + * + * where, the matrix A and the vector b are fixed. 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). + * + * Note that the cost function's quaternion components are only concerned with the imaginary components (qx, qy, qz). + */ +class NormalDeltaPose3DCostFunctor +{ +public: + /** + * @brief Constructor + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in order + * (dx, dy, dz, dqx, dqy, dqz) + * @param[in] b The exposed pose difference in order (dx, dy, dz, dqw, dqx, dqy, dqz) + */ + NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b); + + /** + * @brief Compute the cost values/residuals using the provided variable/parameter values + */ + template + bool operator()( + const T* const position1, + const T* const orientation1, + const T* const position2, + const T* const orientation2, + T* residual) const; + +private: + fuse_core::Matrix6d A_; //!< The residual weighting matrix, most likely the square root information matrix + fuse_core::Vector7d b_; //!< The measured difference between variable pose1 and variable pose2 +}; + +NormalDeltaPose3DCostFunctor::NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b) : + A_(A), + b_(b) +{ +} + +template +bool NormalDeltaPose3DCostFunctor::operator()( + const T* const position1, + const T* const orientation1, + const T* const position2, + const T* const orientation2, + T* residual) const +{ + // Based on the Ceres SLAM pose graph error calculation here: + // https://github.com/ceres-solver/ceres-solver/blob/4fc5d25f9cbfe2aa333425ddad03bdc651335c24/examples/slam/pose_graph_3d/pose_graph_3d_error_term.h#L73 + + // 1. Get the relative orientation change from the variable's orientation1 to orientation2 + T variable_orientation1_inverse[4] = + { + orientation1[0], + -orientation1[1], + -orientation1[2], + -orientation1[3] + }; + + T variable_orientation_delta[4]; + ceres::QuaternionProduct(variable_orientation1_inverse, orientation2, variable_orientation_delta); + + // 2. Get the position change from pose1 to pose2, then rotate it into the frame of pose1 + T variable_position_delta[3] = + { + position2[0] - position1[0], + position2[1] - position1[1], + position2[2] - position1[2] + }; + + T variable_position_delta_rotated[3]; + ceres::QuaternionRotatePoint( + variable_orientation1_inverse, + variable_position_delta, + variable_position_delta_rotated); + + // 3. Get the difference between the orientation delta that we just computed for orientation1 and orientation2, + // and the measurement's orientation delta + T observation_inverse[4] = + { + T(b_(3)), + T(-b_(4)), + T(-b_(5)), + T(-b_(6)) + }; + + T delta_difference_orientation[4]; + ceres::QuaternionProduct( + variable_orientation_delta, + observation_inverse, + delta_difference_orientation); + + // 4. Compute the position delta, and throw everything into a residual at the same time + residual[0] = variable_position_delta_rotated[0] - b_[0]; + residual[1] = variable_position_delta_rotated[1] - b_[1]; + residual[2] = variable_position_delta_rotated[2] - b_[2]; + residual[3] = T(2.0) * delta_difference_orientation[1]; + residual[4] = T(2.0) * delta_difference_orientation[2]; + residual[5] = T(2.0) * delta_difference_orientation[3]; + + // 5. Map it to Eigen, and weight it + Eigen::Map > residual_map(residual); + + residual_map.applyOnTheLeft(A_.template cast()); + + return true; +} + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS_NORMAL_DELTA_POSE_3D_COST_FUNCTOR_H diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h index e53b7ef1c..f5346ed35 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h @@ -100,7 +100,7 @@ class NormalPriorOrientation3DCostFunctor orientation[3] }; - T inverse_observation[4] = + T observation_inverse[4] = { T(b_(0)), T(-b_(1)), @@ -110,12 +110,12 @@ class NormalPriorOrientation3DCostFunctor T output[4]; - ceres::QuaternionProduct(inverse_observation, variable, output); + ceres::QuaternionProduct(variable, observation_inverse, output); // 2. Can use just the imaginary coefficients as the residual - residuals[0] = output[1]; - residuals[1] = output[2]; - residuals[2] = output[3]; + residuals[0] = T(2.0) * output[1]; + residuals[1] = T(2.0) * output[2]; + residuals[2] = T(2.0) * output[3]; // 3. Scale the residuals by the square root information matrix to account for the measurement uncertainty. Eigen::Map> residuals_map(residuals, A_.rows()); diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h new file mode 100644 index 000000000..82781489c --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h @@ -0,0 +1,137 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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_RELATIVE_POSE_3D_STAMPED_CONSTRAINT_H +#define FUSE_CONSTRAINTS_RELATIVE_POSE_3D_STAMPED_CONSTRAINT_H + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief A constraint that represents a measurement on the difference between two 3D poses. + * + * This type of constraint arises in many situations. Many types of incremental odometry measurements (e.g., visual + * odometry) measure the change in the pose, not the pose directly. This constraint holds the measured 3D pose change + * and the measurement uncertainty/covariance. + */ +class RelativePose3DStampedConstraint : public fuse_core::Constraint +{ +public: + SMART_PTR_DEFINITIONS(RelativePose3DStampedConstraint); + + /** + * @brief Constructor + * + * @param[in] position1 The variable representing the position components of the first pose + * @param[in] orientation1 The variable representing the orientation components of the first pose + * @param[in] position2 The variable representing the position components of the second pose + * @param[in] orientation2 The variable representing the orientation components of the second pose + * @param[in] delta The measured change in the pose (7x1 vector: dx, dy, dz, dqw, dqx, dqy, dqz) + * @param[in] covariance The measurement covariance (6x6 matrix: dx, dy, dz, dqx, dqy, dqz) + */ + RelativePose3DStampedConstraint( + const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, + const fuse_core::Vector7d& delta, + const fuse_core::Matrix6d& covariance); + + /** + * @brief Destructor + */ + virtual ~RelativePose3DStampedConstraint() = default; + + /** + * @brief Read-only access to the measured pose change. + */ + const fuse_core::Vector7d& delta() const { return delta_; } + + /** + * @brief Read-only access to the square root information matrix. + */ + const fuse_core::Matrix6d& sqrtInformation() const { return sqrt_information_; } + + /** + * @brief Compute the measurement covariance matrix. + */ + fuse_core::Matrix6d covariance() const { return (sqrt_information_.transpose() * sqrt_information_).inverse(); } + + /** + * @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 Perform a deep copy of the constraint and return a unique pointer to the copy + * + * Unique pointers can be implicitly upgraded to shared pointers if needed. + * + * @return A unique pointer to a new instance of the most-derived constraint + */ + fuse_core::Constraint::UniquePtr clone() const override; + + /** + * @brief Access the cost function for this constraint + * + * 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::Vector7d delta_; //!< The measured pose change (dx, dy, dz, dqw, dqx, dqy, dqz) + fuse_core::Matrix6d sqrt_information_; //!< The square root information matrix (derived from the covariance matrix) +}; + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS_RELATIVE_POSE_3D_STAMPED_CONSTRAINT_H diff --git a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp new file mode 100644 index 000000000..dad6393bb --- /dev/null +++ b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp @@ -0,0 +1,79 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 + + +namespace fuse_constraints +{ + +RelativePose3DStampedConstraint::RelativePose3DStampedConstraint( + const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, + const fuse_core::Vector7d& delta, + const fuse_core::Matrix6d& covariance) : + fuse_core::Constraint{position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}, + delta_(delta), + sqrt_information_(covariance.inverse().llt().matrixU()) +{ +} + +void RelativePose3DStampedConstraint::print(std::ostream& stream) const +{ + stream << type() << "\n" + << " uuid: " << uuid() << "\n" + << " position1 variable: " << variables_.at(0) << "\n" + << " orientation1 variable: " << variables_.at(1) << "\n" + << " position2 variable: " << variables_.at(2) << "\n" + << " orientation2 variable: " << variables_.at(3) << "\n" + << " delta: " << delta().transpose() << "\n" + << " sqrt_info: " << sqrtInformation() << "\n"; +} + +fuse_core::Constraint::UniquePtr RelativePose3DStampedConstraint::clone() const +{ + return RelativePose3DStampedConstraint::make_unique(*this); +} + +ceres::CostFunction* RelativePose3DStampedConstraint::costFunction() const +{ + return new ceres::AutoDiffCostFunction( + new NormalDeltaPose3DCostFunctor(sqrt_information_, delta_)); +} + +} // namespace fuse_constraints diff --git a/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp index fff7224cc..ad057076c 100644 --- a/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_3d_stamped_constraint.cpp @@ -106,7 +106,10 @@ TEST(AbsoluteOrientation3DStampedConstraint, Optimization) mean << 1.0, 0.0, 0.0, 0.0; fuse_core::Matrix3d cov; - cov << 1.0, 0.1, 0.2, 0.1, 2.0, 0.3, 0.2, 0.3, 3.0; + cov << + 1.0, 0.1, 0.2, + 0.1, 2.0, 0.3, + 0.2, 0.3, 3.0; auto constraint = AbsoluteOrientation3DStampedConstraint::make_shared( *orientation_variable, mean, @@ -148,7 +151,11 @@ TEST(AbsoluteOrientation3DStampedConstraint, Optimization) // Assemble the full covariance from the covariance blocks fuse_core::Matrix4d actual_covariance(covariance_vector.data()); - fuse_core::Matrix3d expected_covariance = cov; + fuse_core::Matrix3d expected_covariance; + expected_covariance << + 0.25, 0.025, 0.05, + 0.025, 0.5, 0.075, + 0.05, 0.075, 0.75; EXPECT_TRUE(expected_covariance.isApprox(actual_covariance.block<3, 3>(1, 1), 1.0e-9)); } diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp index 6520fdc9f..c9b45d6d0 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_constraint.cpp @@ -200,7 +200,15 @@ TEST(AbsolutePose3DStampedConstraint, Optimization) fuse_core::Matrix6d actual_covariance; actual_covariance << pos_pos, pos_or.block<3, 3>(0, 1), pos_or.block<3, 3>(0, 1).transpose(), or_or.block<3, 3>(1, 1); - fuse_core::Matrix6d expected_covariance = cov; + fuse_core::Matrix6d expected_covariance; + expected_covariance << + 1.0, 0.1, 0.2, 0.15, 0.2, 0.25, + 0.1, 2.0, 0.6, 0.25, 0.2, 0.15, + 0.2, 0.6, 3.0, 0.1, 0.05, 0.1, + 0.15, 0.25, 0.1, 1.0, 0.075, 0.1, + 0.2, 0.2, 0.05, 0.075, 1.25, 0.125, + 0.25, 0.15, 0.1, 0.1, 0.125, 1.5; + EXPECT_TRUE(expected_covariance.isApprox(actual_covariance, 1.0e-9)); } diff --git a/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp b/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp new file mode 100644 index 000000000..abc083606 --- /dev/null +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_constraint.cpp @@ -0,0 +1,315 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, 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 + +using fuse_variables::Orientation3DStamped; +using fuse_variables::Position3DStamped; +using fuse_constraints::AbsolutePose3DStampedConstraint; +using fuse_constraints::RelativePose3DStampedConstraint; + + +TEST(RelativePose3DStampedConstraint, Constructor) +{ + // Construct a constraint just to make sure it compiles. + Orientation3DStamped orientation1(ros::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(ros::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(ros::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(ros::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + + fuse_core::Vector7d delta; + delta << 1.0, 2.0, 3.0, 0.988, 0.094, 0.079, 0.094; + + // 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, + 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; + + EXPECT_NO_THROW(RelativePose3DStampedConstraint constraint(position1, + orientation1, + position2, + orientation2, + delta, + cov)); +} + +TEST(RelativePose3DStampedConstraint, Covariance) +{ + // Verify the covariance <--> sqrt information conversions are correct + Orientation3DStamped orientation1(ros::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(ros::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(ros::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(ros::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + fuse_core::Vector7d delta; + delta << 1.0, 2.0, 3.0, 0.988, 0.094, 0.079, 0.094; + + // 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, + 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; + + RelativePose3DStampedConstraint constraint(position1, + orientation1, + position2, + orientation2, + delta, + 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_TRUE(expected_cov.isApprox(constraint.covariance(), 1.0e-9)); + EXPECT_TRUE(expected_sqrt_info.isApprox(constraint.sqrtInformation(), 1.0e-9)); +} + +TEST(RelativePose3DStampedConstraint, Optimization) +{ + // Optimize a two-pose system with a pose prior and a relative pose constraint + // Verify the expected poses and covariances are generated. + // Create two poses + auto position1 = Position3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + position1->x() = 1.5; + position1->y() = -3.0; + position1->z() = 10.0; + + auto orientation1 = Orientation3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation1->w() = 0.952; + orientation1->x() = 0.038; + orientation1->y() = -0.189; + orientation1->z() = 0.239; + + auto position2 = Position3DStamped::make_shared(ros::Time(2, 0), fuse_core::uuid::generate("spra")); + position2->x() = -1.5; + position2->y() = 3.0; + position2->z() = -10.0; + + auto orientation2 = Orientation3DStamped::make_shared(ros::Time(2, 0), fuse_core::uuid::generate("spra")); + orientation2->w() = 0.944; + orientation2->x() = -0.128; + orientation2->y() = 0.145; + orientation2->z() = -0.269; + + // Create an absolute pose constraint at the origin + fuse_core::Vector7d mean_origin; + mean_origin << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; + fuse_core::Matrix6d cov_origin = fuse_core::Matrix6d::Identity(); + auto prior = AbsolutePose3DStampedConstraint::make_shared(*position1, + *orientation1, + mean_origin, + cov_origin); + + // Create a relative pose constraint for 1m in the x direction + fuse_core::Vector7d mean_delta; + mean_delta << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; + fuse_core::Matrix6d cov_delta = fuse_core::Matrix6d::Identity(); + auto relative = RelativePose3DStampedConstraint::make_shared(*position1, + *orientation1, + *position2, + *orientation2, + mean_delta, + cov_delta); + + // Build the problem + ceres::Problem problem; + problem.AddParameterBlock( + orientation1->data(), + orientation1->size(), + orientation1->localParameterization()); + problem.AddParameterBlock( + position1->data(), + position1->size(), + position1->localParameterization()); + problem.AddParameterBlock( + orientation2->data(), + orientation2->size(), + orientation2->localParameterization()); + problem.AddParameterBlock( + position2->data(), + position2->size(), + position2->localParameterization()); + std::vector prior_parameter_blocks; + prior_parameter_blocks.push_back(position1->data()); + prior_parameter_blocks.push_back(orientation1->data()); + problem.AddResidualBlock( + prior->costFunction(), + prior->lossFunction(), + prior_parameter_blocks); + std::vector relative_parameter_blocks; + relative_parameter_blocks.push_back(position1->data()); + relative_parameter_blocks.push_back(orientation1->data()); + relative_parameter_blocks.push_back(position2->data()); + relative_parameter_blocks.push_back(orientation2->data()); + problem.AddResidualBlock( + relative->costFunction(), + relative->lossFunction(), + relative_parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(0.0, position1->x(), 1.0e-5); + EXPECT_NEAR(0.0, position1->y(), 1.0e-5); + EXPECT_NEAR(0.0, position1->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation1->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->z(), 1.0e-3); + EXPECT_NEAR(1.0, position2->x(), 1.0e-5); + EXPECT_NEAR(0.0, position2->y(), 1.0e-5); + EXPECT_NEAR(0.0, position2->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation2->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->z(), 1.0e-3); + + // Compute the marginal covariance for pose1 + { + std::vector > covariance_blocks; + covariance_blocks.emplace_back(position1->data(), position1->data()); + covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); + covariance_blocks.emplace_back(position1->data(), orientation1->data()); + + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + + std::vector cov_pos_pos(position1->size() * position1->size()); + covariance.GetCovarianceBlock(position1->data(), position1->data(), cov_pos_pos.data()); + + std::vector cov_or_or(orientation1->size() * orientation1->size()); + covariance.GetCovarianceBlock(orientation1->data(), orientation1->data(), cov_or_or.data()); + + std::vector cov_pos_or(position1->size() * orientation1->size()); + covariance.GetCovarianceBlock(position1->data(), orientation1->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + Eigen::Map pos_pos(cov_pos_pos.data()); + Eigen::Map or_or(cov_or_or.data()); + Eigen::Map> pos_or(cov_pos_or.data()); + + fuse_core::Matrix6d actual_covariance; + actual_covariance << + pos_pos, pos_or.block<3, 3>(0, 1), + pos_or.block<3, 3>(0, 1).transpose(), or_or.block<3, 3>(1, 1); + + fuse_core::Matrix6d expected_covariance; + expected_covariance << + 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.25; + EXPECT_TRUE(expected_covariance.isApprox(actual_covariance, 1.0e-9)); + } + + // Compute the marginal covariance for pose2 + { + std::vector > covariance_blocks; + covariance_blocks.emplace_back(position2->data(), position2->data()); + covariance_blocks.emplace_back(position2->data(), orientation2->data()); + covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); + + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + + std::vector cov_pos_pos(position2->size() * position2->size()); + covariance.GetCovarianceBlock(position2->data(), position2->data(), cov_pos_pos.data()); + + std::vector cov_or_or(orientation2->size() * orientation2->size()); + covariance.GetCovarianceBlock(orientation2->data(), orientation2->data(), cov_or_or.data()); + + std::vector cov_pos_or(position2->size() * orientation2->size()); + covariance.GetCovarianceBlock(position2->data(), orientation2->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + Eigen::Map pos_pos(cov_pos_pos.data()); + Eigen::Map or_or(cov_or_or.data()); + Eigen::Map> pos_or(cov_pos_or.data()); + + fuse_core::Matrix6d actual_covariance; + actual_covariance << + pos_pos, pos_or.block<3, 3>(0, 1), + pos_or.block<3, 3>(0, 1).transpose(), or_or.block<3, 3>(1, 1); + + fuse_core::Matrix6d expected_covariance; + expected_covariance << + 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 3.0, 0.0, 0.0, 0.0, 0.5, + 0.0, 0.0, 3.0, 0.0, -0.5, 0.0, + 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, + 0.0, 0.0, -0.5, 0.0, 0.5, 0.0, + 0.0, 0.5, 0.0, 0.0, 0.0, 0.5; + + EXPECT_TRUE(expected_covariance.isApprox(actual_covariance, 1.0e-3)); + } +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}