From 98cf1f3bebd0973a84f71647d8d9d4b39bb2216d Mon Sep 17 00:00:00 2001 From: Jake McLaughlin Date: Mon, 25 Nov 2024 15:25:18 -0500 Subject: [PATCH] Fix mutliple definition error (#386) * Make implementations inline to fix multiple definition error --- .../normal_delta_pose_3d_cost_functor.h | 94 +++++++++---------- .../normal_prior_pose_3d_cost_functor.h | 52 +++++----- 2 files changed, 65 insertions(+), 81 deletions(-) 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 index 343174853..d85a0a5e4 100644 --- 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 @@ -77,7 +77,12 @@ class NormalDeltaPose3DCostFunctor * (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); + NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b): + A_(A), + b_(b), + orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Orientation residuals will not be scaled + { + } /** * @brief Compute the cost values/residuals using the provided variable/parameter values @@ -88,7 +93,42 @@ class NormalDeltaPose3DCostFunctor const T* const orientation1, const T* const position2, const T* const orientation2, - T* residual) const; + T* residual) const + { + // Compute the position delta between pose1 and pose2 + T orientation1_inverse[4] = + { + orientation1[0], + -orientation1[1], + -orientation1[2], + -orientation1[3] + }; + T position_delta[3] = + { + position2[0] - position1[0], + position2[1] - position1[1], + position2[2] - position1[2] + }; + T position_delta_rotated[3]; + ceres::QuaternionRotatePoint( + orientation1_inverse, + position_delta, + position_delta_rotated); + + // Compute the first three residual terms as (position_delta - b) + residual[0] = position_delta_rotated[0] - T(b_[0]); + residual[1] = position_delta_rotated[1] - T(b_[1]); + residual[2] = position_delta_rotated[2] - T(b_[2]); + + // Use the 3D orientation cost functor to compute the orientation delta + orientation_functor_(orientation1, orientation2, &residual[3]); + + // Map it to Eigen, and weight it + Eigen::Map> residual_map(residual); + residual_map.applyOnTheLeft(A_.template cast()); + + return true; + } private: fuse_core::Matrix6d A_; //!< The residual weighting matrix, most likely the square root information matrix @@ -97,56 +137,6 @@ class NormalDeltaPose3DCostFunctor NormalDeltaOrientation3DCostFunctor orientation_functor_; }; -NormalDeltaPose3DCostFunctor::NormalDeltaPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b) : - A_(A), - b_(b), - orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Orientation residuals will not be scaled -{ -} - -template -bool NormalDeltaPose3DCostFunctor::operator()( - const T* const position1, - const T* const orientation1, - const T* const position2, - const T* const orientation2, - T* residual) const -{ - // Compute the position delta between pose1 and pose2 - T orientation1_inverse[4] = - { - orientation1[0], - -orientation1[1], - -orientation1[2], - -orientation1[3] - }; - T position_delta[3] = - { - position2[0] - position1[0], - position2[1] - position1[1], - position2[2] - position1[2] - }; - T position_delta_rotated[3]; - ceres::QuaternionRotatePoint( - orientation1_inverse, - position_delta, - position_delta_rotated); - - // Compute the first three residual terms as (position_delta - b) - residual[0] = position_delta_rotated[0] - T(b_[0]); - residual[1] = position_delta_rotated[1] - T(b_[1]); - residual[2] = position_delta_rotated[2] - T(b_[2]); - - // Use the 3D orientation cost functor to compute the orientation delta - orientation_functor_(orientation1, orientation2, &residual[3]); - - // 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_pose_3d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.h index a77f5627c..d50159240 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.h @@ -77,13 +77,34 @@ class NormalPriorPose3DCostFunctor * (x, y, z, qx, qy, qz) * @param[in] b The 3D pose measurement or prior in order (x, y, z, qw, qx, qy, qz) */ - NormalPriorPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b); + NormalPriorPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b): + A_(A), + b_(b), + orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Delta will not be scaled + { + } /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. */ template - bool operator()(const T* const position, const T* const orientation, T* residual) const; + bool operator()(const T* const position, const T* const orientation, T* residual) const + { + // Compute the position error + residual[0] = position[0] - T(b_(0)); + residual[1] = position[1] - T(b_(1)); + residual[2] = position[2] - T(b_(2)); + + // Use the 3D orientation cost functor to compute the orientation delta + orientation_functor_(orientation, &residual[3]); + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + Eigen::Map> residual_map(residual); + residual_map.applyOnTheLeft(A_.template cast()); + + return true; + } private: fuse_core::Matrix6d A_; @@ -91,33 +112,6 @@ class NormalPriorPose3DCostFunctor NormalPriorOrientation3DCostFunctor orientation_functor_; }; - -NormalPriorPose3DCostFunctor::NormalPriorPose3DCostFunctor(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b) : - A_(A), - b_(b), - orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Delta will not be scaled -{ -} - -template -bool NormalPriorPose3DCostFunctor::operator()(const T* const position, const T* const orientation, T* residual) const -{ - // Compute the position error - residual[0] = position[0] - T(b_(0)); - residual[1] = position[1] - T(b_(1)); - residual[2] = position[2] - T(b_(2)); - - // Use the 3D orientation cost functor to compute the orientation delta - orientation_functor_(orientation, &residual[3]); - - // Scale the residuals by the square root information matrix to account for - // the measurement uncertainty. - Eigen::Map> residual_map(residual); - residual_map.applyOnTheLeft(A_.template cast()); - - return true; -} - } // namespace fuse_constraints #endif // FUSE_CONSTRAINTS_NORMAL_PRIOR_POSE_3D_COST_FUNCTOR_H