diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 03bdea8c..a72a982f 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -24,6 +24,9 @@ jobs: - name: Checkout source uses: actions/checkout@v4 + - name: VCS import + run: vcs import src --input src/fuse/fuse.repos ; rosdep install --ignore-src --from-paths src -y -r + - name: Setup Docker Buildx uses: docker/setup-buildx-action@v2 diff --git a/fuse.repos b/fuse.repos index 8ad27e25..d569f4ec 100644 --- a/fuse.repos +++ b/fuse.repos @@ -6,4 +6,4 @@ repositories: covariance_geometry_ros: type: git url: https://github.com/giafranchini/covariance_geometry_ros.git - version: iron \ No newline at end of file + version: iron diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp index 63c2d850..e367e174 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp @@ -32,7 +32,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ - #ifndef FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ #define FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ @@ -54,13 +53,12 @@ #include #include - namespace fuse_constraints { /** * @brief A constraint that represents either prior information about a 3D pose, or a direct - * measurement of the 3D pose. + * measurement of the 3D pose. * * A 3D pose is the combination of a 3D position and a 3D orientation variable. As a convenience, * this class applies an absolute constraint on both variables at once. This type of constraint @@ -92,13 +90,10 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * (6x1 vector: x, y, z, roll, pitch, yaw) * @param[in] covariance The measurement/prior covariance (6x6 matrix: x, y, z, roll, pitch, yaw) */ - AbsolutePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector6d & mean, - const fuse_core::Matrix6d & covariance); - + AbsolutePose3DStampedEulerConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_core::Vector6d& mean, const fuse_core::Matrix6d& covariance); + /** * @brief Create a constraint using a partial measurement/prior of the 3D pose * @@ -110,13 +105,11 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * @param[in] partial_covariance The measurement/prior covariance (6x6 matrix: x, y, z, roll, pitch, yaw) * @param[in] variable_indices The indices of the measured variables */ - AbsolutePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector6d & partial_mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & variable_indices); + AbsolutePose3DStampedEulerConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_core::Vector6d& partial_mean, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& variable_indices); /** * @brief Destructor @@ -128,14 +121,20 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * * Order is (x, y, z, roll, pitch, yaw) */ - const fuse_core::Vector6d & mean() const {return mean_;} + const fuse_core::Vector6d& mean() const + { + return mean_; + } /** * @brief Read-only access to the square root information matrix. * * Order is (x, y, z, roll, pitch, yaw) */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -149,7 +148,7 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -161,10 +160,10 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::Vector6d mean_; //!< The measured/prior mean vector for this variable + fuse_core::Vector6d mean_; //!< The measured/prior mean vector for this variable fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix private: @@ -178,12 +177,12 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & mean_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& mean_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp index afb16217..4c120165 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp @@ -41,7 +41,6 @@ #include #include - namespace fuse_constraints { @@ -76,62 +75,42 @@ class NormalDeltaPose3DEulerCostFunctor * order (dx, dy, dz, droll, dpitch, dyaw) * @param[in] b The exposed pose difference in order (dx, dy, dz, droll, dpitch, dyaw) */ - NormalDeltaPose3DEulerCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b); + NormalDeltaPose3DEulerCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector6d& 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; + 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::MatrixXd A_; fuse_core::Vector6d b_; }; -NormalDeltaPose3DEulerCostFunctor::NormalDeltaPose3DEulerCostFunctor( - const fuse_core::MatrixXd & A, - const fuse_core::Vector6d & b) -: A_(A), - b_(b) +NormalDeltaPose3DEulerCostFunctor::NormalDeltaPose3DEulerCostFunctor(const fuse_core::MatrixXd& A, + const fuse_core::Vector6d& b) + : A_(A), b_(b) { CHECK_GT(A_.rows(), 0); CHECK_EQ(A_.cols(), 6); } -template -bool NormalDeltaPose3DEulerCostFunctor::operator()( - const T * const position1, - const T * const orientation1, - const T * const position2, - const T * const orientation2, - T * residual) const +template +bool NormalDeltaPose3DEulerCostFunctor::operator()(const T* const position1, const T* const orientation1, + const T* const position2, const T* const orientation2, + T* residual) const { T full_residuals[6]; T position_delta_rotated[3]; T orientation_delta[4]; T orientation_delta_rpy[3]; - 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 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] }; + // Compute the position residual ceres::QuaternionRotatePoint(orientation1_inverse, position_delta, position_delta_rotated); full_residuals[0] = position_delta_rotated[0] - T(b_(0)); @@ -140,9 +119,12 @@ bool NormalDeltaPose3DEulerCostFunctor::operator()( // Compute the orientation residual ceres::QuaternionProduct(orientation1_inverse, orientation2, orientation_delta); - orientation_delta_rpy[0] = fuse_core::getRoll(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); - orientation_delta_rpy[1] = fuse_core::getPitch(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); - orientation_delta_rpy[2] = fuse_core::getYaw(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); + orientation_delta_rpy[0] = + fuse_core::getRoll(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); + orientation_delta_rpy[1] = + fuse_core::getPitch(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); + orientation_delta_rpy[2] = + fuse_core::getYaw(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); full_residuals[3] = orientation_delta_rpy[0] - T(b_(3)); full_residuals[4] = orientation_delta_rpy[1] - T(b_(4)); full_residuals[5] = orientation_delta_rpy[2] - T(b_(5)); @@ -158,4 +140,4 @@ bool NormalDeltaPose3DEulerCostFunctor::operator()( } // namespace fuse_constraints -#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ \ No newline at end of file +#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp index 4e6c4b60..bcb2ddb8 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp @@ -38,7 +38,6 @@ #include - namespace fuse_constraints { @@ -73,7 +72,7 @@ class NormalPriorOrientation3D : public ceres::SizedCostFunction<3, 4> * order (qx, qy, qz) * @param[in] b The orientation measurement or prior in order (qw, qx, qy, qz) */ - NormalPriorOrientation3D(const fuse_core::Matrix3d & A, const fuse_core::Vector4d & b); + NormalPriorOrientation3D(const fuse_core::Matrix3d& A, const fuse_core::Vector4d& b); /** * @brief Destructor @@ -84,10 +83,7 @@ class NormalPriorOrientation3D : public ceres::SizedCostFunction<3, 4> * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided * variable/parameter values */ - virtual bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const; + virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const; private: fuse_core::Matrix3d A_; //!< The residual weighting matrix, most likely the square root diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp index 7ec76458..8088539d 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp @@ -39,7 +39,6 @@ #include - namespace fuse_constraints { @@ -55,7 +54,7 @@ namespace fuse_constraints * cost(x) = || A * [ y - b(1)] || * || [ z - b(2)] || * || [ quat2angleaxis(b(3-6)^-1 * q)] || - * + * * In case the user is interested in implementing a cost function of the form: * * cost(X) = (X - mu)^T S^{-1} (X - mu) @@ -73,7 +72,7 @@ class NormalPriorPose3D : public ceres::SizedCostFunction<6, 3, 4> * order (x, y, z, roll, pitch, yaw) * @param[in] b The pose measurement or prior in order (x, y, z, qw, qx, qy, qz) */ - NormalPriorPose3D(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b); + NormalPriorPose3D(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b); /** * @brief Destructor @@ -84,10 +83,7 @@ class NormalPriorPose3D : public ceres::SizedCostFunction<6, 3, 4> * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided * variable/parameter values */ - virtual bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const; + virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const; private: fuse_core::Matrix6d A_; //!< The residual weighting matrix, most likely the square root diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp index 0579b30c..723fc3fb 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp @@ -38,7 +38,6 @@ #include - namespace fuse_constraints { @@ -54,7 +53,7 @@ namespace fuse_constraints * cost(x) = || A * [ y - b(1)] || * || [ z - b(2)] || * || [ quat2eul(q) - b(3:5) ] || - * + * * Here, the matrix A can be of variable size, thereby permitting the computation of errors for * partial measurements. The vector b is a fixed-size 6x1. In case the user is interested in * implementing a cost function of the form: @@ -79,7 +78,7 @@ class NormalPriorPose3DEuler : public ceres::SizedCostFunction #include - namespace fuse_constraints { @@ -76,40 +75,35 @@ class NormalPriorPose3DEulerCostFunctor * order (x, y, z, roll, pitch, yaw) * @param[in] b The 3D pose measurement or prior in order (x, y, z, roll, pitch, yaw) */ - NormalPriorPose3DEulerCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b); + NormalPriorPose3DEulerCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector6d& b); /** * @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; + template + bool operator()(const T* const position, const T* const orientation, T* residual) const; private: fuse_core::MatrixXd A_; fuse_core::Vector6d b_; }; -NormalPriorPose3DEulerCostFunctor::NormalPriorPose3DEulerCostFunctor( - const fuse_core::MatrixXd & A, - const fuse_core::Vector6d & b) -: A_(A), - b_(b) +NormalPriorPose3DEulerCostFunctor::NormalPriorPose3DEulerCostFunctor(const fuse_core::MatrixXd& A, + const fuse_core::Vector6d& b) + : A_(A), b_(b) { CHECK_GT(A_.rows(), 0); CHECK_EQ(A_.cols(), 6); } -template -bool NormalPriorPose3DEulerCostFunctor::operator()( - const T * const position, const T * const orientation, - T * residual) const +template +bool NormalPriorPose3DEulerCostFunctor::operator()(const T* const position, const T* const orientation, + T* residual) const { T full_residuals[6]; - T orientation_rpy[3] = { - fuse_core::getRoll(orientation[0], orientation[1], orientation[2], orientation[3]), - fuse_core::getPitch(orientation[0], orientation[1], orientation[2], orientation[3]), - fuse_core::getYaw(orientation[0], orientation[1], orientation[2], orientation[3]) - }; + T orientation_rpy[3] = { fuse_core::getRoll(orientation[0], orientation[1], orientation[2], orientation[3]), + fuse_core::getPitch(orientation[0], orientation[1], orientation[2], orientation[3]), + fuse_core::getYaw(orientation[0], orientation[1], orientation[2], orientation[3]) }; // Compute the position residual full_residuals[0] = position[0] - T(b_(0)); diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp index a5f2fdf9..ab59f791 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp @@ -53,7 +53,6 @@ #include #include - namespace fuse_constraints { @@ -86,15 +85,12 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint * (6x1 vector: dx, dy, dz, droll, dpitch, dyaw) * @param[in] covariance The measurement covariance (6x6 matrix: dx, dy, dz, droll, dpitch, dyaw) */ - RelativePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector6d & delta, - const fuse_core::Matrix6d & covariance); - + RelativePose3DStampedEulerConstraint(const std::string& source, const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, + const fuse_core::Vector6d& delta, const fuse_core::Matrix6d& covariance); + /** * @brief Create a constraint using a measurement/prior of the relative 3D pose * @@ -107,16 +103,14 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint * (6x1 vector: dx, dy, dz, droll, dpitch, dyaw) * @param[in] partial_covariance The measurement subset covariance (max 6x6 matrix: x, y, z, droll, dpitch, dyaw) * @param[in] variable_indices The indices of the measured variables - */ - RelativePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector6d & partial_delta, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & variable_indices); + */ + RelativePose3DStampedEulerConstraint(const std::string& source, const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, + const fuse_core::Vector6d& partial_delta, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& variable_indices); /** * @brief Destructor @@ -126,12 +120,18 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint /** * @brief Read-only access to the measured pose change. */ - const fuse_core::Vector6d & delta() const {return delta_;} + const fuse_core::Vector6d& delta() const + { + return delta_; + } /** * @brief Read-only access to the square root information matrix. */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -143,7 +143,7 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Access the cost function for this constraint @@ -155,10 +155,10 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::Vector6d delta_; //!< The measured pose change (dx, dy, dz, droll, dpitch, dyaw) + fuse_core::Vector6d delta_; //!< The measured pose change (dx, dy, dz, droll, dpitch, dyaw) fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix (derived from the //!< covariance matrix) @@ -173,12 +173,12 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & delta_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& delta_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp index 56206e30..e3bec0ff 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp @@ -45,27 +45,24 @@ namespace fuse_constraints { AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector6d & mean, - const fuse_core::Matrix6d & covariance) -: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT - mean_(mean), - sqrt_information_(covariance.inverse().llt().matrixU()) -{ + const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, const fuse_core::Vector6d& mean, + const fuse_core::Matrix6d& covariance) + : fuse_core::Constraint(source, { position.uuid(), orientation.uuid() }) + , // NOLINT + mean_(mean) + , sqrt_information_(covariance.inverse().llt().matrixU()) +{ } AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector6d & partial_mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & variable_indices) -: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT + const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, const fuse_core::Vector6d& partial_mean, + const fuse_core::MatrixXd& partial_covariance, const std::vector& variable_indices) + : fuse_core::Constraint(source, { position.uuid(), orientation.uuid() }) + , // NOLINT mean_(partial_mean) -{ +{ // Compute the partial sqrt information matrix of the provided cov matrix fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); @@ -76,8 +73,8 @@ AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( // dimensions. But the variable vectors will be full sized. We can make this all work out by // creating a non-square A, where each row computes a cost for one measured dimensions, // and the columns are in the order defined by the variable. - - // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions + + // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); for (size_t i = 0; i < variable_indices.size(); ++i) { @@ -103,7 +100,7 @@ fuse_core::Matrix6d AbsolutePose3DStampedEulerConstraint::covariance() const return pinv * pinv.transpose(); } -void AbsolutePose3DStampedEulerConstraint::print(std::ostream & stream) const +void AbsolutePose3DStampedEulerConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -113,23 +110,24 @@ void AbsolutePose3DStampedEulerConstraint::print(std::ostream & stream) const << " mean: " << mean().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * AbsolutePose3DStampedEulerConstraint::costFunction() const +ceres::CostFunction* AbsolutePose3DStampedEulerConstraint::costFunction() const { return new NormalPriorPose3DEuler(sqrt_information_, mean_); - + // Here we return a cost function that computes the analytic derivatives/jacobians, but we could // use automatic differentiation as follows: // return new ceres::AutoDiffCostFunction( - // new NormalPriorPose3DEulerCostFunctor(sqrt_information_, mean_), - // sqrt_information_.rows()); - + // new NormalPriorPose3DEulerCostFunctor(sqrt_information_, mean_), + // sqrt_information_.rows()); + // And including the followings: // #include // #include diff --git a/fuse_constraints/src/normal_prior_orientation_3d.cpp b/fuse_constraints/src/normal_prior_orientation_3d.cpp index 34269ced..c0537dc2 100644 --- a/fuse_constraints/src/normal_prior_orientation_3d.cpp +++ b/fuse_constraints/src/normal_prior_orientation_3d.cpp @@ -40,32 +40,21 @@ namespace fuse_constraints { -NormalPriorOrientation3D::NormalPriorOrientation3D(const fuse_core::Matrix3d & A, const fuse_core::Vector4d & b) -: A_(A), - b_(b) +NormalPriorOrientation3D::NormalPriorOrientation3D(const fuse_core::Matrix3d& A, const fuse_core::Vector4d& b) + : A_(A), b_(b) { } -bool NormalPriorOrientation3D::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalPriorOrientation3D::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { - double variable[4] = - { + double variable[4] = { parameters[0][0], parameters[0][1], parameters[0][2], parameters[0][3], }; - double observation_inverse[4] = - { - b_(0), - -b_(1), - -b_(2), - -b_(3) - }; + double observation_inverse[4] = { b_(0), -b_(1), -b_(2), -b_(3) }; double difference[4]; double j_product[16]; @@ -73,15 +62,17 @@ bool NormalPriorOrientation3D::Evaluate( // TODO(giafranchini): these jacobians should be populated only if jacobians[1] != nullptr fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); - fuse_core::quaternionToAngleAxis(difference, residuals, j_quat2angle); // orientation angle-axis - + fuse_core::quaternionToAngleAxis(difference, residuals, j_quat2angle); // orientation angle-axis + // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. Eigen::Map residuals_map(residuals); residuals_map.applyOnTheLeft(A_); - if (jacobians != nullptr) { - if (jacobians[0] != nullptr) { + if (jacobians != nullptr) + { + if (jacobians[0] != nullptr) + { Eigen::Map> j_map(jacobians[0]); Eigen::Map j_product_map(j_product); Eigen::Map> j_quat2angle_map(j_quat2angle); diff --git a/fuse_constraints/src/normal_prior_pose_3d.cpp b/fuse_constraints/src/normal_prior_pose_3d.cpp index 581e455d..4fa0864a 100644 --- a/fuse_constraints/src/normal_prior_pose_3d.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d.cpp @@ -40,33 +40,20 @@ namespace fuse_constraints { -NormalPriorPose3D::NormalPriorPose3D(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b) -: A_(A), - b_(b) +NormalPriorPose3D::NormalPriorPose3D(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b) : A_(A), b_(b) { } -bool NormalPriorPose3D::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalPriorPose3D::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { - - double variable[4] = - { + double variable[4] = { parameters[1][0], parameters[1][1], parameters[1][2], parameters[1][3], }; - double observation_inverse[4] = - { - b_(3), - -b_(4), - -b_(5), - -b_(6) - }; + double observation_inverse[4] = { b_(3), -b_(4), -b_(5), -b_(6) }; double difference[4]; double j_product[16]; @@ -78,23 +65,26 @@ bool NormalPriorPose3D::Evaluate( residuals[2] = parameters[0][2] - b_[2]; // Compute orientation residuals - // TODO(giafranchini): this is repeated code, it should be better to include normal_prior_orientation_3d.hpp - // and use that to compute residuals and jacobians. + // TODO(giafranchini): this is repeated code, it should be better to include normal_prior_orientation_3d.hpp + // and use that to compute residuals and jacobians. fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); - fuse_core::quaternionToAngleAxis(difference, &residuals[3], j_quat2angle); // orientation angle-axis - + fuse_core::quaternionToAngleAxis(difference, &residuals[3], j_quat2angle); // orientation angle-axis + // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. Eigen::Map residuals_map(residuals); residuals_map.applyOnTheLeft(A_); - if (jacobians != nullptr) { + if (jacobians != nullptr) + { // Jacobian of the residuals wrt position parameter block - if (jacobians[0] != nullptr) { + if (jacobians[0] != nullptr) + { Eigen::Map(jacobians[0], num_residuals(), 3) = A_.leftCols<3>(); } // Jacobian of the residuals wrt orientation parameter block - if (jacobians[1] != nullptr) { + if (jacobians[1] != nullptr) + { Eigen::Map j_product_map(j_product); Eigen::Map> j_quat2angle_map(j_quat2angle); Eigen::Map j1_map(jacobians[1], num_residuals(), 4); diff --git a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp index c4871aa3..09816802 100644 --- a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp @@ -42,19 +42,15 @@ namespace fuse_constraints { -NormalPriorPose3DEuler::NormalPriorPose3DEuler(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b) -: A_(A), - b_(b) +NormalPriorPose3DEuler::NormalPriorPose3DEuler(const fuse_core::MatrixXd& A, const fuse_core::Vector6d& b) + : A_(A), b_(b) { CHECK_GT(A_.rows(), 0); CHECK_EQ(A_.cols(), 6); set_num_residuals(A_.rows()); } -bool NormalPriorPose3DEuler::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalPriorPose3DEuler::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { fuse_core::Vector6d full_residuals; double orientation_rpy[3]; @@ -74,19 +70,21 @@ bool NormalPriorPose3DEuler::Evaluate( // Scale the residuals by the square root information matrix to account for // the measurement uncertainty. - Eigen::Map> residuals_map(residuals, A_.rows()); + Eigen::Map> residuals_map(residuals, A_.rows()); residuals_map = A_ * full_residuals; - if (jacobians != nullptr) { + if (jacobians != nullptr) + { // Jacobian wrt position - if (jacobians[0] != nullptr) { + if (jacobians[0] != nullptr) + { Eigen::Map(jacobians[0], num_residuals(), 3) = A_.leftCols<3>(); } // Jacobian wrt orientation - if (jacobians[1] != nullptr) { + if (jacobians[1] != nullptr) + { Eigen::Map> j_quat2angle_map(j_quat2rpy); - Eigen::Map(jacobians[1], num_residuals(), 4) = - A_.rightCols<3>() * j_quat2angle_map; + Eigen::Map(jacobians[1], num_residuals(), 4) = A_.rightCols<3>() * j_quat2angle_map; } } return true; diff --git a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp index 0c4e7eee..91de1542 100644 --- a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp @@ -44,33 +44,24 @@ namespace fuse_constraints { RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector6d & delta, - const fuse_core::Matrix6d & covariance) -: fuse_core::Constraint( - source, - {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}), // NOLINT - delta_(delta), - sqrt_information_(covariance.inverse().llt().matrixU()) + const std::string& source, const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, const fuse_core::Vector6d& delta, + const fuse_core::Matrix6d& covariance) + : fuse_core::Constraint(source, { position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid() }) + , // NOLINT + delta_(delta) + , sqrt_information_(covariance.inverse().llt().matrixU()) { } RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector6d & partial_delta, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & variable_indices) -: fuse_core::Constraint( - source, - {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}), // NOLINT + const std::string& source, const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, const fuse_core::Vector6d& partial_delta, + const fuse_core::MatrixXd& partial_covariance, const std::vector& variable_indices) + : fuse_core::Constraint(source, { position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid() }) + , // NOLINT delta_(partial_delta) { // Compute the partial sqrt information matrix of the provided cov matrix @@ -83,8 +74,8 @@ RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( // dimensions. But the variable vectors will be full sized. We can make this all work out by // creating a non-square A, where each row computes a cost for one measured dimensions, // and the columns are in the order defined by the variable. - - // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions + + // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); for (size_t i = 0; i < variable_indices.size(); ++i) { @@ -106,7 +97,7 @@ fuse_core::Matrix6d RelativePose3DStampedEulerConstraint::covariance() const return pinv * pinv.transpose(); } -void RelativePose3DStampedEulerConstraint::print(std::ostream & stream) const +void RelativePose3DStampedEulerConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -119,12 +110,11 @@ void RelativePose3DStampedEulerConstraint::print(std::ostream & stream) const << " sqrt_info: " << sqrtInformation() << "\n"; } -ceres::CostFunction * RelativePose3DStampedEulerConstraint::costFunction() const +ceres::CostFunction* RelativePose3DStampedEulerConstraint::costFunction() const { // TODO(giafranchini): implement cost function with analytical Jacobians return new ceres::AutoDiffCostFunction( - new NormalDeltaPose3DEulerCostFunctor(sqrt_information_, delta_), - sqrt_information_.rows()); + new NormalDeltaPose3DEulerCostFunctor(sqrt_information_, delta_), sqrt_information_.rows()); } } // namespace fuse_constraints diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp index 4e0b0457..43b92de9 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp @@ -47,17 +47,15 @@ #include #include +using fuse_constraints::AbsolutePose3DStampedEulerConstraint; using fuse_variables::Orientation3DStamped; using fuse_variables::Position3DStamped; -using fuse_constraints::AbsolutePose3DStampedEulerConstraint; - TEST(AbsolutePose3DStampedEulerConstraint, Constructor) { // Construct a constraint just to make sure it compiles. Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector6d mean; mean << 1.0, 2.0, 3.0, 0.1, 0.1, 1.0; @@ -66,53 +64,53 @@ TEST(AbsolutePose3DStampedEulerConstraint, Constructor) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - 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 + 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 /* *INDENT-ON* */ EXPECT_NO_THROW( - AbsolutePose3DStampedEulerConstraint constraint( - "test", position_variable, orientation_variable, - mean, cov)); + AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, mean, cov)); } TEST(AbsolutePose3DStampedEulerConstraint, ConstructorPartial) { // Construct a constraint just to make sure it compiles. Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); - std::vector variable_indices {0, 2, 3, 4, 5}; + std::vector variable_indices{ 0, 2, 3, 4, 5 }; fuse_core::Vector6d mean_partial; mean_partial << 1.0, 0.0, 3.0, 0.1, 0.1, 1.0; fuse_core::Matrix5d cov_partial; /* *INDENT-OFF* */ - cov_partial << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov_partial << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT /* *INDENT-ON* */ - EXPECT_NO_THROW( - AbsolutePose3DStampedEulerConstraint constraint( - "test", position_variable, orientation_variable, - mean_partial, cov_partial, variable_indices)); + EXPECT_NO_THROW(AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, + mean_partial, cov_partial, variable_indices)); } TEST(AbsolutePose3DStampedEulerConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); fuse_core::Vector6d mean; mean << 1.0, 2.0, 3.0, 0.1, 0.1, 1.0; @@ -121,26 +119,32 @@ TEST(AbsolutePose3DStampedEulerConstraint, Covariance) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - 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 + 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 /* *INDENT-ON* */ - AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, mean, - cov); + AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, mean, cov); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix6d expected_sqrt_info; /* *INDENT-OFF* */ - 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 + 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 /* *INDENT-ON* */ fuse_core::Matrix6d expected_cov = cov; @@ -153,10 +157,9 @@ TEST(AbsolutePose3DStampedEulerConstraint, CovariancePartial) { // Verify the covariance <--> sqrt information conversions are correct Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); - std::vector variable_indices {0, 2, 3, 4, 5}; + std::vector variable_indices{ 0, 2, 3, 4, 5 }; fuse_core::Vector6d mean; mean << 1.0, 0.0, 3.0, 0.1, 0.1, 1.0; @@ -165,34 +168,35 @@ TEST(AbsolutePose3DStampedEulerConstraint, CovariancePartial) // required precision) fuse_core::Matrix5d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT /* *INDENT-ON* */ - AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, - mean, cov, variable_indices); + AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, mean, cov, + variable_indices); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix expected_sqrt_info; /* *INDENT-OFF* */ - expected_sqrt_info << 1.90431982, 0.0, 1.57962714, -0.37235015, -1.81200356, -0.51202133, // NOLINT - 0. , 0.0, 3.82674687, 2.80341172 , -2.68168479, -2.88943844, // NOLINT - 0. , 0.0, 0. , 1.83006791 , -0.69691741, -1.17412835, // NOLINT - 0. , 0.0, 0. , 0. , 0.95330283, -0.76965441, // NOLINT - 0. , 0.0, 0. , 0. , 0. , 0.68147774; // NOLINT + expected_sqrt_info << 1.90431982, 0.0, 1.57962714, -0.37235015, -1.81200356, -0.51202133, // NOLINT + 0., 0.0, 3.82674687, 2.80341172, -2.68168479, -2.88943844, // NOLINT + 0., 0.0, 0., 1.83006791, -0.69691741, -1.17412835, // NOLINT + 0., 0.0, 0., 0., 0.95330283, -0.76965441, // NOLINT + 0., 0.0, 0., 0., 0., 0.68147774; // NOLINT /* *INDENT-ON* */ fuse_core::Matrix6d expected_cov; /* *INDENT-OFF* */ - expected_cov << 2.0847236144069, 0.0, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT - 1.02943174290333, 0.0, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 0.0, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 0.0, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 0.0, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + expected_cov << 2.0847236144069, 0.0, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT + 1.02943174290333, 0.0, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 0.0, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 0.0, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 0.0, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT /* *INDENT-ON* */ // Compare @@ -204,14 +208,12 @@ TEST(AbsolutePose3DStampedEulerConstraint, 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( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position_variable = Position3DStamped::make_shared(rclcpp::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( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->w() = 0.952; orientation_variable->x() = 0.038; orientation_variable->y() = -0.189; @@ -223,51 +225,36 @@ TEST(AbsolutePose3DStampedEulerConstraint, Optimization) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 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; + cov << 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; /* *INDENT-ON* */ - auto constraint = AbsolutePose3DStampedEulerConstraint::make_shared( - "test", - *position_variable, - *orientation_variable, - mean, - cov); + auto constraint = + AbsolutePose3DStampedEulerConstraint::make_shared("test", *position_variable, *orientation_variable, 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(), + problem.AddParameterBlock(position_variable->data(), position_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - position_variable->localParameterization()); + position_variable->localParameterization()); #else - position_variable->manifold()); + position_variable->manifold()); #endif - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock( - constraint->costFunction(), - constraint->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -284,7 +271,7 @@ TEST(AbsolutePose3DStampedEulerConstraint, Optimization) EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + 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()); @@ -293,18 +280,15 @@ TEST(AbsolutePose3DStampedEulerConstraint, Optimization) 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()); + 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_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()); + 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; @@ -313,13 +297,8 @@ TEST(AbsolutePose3DStampedEulerConstraint, Optimization) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - 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; + 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; /* *INDENT-ON* */ // High tolerance here, but also high values of covariance @@ -330,72 +309,55 @@ TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) { // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. Create the variables. Version for partial measurements - auto position_variable = Position3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position_variable = Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position_variable->x() = 1.5; position_variable->y() = 1.0; position_variable->z() = 10.0; - auto orientation_variable = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation3DStamped::make_shared(rclcpp::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; // Create an absolute pose constraint - std::vector variable_indices {0, 2, 3, 4, 5}; + std::vector variable_indices{ 0, 2, 3, 4, 5 }; fuse_core::Vector6d mean; mean << 1.0, 0.0, 3.0, 0.0, 0.0, 0.0; fuse_core::Matrix5d cov; /* *INDENT-OFF* */ - cov << 1.0, 0.2, 0.3, 0.4, 0.5, - 0.2, 3.0, 0.2, 0.1, 0.2, - 0.3, 0.2, 4.0, 0.3, 0.4, - 0.4, 0.1, 0.3, 5.0, 0.5, - 0.5, 0.2, 0.4, 0.5, 6.0; + cov << 1.0, 0.2, 0.3, 0.4, 0.5, 0.2, 3.0, 0.2, 0.1, 0.2, 0.3, 0.2, 4.0, 0.3, 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, 0.5, 0.2, + 0.4, 0.5, 6.0; /* *INDENT-ON* */ - auto constraint = AbsolutePose3DStampedEulerConstraint::make_shared( - "test", - *position_variable, - *orientation_variable, - mean, - cov, - variable_indices); + auto constraint = AbsolutePose3DStampedEulerConstraint::make_shared("test", *position_variable, *orientation_variable, + mean, cov, variable_indices); // 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(), + problem.AddParameterBlock(position_variable->data(), position_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - position_variable->localParameterization()); + position_variable->localParameterization()); #else - position_variable->manifold()); + position_variable->manifold()); #endif - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock( - constraint->costFunction(), - constraint->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -404,7 +366,7 @@ TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) // Check EXPECT_NEAR(1.0, position_variable->x(), 1.0e-5); - EXPECT_NEAR(1.0, position_variable->y(), 1.0e-5); // This is not measured so it will not change + EXPECT_NEAR(1.0, position_variable->y(), 1.0e-5); // This is not measured so it will not change EXPECT_NEAR(3.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); @@ -412,7 +374,7 @@ TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + 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()); @@ -422,18 +384,15 @@ TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) 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()); + 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_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()); + 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; @@ -442,13 +401,8 @@ TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - expected_covariance << - 1.0, 0.0, 0.2, 0.3, 0.4, 0.5, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.2, 0.0, 3.0, 0.2, 0.1, 0.2, - 0.3, 0.0, 0.2, 4.0, 0.3, 0.4, - 0.4, 0.0, 0.1, 0.3, 5.0, 0.5, - 0.5, 0.0, 0.2, 0.4, 0.5, 6.0; + expected_covariance << 1.0, 0.0, 0.2, 0.3, 0.4, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 3.0, 0.2, 0.1, 0.2, 0.3, + 0.0, 0.2, 4.0, 0.3, 0.4, 0.4, 0.0, 0.1, 0.3, 5.0, 0.5, 0.5, 0.0, 0.2, 0.4, 0.5, 6.0; /* *INDENT-ON* */ // High tolerance here, but also high values of covariance @@ -459,8 +413,7 @@ TEST(AbsolutePose3DStampedEulerConstraint, Serialization) { // Construct a constraint Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector6d mean; mean << 1.0, 2.0, 3.0, 0.1, 0.1, 1.0; @@ -469,16 +422,21 @@ TEST(AbsolutePose3DStampedEulerConstraint, Serialization) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - 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 + 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 /* *INDENT-ON* */ - AbsolutePose3DStampedEulerConstraint expected("test", position_variable, orientation_variable, mean, - cov); + AbsolutePose3DStampedEulerConstraint expected("test", position_variable, orientation_variable, mean, cov); // Serialize the constraint into an archive std::stringstream stream; diff --git a/fuse_constraints/test/test_normal_prior_pose_3d.cpp b/fuse_constraints/test/test_normal_prior_pose_3d.cpp index 8a778c09..43f17f94 100644 --- a/fuse_constraints/test/test_normal_prior_pose_3d.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_3d.cpp @@ -51,8 +51,7 @@ class NormalPriorPose3DTestFixture : public ::testing::Test public: //!< The automatic differentiation cost function type for the pose 3d cost functor using AutoDiffNormalPriorPose3D = - ceres::AutoDiffCostFunction; + ceres::AutoDiffCostFunction; /** * @brief Constructor @@ -63,14 +62,13 @@ class NormalPriorPose3DTestFixture : public ::testing::Test } const fuse_core::Matrix6d covariance = - fuse_core::Vector6d(1e-3, 1e-3, 1e-3, - 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, - //!< y, z, roll, pitch and yaw components + fuse_core::Vector6d(1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, + //!< y, z, roll, pitch and yaw components Eigen::Matrix full_sqrt_information; //!< The full pose 3d sqrt information matrix for the x, y - //!< z, roll, pitch, and yaw components - Eigen::Vector full_mean{1.0, 2.0, 1.0, 1.0, 0.0, 0.0, 0.0}; //!< The full pose 3d mean - //!< components: x, y z, - //!< qw, qx, qy, qz + //!< z, roll, pitch, and yaw components + Eigen::Vector full_mean{ 1.0, 2.0, 1.0, 1.0, 0.0, 0.0, 0.0 }; //!< The full pose 3d mean + //!< components: x, y z, + //!< qw, qx, qy, qz }; TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqual) @@ -78,15 +76,14 @@ TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqual) // Create cost function auto q = Eigen::Quaterniond::UnitRandom(); full_mean << 1.0, 2.0, 1.0, q.w(), q.x(), q.y(), q.z(); // Create automatic differentiation cost function - const fuse_constraints::NormalPriorPose3D cost_function{full_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose3D cost_function{ full_sqrt_information, full_mean }; const auto num_residuals = full_sqrt_information.rows(); AutoDiffNormalPriorPose3D autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DCostFunctor(full_sqrt_information, full_mean), - num_residuals); - + new fuse_constraints::NormalPriorPose3DCostFunctor(full_sqrt_information, full_mean), num_residuals); + // Compare the expected, automatic differentiation, cost function and the actual one // N.B. in ExpectCostFunctionsAreEqual constructor, the first argument is the expected cost function - // and the second argument is the actual cost function + // and the second argument is the actual cost function ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); -} \ No newline at end of file +} diff --git a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp index 5b9f08f6..59150f13 100644 --- a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp @@ -51,8 +51,7 @@ class NormalPriorPose3DEulerTestFixture : public ::testing::Test public: //!< The automatic differentiation cost function type for the pose 3d cost functor using AutoDiffNormalPriorPose3DEuler = - ceres::AutoDiffCostFunction; + ceres::AutoDiffCostFunction; /** * @brief Constructor @@ -63,31 +62,29 @@ class NormalPriorPose3DEulerTestFixture : public ::testing::Test } const fuse_core::Matrix6d covariance = - fuse_core::Vector6d(1e-3, 1e-3, 1e-3, - 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, - //!< y, z, roll, pitch and yaw components + fuse_core::Vector6d(1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, + //!< y, z, roll, pitch and yaw components Eigen::Matrix full_sqrt_information; //!< The full pose 3d sqrt information matrix for the x, y - //!< z, roll, pitch, and yaw components - Eigen::Vector full_mean{1.0, 2.0, 1.0, 0.0, 0.0, 0.0}; //!< The full pose 3d mean - //!< components: x, y z, - //!< roll, pitch, and yaw + //!< z, roll, pitch, and yaw components + Eigen::Vector full_mean{ 1.0, 2.0, 1.0, 0.0, 0.0, 0.0 }; //!< The full pose 3d mean + //!< components: x, y z, + //!< roll, pitch, and yaw }; TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForFullResiduals) { // Create cost function auto rpy = Eigen::Vector3d::Random(); - full_mean << 1.0, 2.0, 1.0, rpy.x(), rpy.y(), rpy.z(); - const fuse_constraints::NormalPriorPose3DEuler cost_function{full_sqrt_information, full_mean}; + full_mean << 1.0, 2.0, 1.0, rpy.x(), rpy.y(), rpy.z(); + const fuse_constraints::NormalPriorPose3DEuler cost_function{ full_sqrt_information, full_mean }; const auto num_residuals = full_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DEulerCostFunctor(full_sqrt_information, full_mean), - num_residuals); - + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(full_sqrt_information, full_mean), num_residuals); + // Compare the expected, automatic differentiation, cost function and the actual one // N.B. in ExpectCostFunctionsAreEqual constructor, the first argument is the expected cost function - // and the second argument is the actual cost function + // and the second argument is the actual cost function ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); } @@ -95,25 +92,25 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu { // Create cost function for a subset of residuals // Version with y position = 0 - std::vector indices = {0, 2, 3, 4, 5}; + std::vector indices = { 0, 2, 3, 4, 5 }; auto rpy = Eigen::Vector3d::Random(); full_mean << 1.0, 0.0, 1.0, rpy.x(), rpy.y(), rpy.z(); fuse_core::Matrix partial_sqrt_information; - for (size_t i = 0; i < indices.size(); ++i) { + for (size_t i = 0; i < indices.size(); ++i) + { partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); } std::cout << "full_mean: " << full_mean << std::endl; std::cout << "partial_sqrt_information: " << partial_sqrt_information << std::endl; - const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose3DEuler cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), - num_residuals); + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); } @@ -122,25 +119,25 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu { // Create cost function for a subset of residuals // Version with roll, pitch = 0 - std::vector indices = {0, 1, 2, 5}; + std::vector indices = { 0, 1, 2, 5 }; Eigen::Vector3d rpy = Eigen::Vector3d::Random(); rpy(0) = 0.0; rpy(1) = 0.0; full_mean << 1.0, 0.0, 1.0, rpy.x(), rpy.y(), rpy.z(); fuse_core::Matrix partial_sqrt_information; - for (size_t i = 0; i < indices.size(); ++i) { + for (size_t i = 0; i < indices.size(); ++i) + { partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); } - const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose3DEuler cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), - num_residuals); + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); } @@ -149,23 +146,23 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu { // Create cost function for a subset of residuals // Version with z = 0, orientation = 0 - std::vector indices = {0, 1}; - Eigen::Vector3d rpy {0.0, 0.0, 0.0}; + std::vector indices = { 0, 1 }; + Eigen::Vector3d rpy{ 0.0, 0.0, 0.0 }; full_mean << 0.1, 0.5, 0.0, rpy.x(), rpy.y(), rpy.z(); fuse_core::Matrix partial_sqrt_information; - for (size_t i = 0; i < indices.size(); ++i) { + for (size_t i = 0; i < indices.size(); ++i) + { partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); } - const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose3DEuler cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), - num_residuals); + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); } @@ -174,24 +171,24 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu { // Create cost function for a subset of residuals // Version with position = 0, roll = 0 - std::vector indices = {4, 5}; + std::vector indices = { 4, 5 }; Eigen::Vector3d rpy = Eigen::Vector3d::Random(); rpy(0) = 0.0; full_mean << 0.0, 0.0, 0.0, rpy.x(), rpy.y(), rpy.z(); fuse_core::Matrix partial_sqrt_information; - for (size_t i = 0; i < indices.size(); ++i) { + for (size_t i = 0; i < indices.size(); ++i) + { partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); } - const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose3DEuler cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), - num_residuals); + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); } diff --git a/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp index 8f266107..283b7535 100644 --- a/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp @@ -49,12 +49,11 @@ #include #include -using fuse_variables::Orientation3DStamped; -using fuse_variables::Position3DStamped; using fuse_constraints::AbsolutePose3DStampedConstraint; using fuse_constraints::AbsolutePose3DStampedEulerConstraint; using fuse_constraints::RelativePose3DStampedEulerConstraint; - +using fuse_variables::Orientation3DStamped; +using fuse_variables::Position3DStamped; TEST(RelativePose3DStampedEulerConstraint, Constructor) { @@ -71,18 +70,22 @@ TEST(RelativePose3DStampedEulerConstraint, Constructor) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - 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 + 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 /* *INDENT-ON* */ - EXPECT_NO_THROW( - RelativePose3DStampedEulerConstraint constraint( - "test", position1, orientation1, position2, - orientation2, delta, cov)); + EXPECT_NO_THROW(RelativePose3DStampedEulerConstraint constraint("test", position1, orientation1, position2, + orientation2, delta, cov)); } TEST(RelativePose3DStampedEulerConstraint, ConstructorPartial) @@ -93,7 +96,7 @@ TEST(RelativePose3DStampedEulerConstraint, ConstructorPartial) Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); - std::vector indices {0, 1, 3, 4, 5}; + std::vector indices{ 0, 1, 3, 4, 5 }; fuse_core::Vector6d delta; delta << 1.0, 2.0, 0.0, 0.988, 0.094, 0.079; @@ -101,17 +104,15 @@ TEST(RelativePose3DStampedEulerConstraint, ConstructorPartial) // required precision) fuse_core::Matrix5d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.96120532313878, 1.35471905449013, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.96120532313878, 1.35471905449013, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT /* *INDENT-ON* */ - EXPECT_NO_THROW( - RelativePose3DStampedEulerConstraint constraint( - "test", position1, orientation1, position2, - orientation2, delta, cov, indices)); + EXPECT_NO_THROW(RelativePose3DStampedEulerConstraint constraint("test", position1, orientation1, position2, + orientation2, delta, cov, indices)); } TEST(RelativePose3DStampedEulerConstraint, Covariance) @@ -128,32 +129,32 @@ TEST(RelativePose3DStampedEulerConstraint, Covariance) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - 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 + 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 /* *INDENT-ON* */ - RelativePose3DStampedEulerConstraint constraint( - "test", - position1, - orientation1, - position2, - orientation2, - delta, - cov); + RelativePose3DStampedEulerConstraint constraint("test", 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; /* *INDENT-OFF* */ - 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 + 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 /* *INDENT-ON* */ fuse_core::Matrix6d expected_cov = cov; @@ -170,7 +171,7 @@ TEST(RelativePose3DStampedEulerConstraint, CovariancePartial) Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); - std::vector indices {0, 1, 3, 4, 5}; + std::vector indices{ 0, 1, 3, 4, 5 }; fuse_core::Vector6d delta; delta << 1.0, 2.0, 0.0, 0.988, 0.094, 0.079; @@ -178,45 +179,39 @@ TEST(RelativePose3DStampedEulerConstraint, CovariancePartial) // required precision) fuse_core::Matrix5d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.96120532313878, 1.35471905449013, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.96120532313878, 1.35471905449013, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT /* *INDENT-ON* */ - RelativePose3DStampedEulerConstraint constraint( - "test", - position1, - orientation1, - position2, - orientation2, - delta, - cov, - indices); + RelativePose3DStampedEulerConstraint constraint("test", position1, orientation1, position2, orientation2, delta, cov, + indices); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix expected_sqrt_info; /* *INDENT-OFF* */ - expected_sqrt_info << 1.7687, -0.1286, 0.0, -1.3877, -0.6508, 0.6747, // NOLINT - 0.0, 1.3117, 0.0, -0.3361, -0.0415, -0.4332, // NOLINT - 0.0, 0.0, 0.0, 1.8301, -0.6969, -1.1741, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.9533, -0.7697, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.6815; // NOLINT + expected_sqrt_info << 1.7687, -0.1286, 0.0, -1.3877, -0.6508, 0.6747, // NOLINT + 0.0, 1.3117, 0.0, -0.3361, -0.0415, -0.4332, // NOLINT + 0.0, 0.0, 0.0, 1.8301, -0.6969, -1.1741, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.9533, -0.7697, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.6815; // NOLINT /* *INDENT-ON* */ /* *INDENT-ON* */ fuse_core::Matrix6d expected_cov; - expected_cov << 2.0847236144069, 1.10752598122138, 0.0, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.0, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT - 1.96120532313878, 1.35471905449013, 0.0, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 0.0, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 0.0, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + expected_cov << 2.0847236144069, 1.10752598122138, 0.0, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.0, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT + 1.96120532313878, 1.35471905449013, 0.0, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 0.0, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 0.0, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT // Compare EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); - EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-4); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-4); } TEST(RelativePose3DStampedEulerConstraint, Optimization) @@ -224,27 +219,23 @@ TEST(RelativePose3DStampedEulerConstraint, 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(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position1 = Position3DStamped::make_shared(rclcpp::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( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation1 = Orientation3DStamped::make_shared(rclcpp::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(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + auto position2 = Position3DStamped::make_shared(rclcpp::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( - rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + auto orientation2 = Orientation3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); orientation2->w() = 0.944; orientation2->x() = -0.128; orientation2->y() = 0.145; @@ -254,78 +245,53 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) 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( - "test", - *position1, - *orientation1, - mean_origin, - cov_origin); + auto prior = AbsolutePose3DStampedConstraint::make_shared("test", *position1, *orientation1, mean_origin, cov_origin); // Create a relative pose constraint for 1m in the x direction fuse_core::Vector6d mean_delta; mean_delta << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; fuse_core::Matrix6d cov_delta = fuse_core::Matrix6d::Identity(); - auto relative = RelativePose3DStampedEulerConstraint::make_shared( - "test", - *position1, - *orientation1, - *position2, - *orientation2, - mean_delta, - cov_delta); + auto relative = RelativePose3DStampedEulerConstraint::make_shared("test", *position1, *orientation1, *position2, + *orientation2, mean_delta, cov_delta); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation1->data(), - orientation1->size(), + problem.AddParameterBlock(orientation1->data(), orientation1->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation1->localParameterization()); + orientation1->localParameterization()); #else - orientation1->manifold()); + orientation1->manifold()); #endif - problem.AddParameterBlock( - position1->data(), - position1->size(), + problem.AddParameterBlock(position1->data(), position1->size(), #if !CERES_SUPPORTS_MANIFOLDS - position1->localParameterization()); + position1->localParameterization()); #else - position1->manifold()); + position1->manifold()); #endif - problem.AddParameterBlock( - orientation2->data(), - orientation2->size(), + problem.AddParameterBlock(orientation2->data(), orientation2->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation2->localParameterization()); + orientation2->localParameterization()); #else - orientation2->manifold()); + orientation2->manifold()); #endif - problem.AddParameterBlock( - position2->data(), - position2->size(), + problem.AddParameterBlock(position2->data(), position2->size(), #if !CERES_SUPPORTS_MANIFOLDS - position2->localParameterization()); + position2->localParameterization()); #else - position2->manifold()); + position2->manifold()); #endif - std::vector prior_parameter_blocks; + 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; + 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); + problem.AddResidualBlock(relative->costFunction(), relative->lossFunction(), relative_parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -350,7 +316,7 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) // Compute the marginal covariance for pose1 { - std::vector> covariance_blocks; + 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()); @@ -363,14 +329,10 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) covariance.GetCovarianceBlock(position1->data(), position1->data(), cov_pos_pos.data()); fuse_core::MatrixXd cov_or_or(3, 3); - covariance.GetCovarianceBlockInTangentSpace( - orientation1->data(), - orientation1->data(), cov_or_or.data()); + covariance.GetCovarianceBlockInTangentSpace(orientation1->data(), orientation1->data(), cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position1->size(), 3); - covariance.GetCovarianceBlockInTangentSpace( - position1->data(), - orientation1->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position1->data(), orientation1->data(), cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -379,13 +341,8 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - 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, 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; + 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, 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; /* *INDENT-ON* */ EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); @@ -393,7 +350,7 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) // Compute the marginal covariance for pose2 { - std::vector> covariance_blocks; + 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()); @@ -406,14 +363,10 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) covariance.GetCovarianceBlock(position2->data(), position2->data(), cov_pos_pos.data()); fuse_core::MatrixXd cov_or_or(3, 3); - covariance.GetCovarianceBlockInTangentSpace( - orientation2->data(), - orientation2->data(), cov_or_or.data()); + covariance.GetCovarianceBlockInTangentSpace(orientation2->data(), orientation2->data(), cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position1->size(), 3); - covariance.GetCovarianceBlockInTangentSpace( - position2->data(), - orientation2->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position2->data(), orientation2->data(), cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -422,13 +375,8 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - expected_covariance << - 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 3.0, 0.0, 0.0, 0.0, 1.0, - 0.0, 0.0, 3.0, 0.0, -1.0, 0.0, - 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, - 0.0, 0.0, -1.0, 0.0, 2.0, 0.0, - 0.0, 1.0, 0.0, 0.0, 0.0, 2.0; + expected_covariance << 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 3.0, 0.0, -1.0, 0.0, + 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 2.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 2.0; /* *INDENT-ON* */ // High tolerance here, but also high values of covariance @@ -441,135 +389,95 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) // 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(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position1 = Position3DStamped::make_shared(rclcpp::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( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation1 = Orientation3DStamped::make_shared(rclcpp::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(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + auto position2 = Position3DStamped::make_shared(rclcpp::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( - rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + auto orientation2 = Orientation3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); orientation2->w() = 0.944; orientation2->x() = -0.128; orientation2->y() = 0.145; orientation2->z() = -0.269; - std::vector indices {0, 1, 3, 4, 5}; + std::vector indices{ 0, 1, 3, 4, 5 }; // 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( - "test", - *position1, - *orientation1, - mean_origin, - cov_origin); + auto prior = AbsolutePose3DStampedConstraint::make_shared("test", *position1, *orientation1, mean_origin, cov_origin); // Create a relative pose constraint for 1m in the x direction fuse_core::Vector6d mean_delta; mean_delta << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; fuse_core::Matrix5d cov_delta = fuse_core::Matrix5d::Identity(); - auto relative = RelativePose3DStampedEulerConstraint::make_shared( - "test", - *position1, - *orientation1, - *position2, - *orientation2, - mean_delta, - cov_delta, - indices); - + auto relative = RelativePose3DStampedEulerConstraint::make_shared("test", *position1, *orientation1, *position2, + *orientation2, mean_delta, cov_delta, indices); + // Create a relative pose constraint for 1m in the y direction - std::vector indices_y {1, 2, 3, 4, 5}; + std::vector indices_y{ 1, 2, 3, 4, 5 }; fuse_core::Vector6d mean_delta_y; mean_delta_y << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0; fuse_core::Matrix5d cov_delta_y = fuse_core::Matrix5d::Identity(); auto relative_y = RelativePose3DStampedEulerConstraint::make_shared( - "test", - *position1, - *orientation1, - *position2, - *orientation2, - mean_delta_y, - cov_delta_y, - indices_y); + "test", *position1, *orientation1, *position2, *orientation2, mean_delta_y, cov_delta_y, indices_y); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation1->data(), - orientation1->size(), + problem.AddParameterBlock(orientation1->data(), orientation1->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation1->localParameterization()); + orientation1->localParameterization()); #else - orientation1->manifold()); + orientation1->manifold()); #endif - problem.AddParameterBlock( - position1->data(), - position1->size(), + problem.AddParameterBlock(position1->data(), position1->size(), #if !CERES_SUPPORTS_MANIFOLDS - position1->localParameterization()); + position1->localParameterization()); #else - position1->manifold()); + position1->manifold()); #endif - problem.AddParameterBlock( - orientation2->data(), - orientation2->size(), + problem.AddParameterBlock(orientation2->data(), orientation2->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation2->localParameterization()); + orientation2->localParameterization()); #else - orientation2->manifold()); + orientation2->manifold()); #endif - problem.AddParameterBlock( - position2->data(), - position2->size(), + problem.AddParameterBlock(position2->data(), position2->size(), #if !CERES_SUPPORTS_MANIFOLDS - position2->localParameterization()); + position2->localParameterization()); #else - position2->manifold()); + position2->manifold()); #endif - std::vector prior_parameter_blocks; + 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; + 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); - std::vector relative_parameter_blocks_y; + problem.AddResidualBlock(relative->costFunction(), relative->lossFunction(), relative_parameter_blocks); + std::vector relative_parameter_blocks_y; relative_parameter_blocks_y.push_back(position1->data()); relative_parameter_blocks_y.push_back(orientation1->data()); relative_parameter_blocks_y.push_back(position2->data()); relative_parameter_blocks_y.push_back(orientation2->data()); - problem.AddResidualBlock( - relative_y->costFunction(), - relative_y->lossFunction(), - relative_parameter_blocks_y); + problem.AddResidualBlock(relative_y->costFunction(), relative_y->lossFunction(), relative_parameter_blocks_y); // Run the solver ceres::Solver::Options options; @@ -594,7 +502,7 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) // Compute the marginal covariance for pose1 { - std::vector> covariance_blocks; + 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()); @@ -608,14 +516,10 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) covariance.GetCovarianceBlock(position1->data(), position1->data(), cov_pos_pos.data()); fuse_core::MatrixXd cov_or_or(orientation1->localSize(), orientation1->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - orientation1->data(), - orientation1->data(), cov_or_or.data()); + covariance.GetCovarianceBlockInTangentSpace(orientation1->data(), orientation1->data(), cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position1->localSize(), orientation1->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - position1->data(), - orientation1->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position1->data(), orientation1->data(), cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -624,13 +528,8 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - 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, 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; + 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, 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; /* *INDENT-ON* */ EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); @@ -638,7 +537,7 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) // Compute the marginal covariance for pose2 { - std::vector> covariance_blocks; + 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()); @@ -652,14 +551,10 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) covariance.GetCovarianceBlock(position2->data(), position2->data(), cov_pos_pos.data()); fuse_core::MatrixXd cov_or_or(orientation1->localSize(), orientation1->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - orientation2->data(), - orientation2->data(), cov_or_or.data()); + covariance.GetCovarianceBlockInTangentSpace(orientation2->data(), orientation2->data(), cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position1->localSize(), orientation1->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - position2->data(), - orientation2->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position2->data(), orientation2->data(), cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -668,13 +563,8 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - expected_covariance << - 2.25, -0.5, 0.0, 0.0, 0.0, -0.5, - -0.5, 2.5, 0.0, 0.0, 0.0, 1.0, - 0.0, 0.0, 3.25, 0.5, -1.0, 0.0, - 0.0, 0.0, 0.5, 1.5, 0.0, 0.0, - 0.0, 0.0, -1.0, 0.0, 1.5, 0.0, - -0.5, 1.0, 0.0, 0.0, 0.0, 1.5; + expected_covariance << 2.25, -0.5, 0.0, 0.0, 0.0, -0.5, -0.5, 2.5, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 3.25, 0.5, -1.0, + 0.0, 0.0, 0.0, 0.5, 1.5, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 1.5, 0.0, -0.5, 1.0, 0.0, 0.0, 0.0, 1.5; /* *INDENT-ON* */ EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-2); @@ -696,16 +586,21 @@ TEST(RelativePose3DStampedEulerConstraint, Serialization) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - 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 + 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 /* *INDENT-ON* */ - RelativePose3DStampedEulerConstraint expected("test", position1, orientation1, position2, orientation2, - delta, cov); + RelativePose3DStampedEulerConstraint expected("test", position1, orientation1, position2, orientation2, delta, cov); // Serialize the constraint into an archive std::stringstream stream; diff --git a/fuse_core/test/test_util.cpp b/fuse_core/test/test_util.cpp index 1ce51eb7..eeab5dbc 100644 --- a/fuse_core/test/test_util.cpp +++ b/fuse_core/test/test_util.cpp @@ -40,43 +40,49 @@ #include #include -struct Quat2RPY { +struct Quat2RPY +{ template - bool operator()(const T * const q, T * rpy) const { + bool operator()(const T* const q, T* rpy) const + { rpy[0] = fuse_core::getRoll(q[0], q[1], q[2], q[3]); rpy[1] = fuse_core::getPitch(q[0], q[1], q[2], q[3]); rpy[2] = fuse_core::getYaw(q[0], q[1], q[2], q[3]); return true; } - static ceres::CostFunction* Create() { + static ceres::CostFunction* Create() + { return (new ceres::AutoDiffCostFunction(new Quat2RPY())); } }; -struct QuatProd { +struct QuatProd +{ template - bool operator()( - const T * const q1, - const T * const q2, - T * q_out) const { - ceres::QuaternionProduct(q1, q2, q_out); - return true; - } - - static ceres::CostFunction* Create() { + bool operator()(const T* const q1, const T* const q2, T* q_out) const + { + ceres::QuaternionProduct(q1, q2, q_out); + return true; + } + + static ceres::CostFunction* Create() + { return (new ceres::AutoDiffCostFunction(new QuatProd())); } }; -struct Quat2AngleAxis { +struct Quat2AngleAxis +{ template - bool operator()(const T * const q, T * aa) const { - ceres::QuaternionToAngleAxis(q, aa); - return true; - } + bool operator()(const T* const q, T* aa) const + { + ceres::QuaternionToAngleAxis(q, aa); + return true; + } - static ceres::CostFunction* Create() { + static ceres::CostFunction* Create() + { return (new ceres::AutoDiffCostFunction(new Quat2AngleAxis())); } }; @@ -128,7 +134,7 @@ TEST(Util, wrapAngle2D) TEST(Util, quaternion2rpy) { // Test correct conversion from quaternion to roll-pitch-yaw - double q[4] = {1.0, 0.0, 0.0, 0.0}; + double q[4] = { 1.0, 0.0, 0.0, 0.0 }; double rpy[3]; fuse_core::quaternion2rpy(q, rpy); EXPECT_EQ(0.0, rpy[0]); @@ -152,19 +158,19 @@ TEST(Util, quaternion2rpy) q[1] = q_eigen.x(); q[2] = q_eigen.y(); q[3] = q_eigen.z(); - + fuse_core::quaternion2rpy(q, rpy, J_analytic); - double * jacobians[1] = {J_autodiff}; - const double * parameters[1] = {q}; + double* jacobians[1] = { J_autodiff }; + const double* parameters[1] = { q }; - ceres::CostFunction * quat2rpy_cf = Quat2RPY::Create(); + ceres::CostFunction* quat2rpy_cf = Quat2RPY::Create(); double rpy_autodiff[3]; quat2rpy_cf->Evaluate(parameters, rpy_autodiff, jacobians); - + Eigen::Map> J_autodiff_map(jacobians[0]); Eigen::Map> J_analytic_map(J_analytic); - + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); } @@ -174,47 +180,35 @@ TEST(Util, quaternionProduct) const Eigen::Quaterniond q1_eigen = Eigen::Quaterniond::UnitRandom(); const Eigen::Quaterniond q2_eigen = Eigen::Quaterniond::UnitRandom(); double q_out[4]; - double q1[4] - { - q1_eigen.w(), - q1_eigen.x(), - q1_eigen.y(), - q1_eigen.z() - }; + double q1[4]{ q1_eigen.w(), q1_eigen.x(), q1_eigen.y(), q1_eigen.z() }; + + double q2[4]{ q2_eigen.w(), q2_eigen.x(), q2_eigen.y(), q2_eigen.z() }; - double q2[4] - { - q2_eigen.w(), - q2_eigen.x(), - q2_eigen.y(), - q2_eigen.z() - }; - // Atm only the jacobian wrt the second quaternion is implemented. If the computation will be - // extended in future, we just have to compare J_analytic_q1 with the other automatic J_autodiff_q1. - double J_analytic_q1[16], J_analytic_q2[16]; // Analytical Jacobians wrt first and second quaternion - double J_autodiff_q1[16], J_autodiff_q2[16]; // Autodiff Jacobians wrt first and second quaternion - - fuse_core::quaternionProduct(q1, q2, q_out, J_analytic_q2); + // extended in future, we just have to compare J_analytic_q1 with the other automatic J_autodiff_q1. + double J_analytic_q1[16], J_analytic_q2[16]; // Analytical Jacobians wrt first and second quaternion + double J_autodiff_q1[16], J_autodiff_q2[16]; // Autodiff Jacobians wrt first and second quaternion + + fuse_core::quaternionProduct(q1, q2, q_out, J_analytic_q2); - double * jacobians[2]; + double* jacobians[2]; jacobians[0] = J_autodiff_q1; jacobians[1] = J_autodiff_q2; - const double * parameters[2]; + const double* parameters[2]; parameters[0] = q1; parameters[1] = q2; - ceres::CostFunction * quat_prod_cf = QuatProd::Create(); + ceres::CostFunction* quat_prod_cf = QuatProd::Create(); double q_out_autodiff[4]; quat_prod_cf->Evaluate(parameters, q_out_autodiff, jacobians); - + Eigen::Map> J_autodiff_q1_map(jacobians[0]); Eigen::Map> J_autodiff_q2_map(jacobians[1]); // Eigen::Map> J_analytic_q1_map(J_analytic_q1); Eigen::Map> J_analytic_q2_map(J_analytic_q2); - + EXPECT_TRUE(J_analytic_q2_map.isApprox(J_autodiff_q2_map)); } @@ -223,29 +217,22 @@ TEST(Util, quaternionToAngleAxis) // Test correct quaternion to angle-axis function jacobian const Eigen::Quaterniond q_eigen = Eigen::Quaterniond::UnitRandom(); double angle_axis[3]; - double q[4] - { - q_eigen.w(), - q_eigen.x(), - q_eigen.y(), - q_eigen.z() - }; - - double J_analytic[12]; + double q[4]{ q_eigen.w(), q_eigen.x(), q_eigen.y(), q_eigen.z() }; + + double J_analytic[12]; double J_autodiff[12]; - - fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); - double * jacobians[1] = {J_autodiff}; - const double * parameters[1] = {q}; + fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); + + double* jacobians[1] = { J_autodiff }; + const double* parameters[1] = { q }; - ceres::CostFunction * quat2angle_axis_cf = Quat2AngleAxis::Create(); + ceres::CostFunction* quat2angle_axis_cf = Quat2AngleAxis::Create(); double angle_axis_autodiff[3]; quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); - + Eigen::Map> J_autodiff_map(jacobians[0]); Eigen::Map> J_analytic_map(J_analytic); - + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); } - diff --git a/fuse_models/include/fuse_models/imu_3d.hpp b/fuse_models/include/fuse_models/imu_3d.hpp index 8dc57000..2ff2fc0f 100644 --- a/fuse_models/include/fuse_models/imu_3d.hpp +++ b/fuse_models/include/fuse_models/imu_3d.hpp @@ -50,7 +50,6 @@ #include #include - namespace fuse_models { @@ -115,16 +114,14 @@ class Imu3D : public fuse_core::AsyncSensorModel /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for pose messages * @param[in] msg - The IMU message to process */ - void process(const sensor_msgs::msg::Imu & msg); + void process(const sensor_msgs::msg::Imu& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -158,23 +155,18 @@ class Imu3D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose and twist coavriance or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, - const bool validate, - fuse_core::Transaction & transaction); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; diff --git a/fuse_models/include/fuse_models/odometry_3d.hpp b/fuse_models/include/fuse_models/odometry_3d.hpp index 8a888f3a..0ba4df86 100644 --- a/fuse_models/include/fuse_models/odometry_3d.hpp +++ b/fuse_models/include/fuse_models/odometry_3d.hpp @@ -50,7 +50,6 @@ #include #include - namespace fuse_models { @@ -106,16 +105,14 @@ class Odometry3D : public fuse_core::AsyncSensorModel /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for pose messages * @param[in] msg - The pose message to process */ - void process(const nav_msgs::msg::Odometry & msg); + void process(const nav_msgs::msg::Odometry& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -149,23 +146,18 @@ class Odometry3D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose and twist coavriance or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, - const bool validate, - fuse_core::Transaction & transaction); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; diff --git a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp index f1c70f44..dc623ad9 100644 --- a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp +++ b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp @@ -56,7 +56,6 @@ #include #include - namespace fuse_models { @@ -123,9 +122,8 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher /** * @brief Shadowing extension to the AsyncPublisher::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; protected: /** @@ -142,9 +140,8 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher * @param[in] graph A read-only pointer to the graph object, allowing queries to be * performed whenever needed */ - void notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) override; + void notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) override; /** * @brief Perform any required operations before the first call to notify() occurs @@ -176,17 +173,11 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher * structure * @return true if the checks pass, false otherwise */ - bool getState( - const fuse_core::Graph & graph, - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id, - fuse_core::UUID & position_uuid, - fuse_core::UUID & orientation_uuid, - fuse_core::UUID & velocity_linear_uuid, - fuse_core::UUID & velocity_angular_uuid, - fuse_core::UUID & acceleration_linear_uuid, - nav_msgs::msg::Odometry & odometry, - geometry_msgs::msg::AccelWithCovarianceStamped & acceleration); + bool getState(const fuse_core::Graph& graph, const rclcpp::Time& stamp, const fuse_core::UUID& device_id, + fuse_core::UUID& position_uuid, fuse_core::UUID& orientation_uuid, + fuse_core::UUID& velocity_linear_uuid, fuse_core::UUID& velocity_angular_uuid, + fuse_core::UUID& acceleration_linear_uuid, nav_msgs::msg::Odometry& odometry, + geometry_msgs::msg::AccelWithCovarianceStamped& acceleration); /** * @brief Timer callback method for the filtered state publication and tf broadcasting @@ -198,31 +189,24 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher * @brief Object that searches for the most recent common timestamp for a set of variables */ using Synchronizer = fuse_publishers::StampedVariableSynchronizer< - fuse_variables::Orientation3DStamped, - fuse_variables::Position3DStamped, - fuse_variables::VelocityLinear3DStamped, - fuse_variables::VelocityAngular3DStamped, - fuse_variables::AccelerationLinear3DStamped>; - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Timers, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncPublisher interfaces_ - - fuse_core::UUID device_id_; //!< The UUID of this device + fuse_variables::Orientation3DStamped, fuse_variables::Position3DStamped, fuse_variables::VelocityLinear3DStamped, + fuse_variables::VelocityAngular3DStamped, fuse_variables::AccelerationLinear3DStamped>; + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncPublisher interfaces_ + + fuse_core::UUID device_id_; //!< The UUID of this device rclcpp::Clock::SharedPtr clock_; //!< The publisher's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The publisher's logger + rclcpp::Logger logger_; //!< The publisher's logger ParameterType params_; rclcpp::Time latest_stamp_; rclcpp::Time latest_covariance_stamp_; - bool latest_covariance_valid_{false}; //!< Whether the latest covariance computed is valid or not + bool latest_covariance_valid_{ false }; //!< Whether the latest covariance computed is valid or not nav_msgs::msg::Odometry odom_output_; geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output_; @@ -237,10 +221,10 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher std::shared_ptr tf_broadcaster_ = nullptr; std::unique_ptr tf_listener_; - fuse_core::DelayedThrottleFilter delayed_throttle_filter_{10.0}; //!< A ros::console filter to - //!< print delayed throttle - //!< messages, that can be reset - //!< on start + fuse_core::DelayedThrottleFilter delayed_throttle_filter_{ 10.0 }; //!< A ros::console filter to + //!< print delayed throttle + //!< messages, that can be reset + //!< on start rclcpp::TimerBase::SharedPtr publish_timer_; diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp index f8e4ccab..cdb01668 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -66,8 +66,8 @@ namespace fuse_models * - ~buffer_length (double) The length of the graph state buffer and state history, in seconds * - ~process_noise_diagonal (vector of doubles) An 15-dimensional vector containing the diagonal * values for the process noise covariance matrix. - * Variable order is (x, y, z, roll, pitch, yaw, - * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * Variable order is (x, y, z, roll, pitch, yaw, + * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc). */ class Omnidirectional3D : public fuse_core::AsyncMotionModel @@ -90,11 +90,10 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel /** * @brief Shadowing extension to the AsyncMotionModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; - void print(std::ostream & stream = std::cout) const; + void print(std::ostream& stream = std::cout) const; protected: /** @@ -102,19 +101,20 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel */ struct StateHistoryElement { - fuse_core::UUID position_uuid; //!< The uuid of the associated position variable - fuse_core::UUID orientation_uuid; //!< The uuid of the associated orientation variable - fuse_core::UUID vel_linear_uuid; //!< The uuid of the associated linear velocity variable - fuse_core::UUID vel_angular_uuid; //!< The uuid of the associated angular velocity variable - fuse_core::UUID acc_linear_uuid; //!< The uuid of the associated linear acceleration - //!< variable - fuse_core::Vector3d position = fuse_core::Vector3d::Zero(); //!< Map-frame position - Eigen::Quaterniond orientation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); //!< Map-frame orientation (quaternion) - fuse_core::Vector3d vel_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear velocities - fuse_core::Vector3d vel_angular = fuse_core::Vector3d::Zero(); //!< Body-frame angular velocities - fuse_core::Vector3d acc_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear (angular not needed) accelerations - - void print(std::ostream & stream = std::cout) const; + fuse_core::UUID position_uuid; //!< The uuid of the associated position variable + fuse_core::UUID orientation_uuid; //!< The uuid of the associated orientation variable + fuse_core::UUID vel_linear_uuid; //!< The uuid of the associated linear velocity variable + fuse_core::UUID vel_angular_uuid; //!< The uuid of the associated angular velocity variable + fuse_core::UUID acc_linear_uuid; //!< The uuid of the associated linear acceleration + //!< variable + fuse_core::Vector3d position = fuse_core::Vector3d::Zero(); //!< Map-frame position + Eigen::Quaterniond orientation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); //!< Map-frame orientation (quaternion) + fuse_core::Vector3d vel_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear velocities + fuse_core::Vector3d vel_angular = fuse_core::Vector3d::Zero(); //!< Body-frame angular velocities + fuse_core::Vector3d acc_linear = + fuse_core::Vector3d::Zero(); //!< Body-frame linear (angular not needed) accelerations + + void print(std::ostream& stream = std::cout) const; /** * @brief Validate the state components: pose, linear velocities, angular velocities and linear @@ -136,7 +136,7 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel * constraints * @return True if the motion models were generated successfully, false otherwise */ - bool applyCallback(fuse_core::Transaction & transaction) override; + bool applyCallback(fuse_core::Transaction& transaction) override; /** * @brief Generate a single motion model segment between the specified timestamps. @@ -156,11 +156,9 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel * ending_stamp. The variables should include initial values for the * optimizer. */ - void generateMotionModel( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables); + void generateMotionModel(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables); /** * @brief Callback fired in the local callback queue thread(s) whenever a new Graph is received @@ -187,10 +185,8 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel * @param[in] state_history The state history object to be updated * @param[in] buffer_length States older than this in the history will be pruned */ - static void updateStateHistoryEstimates( - const fuse_core::Graph & graph, - StateHistory & state_history, - const rclcpp::Duration & buffer_length); + static void updateStateHistoryEstimates(const fuse_core::Graph& graph, StateHistory& state_history, + const rclcpp::Duration& buffer_length); /** * @brief Validate the motion model state #1, state #2 and process noise covariance @@ -203,41 +199,37 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel * @param[in] process_noise_covariance The process noise covariance, after it is scaled and * multiplied by dt */ - static void validateMotionModel( - const StateHistoryElement & state1, const StateHistoryElement & state2, - const fuse_core::Matrix15d & process_noise_covariance); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + static void validateMotionModel(const StateHistoryElement& state1, const StateHistoryElement& state2, + const fuse_core::Matrix15d& process_noise_covariance); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger rclcpp::Duration buffer_length_; //!< The length of the state history fuse_core::UUID device_id_; //!< The UUID of the device to be published fuse_core::TimestampManager timestamp_manager_; //!< Tracks timestamps and previously created //!< motion model segments fuse_core::Matrix15d process_noise_covariance_; //!< Process noise covariance matrix - bool scale_process_noise_{false}; //!< Whether to scale the process noise + bool scale_process_noise_{ false }; //!< Whether to scale the process noise //!< covariance pose by the norm of the current //!< state twist - double velocity_linear_norm_min_{1e-3}; //!< The minimum linear velocity norm allowed when + double velocity_linear_norm_min_{ 1e-3 }; //!< The minimum linear velocity norm allowed when //!< scaling the process noise covariance - double velocity_angular_norm_min_{1e-3}; //!< The minimum twist norm allowed when + double velocity_angular_norm_min_{ 1e-3 }; //!< The minimum twist norm allowed when //!< scaling the process noise covariance - bool disable_checks_{false}; //!< Whether to disable the validation checks for the current and - //!< predicted state, including the process noise covariance after - //!< it is scaled and multiplied by dt - StateHistory state_history_; //!< History of optimized graph pose estimates + bool disable_checks_{ false }; //!< Whether to disable the validation checks for the current and + //!< predicted state, including the process noise covariance after + //!< it is scaled and multiplied by dt + StateHistory state_history_; //!< History of optimized graph pose estimates }; -std::ostream & operator<<(std::ostream & stream, const Omnidirectional3D & unicycle_2d); +std::ostream& operator<<(std::ostream& stream, const Omnidirectional3D& unicycle_2d); } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp index ecbf3902..a9757f1c 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp @@ -49,7 +49,6 @@ #include #include - namespace fuse_models { @@ -58,8 +57,8 @@ namespace fuse_models * motion model. * * This class publishes a transaction that contains a prior on each state subvariable used in the - * Omnidirectional 3D motion model (x, y, z, qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, - * yaw_vel, x_acc, y_acc, z_acc). When the sensor is first loaded, it publishes a single transaction + * Omnidirectional 3D motion model (x, y, z, qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * yaw_vel, x_acc, y_acc, z_acc). When the sensor is first loaded, it publishes a single transaction * with the configured initial state and covariance. * Additionally, whenever a pose is received, either on the set_pose service or the topic, this * ignition sensor resets the optimizer then publishes a new transaction with a prior at the @@ -73,12 +72,12 @@ namespace fuse_models * - ~initial_sigma (vector of doubles) A 15-dimensional vector containing the standard deviations * for the initial state values. The covariance matrix is * created placing the squared standard deviations along the - * diagonal of an 15x15 matrix. Variable order is (x, y, z, - * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * diagonal of an 15x15 matrix. Variable order is (x, y, z, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, * yaw_vel, x_acc, y_acc, z_acc). * - ~initial_state (vector of doubles) A 15-dimensional vector containing the initial values for - * the state. Variable order is (x, y, z, - * qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * the state. Variable order is (x, y, z, + * qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, * yaw_vel, x_acc, y_acc, z_acc). * - ~queue_size (int, default: 10) The subscriber queue size for the pose messages * - ~reset_service (string, default: "~/reset") The name of the reset service to call before @@ -111,10 +110,8 @@ class Omnidirectional3DIgnition : public fuse_core::AsyncSensorModel /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Subscribe to the input topic to start sending transactions to the optimizer @@ -141,23 +138,20 @@ class Omnidirectional3DIgnition : public fuse_core::AsyncSensorModel /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - void subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg); + void subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg); /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - bool setPoseServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr, - const fuse_msgs::srv::SetPose::Request::SharedPtr req); + bool setPoseServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr, const fuse_msgs::srv::SetPose::Request::SharedPtr req); /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - bool setPoseDeprecatedServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req); + bool setPoseDeprecatedServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req); protected: /** @@ -173,37 +167,31 @@ class Omnidirectional3DIgnition : public fuse_core::AsyncSensorModel * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, z, roll, pitch, yaw) */ - void process( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - std::function post_process = nullptr); + void process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, std::function post_process = nullptr); /** * @brief Create and send a prior transaction based on the supplied pose * - * The omnidirectional 3d state members not included in the pose message (x_vel, y_vel, z_vel, roll_vel, pitch_vel, - * yaw_vel, x_acc, y_acc, z_acc) will use the initial state values and standard deviations configured on the + * The omnidirectional 3d state members not included in the pose message (x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * yaw_vel, x_acc, y_acc, z_acc) will use the initial state values and standard deviations configured on the * parameter server. * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, z, roll, pitch, yaw) */ - void sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Graph, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Services, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ - - std::atomic_bool started_; //!< Flag indicating the sensor has been started - bool initial_transaction_sent_; //!< Flag indicating an initial transaction has been sent already - fuse_core::UUID device_id_; //!< The UUID of this device + void sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped& pose); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + std::atomic_bool started_; //!< Flag indicating the sensor has been started + bool initial_transaction_sent_; //!< Flag indicating an initial transaction has been sent already + fuse_core::UUID device_id_; //!< The UUID of this device rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; //!< Object containing all of the configuration parameters diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp index 1a9a3501..16755462 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp @@ -75,39 +75,14 @@ namespace fuse_models * @param[out] acc_linear2_y - Second Y acceleration * @param[out] acc_linear2_z - Second Z acceleration */ -template -inline void predict( - const T position1_x, - const T position1_y, - const T position1_z, - const T orientation1_r, - const T orientation1_p, - const T orientation1_y, - const T vel_linear1_x, - const T vel_linear1_y, - const T vel_linear1_z, - const T vel_angular1_r, - const T vel_angular1_p, - const T vel_angular1_y, - const T acc_linear1_x, - const T acc_linear1_y, - const T acc_linear1_z, - const T dt, - T & position2_x, - T & position2_y, - T & position2_z, - T & orientation2_r, - T & orientation2_p, - T & orientation2_y, - T & vel_linear2_x, - T & vel_linear2_y, - T & vel_linear2_z, - T & vel_angular2_r, - T & vel_angular2_p, - T & vel_angular2_y, - T & acc_linear2_x, - T & acc_linear2_y, - T & acc_linear2_z) +template +inline void predict(const T position1_x, const T position1_y, const T position1_z, const T orientation1_r, + const T orientation1_p, const T orientation1_y, const T vel_linear1_x, const T vel_linear1_y, + const T vel_linear1_z, const T vel_angular1_r, const T vel_angular1_p, const T vel_angular1_y, + const T acc_linear1_x, const T acc_linear1_y, const T acc_linear1_z, const T dt, T& position2_x, + T& position2_y, T& position2_z, T& orientation2_r, T& orientation2_p, T& orientation2_y, + T& vel_linear2_x, T& vel_linear2_y, T& vel_linear2_z, T& vel_angular2_r, T& vel_angular2_p, + T& vel_angular2_y, T& acc_linear2_x, T& acc_linear2_y, T& acc_linear2_z) { // 3D material point projection model which matches the one used by r_l. const T sr = ceres::sin(orientation1_r); @@ -120,23 +95,28 @@ inline void predict( const T dt2 = T(0.5) * dt * dt; // Project the state - position2_x = position1_x + - ((cy * cp) * vel_linear1_x + (cy * sp * sr - sy * cr) * vel_linear1_y + (cy * sp * cr + sy * sr) * vel_linear1_z) * dt + - ((cy * cp) * acc_linear1_x + (cy * sp * sr - sy * cr) * acc_linear1_y + (cy * sp * cr + sy * sr) * acc_linear1_z) * dt2; + position2_x = position1_x + + ((cy * cp) * vel_linear1_x + (cy * sp * sr - sy * cr) * vel_linear1_y + + (cy * sp * cr + sy * sr) * vel_linear1_z) * + dt + + ((cy * cp) * acc_linear1_x + (cy * sp * sr - sy * cr) * acc_linear1_y + + (cy * sp * cr + sy * sr) * acc_linear1_z) * + dt2; position2_y = position1_y + - ((sy * cp) * vel_linear1_x + (sy * sp * sr + cy * cr) * vel_linear1_y + (sy * sp * cr - cy * sr) * vel_linear1_z) * dt + - ((sy * cp) * acc_linear1_x + (sy * sp * sr + cy * cr) * acc_linear1_y + (sy * sp * cr - cy * sr) * acc_linear1_z) * dt2; - position2_z = position1_z + - ((-sp) * vel_linear1_x + (cp * sr) * vel_linear1_y + (cp * cr) * vel_linear1_z) * dt + - ((-sp) * acc_linear1_x + (cp * sr) * acc_linear1_y + (cp * cr) * acc_linear1_z) * dt2; - - orientation2_r = orientation1_r + - (vel_angular1_r + sr * sp * cpi * vel_angular1_p + cr * sp * cpi * vel_angular1_y) * dt; - orientation2_p = orientation1_p + - (cr * vel_angular1_p - sr * vel_angular1_y) * dt; - orientation2_y = orientation1_y + - (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; - + ((sy * cp) * vel_linear1_x + (sy * sp * sr + cy * cr) * vel_linear1_y + + (sy * sp * cr - cy * sr) * vel_linear1_z) * + dt + + ((sy * cp) * acc_linear1_x + (sy * sp * sr + cy * cr) * acc_linear1_y + + (sy * sp * cr - cy * sr) * acc_linear1_z) * + dt2; + position2_z = position1_z + ((-sp) * vel_linear1_x + (cp * sr) * vel_linear1_y + (cp * cr) * vel_linear1_z) * dt + + ((-sp) * acc_linear1_x + (cp * sr) * acc_linear1_y + (cp * cr) * acc_linear1_z) * dt2; + + orientation2_r = + orientation1_r + (vel_angular1_r + sr * sp * cpi * vel_angular1_p + cr * sp * cpi * vel_angular1_y) * dt; + orientation2_p = orientation1_p + (cr * vel_angular1_p - sr * vel_angular1_y) * dt; + orientation2_y = orientation1_y + (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; + vel_linear2_x = vel_linear1_x + acc_linear1_x * dt; vel_linear2_y = vel_linear1_y + acc_linear1_y * dt; vel_linear2_z = vel_linear1_z + acc_linear1_z * dt; @@ -189,40 +169,16 @@ inline void predict( * @param[out] acc_linear2_z - Second Z acceleration * @param[out] jacobians - Jacobians wrt the state */ -inline void predict( - const double position1_x, - const double position1_y, - const double position1_z, - const double orientation1_r, - const double orientation1_p, - const double orientation1_y, - const double vel_linear1_x, - const double vel_linear1_y, - const double vel_linear1_z, - const double vel_angular1_r, - const double vel_angular1_p, - const double vel_angular1_y, - const double acc_linear1_x, - const double acc_linear1_y, - const double acc_linear1_z, - const double dt, - double & position2_x, - double & position2_y, - double & position2_z, - double & orientation2_r, - double & orientation2_p, - double & orientation2_y, - double & vel_linear2_x, - double & vel_linear2_y, - double & vel_linear2_z, - double & vel_angular2_r, - double & vel_angular2_p, - double & vel_angular2_y, - double & acc_linear2_x, - double & acc_linear2_y, - double & acc_linear2_z, - double ** jacobians, - double * jacobian_quat2rpy) +inline void predict(const double position1_x, const double position1_y, const double position1_z, + const double orientation1_r, const double orientation1_p, const double orientation1_y, + const double vel_linear1_x, const double vel_linear1_y, const double vel_linear1_z, + const double vel_angular1_r, const double vel_angular1_p, const double vel_angular1_y, + const double acc_linear1_x, const double acc_linear1_y, const double acc_linear1_z, const double dt, + double& position2_x, double& position2_y, double& position2_z, double& orientation2_r, + double& orientation2_p, double& orientation2_y, double& vel_linear2_x, double& vel_linear2_y, + double& vel_linear2_z, double& vel_angular2_r, double& vel_angular2_p, double& vel_angular2_y, + double& acc_linear2_x, double& acc_linear2_y, double& acc_linear2_z, double** jacobians, + double* jacobian_quat2rpy) { // 3D material point projection model which matches the one used by r_l. const double sr = ceres::sin(orientation1_r); @@ -235,23 +191,28 @@ inline void predict( const double dt2 = 0.5 * dt * dt; // Project the state - position2_x = position1_x + - ((cp * cy) * vel_linear1_x + (sr * sp * cy - cr * sy) * vel_linear1_y + (cr * sp * cy + sr * sy) * vel_linear1_z) * dt + - ((cp * cy) * acc_linear1_x + (sr * sp * cy - cr * sy) * acc_linear1_y + (cr * sp * cy + sr * sy) * acc_linear1_z) * dt2; + position2_x = position1_x + + ((cp * cy) * vel_linear1_x + (sr * sp * cy - cr * sy) * vel_linear1_y + + (cr * sp * cy + sr * sy) * vel_linear1_z) * + dt + + ((cp * cy) * acc_linear1_x + (sr * sp * cy - cr * sy) * acc_linear1_y + + (cr * sp * cy + sr * sy) * acc_linear1_z) * + dt2; position2_y = position1_y + - ((cp * sy) * vel_linear1_x + (sr * sp * sy + cr * cy) * vel_linear1_y + (cr * sp * sy - sr * cy) * vel_linear1_z) * dt + - ((cp * sy) * acc_linear1_x + (sr * sp * sy + cr * cy) * acc_linear1_y + (cr * sp * sy - sr * cy) * acc_linear1_z) * dt2; - position2_z = position1_z + - ((-sp) * vel_linear1_x + (sr * cp) * vel_linear1_y + (cr * cp) * vel_linear1_z) * dt + - ((-sp) * acc_linear1_x + (sr * cp) * acc_linear1_y + (cr * cp) * acc_linear1_z) * dt2; - - orientation2_r = orientation1_r + - (vel_angular1_r + sr * sp * cpi * vel_angular1_p + cr * sp * cpi * vel_angular1_y) * dt; - orientation2_p = orientation1_p + - (cr * vel_angular1_p - sr * vel_angular1_y) * dt; - orientation2_y = orientation1_y + - (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; - + ((cp * sy) * vel_linear1_x + (sr * sp * sy + cr * cy) * vel_linear1_y + + (cr * sp * sy - sr * cy) * vel_linear1_z) * + dt + + ((cp * sy) * acc_linear1_x + (sr * sp * sy + cr * cy) * acc_linear1_y + + (cr * sp * sy - sr * cy) * acc_linear1_z) * + dt2; + position2_z = position1_z + ((-sp) * vel_linear1_x + (sr * cp) * vel_linear1_y + (cr * cp) * vel_linear1_z) * dt + + ((-sp) * acc_linear1_x + (sr * cp) * acc_linear1_y + (cr * cp) * acc_linear1_z) * dt2; + + orientation2_r = + orientation1_r + (vel_angular1_r + sr * sp * cpi * vel_angular1_p + cr * sp * cpi * vel_angular1_y) * dt; + orientation2_p = orientation1_p + (cr * vel_angular1_p - sr * vel_angular1_y) * dt; + orientation2_y = orientation1_y + (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; + vel_linear2_x = vel_linear1_x + acc_linear1_x * dt; vel_linear2_y = vel_linear1_y + acc_linear1_y * dt; vel_linear2_z = vel_linear1_z + acc_linear1_z * dt; @@ -268,9 +229,11 @@ inline void predict( fuse_core::wrapAngle2D(orientation2_p); fuse_core::wrapAngle2D(orientation2_y); - if (jacobians) { + if (jacobians) + { // Jacobian wrt position1 - if (jacobians[0]) { + if (jacobians[0]) + { Eigen::Map> jacobian(jacobians[0]); jacobian.setZero(); // partial derivatives of position2 wrt orientation1 @@ -278,49 +241,51 @@ inline void predict( jacobian(1, 1) = 1.0; jacobian(2, 2) = 1.0; } - // Jacobian wrt orientation1 - if (jacobians[1]) { + // Jacobian wrt orientation1 + if (jacobians[1]) + { Eigen::Map> jacobian(jacobians[1]); Eigen::Map> j_quat2rpy_map(jacobian_quat2rpy); fuse_core::Matrix j_tmp; jacobian.setZero(); j_tmp.setZero(); - + // partial derivatives of position2_x wrt orientation1 - j_tmp(0, 0) = - ((cr * sp * cy + sr * sy) * vel_linear1_y + (- sr * sp * cy + cr * sy) * vel_linear1_z) * dt + - ((cr * sp * cy + sr * sy) * acc_linear1_y + (- sr * sp * cy + cr * sy) * acc_linear1_z) * dt2; - j_tmp(0, 1) = - ((-sp * cy) * vel_linear1_x + (sr * cp * cy) * vel_linear1_y + (cr * cp * cy) * vel_linear1_z) * dt + - ((-sp * cy) * acc_linear1_x + (sr * cp * cy) * acc_linear1_y + (cr * cp * cy) * acc_linear1_z) * dt2; - j_tmp(0, 2) = - ((-cp * sy) * vel_linear1_x + (- sr * sp * sy - cr * cy) * vel_linear1_y + (- cr * sp * sy + sr * cy) * vel_linear1_z) * dt + - ((-cp * sy) * acc_linear1_x + (- sr * sp * sy - cr * cy) * acc_linear1_y + (- cr * sp * sy + sr * cy) * acc_linear1_z) * dt2; - - // partial derivatives of position2_y wrt orientation1 - j_tmp(1, 0) = - ((- sr * cy + cr * sp * sy) * vel_linear1_y + (- cr * cy - sr * sp * sy) * vel_linear1_z) * dt + - ((- sr * cy + cr * sp * sy) * acc_linear1_y + (- cr * cy - sr * sp * sy) * acc_linear1_z) * dt2; - j_tmp(1, 1) = - ((-sp * sy) * vel_linear1_x + (sr * cp * sy) * vel_linear1_y + (cr * cp * sy) * vel_linear1_z) * dt + - ((-sp * sy) * acc_linear1_x + (sr * cp * sy) * acc_linear1_y + (cr * cp * sy) * acc_linear1_z) * dt2; - j_tmp(1, 2) = - ((cp * cy) * vel_linear1_x + (- cr * sy + sr * sp * cy) * vel_linear1_y + (sr * sy + cr * sp * cy) * vel_linear1_z) * dt + - ((cp * cy) * acc_linear1_x + (- cr * sy + sr * sp * cy) * acc_linear1_y + (sr * sy + cr * sp * cy) * acc_linear1_z) * dt2; - + j_tmp(0, 0) = ((cr * sp * cy + sr * sy) * vel_linear1_y + (-sr * sp * cy + cr * sy) * vel_linear1_z) * dt + + ((cr * sp * cy + sr * sy) * acc_linear1_y + (-sr * sp * cy + cr * sy) * acc_linear1_z) * dt2; + j_tmp(0, 1) = + ((-sp * cy) * vel_linear1_x + (sr * cp * cy) * vel_linear1_y + (cr * cp * cy) * vel_linear1_z) * dt + + ((-sp * cy) * acc_linear1_x + (sr * cp * cy) * acc_linear1_y + (cr * cp * cy) * acc_linear1_z) * dt2; + j_tmp(0, 2) = ((-cp * sy) * vel_linear1_x + (-sr * sp * sy - cr * cy) * vel_linear1_y + + (-cr * sp * sy + sr * cy) * vel_linear1_z) * + dt + + ((-cp * sy) * acc_linear1_x + (-sr * sp * sy - cr * cy) * acc_linear1_y + + (-cr * sp * sy + sr * cy) * acc_linear1_z) * + dt2; + + // partial derivatives of position2_y wrt orientation1 + j_tmp(1, 0) = ((-sr * cy + cr * sp * sy) * vel_linear1_y + (-cr * cy - sr * sp * sy) * vel_linear1_z) * dt + + ((-sr * cy + cr * sp * sy) * acc_linear1_y + (-cr * cy - sr * sp * sy) * acc_linear1_z) * dt2; + j_tmp(1, 1) = + ((-sp * sy) * vel_linear1_x + (sr * cp * sy) * vel_linear1_y + (cr * cp * sy) * vel_linear1_z) * dt + + ((-sp * sy) * acc_linear1_x + (sr * cp * sy) * acc_linear1_y + (cr * cp * sy) * acc_linear1_z) * dt2; + j_tmp(1, 2) = ((cp * cy) * vel_linear1_x + (-cr * sy + sr * sp * cy) * vel_linear1_y + + (sr * sy + cr * sp * cy) * vel_linear1_z) * + dt + + ((cp * cy) * acc_linear1_x + (-cr * sy + sr * sp * cy) * acc_linear1_y + + (sr * sy + cr * sp * cy) * acc_linear1_z) * + dt2; + // partial derivatives of position2_z wrt orientation1 - j_tmp(2, 0) = - ((cr * cp) * vel_linear1_y - (sr * cp) * vel_linear1_z) * dt + - ((cr * cp) * acc_linear1_y - (sr * cp) * acc_linear1_z) * dt2; - j_tmp(2, 1) = - (-cp * vel_linear1_x - (sr * sp) * vel_linear1_y - (cr * sp) * vel_linear1_z) * dt + - (-cp * acc_linear1_x - (sr * sp) * acc_linear1_y - (cr * sp) * acc_linear1_z) * dt2; + j_tmp(2, 0) = ((cr * cp) * vel_linear1_y - (sr * cp) * vel_linear1_z) * dt + + ((cr * cp) * acc_linear1_y - (sr * cp) * acc_linear1_z) * dt2; + j_tmp(2, 1) = (-cp * vel_linear1_x - (sr * sp) * vel_linear1_y - (cr * sp) * vel_linear1_z) * dt + + (-cp * acc_linear1_x - (sr * sp) * acc_linear1_y - (cr * sp) * acc_linear1_z) * dt2; // partial derivatives of orientation2_r wrt orientation1 - j_tmp(3, 0) = 1.0 + - ((cr * sp * cpi) * vel_angular1_p - (sr * sp * cpi) * vel_angular1_y) * dt; - j_tmp(3, 1) = - ((sr + sr * sp * sp * cpi * cpi) * vel_angular1_p + (cr + cr *sp * sp * cpi * cpi) * vel_angular1_y) * dt; + j_tmp(3, 0) = 1.0 + ((cr * sp * cpi) * vel_angular1_p - (sr * sp * cpi) * vel_angular1_y) * dt; + j_tmp(3, 1) = + ((sr + sr * sp * sp * cpi * cpi) * vel_angular1_p + (cr + cr * sp * sp * cpi * cpi) * vel_angular1_y) * dt; // partial derivatives of orientation2_p wrt orientation1 j_tmp(4, 0) = (-sr * vel_angular1_p - cr * vel_angular1_y) * dt; @@ -332,16 +297,17 @@ inline void predict( j_tmp(5, 2) = 1.0; jacobian.block<6, 4>(0, 0) = j_tmp * j_quat2rpy_map; - } + } // Jacobian wrt vel_linear1 - if (jacobians[2]) { + if (jacobians[2]) + { Eigen::Map> jacobian(jacobians[2]); jacobian.setZero(); // partial derivatives of position1_x wrt vel_linear1 jacobian(0, 0) = cp * cy * dt; jacobian(0, 1) = (sr * sp * cy - cr * sy) * dt; - jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt; + jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt; // partial derivatives of position1_y wrt vel_linear1 jacobian(1, 0) = cp * sy * dt; jacobian(1, 1) = (sr * sp * sy + cr * cy) * dt; @@ -359,7 +325,8 @@ inline void predict( } // Jacobian wrt vel_angular1 - if (jacobians[3]) { + if (jacobians[3]) + { Eigen::Map> jacobian(jacobians[3]); jacobian.setZero(); @@ -382,13 +349,14 @@ inline void predict( } // Jacobian wrt acc_linear1 - if (jacobians[4]) { + if (jacobians[4]) + { Eigen::Map> jacobian(jacobians[4]); jacobian.setZero(); // partial derivatives of position1_x wrt acc_linear1 jacobian(0, 0) = cp * cy * dt2; jacobian(0, 1) = (sr * sp * cy - cr * sy) * dt2; - jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt2; + jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt2; // partial derivatives of position1_y wrt acc_linear1 jacobian(1, 0) = cp * sy * dt2; jacobian(1, 1) = (sr * sp * sy + cr * cy) * dt2; @@ -427,52 +395,16 @@ inline void predict( * @param[out] vel_angular2 - Second yaw velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) * @param[out] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1, z at index 2) */ -template -inline void predict( - const T * const position1, - const T * const orientation1, - const T * const vel_linear1, - const T * const vel_angular1, - const T * const acc_linear1, - const T dt, - T * const position2, - T * const orientation2, - T * const vel_linear2, - T * const vel_angular2, - T * const acc_linear2) +template +inline void predict(const T* const position1, const T* const orientation1, const T* const vel_linear1, + const T* const vel_angular1, const T* const acc_linear1, const T dt, T* const position2, + T* const orientation2, T* const vel_linear2, T* const vel_angular2, T* const acc_linear2) { - predict( - position1[0], - position1[1], - position1[2], - orientation1[0], - orientation1[1], - orientation1[2], - vel_linear1[0], - vel_linear1[1], - vel_linear1[2], - vel_angular1[0], - vel_angular1[1], - vel_angular1[2], - acc_linear1[0], - acc_linear1[1], - acc_linear1[2], - dt, - position2[0], - position2[1], - position2[2], - orientation2[0], - orientation2[1], - orientation2[2], - vel_linear2[0], - vel_linear2[1], - vel_linear2[2], - vel_angular2[0], - vel_angular2[1], - vel_angular2[2], - acc_linear2[0], - acc_linear2[1], - acc_linear2[2]); + predict(position1[0], position1[1], position1[2], orientation1[0], orientation1[1], orientation1[2], vel_linear1[0], + vel_linear1[1], vel_linear1[2], vel_angular1[0], vel_angular1[1], vel_angular1[2], acc_linear1[0], + acc_linear1[1], acc_linear1[2], dt, position2[0], position2[1], position2[2], orientation2[0], + orientation2[1], orientation2[2], vel_linear2[0], vel_linear2[1], vel_linear2[2], vel_angular2[0], + vel_angular2[1], vel_angular2[2], acc_linear2[0], acc_linear2[1], acc_linear2[2]); } /** * @brief Given a state and time delta, predicts a new state @@ -488,62 +420,26 @@ inline void predict( * @param[out] vel_angular2 - Second angular velocity * @param[out] acc_linear2 - Second linear acceleration */ -inline void predict( - const fuse_core::Vector3d & position1, - const Eigen::Quaterniond & orientation1, - const fuse_core::Vector3d & vel_linear1, - const fuse_core::Vector3d & vel_angular1, - const fuse_core::Vector3d & acc_linear1, - const double dt, - fuse_core::Vector3d & position2, - Eigen::Quaterniond & orientation2, - fuse_core::Vector3d & vel_linear2, - fuse_core::Vector3d & vel_angular2, - fuse_core::Vector3d & acc_linear2) +inline void predict(const fuse_core::Vector3d& position1, const Eigen::Quaterniond& orientation1, + const fuse_core::Vector3d& vel_linear1, const fuse_core::Vector3d& vel_angular1, + const fuse_core::Vector3d& acc_linear1, const double dt, fuse_core::Vector3d& position2, + Eigen::Quaterniond& orientation2, fuse_core::Vector3d& vel_linear2, + fuse_core::Vector3d& vel_angular2, fuse_core::Vector3d& acc_linear2) { - fuse_core::Vector3d rpy( - fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), - fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), - fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()) - ); - - predict( - position1.x(), - position1.y(), - position1.z(), - rpy.x(), - rpy.y(), - rpy.z(), - vel_linear1.x(), - vel_linear1.y(), - vel_linear1.z(), - vel_angular1.x(), - vel_angular1.y(), - vel_angular1.z(), - acc_linear1.x(), - acc_linear1.y(), - acc_linear1.z(), - dt, - position2.x(), - position2.y(), - position2.z(), - rpy.x(), - rpy.y(), - rpy.z(), - vel_linear2.x(), - vel_linear2.y(), - vel_linear2.z(), - vel_angular2.x(), - vel_angular2.y(), - vel_angular2.z(), - acc_linear2.x(), - acc_linear2.y(), - acc_linear2.z()); + fuse_core::Vector3d rpy(fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), + fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), + fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z())); + + predict(position1.x(), position1.y(), position1.z(), rpy.x(), rpy.y(), rpy.z(), vel_linear1.x(), vel_linear1.y(), + vel_linear1.z(), vel_angular1.x(), vel_angular1.y(), vel_angular1.z(), acc_linear1.x(), acc_linear1.y(), + acc_linear1.z(), dt, position2.x(), position2.y(), position2.z(), rpy.x(), rpy.y(), rpy.z(), vel_linear2.x(), + vel_linear2.y(), vel_linear2.z(), vel_angular2.x(), vel_angular2.y(), vel_angular2.z(), acc_linear2.x(), + acc_linear2.y(), acc_linear2.z()); // Convert back to quaternion orientation2 = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); } /** @@ -561,22 +457,13 @@ inline void predict( * @param[out] acc_linear2 - Second linear acceleration * @param[out] jacobian - Jacobian wrt the state */ -inline void predict( - const fuse_core::Vector3d & position1, - const Eigen::Quaterniond & orientation1, - const fuse_core::Vector3d & vel_linear1, - const fuse_core::Vector3d & vel_angular1, - const fuse_core::Vector3d & acc_linear1, - const double dt, - fuse_core::Vector3d & position2, - Eigen::Quaterniond & orientation2, - fuse_core::Vector3d & vel_linear2, - fuse_core::Vector3d & vel_angular2, - fuse_core::Vector3d & acc_linear2, - fuse_core::Matrix15d & jacobian) +inline void predict(const fuse_core::Vector3d& position1, const Eigen::Quaterniond& orientation1, + const fuse_core::Vector3d& vel_linear1, const fuse_core::Vector3d& vel_angular1, + const fuse_core::Vector3d& acc_linear1, const double dt, fuse_core::Vector3d& position2, + Eigen::Quaterniond& orientation2, fuse_core::Vector3d& vel_linear2, + fuse_core::Vector3d& vel_angular2, fuse_core::Vector3d& acc_linear2, fuse_core::Matrix15d& jacobian) { - - double quat[4] = {orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()}; + double quat[4] = { orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z() }; double rpy[3]; double jacobian_quat2rpy[12]; fuse_core::quaternion2rpy(quat, rpy, jacobian_quat2rpy); @@ -584,62 +471,34 @@ inline void predict( // fuse_core::Matrix15d is Eigen::RowMajor, so we cannot use pointers to the columns where each // parameter block starts. Instead, we need to create a vector of Eigen::RowMajor matrices per // parameter block and later reconstruct the fuse_core::Matrix15d with the full jacobian. The - // parameter blocks have the following sizes: {position1: 3, orientation1: 4, vel_linear1: 3, + // parameter blocks have the following sizes: {position1: 3, orientation1: 4, vel_linear1: 3, // vel_angular1: 3, acc_linear1: 3} - static constexpr size_t num_residuals{15}; - static constexpr size_t num_parameter_blocks{5}; - static const std::array block_sizes = {3, 4, 3, 3, 3}; + static constexpr size_t num_residuals{ 15 }; + static constexpr size_t num_parameter_blocks{ 5 }; + static const std::array block_sizes = { 3, 4, 3, 3, 3 }; std::array J; - std::array jacobians; + std::array jacobians; - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } - predict( - position1.x(), - position1.y(), - position1.z(), - rpy[0], - rpy[1], - rpy[2], - vel_linear1.x(), - vel_linear1.y(), - vel_linear1.z(), - vel_angular1.x(), - vel_angular1.y(), - vel_angular1.z(), - acc_linear1.x(), - acc_linear1.y(), - acc_linear1.z(), - dt, - position2.x(), - position2.y(), - position2.z(), - rpy[0], - rpy[1], - rpy[2], - vel_linear2.x(), - vel_linear2.y(), - vel_linear2.z(), - vel_angular2.x(), - vel_angular2.y(), - vel_angular2.z(), - acc_linear2.x(), - acc_linear2.y(), - acc_linear2.z(), - jacobians.data(), - jacobian_quat2rpy); + predict(position1.x(), position1.y(), position1.z(), rpy[0], rpy[1], rpy[2], vel_linear1.x(), vel_linear1.y(), + vel_linear1.z(), vel_angular1.x(), vel_angular1.y(), vel_angular1.z(), acc_linear1.x(), acc_linear1.y(), + acc_linear1.z(), dt, position2.x(), position2.y(), position2.z(), rpy[0], rpy[1], rpy[2], vel_linear2.x(), + vel_linear2.y(), vel_linear2.z(), vel_angular2.x(), vel_angular2.y(), vel_angular2.z(), acc_linear2.x(), + acc_linear2.y(), acc_linear2.z(), jacobians.data(), jacobian_quat2rpy); jacobian << J[0], J[1], J[2], J[3], J[4]; // Convert back to quaternion orientation2 = Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX()); } } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp index 730d48de..0fcaf906 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp @@ -42,7 +42,6 @@ #include #include - namespace fuse_models { @@ -74,7 +73,7 @@ namespace fuse_models * || [ x_acc_t2 - proj(x_acc_t1) ] || * || [ y_acc_t2 - proj(y_acc_t1) ] || * || [ z_acc_t2 - proj(z_acc_t1) ] || - * + * * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and * proj is a function that projects the state variables from time t1 to time t2. In case the user is * interested in implementing a cost function of the form @@ -94,22 +93,22 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 * * @param[in] dt The time delta across which to generate the kinematic model cost * @param[in] A The residual weighting matrix, most likely the square root information matrix in - * order (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * order (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ - Omnidirectional3DStateCostFunction(const double dt, const fuse_core::Matrix15d & A); + Omnidirectional3DStateCostFunction(const double dt, const fuse_core::Matrix15d& A); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. * * @param[in] parameters - Parameter blocks: - * 0 : position1 - First position (array with x at index 0, y at index 1, + * 0 : position1 - First position (array with x at index 0, y at index 1, * z at index 2) * 1 : orientation1 - First orientation (array with w at index 0, x at index 1, * y at index 2, z at index 3) * 2 : vel_linear1 - First linear velocity (array with x at index 0, y at * index 1, z at index 2) - * 3 : vel_angular1 - First angular velocity (array with vroll at index 0, + * 3 : vel_angular1 - First angular velocity (array with vroll at index 0, * vpitch at index 1, vyaw at index 2) * 4 : acc_linear1 - First linear acceleration (array with x at index 0, y * at index 1, z at index 2) @@ -121,7 +120,7 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 * index 1, z at index 2) * 8 : vel_angular2 - Second angular velocity (array with vroll at index 0, * vpitch at index 1, vyaw at index 2) - * 9 : acc_linear2 - Second linear acceleration (array with x at index 0, y at + * 9 : acc_linear2 - Second linear acceleration (array with x at index 0, y at * index 1) * @param[out] residual - The computed residual (error) * @param[out] jacobians - Jacobians of the residuals wrt the parameters. Only computed if not @@ -130,10 +129,7 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 * @return The return value indicates whether the computation of the residuals and/or jacobians * was successful or not. */ - bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const override + bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override { double position_pred[3]; double orientation_pred_rpy[3]; @@ -147,40 +143,25 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 fuse_core::quaternion2rpy(parameters[1], orientation1_rpy, j1_quat2rpy); fuse_core::quaternion2rpy(parameters[6], orientation2_rpy, j2_quat2rpy); - predict( - parameters[0][0], // position1_x - parameters[0][1], // position1_y - parameters[0][2], // position1_z - orientation1_rpy[0], // orientation1_roll - orientation1_rpy[1], // orientation1_pitch - orientation1_rpy[2], // orientation1_yaw - parameters[2][0], // vel_linear1_x - parameters[2][1], // vel_linear1_y - parameters[2][2], // vel_linear1_z - parameters[3][0], // vel_angular1_roll - parameters[3][1], // vel_angular1_pitch - parameters[3][2], // vel_angular1_yaw - parameters[4][0], // acc_linear1_x - parameters[4][1], // acc_linear1_y - parameters[4][2], // acc_linear1_z - dt_, - position_pred[0], - position_pred[1], - position_pred[2], - orientation_pred_rpy[0], - orientation_pred_rpy[1], - orientation_pred_rpy[2], - vel_linear_pred[0], - vel_linear_pred[1], - vel_linear_pred[2], - vel_angular_pred[0], - vel_angular_pred[1], - vel_angular_pred[2], - acc_linear_pred[0], - acc_linear_pred[1], - acc_linear_pred[2], - jacobians, - j1_quat2rpy); + predict(parameters[0][0], // position1_x + parameters[0][1], // position1_y + parameters[0][2], // position1_z + orientation1_rpy[0], // orientation1_roll + orientation1_rpy[1], // orientation1_pitch + orientation1_rpy[2], // orientation1_yaw + parameters[2][0], // vel_linear1_x + parameters[2][1], // vel_linear1_y + parameters[2][2], // vel_linear1_z + parameters[3][0], // vel_angular1_roll + parameters[3][1], // vel_angular1_pitch + parameters[3][2], // vel_angular1_yaw + parameters[4][0], // acc_linear1_x + parameters[4][1], // acc_linear1_y + parameters[4][2], // acc_linear1_z + dt_, position_pred[0], position_pred[1], position_pred[2], orientation_pred_rpy[0], orientation_pred_rpy[1], + orientation_pred_rpy[2], vel_linear_pred[0], vel_linear_pred[1], vel_linear_pred[2], vel_angular_pred[0], + vel_angular_pred[1], vel_angular_pred[2], acc_linear_pred[0], acc_linear_pred[1], acc_linear_pred[2], + jacobians, j1_quat2rpy); residuals[0] = parameters[5][0] - position_pred[0]; residuals[1] = parameters[5][1] - position_pred[1]; @@ -207,64 +188,75 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 Eigen::Map residuals_map(residuals); residuals_map.applyOnTheLeft(A_); - if (jacobians) { + if (jacobians) + { // Update jacobian wrt position1 - if (jacobians[0]) { + if (jacobians[0]) + { Eigen::Map> jacobian(jacobians[0]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt orientation1 - if (jacobians[1]) { + if (jacobians[1]) + { Eigen::Map> jacobian(jacobians[1]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt vel_linear1 - if (jacobians[2]) { + if (jacobians[2]) + { Eigen::Map> jacobian(jacobians[2]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt vel_yaw1 - if (jacobians[3]) { + if (jacobians[3]) + { Eigen::Map> jacobian(jacobians[3]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt acc_linear1 - if (jacobians[4]) { + if (jacobians[4]) + { Eigen::Map> jacobian(jacobians[4]); jacobian.applyOnTheLeft(-A_); } - + // Jacobian wrt position2 - if (jacobians[5]) { + if (jacobians[5]) + { Eigen::Map> jacobian(jacobians[5]); jacobian = A_.block<15, 3>(0, 0); } // Jacobian wrt orientation2 - if (jacobians[6]) { + if (jacobians[6]) + { Eigen::Map> jacobian(jacobians[6]); Eigen::Map> j2_quat2rpy_map(j2_quat2rpy); jacobian = A_.block<15, 3>(0, 3) * j2_quat2rpy_map; } // Jacobian wrt vel_linear2 - if (jacobians[7]) { + if (jacobians[7]) + { Eigen::Map> jacobian(jacobians[7]); jacobian = A_.block<15, 3>(0, 6); } // Jacobian wrt vel_angular2 - if (jacobians[8]) { + if (jacobians[8]) + { Eigen::Map> jacobian(jacobians[8]); jacobian = A_.block<15, 3>(0, 9); } // Jacobian wrt acc_linear2 - if (jacobians[9]) { + if (jacobians[9]) + { Eigen::Map> jacobian(jacobians[9]); jacobian = A_.block<15, 3>(0, 12); } @@ -279,11 +271,8 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 //!< information matrix }; -Omnidirectional3DStateCostFunction::Omnidirectional3DStateCostFunction( - const double dt, - const fuse_core::Matrix15d & A) -: dt_(dt), - A_(A) +Omnidirectional3DStateCostFunction::Omnidirectional3DStateCostFunction(const double dt, const fuse_core::Matrix15d& A) + : dt_(dt), A_(A) { } diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp index d95523a0..0ccafd8f 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp @@ -40,7 +40,6 @@ #include #include - namespace fuse_models { @@ -92,66 +91,52 @@ class Omnidirectional3DStateCostFunctor * * @param[in] dt The time delta across which to generate the kinematic model cost * @param[in] A The residual weighting matrix, most likely the square root information matrix in - * order (x, y, z, qx, qy, qz, qw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * order (x, y, z, qx, qy, qz, qw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ - Omnidirectional3DStateCostFunctor(const double dt, const fuse_core::Matrix15d & A); + Omnidirectional3DStateCostFunctor(const double dt, const fuse_core::Matrix15d& A); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. * @param[in] position1 - First position (array with x at index 0, y at index 1, z at index 2) - * @param[in] orientation1 - First orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) check this order + * @param[in] orientation1 - First orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) + * check this order * @param[in] vel_linear1 - First linear velocity (array with x at index 0, y at index 1, z at index 2) * @param[in] vel_angular1 - First angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) * @param[in] acc_linear1 - First linear acceleration (array with x at index 0, y at index 1, z at index 2) * @param[in] dt - The time delta across which to predict the state * @param[out] position2 - Second position (array with x at index 0, y at index 1, z at index 2) - * @param[out] orientation2 - Second orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) check this order + * @param[out] orientation2 - Second orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) + * check this order * @param[out] vel_linear2 - Second velocity (array with x at index 0, y at index 1, z at index 2) - * @param[out] vel_angular2 - Second angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) + * @param[out] vel_angular2 - Second angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index + * 2) * @param[out] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1, z at index 2) */ - template - bool operator()( - const T * const position1, - const T * const orientation1, - const T * const vel_linear1, - const T * const vel_angular1, - const T * const acc_linear1, - const T * const position2, - const T * const orientation2, - const T * const vel_linear2, - const T * const vel_angular2, - const T * const acc_linear2, - T * residual) const; + template + bool operator()(const T* const position1, const T* const orientation1, const T* const vel_linear1, + const T* const vel_angular1, const T* const acc_linear1, const T* const position2, + const T* const orientation2, const T* const vel_linear2, const T* const vel_angular2, + const T* const acc_linear2, T* residual) const; private: double dt_; fuse_core::Matrix15d A_; //!< The residual weighting matrix, most likely the square root - //!< information matrix + //!< information matrix }; -Omnidirectional3DStateCostFunctor::Omnidirectional3DStateCostFunctor( - const double dt, - const fuse_core::Matrix15d & A) -: dt_(dt), - A_(A) +Omnidirectional3DStateCostFunctor::Omnidirectional3DStateCostFunctor(const double dt, const fuse_core::Matrix15d& A) + : dt_(dt), A_(A) { } -template -bool Omnidirectional3DStateCostFunctor::operator()( - const T * const position1, - const T * const orientation1, - const T * const vel_linear1, - const T * const vel_angular1, - const T * const acc_linear1, - const T * const position2, - const T * const orientation2, - const T * const vel_linear2, - const T * const vel_angular2, - const T * const acc_linear2, - T * residual) const +template +bool Omnidirectional3DStateCostFunctor::operator()(const T* const position1, const T* const orientation1, + const T* const vel_linear1, const T* const vel_angular1, + const T* const acc_linear1, const T* const position2, + const T* const orientation2, const T* const vel_linear2, + const T* const vel_angular2, const T* const acc_linear2, + T* residual) const { T position_pred[3]; T orientation_pred[3]; @@ -160,30 +145,16 @@ bool Omnidirectional3DStateCostFunctor::operator()( T acc_linear_pred[3]; // Convert orientation variables from quaternion to roll-pitch-yaw - const T orientation1_rpy[3] { - fuse_core::getRoll(orientation1[0], orientation1[1], orientation1[2], orientation1[3]), - fuse_core::getPitch(orientation1[0], orientation1[1], orientation1[2], orientation1[3]), - fuse_core::getYaw(orientation1[0], orientation1[1], orientation1[2], orientation1[3]) - }; - const T orientation2_rpy[3] { - fuse_core::getRoll(orientation2[0], orientation2[1], orientation2[2], orientation2[3]), - fuse_core::getPitch(orientation2[0], orientation2[1], orientation2[2], orientation2[3]), - fuse_core::getYaw(orientation2[0], orientation2[1], orientation2[2], orientation2[3]) - }; - - predict( - position1, - orientation1_rpy, - vel_linear1, - vel_angular1, - acc_linear1, - T(dt_), - position_pred, - orientation_pred, - vel_linear_pred, - vel_angular_pred, - acc_linear_pred); - + const T orientation1_rpy[3]{ fuse_core::getRoll(orientation1[0], orientation1[1], orientation1[2], orientation1[3]), + fuse_core::getPitch(orientation1[0], orientation1[1], orientation1[2], orientation1[3]), + fuse_core::getYaw(orientation1[0], orientation1[1], orientation1[2], orientation1[3]) }; + const T orientation2_rpy[3]{ fuse_core::getRoll(orientation2[0], orientation2[1], orientation2[2], orientation2[3]), + fuse_core::getPitch(orientation2[0], orientation2[1], orientation2[2], orientation2[3]), + fuse_core::getYaw(orientation2[0], orientation2[1], orientation2[2], orientation2[3]) }; + + predict(position1, orientation1_rpy, vel_linear1, vel_angular1, acc_linear1, T(dt_), position_pred, orientation_pred, + vel_linear_pred, vel_angular_pred, acc_linear_pred); + Eigen::Map> residuals_map(residual); residuals_map(0) = position2[0] - position_pred[0]; residuals_map(1) = position2[1] - position_pred[1]; diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp index a61de7ba..18273e19 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp @@ -53,7 +53,6 @@ #include #include - namespace fuse_models { @@ -91,22 +90,21 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint * @param[in] velocity_angular2 Angular velocity component variable of the second state * @param[in] acceleration_linear2 Linear acceleration component variable of the second state * @param[in] covariance The covariance matrix used to weight the constraint. Order is (x, y, z, - * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ - Omnidirectional3DStateKinematicConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::VelocityLinear3DStamped & velocity_linear1, - const fuse_variables::VelocityAngular3DStamped & velocity_angular1, - const fuse_variables::AccelerationLinear3DStamped & acceleration_linear1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_variables::VelocityLinear3DStamped & velocity_linear2, - const fuse_variables::VelocityAngular3DStamped & velocity_angular2, - const fuse_variables::AccelerationLinear3DStamped & acceleration_linear2, - const fuse_core::Matrix15d & covariance); + Omnidirectional3DStateKinematicConstraint(const std::string& source, + const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::VelocityLinear3DStamped& velocity_linear1, + const fuse_variables::VelocityAngular3DStamped& velocity_angular1, + const fuse_variables::AccelerationLinear3DStamped& acceleration_linear1, + const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, + const fuse_variables::VelocityLinear3DStamped& velocity_linear2, + const fuse_variables::VelocityAngular3DStamped& velocity_angular2, + const fuse_variables::AccelerationLinear3DStamped& acceleration_linear2, + const fuse_core::Matrix15d& covariance); /** * @brief Destructor @@ -117,20 +115,26 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint * @brief Read-only access to the time delta between the first and second state (really, between * the position1 and position2 variables in the constructor) */ - double dt() const {return dt_;} + double dt() const + { + return dt_; + } /** * @brief Read-only access to the square root information matrix. * - * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ - const fuse_core::Matrix15d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix15d& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. * - * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ fuse_core::Matrix15d covariance() const @@ -143,7 +147,7 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -155,10 +159,10 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - double dt_; //!< The time delta for the constraint + double dt_; //!< The time delta for the constraint fuse_core::Matrix15d sqrt_information_; //!< The square root information matrix private: @@ -172,12 +176,12 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint * @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 */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & dt_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& dt_; + archive& sqrt_information_; } }; diff --git a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp index 1c0dda5c..c0df539a 100644 --- a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp @@ -45,7 +45,6 @@ #include #include - namespace fuse_models { @@ -65,147 +64,80 @@ struct Imu3DParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { - angular_velocity_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_dimensions")); - linear_acceleration_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "linear_acceleration_dimensions")); + angular_velocity_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "angular_velocity_dimensions")); + linear_acceleration_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "linear_acceleration_dimensions")); orientation_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "orientation_dimensions")); - - differential = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "differential"), - differential); + interfaces, fuse_core::joinParameterName(ns, "orientation_dimensions")); + + differential = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "differential"), differential); disable_checks = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "disable_checks"), - disable_checks); - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_timeout"), tf_timeout, - false); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_period"), throttle_period, - false); - throttle_use_wall_time = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_use_wall_time"), - throttle_use_wall_time); - - remove_gravitational_acceleration = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, "remove_gravitational_acceleration"), remove_gravitational_acceleration); - gravitational_acceleration = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "gravitational_acceleration"), - gravitational_acceleration); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "disable_checks"), disable_checks); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_timeout"), tf_timeout, false); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "throttle_period"), throttle_period, false); + throttle_use_wall_time = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "throttle_use_wall_time"), + throttle_use_wall_time); + + remove_gravitational_acceleration = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "remove_gravitational_acceleration"), + remove_gravitational_acceleration); + gravitational_acceleration = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "gravitational_acceleration"), gravitational_acceleration); fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); - if (differential) { - independent = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "independent"), - independent); - use_twist_covariance = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "use_twist_covariance"), - use_twist_covariance); - - minimum_pose_relative_covariance = - fuse_core::getCovarianceDiagonalParam<6>( - interfaces, - fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); - twist_covariance_offset = - fuse_core::getCovarianceDiagonalParam<6>( - interfaces, - fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); + if (differential) + { + independent = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "independent"), independent); + use_twist_covariance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_twist_covariance"), + use_twist_covariance); + + minimum_pose_relative_covariance = fuse_core::getCovarianceDiagonalParam<6>( + interfaces, fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + twist_covariance_offset = fuse_core::getCovarianceDiagonalParam<6>( + interfaces, fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); } - acceleration_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "acceleration_target_frame"), - acceleration_target_frame); - orientation_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "orientation_target_frame"), - orientation_target_frame); + acceleration_target_frame = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "acceleration_target_frame"), acceleration_target_frame); + orientation_target_frame = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "orientation_target_frame"), orientation_target_frame); twist_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "twist_target_frame"), - twist_target_frame); - - pose_loss = - fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "twist_target_frame"), twist_target_frame); + + pose_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); angular_velocity_loss = - fuse_core::loadLossConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "angular_velocity_loss")); linear_acceleration_loss = - fuse_core::loadLossConfig( - interfaces, - fuse_core::joinParameterName(ns, "linear_acceleration_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "linear_acceleration_loss")); } - bool differential {false}; - bool disable_checks {false}; - bool independent {true}; - bool use_twist_covariance {true}; + bool differential{ false }; + bool disable_checks{ false }; + bool independent{ true }; + bool use_twist_covariance{ true }; fuse_core::Matrix6d minimum_pose_relative_covariance; //!< Minimum pose relative covariance //!< matrix - fuse_core::Matrix6d twist_covariance_offset; //!< Offset already added to the twist covariance - //!< matrix, that will be substracted in order to - //!< recover the raw values - bool remove_gravitational_acceleration {false}; - int queue_size {10}; - rclcpp::Duration tf_timeout {0, 0}; //!< The maximum time to wait for a transform to become - //!< available - rclcpp::Duration throttle_period {0, 0}; //!< The throttle period duration in seconds - bool throttle_use_wall_time {false}; //!< Whether to throttle using ros::WallTime or not - double gravitational_acceleration {9.80665}; - std::string acceleration_target_frame {}; - std::string orientation_target_frame {}; - std::string topic {}; - std::string twist_target_frame {}; + fuse_core::Matrix6d twist_covariance_offset; //!< Offset already added to the twist covariance + //!< matrix, that will be substracted in order to + //!< recover the raw values + bool remove_gravitational_acceleration{ false }; + int queue_size{ 10 }; + rclcpp::Duration tf_timeout{ 0, 0 }; //!< The maximum time to wait for a transform to become + //!< available + rclcpp::Duration throttle_period{ 0, 0 }; //!< The throttle period duration in seconds + bool throttle_use_wall_time{ false }; //!< Whether to throttle using ros::WallTime or not + double gravitational_acceleration{ 9.80665 }; + std::string acceleration_target_frame{}; + std::string orientation_target_frame{}; + std::string topic{}; + std::string twist_target_frame{}; std::vector angular_velocity_indices; std::vector linear_acceleration_indices; std::vector orientation_indices; diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp index 1e01a073..734e75c9 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp @@ -46,7 +46,6 @@ #include #include - namespace fuse_models { @@ -66,137 +65,73 @@ struct Odometry3DParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { - position_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "position_dimensions")); - orientation_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "orientation_dimensions")); - linear_velocity_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "linear_velocity_dimensions")); - angular_velocity_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_dimensions")); - - differential = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "differential"), - differential); + position_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "position_dimensions")); + orientation_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "orientation_dimensions")); + linear_velocity_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "linear_velocity_dimensions")); + angular_velocity_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "angular_velocity_dimensions")); + + differential = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "differential"), differential); disable_checks = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "disable_checks"), - disable_checks); - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_timeout"), tf_timeout, - false); - - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_period"), throttle_period, - false); - throttle_use_wall_time = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_use_wall_time"), - throttle_use_wall_time); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "disable_checks"), disable_checks); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_timeout"), tf_timeout, false); + + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "throttle_period"), throttle_period, false); + throttle_use_wall_time = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "throttle_use_wall_time"), + throttle_use_wall_time); fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); twist_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "twist_target_frame"), - twist_target_frame); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "twist_target_frame"), twist_target_frame); pose_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "pose_target_frame"), - pose_target_frame); - - if (differential) { - independent = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "independent"), - independent); - use_twist_covariance = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "use_twist_covariance"), - use_twist_covariance); - - minimum_pose_relative_covariance = - fuse_core::getCovarianceDiagonalParam<6>( - interfaces, - fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); - twist_covariance_offset = - fuse_core::getCovarianceDiagonalParam<6>( - interfaces, - fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "pose_target_frame"), pose_target_frame); + + if (differential) + { + independent = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "independent"), independent); + use_twist_covariance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_twist_covariance"), + use_twist_covariance); + + minimum_pose_relative_covariance = fuse_core::getCovarianceDiagonalParam<6>( + interfaces, fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + twist_covariance_offset = fuse_core::getCovarianceDiagonalParam<6>( + interfaces, fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); } - pose_loss = - fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + pose_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); linear_velocity_loss = - fuse_core::loadLossConfig( - interfaces, fuse_core::joinParameterName( - ns, - "linear_velocity_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "linear_velocity_loss")); angular_velocity_loss = - fuse_core::loadLossConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "angular_velocity_loss")); } - bool differential {false}; - bool disable_checks {false}; - bool independent {true}; - bool use_twist_covariance {true}; + bool differential{ false }; + bool disable_checks{ false }; + bool independent{ true }; + bool use_twist_covariance{ true }; fuse_core::Matrix6d minimum_pose_relative_covariance; //!< Minimum pose relative covariance //!< matrix - fuse_core::Matrix6d twist_covariance_offset; //!< Offset already added to the twist covariance - //!< matrix, that will be substracted in order to - //!< recover the raw values - int queue_size {10}; - rclcpp::Duration tf_timeout {0, 0}; //!< The maximum time to wait for a transform to become - //!< available - rclcpp::Duration throttle_period {0, 0}; //!< The throttle period duration in seconds - bool throttle_use_wall_time {false}; //!< Whether to throttle using ros::WallTime or not - std::string topic {}; - std::string pose_target_frame {}; - std::string twist_target_frame {}; + fuse_core::Matrix6d twist_covariance_offset; //!< Offset already added to the twist covariance + //!< matrix, that will be substracted in order to + //!< recover the raw values + int queue_size{ 10 }; + rclcpp::Duration tf_timeout{ 0, 0 }; //!< The maximum time to wait for a transform to become + //!< available + rclcpp::Duration throttle_period{ 0, 0 }; //!< The throttle period duration in seconds + bool throttle_use_wall_time{ false }; //!< Whether to throttle using ros::WallTime or not + std::string topic{}; + std::string pose_target_frame{}; + std::string twist_target_frame{}; std::vector position_indices; std::vector orientation_indices; std::vector linear_velocity_indices; diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp index 64d2d138..21cb0a29 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp @@ -49,7 +49,6 @@ #include - namespace fuse_models { @@ -71,174 +70,95 @@ struct Odometry3DPublisherParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { - publish_tf = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "publish_tf"), - publish_tf); - invert_tf = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "invert_tf"), - invert_tf); - predict_to_current_time = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "predict_to_current_time"), - predict_to_current_time); - predict_with_acceleration = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "predict_with_acceleration"), - predict_with_acceleration); + publish_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_tf"), publish_tf); + invert_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "invert_tf"), invert_tf); + predict_to_current_time = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "predict_to_current_time"), predict_to_current_time); + predict_with_acceleration = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "predict_with_acceleration"), predict_with_acceleration); publish_frequency = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "publish_frequency"), - publish_frequency); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_frequency"), publish_frequency); process_noise_covariance = fuse_core::getCovarianceDiagonalParam<15>( - interfaces, fuse_core::joinParameterName( - ns, - "process_noise_diagonal"), 0.0); + interfaces, fuse_core::joinParameterName(ns, "process_noise_diagonal"), 0.0); scale_process_noise = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "scale_process_noise"), - scale_process_noise); - velocity_linear_norm_min_ = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "velocity_linear_norm_min"), - velocity_linear_norm_min_); - velocity_angular_norm_min_ = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "velocity_angular_norm_min"), - velocity_angular_norm_min_); - fuse_core::getPositiveParam( - interfaces, - fuse_core::joinParameterName( - ns, - "covariance_throttle_period"), covariance_throttle_period, - false); - - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_cache_time"), tf_cache_time, - false); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_timeout"), tf_timeout, - false); - - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - - map_frame_id = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "map_frame_id"), - map_frame_id); - odom_frame_id = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "odom_frame_id"), - odom_frame_id); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "scale_process_noise"), scale_process_noise); + velocity_linear_norm_min_ = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "velocity_linear_norm_min"), velocity_linear_norm_min_); + velocity_angular_norm_min_ = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "velocity_angular_norm_min"), velocity_angular_norm_min_); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "covariance_throttle_period"), + covariance_throttle_period, false); + + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_cache_time"), tf_cache_time, false); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_timeout"), tf_timeout, false); + + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + + map_frame_id = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "map_frame_id"), map_frame_id); + odom_frame_id = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "odom_frame_id"), odom_frame_id); base_link_frame_id = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "base_link_frame_id"), - base_link_frame_id); - base_link_output_frame_id = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "base_link_output_frame_id"), - base_link_output_frame_id); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "base_link_frame_id"), base_link_frame_id); + base_link_output_frame_id = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "base_link_output_frame_id"), base_link_output_frame_id); world_frame_id = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "world_frame_id"), - world_frame_id); - - const bool frames_valid = - map_frame_id != odom_frame_id && - map_frame_id != base_link_frame_id && - map_frame_id != base_link_output_frame_id && - odom_frame_id != base_link_frame_id && - odom_frame_id != base_link_output_frame_id && - (world_frame_id == map_frame_id || world_frame_id == odom_frame_id); - - if (!frames_valid) { - RCLCPP_FATAL_STREAM( - interfaces.get_node_logging_interface()->get_logger(), - "Invalid frame configuration! Please note:\n" - << " - The values for map_frame_id, odom_frame_id, and base_link_frame_id must be " - << "unique\n" - << " - The values for map_frame_id, odom_frame_id, and base_link_output_frame_id must be " - << "unique\n" - << " - The world_frame_id must be the same as the map_frame_id or odom_frame_id\n"); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "world_frame_id"), world_frame_id); + + const bool frames_valid = map_frame_id != odom_frame_id && map_frame_id != base_link_frame_id && + map_frame_id != base_link_output_frame_id && odom_frame_id != base_link_frame_id && + odom_frame_id != base_link_output_frame_id && + (world_frame_id == map_frame_id || world_frame_id == odom_frame_id); + + if (!frames_valid) + { + RCLCPP_FATAL_STREAM(interfaces.get_node_logging_interface()->get_logger(), + "Invalid frame configuration! Please note:\n" + << " - The values for map_frame_id, odom_frame_id, and base_link_frame_id must be " + << "unique\n" + << " - The values for map_frame_id, odom_frame_id, and base_link_output_frame_id must be " + << "unique\n" + << " - The world_frame_id must be the same as the map_frame_id or odom_frame_id\n"); assert(frames_valid); } topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); acceleration_topic = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "acceleration_topic"), - acceleration_topic); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "acceleration_topic"), acceleration_topic); fuse_core::loadCovarianceOptionsFromROS(interfaces, covariance_options, "covariance_options"); } - bool publish_tf {true}; //!< Whether to publish/broadcast the TF transform or not - bool invert_tf{false}; //!< Whether to broadcast the inverse of the TF transform or not. When - //!< the inverse is broadcasted, the transform is inverted and the - //!< header.frame_id and child_frame_id are swapped, i.e. the odometry - //!< output header.frame_id is set to the base_link_output_frame_id and - //!< the child_frame_id to the world_frame_id - bool predict_to_current_time {false}; - bool predict_with_acceleration {false}; - double publish_frequency {10.0}; - fuse_core::Matrix15d process_noise_covariance; //!< Process noise covariance matrix - bool scale_process_noise{false}; - double velocity_linear_norm_min_{1e-3}; - double velocity_angular_norm_min_{1e-3}; - rclcpp::Duration covariance_throttle_period {0, 0}; //!< The throttle period duration in seconds - //!< to compute the covariance - rclcpp::Duration tf_cache_time {10, 0}; - rclcpp::Duration tf_timeout {0, static_cast(RCUTILS_S_TO_NS(0.1))}; - int queue_size {1}; - std::string map_frame_id {"map"}; - std::string odom_frame_id {"odom"}; - std::string base_link_frame_id {"base_link"}; - std::string base_link_output_frame_id {base_link_frame_id}; - std::string world_frame_id {odom_frame_id}; - std::string topic {"odometry/filtered"}; - std::string acceleration_topic {"acceleration/filtered"}; + bool publish_tf{ true }; //!< Whether to publish/broadcast the TF transform or not + bool invert_tf{ false }; //!< Whether to broadcast the inverse of the TF transform or not. When + //!< the inverse is broadcasted, the transform is inverted and the + //!< header.frame_id and child_frame_id are swapped, i.e. the odometry + //!< output header.frame_id is set to the base_link_output_frame_id and + //!< the child_frame_id to the world_frame_id + bool predict_to_current_time{ false }; + bool predict_with_acceleration{ false }; + double publish_frequency{ 10.0 }; + fuse_core::Matrix15d process_noise_covariance; //!< Process noise covariance matrix + bool scale_process_noise{ false }; + double velocity_linear_norm_min_{ 1e-3 }; + double velocity_angular_norm_min_{ 1e-3 }; + rclcpp::Duration covariance_throttle_period{ 0, 0 }; //!< The throttle period duration in seconds + //!< to compute the covariance + rclcpp::Duration tf_cache_time{ 10, 0 }; + rclcpp::Duration tf_timeout{ 0, static_cast(RCUTILS_S_TO_NS(0.1)) }; + int queue_size{ 1 }; + std::string map_frame_id{ "map" }; + std::string odom_frame_id{ "odom" }; + std::string base_link_frame_id{ "base_link" }; + std::string base_link_output_frame_id{ base_link_frame_id }; + std::string world_frame_id{ odom_frame_id }; + std::string topic{ "odometry/filtered" }; + std::string acceleration_topic{ "acceleration/filtered" }; ceres::Covariance::Options covariance_options; }; diff --git a/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp index 053ac557..b84d1c25 100644 --- a/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp @@ -44,7 +44,6 @@ #include #include - namespace fuse_models { @@ -64,88 +63,54 @@ struct Omnidirectional3DIgnitionParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { publish_on_startup = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "publish_on_startup"), - publish_on_startup); - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - reset_service = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "reset_service"), - reset_service); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_on_startup"), publish_on_startup); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + reset_service = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "reset_service"), reset_service); set_pose_service = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "set_pose_service"), - set_pose_service); - set_pose_deprecated_service = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "set_pose_deprecated_service"), - set_pose_deprecated_service); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "set_pose_service"), set_pose_service); + set_pose_deprecated_service = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "set_pose_deprecated_service"), set_pose_deprecated_service); topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); std::vector sigma_vector; - sigma_vector = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "initial_sigma"), - sigma_vector); - if (!sigma_vector.empty()) { - if (sigma_vector.size() != 15) { - throw std::invalid_argument( - "The supplied initial_sigma parameter must be length 15, but " - "is actually length " + - std::to_string(sigma_vector.size())); + sigma_vector = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "initial_sigma"), sigma_vector); + if (!sigma_vector.empty()) + { + if (sigma_vector.size() != 15) + { + throw std::invalid_argument("The supplied initial_sigma parameter must be length 15, but " + "is actually length " + + std::to_string(sigma_vector.size())); } - auto is_sigma_valid = [](const double sigma) - { - return std::isfinite(sigma) && (sigma > 0); - }; - if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) { - throw std::invalid_argument( - "The supplied initial_sigma parameter must contain valid floating point values. " - "NaN, Inf, and values <= 0 are not acceptable."); + auto is_sigma_valid = [](const double sigma) { return std::isfinite(sigma) && (sigma > 0); }; + if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) + { + throw std::invalid_argument("The supplied initial_sigma parameter must contain valid floating point values. " + "NaN, Inf, and values <= 0 are not acceptable."); } initial_sigma.swap(sigma_vector); } std::vector state_vector; - state_vector = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "initial_state"), - state_vector); - if (!state_vector.empty()) { - if (state_vector.size() != 15) { - throw std::invalid_argument( - "The supplied initial_state parameter must be length 15, but is actually length " + - std::to_string(state_vector.size())); + state_vector = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "initial_state"), state_vector); + if (!state_vector.empty()) + { + if (state_vector.size() != 15) + { + throw std::invalid_argument("The supplied initial_state parameter must be length 15, but is actually length " + + std::to_string(state_vector.size())); } - auto is_state_valid = [](const double state) - { - return std::isfinite(state); - }; - if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) { - throw std::invalid_argument( - "The supplied initial_state parameter must contain valid floating point values. " - "NaN, Inf, etc are not acceptable."); + auto is_state_valid = [](const double state) { return std::isfinite(state); }; + if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) + { + throw std::invalid_argument("The supplied initial_state parameter must contain valid floating point values. " + "NaN, Inf, etc are not acceptable."); } initial_state.swap(state_vector); } @@ -153,37 +118,36 @@ struct Omnidirectional3DIgnitionParams : public ParameterBase loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "loss")); } - /** * @brief Flag indicating if an initial state transaction should be sent on startup, or only in * response to a set_pose service call or topic message. */ - bool publish_on_startup {true}; + bool publish_on_startup{ true }; /** * @brief The size of the subscriber queue for the set_pose topic */ - int queue_size {10}; + int queue_size{ 10 }; /** * @brief The name of the reset service to call before sending transactions to the optimizer */ - std::string reset_service {"~/reset"}; + std::string reset_service{ "~/reset" }; /** * @brief The name of the set_pose service to advertise */ - std::string set_pose_service {"set_pose"}; + std::string set_pose_service{ "set_pose" }; /** * @brief The name of the deprecated set_pose service without return codes */ - std::string set_pose_deprecated_service {"set_pose_deprecated"}; + std::string set_pose_deprecated_service{ "set_pose_deprecated" }; /** * @brief The topic name for received PoseWithCovarianceStamped messages */ - std::string topic {"set_pose"}; + std::string topic{ "set_pose" }; /** * @brief The uncertainty of the initial state value @@ -193,18 +157,14 @@ struct Omnidirectional3DIgnitionParams : public ParameterBase * The covariance matrix is created placing the squared standard deviations along the diagonal of * an 15x15 matrix. */ - std::vector initial_sigma {1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, - 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, - 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9 - }; + std::vector initial_sigma{ 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, + 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9 }; /** - * @brief The initial value of the 15-dimension state vector (x, y, z, roll, pitch, yaw, + * @brief The initial value of the 15-dimension state vector (x, y, z, roll, pitch, yaw, * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc) */ - std::vector initial_state {0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0}; + std::vector initial_state{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; /** * @brief Loss function */ diff --git a/fuse_models/src/imu_3d.cpp b/fuse_models/src/imu_3d.cpp index 9fdc11d4..1a12217c 100644 --- a/fuse_models/src/imu_3d.cpp +++ b/fuse_models/src/imu_3d.cpp @@ -45,7 +45,6 @@ #include #include - // Register this sensor model with ROS as a plugin. PLUGINLIB_EXPORT_CLASS(fuse_models::Imu3D, fuse_core::SensorModel) @@ -53,17 +52,15 @@ namespace fuse_models { Imu3D::Imu3D() -: fuse_core::AsyncSensorModel(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - throttled_callback_(std::bind(&Imu3D::process, this, std::placeholders::_1)) + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&Imu3D::process, this, std::placeholders::_1)) { } -void Imu3D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Imu3D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -81,35 +78,29 @@ void Imu3D::onInit() throttled_callback_.setThrottlePeriod(params_.throttle_period); - if (!params_.throttle_use_wall_time) { + if (!params_.throttle_use_wall_time) + { throttled_callback_.setClock(clock_); } - if (params_.orientation_indices.empty() && - params_.linear_acceleration_indices.empty() && - params_.angular_velocity_indices.empty()) + if (params_.orientation_indices.empty() && params_.linear_acceleration_indices.empty() && + params_.angular_velocity_indices.empty()) { - RCLCPP_WARN_STREAM( - logger_, - "No dimensions were specified. Data from topic " << params_.topic - << " will be ignored."); + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << params_.topic << " will be ignored."); } tf_buffer_ = std::make_unique(clock_); - tf_listener_ = std::make_unique( - *tf_buffer_, - interfaces_.get_node_base_interface(), - interfaces_.get_node_logging_interface(), - interfaces_.get_node_parameters_interface(), - interfaces_.get_node_topics_interface() - ); + tf_listener_ = std::make_unique(*tf_buffer_, interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); } void Imu3D::onStart() { - if (!params_.orientation_indices.empty() || - !params_.linear_acceleration_indices.empty() || - !params_.angular_velocity_indices.empty()) + if (!params_.orientation_indices.empty() || !params_.linear_acceleration_indices.empty() || + !params_.angular_velocity_indices.empty()) { previous_pose_.reset(); @@ -117,16 +108,10 @@ void Imu3D::onStart() sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind( - &ImuThrottledCallback::callback, - &throttled_callback_, - std::placeholders::_1 - ), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&ImuThrottledCallback::callback, &throttled_callback_, + std::placeholders::_1), + sub_options); } } @@ -135,7 +120,7 @@ void Imu3D::onStop() sub_.reset(); } -void Imu3D::process(const sensor_msgs::msg::Imu & msg) +void Imu3D::process(const sensor_msgs::msg::Imu& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); @@ -167,40 +152,24 @@ void Imu3D::process(const sensor_msgs::msg::Imu & msg) twist.twist.covariance[33] = msg.angular_velocity_covariance[6]; twist.twist.covariance[34] = msg.angular_velocity_covariance[7]; twist.twist.covariance[35] = msg.angular_velocity_covariance[8]; - + const bool validate = !params_.disable_checks; - if (params_.differential) { + if (params_.differential) + { processDifferential(*pose, twist, validate, *transaction); - } else { - common::processAbsolutePose3DWithCovariance( - name(), - device_id_, - *pose, - params_.pose_loss, - params_.orientation_target_frame, - {}, - params_.orientation_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + } + else + { + common::processAbsolutePose3DWithCovariance(name(), device_id_, *pose, params_.pose_loss, + params_.orientation_target_frame, {}, params_.orientation_indices, + *tf_buffer_, validate, *transaction, params_.tf_timeout); } // Handle the twist data (only include indices for angular velocity) - common::processTwist3DWithCovariance( - name(), - device_id_, - twist, - nullptr, - params_.angular_velocity_loss, - params_.twist_target_frame, - {}, - params_.angular_velocity_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + common::processTwist3DWithCovariance(name(), device_id_, twist, nullptr, params_.angular_velocity_loss, + params_.twist_target_frame, {}, params_.angular_velocity_indices, *tf_buffer_, + validate, *transaction, params_.tf_timeout); // Handle the acceleration data geometry_msgs::msg::AccelWithCovarianceStamped accel; @@ -217,7 +186,8 @@ void Imu3D::process(const sensor_msgs::msg::Imu & msg) accel.accel.covariance[14] = msg.linear_acceleration_covariance[8]; // Optionally remove the acceleration due to gravity - if (params_.remove_gravitational_acceleration) { + if (params_.remove_gravitational_acceleration) + { geometry_msgs::msg::Vector3 accel_gravity; accel_gravity.z = params_.gravitational_acceleration; geometry_msgs::msg::TransformStamped orientation_trans; @@ -230,88 +200,64 @@ void Imu3D::process(const sensor_msgs::msg::Imu & msg) accel.accel.accel.linear.z -= accel_gravity.z; } - common::processAccel3DWithCovariance( - name(), - device_id_, - accel, - params_.linear_acceleration_loss, - params_.acceleration_target_frame, - params_.linear_acceleration_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + common::processAccel3DWithCovariance(name(), device_id_, accel, params_.linear_acceleration_loss, + params_.acceleration_target_frame, params_.linear_acceleration_indices, + *tf_buffer_, validate, *transaction, params_.tf_timeout); // Send the transaction object to the plugin's parent sendTransaction(transaction); } -void Imu3D::processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, - const bool validate, - fuse_core::Transaction & transaction) +void Imu3D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique(); transformed_pose->header.frame_id = - params_.orientation_target_frame.empty() ? pose.header.frame_id : params_. - orientation_target_frame; - - if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose, params_.tf_timeout)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform pose message with stamp " << rclcpp::Time( - pose.header.stamp).nanoseconds() - << " to orientation target frame " << - params_.orientation_target_frame); + params_.orientation_target_frame.empty() ? pose.header.frame_id : params_.orientation_target_frame; + + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose, params_.tf_timeout)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " + << rclcpp::Time(pose.header.stamp).nanoseconds() << " to orientation target frame " + << params_.orientation_target_frame); return; } - if (!previous_pose_) { + if (!previous_pose_) + { previous_pose_ = std::move(transformed_pose); - return; + return; } - if (params_.use_twist_covariance) { + if (params_.use_twist_covariance) + { geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; transformed_twist.header.frame_id = - params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; - - if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform twist message with stamp " << rclcpp::Time( - twist.header.stamp).nanoseconds() - << " to twist target frame " << - params_.twist_target_frame); - } else { - common::processDifferentialPose3DWithTwistCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - twist, - params_.minimum_pose_relative_covariance, - params_.twist_covariance_offset, - params_.pose_loss, - {}, - params_.orientation_indices, - validate, - transaction); + params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; + + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " + << rclcpp::Time(twist.header.stamp).nanoseconds() << " to twist target frame " + << params_.twist_target_frame); } - } else { - common::processDifferentialPose3DWithCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - params_.independent, - params_.minimum_pose_relative_covariance, - params_.pose_loss, - {}, - params_.orientation_indices, - validate, - transaction); + else + { + common::processDifferentialPose3DWithTwistCovariance(name(), device_id_, *previous_pose_, *transformed_pose, + twist, params_.minimum_pose_relative_covariance, + params_.twist_covariance_offset, params_.pose_loss, {}, + params_.orientation_indices, validate, transaction); + } + } + else + { + common::processDifferentialPose3DWithCovariance(name(), device_id_, *previous_pose_, *transformed_pose, + params_.independent, params_.minimum_pose_relative_covariance, + params_.pose_loss, {}, params_.orientation_indices, validate, + transaction); } previous_pose_ = std::move(transformed_pose); diff --git a/fuse_models/src/odometry_3d.cpp b/fuse_models/src/odometry_3d.cpp index 85cb1cc7..cd05a3c2 100644 --- a/fuse_models/src/odometry_3d.cpp +++ b/fuse_models/src/odometry_3d.cpp @@ -51,17 +51,15 @@ namespace fuse_models { Odometry3D::Odometry3D() -: fuse_core::AsyncSensorModel(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - throttled_callback_(std::bind(&Odometry3D::process, this, std::placeholders::_1)) + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&Odometry3D::process, this, std::placeholders::_1)) { } -void Odometry3D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Odometry3D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -79,51 +77,40 @@ void Odometry3D::onInit() throttled_callback_.setThrottlePeriod(params_.throttle_period); - if (!params_.throttle_use_wall_time) { + if (!params_.throttle_use_wall_time) + { throttled_callback_.setClock(clock_); } if (params_.position_indices.empty() && params_.orientation_indices.empty() && - params_.linear_velocity_indices.empty() && params_.angular_velocity_indices.empty()) + params_.linear_velocity_indices.empty() && params_.angular_velocity_indices.empty()) { - RCLCPP_WARN_STREAM( - logger_, - "No dimensions were specified. Data from topic " << params_.topic - << " will be ignored."); + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << params_.topic << " will be ignored."); } tf_buffer_ = std::make_unique(clock_); - tf_listener_ = std::make_unique( - *tf_buffer_, - interfaces_.get_node_base_interface(), - interfaces_.get_node_logging_interface(), - interfaces_.get_node_parameters_interface(), - interfaces_.get_node_topics_interface() - ); + tf_listener_ = std::make_unique(*tf_buffer_, interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); } void Odometry3D::onStart() { if (!params_.position_indices.empty() || !params_.orientation_indices.empty() || - !params_.linear_velocity_indices.empty() || !params_.angular_velocity_indices.empty()) + !params_.linear_velocity_indices.empty() || !params_.angular_velocity_indices.empty()) { - previous_pose_.reset(); + previous_pose_.reset(); - rclcpp::SubscriptionOptions sub_options; - sub_options.callback_group = cb_group_; + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; - sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind( - &OdometryThrottledCallback::callback< - const nav_msgs::msg::Odometry &>, - &throttled_callback_, - std::placeholders::_1 - ), - sub_options - ); + sub_ = rclcpp::create_subscription( + interfaces_, params_.topic, params_.queue_size, + std::bind(&OdometryThrottledCallback::callback, &throttled_callback_, + std::placeholders::_1), + sub_options); } } @@ -132,7 +119,7 @@ void Odometry3D::onStop() sub_.reset(); } -void Odometry3D::process(const nav_msgs::msg::Odometry & msg) +void Odometry3D::process(const nav_msgs::msg::Odometry& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); @@ -150,106 +137,78 @@ void Odometry3D::process(const nav_msgs::msg::Odometry & msg) const bool validate = !params_.disable_checks; - if (params_.differential) { + if (params_.differential) + { processDifferential(*pose, twist, validate, *transaction); - } else { - common::processAbsolutePose3DWithCovariance( - name(), - device_id_, - *pose, - params_.pose_loss, - params_.pose_target_frame, - params_.position_indices, - params_.orientation_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + } + else + { + common::processAbsolutePose3DWithCovariance(name(), device_id_, *pose, params_.pose_loss, params_.pose_target_frame, + params_.position_indices, params_.orientation_indices, *tf_buffer_, + validate, *transaction, params_.tf_timeout); } // Handle the twist data - common::processTwist3DWithCovariance( - name(), - device_id_, - twist, - params_.linear_velocity_loss, - params_.angular_velocity_loss, - params_.twist_target_frame, - params_.linear_velocity_indices, - params_.angular_velocity_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + common::processTwist3DWithCovariance(name(), device_id_, twist, params_.linear_velocity_loss, + params_.angular_velocity_loss, params_.twist_target_frame, + params_.linear_velocity_indices, params_.angular_velocity_indices, *tf_buffer_, + validate, *transaction, params_.tf_timeout); // Send the transaction object to the plugin's parent sendTransaction(transaction); } -void Odometry3D::processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, - const bool validate, - fuse_core::Transaction & transaction) +void Odometry3D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique(); transformed_pose->header.frame_id = - params_.pose_target_frame.empty() ? pose.header.frame_id : params_.pose_target_frame; + params_.pose_target_frame.empty() ? pose.header.frame_id : params_.pose_target_frame; - if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform pose message with stamp " - << rclcpp::Time( - pose.header.stamp).nanoseconds() << " to pose target frame " << params_.pose_target_frame); + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " + << rclcpp::Time(pose.header.stamp).nanoseconds() << " to pose target frame " + << params_.pose_target_frame); return; } - if (!previous_pose_) { + if (!previous_pose_) + { previous_pose_ = std::move(transformed_pose); return; } - if (params_.use_twist_covariance) { + if (params_.use_twist_covariance) + { geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; transformed_twist.header.frame_id = - params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; - - if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform twist message with stamp " << rclcpp::Time( - twist.header.stamp).nanoseconds() - << " to twist target frame " << - params_.twist_target_frame); - } else { - common::processDifferentialPose3DWithTwistCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - transformed_twist, - params_.minimum_pose_relative_covariance, - params_.twist_covariance_offset, - params_.pose_loss, - params_.position_indices, - params_.orientation_indices, - validate, - transaction); + params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; + + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " + << rclcpp::Time(twist.header.stamp).nanoseconds() << " to twist target frame " + << params_.twist_target_frame); + } + else + { + common::processDifferentialPose3DWithTwistCovariance(name(), device_id_, *previous_pose_, *transformed_pose, + transformed_twist, params_.minimum_pose_relative_covariance, + params_.twist_covariance_offset, params_.pose_loss, + params_.position_indices, params_.orientation_indices, + validate, transaction); } - } else { - common::processDifferentialPose3DWithCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - params_.independent, - params_.minimum_pose_relative_covariance, - params_.pose_loss, - params_.position_indices, - params_.orientation_indices, - validate, - transaction); + } + else + { + common::processDifferentialPose3DWithCovariance(name(), device_id_, *previous_pose_, *transformed_pose, + params_.independent, params_.minimum_pose_relative_covariance, + params_.pose_loss, params_.position_indices, + params_.orientation_indices, validate, transaction); } previous_pose_ = std::move(transformed_pose); } diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index 6fb94e5e..ec2649f9 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -65,17 +65,16 @@ namespace fuse_models { Odometry3DPublisher::Odometry3DPublisher() -: fuse_core::AsyncPublisher(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - latest_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)), - latest_covariance_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) + : fuse_core::AsyncPublisher(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , latest_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) + , latest_covariance_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { } void Odometry3DPublisher::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -93,54 +92,45 @@ void Odometry3DPublisher::onInit() params_.loadFromROS(interfaces_, name_); - if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { + if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) + { tf_buffer_ = std::make_unique( - clock_, - params_.tf_cache_time.to_chrono() - // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h - // TODO(methylDragon): See above ^ + clock_, params_.tf_cache_time.to_chrono() + // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h + // TODO(methylDragon): See above ^ ); - tf_listener_ = std::make_unique( - *tf_buffer_, - interfaces_.get_node_base_interface(), - interfaces_.get_node_logging_interface(), - interfaces_.get_node_parameters_interface(), - interfaces_.get_node_topics_interface()); + tf_listener_ = std::make_unique(*tf_buffer_, interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); } // Advertise the topics rclcpp::PublisherOptions pub_options; pub_options.callback_group = cb_group_; - odom_pub_ = rclcpp::create_publisher( - interfaces_, - params_.topic, - params_.queue_size, - pub_options); + odom_pub_ = + rclcpp::create_publisher(interfaces_, params_.topic, params_.queue_size, pub_options); acceleration_pub_ = rclcpp::create_publisher( - interfaces_, - params_.acceleration_topic, - params_.queue_size, - pub_options); + interfaces_, params_.acceleration_topic, params_.queue_size, pub_options); } -void Odometry3DPublisher::notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) +void Odometry3DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) { // Find the most recent common timestamp const auto latest_stamp = synchronizer_.findLatestCommonStamp(*transaction, *graph); - if (0u == latest_stamp.nanoseconds()) { + if (0u == latest_stamp.nanoseconds()) + { { std::lock_guard lock(mutex_); latest_stamp_ = latest_stamp; } - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Failed to find a matching set of state variables with device id '" - << device_id_ << "'."); + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a matching set of state variables with device id '" << device_id_ + << "'."); return; } @@ -153,17 +143,8 @@ void Odometry3DPublisher::notifyCallback( nav_msgs::msg::Odometry odom_output; geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output; - if (!getState( - *graph, - latest_stamp, - device_id_, - position_uuid, - orientation_uuid, - velocity_linear_uuid, - velocity_angular_uuid, - acceleration_linear_uuid, - odom_output, - acceleration_output)) + if (!getState(*graph, latest_stamp, device_id_, position_uuid, orientation_uuid, velocity_linear_uuid, + velocity_angular_uuid, acceleration_linear_uuid, odom_output, acceleration_output)) { std::lock_guard lock(mutex_); latest_stamp_ = latest_stamp; @@ -180,14 +161,16 @@ void Odometry3DPublisher::notifyCallback( // Don't waste CPU computing the covariance if nobody is listening rclcpp::Time latest_covariance_stamp = latest_covariance_stamp_; bool latest_covariance_valid = latest_covariance_valid_; - if (odom_pub_->get_subscription_count() > 0 || acceleration_pub_->get_subscription_count() > 0) { + if (odom_pub_->get_subscription_count() > 0 || acceleration_pub_->get_subscription_count() > 0) + { // Throttle covariance computation if (params_.covariance_throttle_period.nanoseconds() == 0 || - latest_stamp - latest_covariance_stamp > params_.covariance_throttle_period) + latest_stamp - latest_covariance_stamp > params_.covariance_throttle_period) { latest_covariance_stamp = latest_stamp; - try { + try + { std::vector> covariance_requests; covariance_requests.emplace_back(position_uuid, position_uuid); covariance_requests.emplace_back(position_uuid, orientation_uuid); @@ -200,47 +183,47 @@ void Odometry3DPublisher::notifyCallback( std::vector> covariance_matrices; graph->getCovariance(covariance_requests, covariance_matrices, params_.covariance_options, true); - odom_output.pose.covariance[0] = covariance_matrices[0][0]; // cov(x, x) - odom_output.pose.covariance[1] = covariance_matrices[0][1]; // cov(x, y) - odom_output.pose.covariance[2] = covariance_matrices[0][2]; // cov(x, z) - odom_output.pose.covariance[3] = covariance_matrices[1][0]; // cov(x, roll) - odom_output.pose.covariance[4] = covariance_matrices[1][1]; // cov(x, pitch) - odom_output.pose.covariance[5] = covariance_matrices[1][2]; // cov(x, yaw) - - odom_output.pose.covariance[6] = covariance_matrices[0][3]; // cov(y, x) - odom_output.pose.covariance[7] = covariance_matrices[0][4]; // cov(y, y) - odom_output.pose.covariance[8] = covariance_matrices[0][5]; // cov(y, z) - odom_output.pose.covariance[9] = covariance_matrices[1][3]; // cov(y, roll) - odom_output.pose.covariance[10] = covariance_matrices[1][4]; // cov(y, pitch) - odom_output.pose.covariance[11] = covariance_matrices[1][5]; // cov(y, yaw) - - odom_output.pose.covariance[12] = covariance_matrices[0][6]; // cov(z, x) - odom_output.pose.covariance[13] = covariance_matrices[0][7]; // cov(z, y) - odom_output.pose.covariance[14] = covariance_matrices[0][8]; // cov(z, z) - odom_output.pose.covariance[15] = covariance_matrices[1][6]; // cov(z, roll) - odom_output.pose.covariance[16] = covariance_matrices[1][7]; // cov(z, pitch) - odom_output.pose.covariance[17] = covariance_matrices[1][8]; // cov(z, yaw) - - odom_output.pose.covariance[18] = covariance_matrices[1][0]; // cov(roll, x) - odom_output.pose.covariance[19] = covariance_matrices[1][3]; // cov(roll, y) - odom_output.pose.covariance[20] = covariance_matrices[1][6]; // cov(roll, z) - odom_output.pose.covariance[21] = covariance_matrices[2][0]; // cov(roll, roll) - odom_output.pose.covariance[22] = covariance_matrices[2][1]; // cov(roll, pitch) - odom_output.pose.covariance[23] = covariance_matrices[2][2]; // cov(roll, yaw) - - odom_output.pose.covariance[24] = covariance_matrices[1][1]; // cov(pitch, x) - odom_output.pose.covariance[25] = covariance_matrices[1][4]; // cov(pitch, y) - odom_output.pose.covariance[26] = covariance_matrices[1][7]; // cov(pitch, z) - odom_output.pose.covariance[27] = covariance_matrices[2][1]; // cov(pitch, roll) - odom_output.pose.covariance[28] = covariance_matrices[2][4]; // cov(pitch, pitch) - odom_output.pose.covariance[29] = covariance_matrices[2][5]; // cov(pitch, yaw) - - odom_output.pose.covariance[30] = covariance_matrices[1][2]; // cov(yaw, x) - odom_output.pose.covariance[31] = covariance_matrices[1][5]; // cov(yaw, y) - odom_output.pose.covariance[32] = covariance_matrices[1][8]; // cov(yaw, z) - odom_output.pose.covariance[33] = covariance_matrices[2][2]; // cov(yaw, roll) - odom_output.pose.covariance[34] = covariance_matrices[2][5]; // cov(yaw, pitch) - odom_output.pose.covariance[35] = covariance_matrices[2][8]; // cov(yaw, yaw) + odom_output.pose.covariance[0] = covariance_matrices[0][0]; // cov(x, x) + odom_output.pose.covariance[1] = covariance_matrices[0][1]; // cov(x, y) + odom_output.pose.covariance[2] = covariance_matrices[0][2]; // cov(x, z) + odom_output.pose.covariance[3] = covariance_matrices[1][0]; // cov(x, roll) + odom_output.pose.covariance[4] = covariance_matrices[1][1]; // cov(x, pitch) + odom_output.pose.covariance[5] = covariance_matrices[1][2]; // cov(x, yaw) + + odom_output.pose.covariance[6] = covariance_matrices[0][3]; // cov(y, x) + odom_output.pose.covariance[7] = covariance_matrices[0][4]; // cov(y, y) + odom_output.pose.covariance[8] = covariance_matrices[0][5]; // cov(y, z) + odom_output.pose.covariance[9] = covariance_matrices[1][3]; // cov(y, roll) + odom_output.pose.covariance[10] = covariance_matrices[1][4]; // cov(y, pitch) + odom_output.pose.covariance[11] = covariance_matrices[1][5]; // cov(y, yaw) + + odom_output.pose.covariance[12] = covariance_matrices[0][6]; // cov(z, x) + odom_output.pose.covariance[13] = covariance_matrices[0][7]; // cov(z, y) + odom_output.pose.covariance[14] = covariance_matrices[0][8]; // cov(z, z) + odom_output.pose.covariance[15] = covariance_matrices[1][6]; // cov(z, roll) + odom_output.pose.covariance[16] = covariance_matrices[1][7]; // cov(z, pitch) + odom_output.pose.covariance[17] = covariance_matrices[1][8]; // cov(z, yaw) + + odom_output.pose.covariance[18] = covariance_matrices[1][0]; // cov(roll, x) + odom_output.pose.covariance[19] = covariance_matrices[1][3]; // cov(roll, y) + odom_output.pose.covariance[20] = covariance_matrices[1][6]; // cov(roll, z) + odom_output.pose.covariance[21] = covariance_matrices[2][0]; // cov(roll, roll) + odom_output.pose.covariance[22] = covariance_matrices[2][1]; // cov(roll, pitch) + odom_output.pose.covariance[23] = covariance_matrices[2][2]; // cov(roll, yaw) + + odom_output.pose.covariance[24] = covariance_matrices[1][1]; // cov(pitch, x) + odom_output.pose.covariance[25] = covariance_matrices[1][4]; // cov(pitch, y) + odom_output.pose.covariance[26] = covariance_matrices[1][7]; // cov(pitch, z) + odom_output.pose.covariance[27] = covariance_matrices[2][1]; // cov(pitch, roll) + odom_output.pose.covariance[28] = covariance_matrices[2][4]; // cov(pitch, pitch) + odom_output.pose.covariance[29] = covariance_matrices[2][5]; // cov(pitch, yaw) + + odom_output.pose.covariance[30] = covariance_matrices[1][2]; // cov(yaw, x) + odom_output.pose.covariance[31] = covariance_matrices[1][5]; // cov(yaw, y) + odom_output.pose.covariance[32] = covariance_matrices[1][8]; // cov(yaw, z) + odom_output.pose.covariance[33] = covariance_matrices[2][2]; // cov(yaw, roll) + odom_output.pose.covariance[34] = covariance_matrices[2][5]; // cov(yaw, pitch) + odom_output.pose.covariance[35] = covariance_matrices[2][8]; // cov(yaw, yaw) odom_output.twist.covariance[0] = covariance_matrices[3][0]; odom_output.twist.covariance[1] = covariance_matrices[3][1]; @@ -262,7 +245,7 @@ void Odometry3DPublisher::notifyCallback( odom_output.twist.covariance[15] = covariance_matrices[4][6]; odom_output.twist.covariance[16] = covariance_matrices[4][7]; odom_output.twist.covariance[17] = covariance_matrices[4][8]; - + odom_output.twist.covariance[18] = covariance_matrices[4][0]; odom_output.twist.covariance[19] = covariance_matrices[4][3]; odom_output.twist.covariance[20] = covariance_matrices[4][6]; @@ -283,7 +266,7 @@ void Odometry3DPublisher::notifyCallback( odom_output.twist.covariance[33] = covariance_matrices[5][2]; odom_output.twist.covariance[34] = covariance_matrices[5][5]; odom_output.twist.covariance[35] = covariance_matrices[5][8]; - + acceleration_output.accel.covariance[0] = covariance_matrices[6][0]; acceleration_output.accel.covariance[1] = covariance_matrices[6][1]; acceleration_output.accel.covariance[2] = covariance_matrices[6][2]; @@ -296,37 +279,39 @@ void Odometry3DPublisher::notifyCallback( // test if covariances are symmetric Eigen::Map odom_cov_map(odom_output.pose.covariance.data()); - if (!odom_cov_map.isApprox(odom_cov_map.transpose())) { + if (!odom_cov_map.isApprox(odom_cov_map.transpose())) + { throw std::runtime_error("Odometry covariance matrix is not symmetric"); } Eigen::Map twist_cov_map(odom_output.twist.covariance.data()); - if (!twist_cov_map.isApprox(twist_cov_map.transpose())) { + if (!twist_cov_map.isApprox(twist_cov_map.transpose())) + { throw std::runtime_error("Twist covariance matrix is not symmetric"); } Eigen::Map accel_cov_map(acceleration_output.accel.covariance.data()); - if (!accel_cov_map.isApprox(accel_cov_map.transpose())) { + if (!accel_cov_map.isApprox(accel_cov_map.transpose())) + { throw std::runtime_error("Acceleration covariance matrix is not symmetric"); } latest_covariance_valid = true; - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM( - logger_, - "An error occurred computing the covariance information for " - << latest_stamp.nanoseconds() - << ". The covariance will be set to zero.\n" - << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM(logger_, "An error occurred computing the covariance information for " + << latest_stamp.nanoseconds() << ". The covariance will be set to zero.\n" + << e.what()); std::fill(odom_output.pose.covariance.begin(), odom_output.pose.covariance.end(), 0.0); std::fill(odom_output.twist.covariance.begin(), odom_output.twist.covariance.end(), 0.0); - std::fill( - acceleration_output.accel.covariance.begin(), - acceleration_output.accel.covariance.end(), 0.0); + std::fill(acceleration_output.accel.covariance.begin(), acceleration_output.accel.covariance.end(), 0.0); latest_covariance_valid = false; } - } else { + } + else + { // This covariance computation cycle has been skipped, so simply take the last covariance // computed // @@ -358,13 +343,9 @@ void Odometry3DPublisher::onStart() acceleration_output_ = geometry_msgs::msg::AccelWithCovarianceStamped(); // TODO(CH3): Add this to a separate callback group for async behavior - publish_timer_ = rclcpp::create_timer( - interfaces_, - clock_, - std::chrono::duration(1.0 / params_.publish_frequency), - std::move(std::bind(&Odometry3DPublisher::publishTimerCallback, this)), - cb_group_ - ); + publish_timer_ = + rclcpp::create_timer(interfaces_, clock_, std::chrono::duration(1.0 / params_.publish_frequency), + std::move(std::bind(&Odometry3DPublisher::publishTimerCallback, this)), cb_group_); delayed_throttle_filter_.reset(); } @@ -374,40 +355,34 @@ void Odometry3DPublisher::onStop() publish_timer_->cancel(); } -bool Odometry3DPublisher::getState( - const fuse_core::Graph & graph, - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id, - fuse_core::UUID & position_uuid, - fuse_core::UUID & orientation_uuid, - fuse_core::UUID & velocity_linear_uuid, - fuse_core::UUID & velocity_angular_uuid, - fuse_core::UUID & acceleration_linear_uuid, - nav_msgs::msg::Odometry & odometry, - geometry_msgs::msg::AccelWithCovarianceStamped & acceleration) +bool Odometry3DPublisher::getState(const fuse_core::Graph& graph, const rclcpp::Time& stamp, + const fuse_core::UUID& device_id, fuse_core::UUID& position_uuid, + fuse_core::UUID& orientation_uuid, fuse_core::UUID& velocity_linear_uuid, + fuse_core::UUID& velocity_angular_uuid, fuse_core::UUID& acceleration_linear_uuid, + nav_msgs::msg::Odometry& odometry, + geometry_msgs::msg::AccelWithCovarianceStamped& acceleration) { - try { + try + { position_uuid = fuse_variables::Position3DStamped(stamp, device_id).uuid(); - auto position_variable = dynamic_cast( - graph.getVariable(position_uuid)); + auto position_variable = dynamic_cast(graph.getVariable(position_uuid)); orientation_uuid = fuse_variables::Orientation3DStamped(stamp, device_id).uuid(); - auto orientation_variable = dynamic_cast( - graph.getVariable(orientation_uuid)); + auto orientation_variable = + dynamic_cast(graph.getVariable(orientation_uuid)); velocity_linear_uuid = fuse_variables::VelocityLinear3DStamped(stamp, device_id).uuid(); - auto velocity_linear_variable = dynamic_cast( - graph.getVariable(velocity_linear_uuid)); + auto velocity_linear_variable = + dynamic_cast(graph.getVariable(velocity_linear_uuid)); velocity_angular_uuid = fuse_variables::VelocityAngular3DStamped(stamp, device_id).uuid(); - auto velocity_angular_variable = dynamic_cast( - graph.getVariable(velocity_angular_uuid)); + auto velocity_angular_variable = + dynamic_cast(graph.getVariable(velocity_angular_uuid)); acceleration_linear_uuid = fuse_variables::AccelerationLinear3DStamped(stamp, device_id).uuid(); auto acceleration_linear_variable = - dynamic_cast( - graph.getVariable(acceleration_linear_uuid)); - + dynamic_cast(graph.getVariable(acceleration_linear_uuid)); + odometry.pose.pose.position.x = position_variable.x(); odometry.pose.pose.position.y = position_variable.y(); odometry.pose.pose.position.z = position_variable.z(); @@ -428,15 +403,17 @@ bool Odometry3DPublisher::getState( acceleration.accel.accel.angular.x = 0.0; acceleration.accel.accel.angular.y = 0.0; acceleration.accel.accel.angular.z = 0.0; - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Failed to find a state at time " << stamp.nanoseconds() << ". Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a state at time " << stamp.nanoseconds() << ". Error: " << e.what()); return false; - } catch (...) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Failed to find a state at time " << stamp.nanoseconds() << ". Error: unknown"); + } + catch (...) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a state at time " << stamp.nanoseconds() << ". Error: unknown"); return false; } @@ -460,10 +437,10 @@ void Odometry3DPublisher::publishTimerCallback() acceleration_output = acceleration_output_; } - if (0u == latest_stamp.nanoseconds()) { - RCLCPP_WARN_STREAM_EXPRESSION( - logger_, delayed_throttle_filter_.isEnabled(), - "No valid state data yet. Delaying tf broadcast."); + if (0u == latest_stamp.nanoseconds()) + { + RCLCPP_WARN_STREAM_EXPRESSION(logger_, delayed_throttle_filter_.isEnabled(), + "No valid state data yet. Delaying tf broadcast."); return; } @@ -471,7 +448,8 @@ void Odometry3DPublisher::publishTimerCallback() tf2::fromMsg(odom_output.pose.pose, pose); // If requested, we need to project our state forward in time using the 3D kinematic model - if (params_.predict_to_current_time) { + if (params_.predict_to_current_time) + { rclcpp::Time timer_now = interfaces_.get_node_clock_interface()->get_clock()->now(); // Convert pose in Eigen representation @@ -482,34 +460,24 @@ void Odometry3DPublisher::publishTimerCallback() orientation.y() = pose.getRotation().y(); orientation.z() = pose.getRotation().z(); orientation.w() = pose.getRotation().w(); - velocity_linear << odom_output.twist.twist.linear.x, - odom_output.twist.twist.linear.y, odom_output.twist.twist.linear.z; - velocity_angular << odom_output.twist.twist.angular.x, - odom_output.twist.twist.angular.y, odom_output.twist.twist.angular.z; + velocity_linear << odom_output.twist.twist.linear.x, odom_output.twist.twist.linear.y, + odom_output.twist.twist.linear.z; + velocity_angular << odom_output.twist.twist.angular.x, odom_output.twist.twist.angular.y, + odom_output.twist.twist.angular.z; const double dt = timer_now.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(); fuse_core::Matrix15d jacobian; fuse_core::Vector3d acceleration_linear; - if (params_.predict_with_acceleration) { - acceleration_linear << acceleration_output.accel.accel.linear.x, - acceleration_output.accel.accel.linear.y, acceleration_output.accel.accel.linear.z; + if (params_.predict_with_acceleration) + { + acceleration_linear << acceleration_output.accel.accel.linear.x, acceleration_output.accel.accel.linear.y, + acceleration_output.accel.accel.linear.z; } - predict( - position, - orientation, - velocity_linear, - velocity_angular, - acceleration_linear, - dt, - position, - orientation, - velocity_linear, - velocity_angular, - acceleration_linear, - jacobian); + predict(position, orientation, velocity_linear, velocity_angular, acceleration_linear, dt, position, orientation, + velocity_linear, velocity_angular, acceleration_linear, jacobian); // Convert back to tf2 representation pose.setOrigin(tf2::Vector3(position.x(), position.y(), position.z())); @@ -530,7 +498,8 @@ void Odometry3DPublisher::publishTimerCallback() odom_output.twist.twist.angular.y = velocity_angular.y(); odom_output.twist.twist.angular.z = velocity_angular.z(); - if (params_.predict_with_acceleration) { + if (params_.predict_with_acceleration) + { acceleration_output.accel.accel.linear.x = acceleration_linear.x(); acceleration_output.accel.accel.linear.y = acceleration_linear.y(); acceleration_output.accel.accel.linear.z = acceleration_linear.z(); @@ -541,7 +510,8 @@ void Odometry3DPublisher::publishTimerCallback() // Either the last covariance computation was skipped because there was no subscriber, // or it failed - if (latest_covariance_valid) { + if (latest_covariance_valid) + { fuse_core::Matrix15d covariance; covariance.setZero(); Eigen::Map pose_covariance(odom_output.pose.covariance.data()); @@ -555,13 +525,10 @@ void Odometry3DPublisher::publishTimerCallback() covariance = jacobian * covariance * jacobian.transpose(); auto process_noise_covariance = params_.process_noise_covariance; - if (params_.scale_process_noise) { - common::scaleProcessNoiseCovariance( - process_noise_covariance, - velocity_linear, - velocity_angular, - params_.velocity_linear_norm_min_, - params_.velocity_angular_norm_min_); + if (params_.scale_process_noise) + { + common::scaleProcessNoiseCovariance(process_noise_covariance, velocity_linear, velocity_angular, + params_.velocity_linear_norm_min_, params_.velocity_angular_norm_min_); } covariance.noalias() += dt * process_noise_covariance; @@ -575,11 +542,13 @@ void Odometry3DPublisher::publishTimerCallback() odom_pub_->publish(odom_output); acceleration_pub_->publish(acceleration_output); - if (params_.publish_tf) { + if (params_.publish_tf) + { auto frame_id = odom_output.header.frame_id; auto child_frame_id = odom_output.child_frame_id; - if (params_.invert_tf) { + if (params_.invert_tf) + { pose = pose.inverse(); std::swap(frame_id, child_frame_id); } @@ -588,24 +557,25 @@ void Odometry3DPublisher::publishTimerCallback() trans.header.stamp = odom_output.header.stamp; trans.header.frame_id = frame_id; trans.child_frame_id = child_frame_id; - trans.transform = tf2::toMsg(pose); - if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { - try { - auto base_to_odom = tf_buffer_->lookupTransform( - params_.base_link_frame_id, - params_.odom_frame_id, - trans.header.stamp, - params_.tf_timeout); + trans.transform = tf2::toMsg(pose); + if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) + { + try + { + auto base_to_odom = tf_buffer_->lookupTransform(params_.base_link_frame_id, params_.odom_frame_id, + trans.header.stamp, params_.tf_timeout); geometry_msgs::msg::TransformStamped map_to_odom; tf2::doTransform(base_to_odom, map_to_odom, trans); map_to_odom.child_frame_id = params_.odom_frame_id; trans = map_to_odom; - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Could not lookup the " << params_.base_link_frame_id << "->" - << params_.odom_frame_id << " transform. Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Could not lookup the " << params_.base_link_frame_id << "->" + << params_.odom_frame_id + << " transform. Error: " << e.what()); return; } diff --git a/fuse_models/src/omnidirectional_3d.cpp b/fuse_models/src/omnidirectional_3d.cpp index 92534d25..5def0f0c 100644 --- a/fuse_models/src/omnidirectional_3d.cpp +++ b/fuse_models/src/omnidirectional_3d.cpp @@ -65,25 +65,25 @@ PLUGINLIB_EXPORT_CLASS(fuse_models::Omnidirectional3D, fuse_core::MotionModel) namespace std { -inline bool isfinite(const fuse_core::Vector3d & vector) +inline bool isfinite(const fuse_core::Vector3d& vector) { return std::isfinite(vector.x()) && std::isfinite(vector.y() && std::isfinite(vector.z())); } -inline bool isNormalized(const Eigen::Quaterniond & q) +inline bool isNormalized(const Eigen::Quaterniond& q) { return std::sqrt(q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z()) - 1.0 < Eigen::NumTraits::dummy_precision(); } -std::string to_string(const fuse_core::Vector3d & vector) +std::string to_string(const fuse_core::Vector3d& vector) { std::ostringstream oss; oss << vector; return oss.str(); } -std::string to_string(const Eigen::Quaterniond & quaternion) +std::string to_string(const Eigen::Quaterniond& quaternion) { std::ostringstream oss; oss << quaternion; @@ -94,21 +94,20 @@ std::string to_string(const Eigen::Quaterniond & quaternion) namespace fuse_core { -template -inline void validateCovariance( - const Eigen::DenseBase & covariance, - const double precision = Eigen::NumTraits::dummy_precision()) +template +inline void validateCovariance(const Eigen::DenseBase& covariance, + const double precision = Eigen::NumTraits::dummy_precision()) { - if (!fuse_core::isSymmetric(covariance, precision)) { - throw std::runtime_error( - "Non-symmetric partial covariance matrix\n" + - fuse_core::to_string(covariance, Eigen::FullPrecision)); + if (!fuse_core::isSymmetric(covariance, precision)) + { + throw std::runtime_error("Non-symmetric partial covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); } - if (!fuse_core::isPositiveDefinite(covariance)) { - throw std::runtime_error( - "Non-positive-definite partial covariance matrix\n" + - fuse_core::to_string(covariance, Eigen::FullPrecision)); + if (!fuse_core::isPositiveDefinite(covariance)) + { + throw std::runtime_error("Non-positive-definite partial covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); } } @@ -118,24 +117,26 @@ namespace fuse_models { Omnidirectional3D::Omnidirectional3D() -: fuse_core::AsyncMotionModel(1), // Thread count = 1 for local callback queue - logger_(rclcpp::get_logger("uninitialized")), - buffer_length_(rclcpp::Duration::max()), - device_id_(fuse_core::uuid::NIL), - timestamp_manager_(&Omnidirectional3D::generateMotionModel, this, rclcpp::Duration::max()) + : fuse_core::AsyncMotionModel(1) + , // Thread count = 1 for local callback queue + logger_(rclcpp::get_logger("uninitialized")) + , buffer_length_(rclcpp::Duration::max()) + , device_id_(fuse_core::uuid::NIL) + , timestamp_manager_(&Omnidirectional3D::generateMotionModel, this, rclcpp::Duration::max()) { } -void Omnidirectional3D::print(std::ostream & stream) const +void Omnidirectional3D::print(std::ostream& stream) const { stream << "state history:\n"; - for (const auto & state : state_history_) { + for (const auto& state : state_history_) + { stream << "- stamp: " << state.first.nanoseconds() << "\n"; state.second.print(stream); } } -void Omnidirectional3D::StateHistoryElement::print(std::ostream & stream) const +void Omnidirectional3D::StateHistoryElement::print(std::ostream& stream) const { stream << " position uuid: " << position_uuid << "\n" << " orientation uuid: " << orientation_uuid << "\n" @@ -151,34 +152,41 @@ void Omnidirectional3D::StateHistoryElement::print(std::ostream & stream) const void Omnidirectional3D::StateHistoryElement::validate() const { - if (!std::isfinite(position)) { + if (!std::isfinite(position)) + { throw std::runtime_error("Invalid position " + std::to_string(position)); } - if (!std::isNormalized(orientation)) { + if (!std::isNormalized(orientation)) + { throw std::runtime_error("Invalid orientation " + std::to_string(orientation)); } - if (!std::isfinite(vel_linear)) { + if (!std::isfinite(vel_linear)) + { throw std::runtime_error("Invalid linear velocity " + std::to_string(vel_linear)); } - if (!std::isfinite(vel_angular)) { + if (!std::isfinite(vel_angular)) + { throw std::runtime_error("Invalid angular velocity " + std::to_string(vel_angular)); } - if (!std::isfinite(acc_linear)) { + if (!std::isfinite(acc_linear)) + { throw std::runtime_error("Invalid linear acceleration " + std::to_string(acc_linear)); } } -bool Omnidirectional3D::applyCallback(fuse_core::Transaction & transaction) +bool Omnidirectional3D::applyCallback(fuse_core::Transaction& transaction) { // Use the timestamp manager to generate just the required motion model segments. The timestamp // manager, in turn, makes calls to the generateMotionModel() function. - try { + try + { // Now actually generate the motion model segments - timestamp_manager_.query(transaction, true); - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "An error occurred while completing the motion model query. Error: " << e.what()); + timestamp_manager_.query(transaction, true); + } + catch (const std::exception& e) + { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "An error occurred while completing the motion model query. Error: " << e.what()); return false; } return true; @@ -189,9 +197,8 @@ void Omnidirectional3D::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) updateStateHistoryEstimates(*graph, state_history_, buffer_length_); } -void Omnidirectional3D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) +void Omnidirectional3D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncMotionModel::initialize(interfaces, name); @@ -203,62 +210,36 @@ void Omnidirectional3D::onInit() clock_ = interfaces_.get_node_clock_interface()->get_clock(); std::vector process_noise_diagonal; - process_noise_diagonal = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "process_noise_diagonal"), - process_noise_diagonal); - - if (process_noise_diagonal.size() != 15) { + process_noise_diagonal = fuse_core::getParam( + interfaces_, fuse_core::joinParameterName(name_, "process_noise_diagonal"), process_noise_diagonal); + + if (process_noise_diagonal.size() != 15) + { throw std::runtime_error("Process noise diagonal must be of length 15!"); } process_noise_covariance_ = fuse_core::Vector15d(process_noise_diagonal.data()).asDiagonal(); - scale_process_noise_ = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "scale_process_noise"), - scale_process_noise_); - velocity_linear_norm_min_ = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "velocity_linear_norm_min"), - velocity_linear_norm_min_); - - velocity_angular_norm_min_ = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "velocity_angular_norm_min"), - velocity_angular_norm_min_); + scale_process_noise_ = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "scale_process_noise"), + scale_process_noise_); + velocity_linear_norm_min_ = fuse_core::getParam( + interfaces_, fuse_core::joinParameterName(name_, "velocity_linear_norm_min"), velocity_linear_norm_min_); + + velocity_angular_norm_min_ = fuse_core::getParam( + interfaces_, fuse_core::joinParameterName(name_, "velocity_angular_norm_min"), velocity_angular_norm_min_); disable_checks_ = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "disable_checks"), - disable_checks_); + fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "disable_checks"), disable_checks_); double buffer_length = 3.0; - buffer_length = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "buffer_length"), - buffer_length); - - if (buffer_length < 0.0) { - throw std::runtime_error( - "Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); + buffer_length = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "buffer_length"), buffer_length); + + if (buffer_length < 0.0) + { + throw std::runtime_error("Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); } - buffer_length_ = - (buffer_length == - 0.0) ? rclcpp::Duration::max() : rclcpp::Duration::from_seconds(buffer_length); + buffer_length_ = (buffer_length == 0.0) ? rclcpp::Duration::max() : rclcpp::Duration::from_seconds(buffer_length); timestamp_manager_.bufferLength(buffer_length_); device_id_ = fuse_variables::loadDeviceId(interfaces_); @@ -270,29 +251,27 @@ void Omnidirectional3D::onStart() state_history_.clear(); } -void Omnidirectional3D::generateMotionModel( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables) +void Omnidirectional3D::generateMotionModel(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables) { - assert( - beginning_stamp < ending_stamp || - (beginning_stamp == ending_stamp && state_history_.empty())); + assert(beginning_stamp < ending_stamp || (beginning_stamp == ending_stamp && state_history_.empty())); StateHistoryElement base_state; - rclcpp::Time base_time{0, 0, RCL_ROS_TIME}; + rclcpp::Time base_time{ 0, 0, RCL_ROS_TIME }; // Find an entry that is > beginning_stamp // The entry that is <= will be the one before it auto base_state_pair_it = state_history_.upper_bound(beginning_stamp); - if (base_state_pair_it == state_history_.begin()) { - RCLCPP_WARN_STREAM_EXPRESSION( - logger_, !state_history_.empty(), - "Unable to locate a state in this history with stamp <= " - << beginning_stamp.nanoseconds() << ". Variables will all be initialized to 0."); + if (base_state_pair_it == state_history_.begin()) + { + RCLCPP_WARN_STREAM_EXPRESSION(logger_, !state_history_.empty(), + "Unable to locate a state in this history with stamp <= " + << beginning_stamp.nanoseconds() << ". Variables will all be initialized to 0."); base_time = beginning_stamp; - } else { + } + else + { --base_state_pair_it; base_time = base_state_pair_it->first; base_state = base_state_pair_it->second; @@ -301,76 +280,47 @@ void Omnidirectional3D::generateMotionModel( StateHistoryElement state1; // If the nearest state we had was before the beginning stamp, we need to project that state to // the beginning stamp - if (base_time != beginning_stamp) { - predict( - base_state.position, - base_state.orientation, - base_state.vel_linear, - base_state.vel_angular, - base_state.acc_linear, - (beginning_stamp - base_time).seconds(), - state1.position, - state1.orientation, - state1.vel_linear, - state1.vel_angular, - state1.acc_linear); - } else { + if (base_time != beginning_stamp) + { + predict(base_state.position, base_state.orientation, base_state.vel_linear, base_state.vel_angular, + base_state.acc_linear, (beginning_stamp - base_time).seconds(), state1.position, state1.orientation, + state1.vel_linear, state1.vel_angular, state1.acc_linear); + } + else + { state1 = base_state; } - + // If dt is zero, we only need to update the state history: const double dt = (ending_stamp - beginning_stamp).seconds(); - if (dt == 0.0) { + if (dt == 0.0) + { state1.position_uuid = fuse_variables::Position3DStamped(beginning_stamp, device_id_).uuid(); state1.orientation_uuid = fuse_variables::Orientation3DStamped(beginning_stamp, device_id_).uuid(); - state1.vel_linear_uuid = - fuse_variables::VelocityLinear3DStamped(beginning_stamp, device_id_).uuid(); - state1.vel_angular_uuid = - fuse_variables::VelocityAngular3DStamped(beginning_stamp, device_id_).uuid(); - state1.acc_linear_uuid = - fuse_variables::AccelerationLinear3DStamped(beginning_stamp, device_id_).uuid(); + state1.vel_linear_uuid = fuse_variables::VelocityLinear3DStamped(beginning_stamp, device_id_).uuid(); + state1.vel_angular_uuid = fuse_variables::VelocityAngular3DStamped(beginning_stamp, device_id_).uuid(); + state1.acc_linear_uuid = fuse_variables::AccelerationLinear3DStamped(beginning_stamp, device_id_).uuid(); state_history_.emplace(beginning_stamp, std::move(state1)); return; } // Now predict to get an initial guess for the state at the ending stamp StateHistoryElement state2; - predict( - state1.position, - state1.orientation, - state1.vel_linear, - state1.vel_angular, - state1.acc_linear, - dt, - state2.position, - state2.orientation, - state2.vel_linear, - state2.vel_angular, - state2.acc_linear); - - // Define the fuse variables required for this constraint + predict(state1.position, state1.orientation, state1.vel_linear, state1.vel_angular, state1.acc_linear, dt, + state2.position, state2.orientation, state2.vel_linear, state2.vel_angular, state2.acc_linear); + + // Define the fuse variables required for this constraint auto position1 = fuse_variables::Position3DStamped::make_shared(beginning_stamp, device_id_); auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(beginning_stamp, device_id_); - auto velocity_linear1 = fuse_variables::VelocityLinear3DStamped::make_shared( - beginning_stamp, - device_id_); - auto velocity_angular1 = fuse_variables::VelocityAngular3DStamped::make_shared( - beginning_stamp, - device_id_); - auto acceleration_linear1 = fuse_variables::AccelerationLinear3DStamped::make_shared( - beginning_stamp, device_id_); + auto velocity_linear1 = fuse_variables::VelocityLinear3DStamped::make_shared(beginning_stamp, device_id_); + auto velocity_angular1 = fuse_variables::VelocityAngular3DStamped::make_shared(beginning_stamp, device_id_); + auto acceleration_linear1 = fuse_variables::AccelerationLinear3DStamped::make_shared(beginning_stamp, device_id_); auto position2 = fuse_variables::Position3DStamped::make_shared(ending_stamp, device_id_); auto orientation2 = fuse_variables::Orientation3DStamped::make_shared(ending_stamp, device_id_); - auto velocity_linear2 = fuse_variables::VelocityLinear3DStamped::make_shared( - ending_stamp, - device_id_); - auto velocity_angular2 = fuse_variables::VelocityAngular3DStamped::make_shared( - ending_stamp, - device_id_); - auto acceleration_linear2 = fuse_variables::AccelerationLinear3DStamped::make_shared( - ending_stamp, - device_id_); + auto velocity_linear2 = fuse_variables::VelocityLinear3DStamped::make_shared(ending_stamp, device_id_); + auto velocity_angular2 = fuse_variables::VelocityAngular3DStamped::make_shared(ending_stamp, device_id_); + auto acceleration_linear2 = fuse_variables::AccelerationLinear3DStamped::make_shared(ending_stamp, device_id_); position1->data()[fuse_variables::Position3DStamped::X] = state1.position.x(); position1->data()[fuse_variables::Position3DStamped::Y] = state1.position.y(); @@ -380,22 +330,19 @@ void Omnidirectional3D::generateMotionModel( orientation1->data()[fuse_variables::Orientation3DStamped::Y] = state1.orientation.y(); orientation1->data()[fuse_variables::Orientation3DStamped::Z] = state1.orientation.z(); orientation1->data()[fuse_variables::Orientation3DStamped::W] = state1.orientation.w(); - + velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::X] = state1.vel_linear.x(); velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::Y] = state1.vel_linear.y(); velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::Z] = state1.vel_linear.z(); - velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state1.vel_angular.x(); + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state1.vel_angular.x(); velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::PITCH] = state1.vel_angular.y(); - velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state1.vel_angular.z(); - - acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::X] = - state1.acc_linear.x(); - acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::Y] = - state1.acc_linear.y(); - acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::Z] = - state1.acc_linear.z(); - + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state1.vel_angular.z(); + + acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::X] = state1.acc_linear.x(); + acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::Y] = state1.acc_linear.y(); + acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::Z] = state1.acc_linear.z(); + position2->data()[fuse_variables::Position3DStamped::X] = state2.position.x(); position2->data()[fuse_variables::Position3DStamped::Y] = state2.position.y(); position2->data()[fuse_variables::Position3DStamped::Z] = state2.position.z(); @@ -404,21 +351,18 @@ void Omnidirectional3D::generateMotionModel( orientation2->data()[fuse_variables::Orientation3DStamped::Y] = state2.orientation.y(); orientation2->data()[fuse_variables::Orientation3DStamped::Z] = state2.orientation.z(); orientation2->data()[fuse_variables::Orientation3DStamped::W] = state2.orientation.w(); - + velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::X] = state2.vel_linear.x(); velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::Y] = state2.vel_linear.y(); velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::Z] = state2.vel_linear.z(); - velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state2.vel_angular.x(); + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state2.vel_angular.x(); velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::PITCH] = state2.vel_angular.y(); - velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state2.vel_angular.z(); + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state2.vel_angular.z(); - acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::X] = - state2.acc_linear.x(); - acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::Y] = - state2.acc_linear.y(); - acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::Z] = - state2.acc_linear.z(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::X] = state2.acc_linear.x(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::Y] = state2.acc_linear.y(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::Z] = state2.acc_linear.z(); state1.position_uuid = position1->uuid(); state1.orientation_uuid = orientation1->uuid(); @@ -437,40 +381,33 @@ void Omnidirectional3D::generateMotionModel( // Scale process noise covariance pose by the norm of the current state twist auto process_noise_covariance = process_noise_covariance_; - if (scale_process_noise_) { - common::scaleProcessNoiseCovariance( - process_noise_covariance, state1.vel_linear, state1.vel_angular, - velocity_linear_norm_min_, velocity_angular_norm_min_); + if (scale_process_noise_) + { + common::scaleProcessNoiseCovariance(process_noise_covariance, state1.vel_linear, state1.vel_angular, + velocity_linear_norm_min_, velocity_angular_norm_min_); } // Validate process_noise_covariance *= dt; - if (!disable_checks_) { - try { + if (!disable_checks_) + { + try + { validateMotionModel(state1, state2, process_noise_covariance); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Invalid '" << name_ << "' motion model: " << ex.what()); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Invalid '" << name_ << "' motion model: " << ex.what()); return; } } // Create the constraints for this motion model segment auto constraint = fuse_models::Omnidirectional3DStateKinematicConstraint::make_shared( - name(), - *position1, - *orientation1, - *velocity_linear1, - *velocity_angular1, - *acceleration_linear1, - *position2, - *orientation2, - *velocity_linear2, - *velocity_angular2, - *acceleration_linear2, - process_noise_covariance); + name(), *position1, *orientation1, *velocity_linear1, *velocity_angular1, *acceleration_linear1, *position2, + *orientation2, *velocity_linear2, *velocity_angular2, *acceleration_linear2, process_noise_covariance); // Update the output variables constraints.push_back(constraint); @@ -486,22 +423,24 @@ void Omnidirectional3D::generateMotionModel( variables.push_back(acceleration_linear2); } -void Omnidirectional3D::updateStateHistoryEstimates( - const fuse_core::Graph & graph, - StateHistory & state_history, - const rclcpp::Duration & buffer_length) +void Omnidirectional3D::updateStateHistoryEstimates(const fuse_core::Graph& graph, StateHistory& state_history, + const rclcpp::Duration& buffer_length) { - if (state_history.empty()) { + if (state_history.empty()) + { return; } // Compute the expiration time carefully, as ROS can't handle negative times - const auto & ending_stamp = state_history.rbegin()->first; + const auto& ending_stamp = state_history.rbegin()->first; rclcpp::Time expiration_time; - if (ending_stamp.seconds() > buffer_length.seconds()) { + if (ending_stamp.seconds() > buffer_length.seconds()) + { expiration_time = ending_stamp - buffer_length; - } else { + } + else + { // NOTE(CH3): Uninitialized. But okay because it's just used for comparison. expiration_time = rclcpp::Time(0, 0, ending_stamp.get_clock_type()); } @@ -511,7 +450,8 @@ void Omnidirectional3D::updateStateHistoryEstimates( // - at least one entry remains at all times // - the history covers *at least* until the expiration time. Longer is acceptable. auto expiration_iter = state_history.upper_bound(expiration_time); - if (expiration_iter != state_history.begin()) { + if (expiration_iter != state_history.begin()) + { // expiration_iter points to the first element > expiration_time. // Back up one entry, to a point that is <= expiration_time state_history.erase(state_history.begin(), std::prev(expiration_iter)); @@ -520,23 +460,20 @@ void Omnidirectional3D::updateStateHistoryEstimates( // Update the states in the state history with information from the graph // If a state is not in the graph yet, predict the state in question from the closest previous // state - for (auto current_iter = state_history.begin(); current_iter != state_history.end(); - ++current_iter) + for (auto current_iter = state_history.begin(); current_iter != state_history.end(); ++current_iter) { - const auto & current_stamp = current_iter->first; - auto & current_state = current_iter->second; - if (graph.variableExists(current_state.position_uuid) && - graph.variableExists(current_state.orientation_uuid) && - graph.variableExists(current_state.vel_linear_uuid) && - graph.variableExists(current_state.vel_angular_uuid) && - graph.variableExists(current_state.acc_linear_uuid)) + const auto& current_stamp = current_iter->first; + auto& current_state = current_iter->second; + if (graph.variableExists(current_state.position_uuid) && graph.variableExists(current_state.orientation_uuid) && + graph.variableExists(current_state.vel_linear_uuid) && graph.variableExists(current_state.vel_angular_uuid) && + graph.variableExists(current_state.acc_linear_uuid)) { // This pose does exist in the graph. Update it directly. - const auto & position = graph.getVariable(current_state.position_uuid); - const auto & orientation = graph.getVariable(current_state.orientation_uuid); - const auto & vel_linear = graph.getVariable(current_state.vel_linear_uuid); - const auto & vel_angular = graph.getVariable(current_state.vel_angular_uuid); - const auto & acc_linear = graph.getVariable(current_state.acc_linear_uuid); + const auto& position = graph.getVariable(current_state.position_uuid); + const auto& orientation = graph.getVariable(current_state.orientation_uuid); + const auto& vel_linear = graph.getVariable(current_state.vel_linear_uuid); + const auto& vel_angular = graph.getVariable(current_state.vel_angular_uuid); + const auto& acc_linear = graph.getVariable(current_state.acc_linear_uuid); current_state.position.x() = position.data()[fuse_variables::Position3DStamped::X]; current_state.position.y() = position.data()[fuse_variables::Position3DStamped::Y]; @@ -547,74 +484,67 @@ void Omnidirectional3D::updateStateHistoryEstimates( current_state.orientation.z() = orientation.data()[fuse_variables::Orientation3DStamped::Z]; current_state.orientation.w() = orientation.data()[fuse_variables::Orientation3DStamped::W]; - current_state.vel_linear.x() = - vel_linear.data()[fuse_variables::VelocityLinear3DStamped::X]; - current_state.vel_linear.y() = - vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Y]; - current_state.vel_linear.z() = - vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Z]; - - current_state.vel_angular.x() = - vel_angular.data()[fuse_variables::VelocityAngular3DStamped::ROLL]; - current_state.vel_angular.y() = - vel_angular.data()[fuse_variables::VelocityAngular3DStamped::PITCH]; - current_state.vel_angular.z() = - vel_angular.data()[fuse_variables::VelocityAngular3DStamped::YAW]; - - current_state.acc_linear.x() = - acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::X]; - current_state.acc_linear.y() = - acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::Y]; - current_state.acc_linear.z() = - acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::Z]; - } else if (current_iter != state_history.begin()) { + current_state.vel_linear.x() = vel_linear.data()[fuse_variables::VelocityLinear3DStamped::X]; + current_state.vel_linear.y() = vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Y]; + current_state.vel_linear.z() = vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Z]; + + current_state.vel_angular.x() = vel_angular.data()[fuse_variables::VelocityAngular3DStamped::ROLL]; + current_state.vel_angular.y() = vel_angular.data()[fuse_variables::VelocityAngular3DStamped::PITCH]; + current_state.vel_angular.z() = vel_angular.data()[fuse_variables::VelocityAngular3DStamped::YAW]; + + current_state.acc_linear.x() = acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::X]; + current_state.acc_linear.y() = acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::Y]; + current_state.acc_linear.z() = acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::Z]; + } + else if (current_iter != state_history.begin()) + { auto previous_iter = std::prev(current_iter); - const auto & previous_stamp = previous_iter->first; - const auto & previous_state = previous_iter->second; - + const auto& previous_stamp = previous_iter->first; + const auto& previous_state = previous_iter->second; + // This state is not in the graph yet, so we can't update/correct the value in our state // history. However, the state *before* this one may have been corrected (or one of its // predecessors may have been), so we can use that corrected value, along with our prediction // logic, to provide a more accurate update to this state. - predict( - previous_state.position, - previous_state.orientation, - previous_state.vel_linear, - previous_state.vel_angular, - previous_state.acc_linear, - (current_stamp - previous_stamp).seconds(), - current_state.position, - current_state.orientation, - current_state.vel_linear, - current_state.vel_angular, - current_state.acc_linear); + predict(previous_state.position, previous_state.orientation, previous_state.vel_linear, + previous_state.vel_angular, previous_state.acc_linear, (current_stamp - previous_stamp).seconds(), + current_state.position, current_state.orientation, current_state.vel_linear, current_state.vel_angular, + current_state.acc_linear); } } } -void Omnidirectional3D::validateMotionModel( - const StateHistoryElement & state1, const StateHistoryElement & state2, - const fuse_core::Matrix15d & process_noise_covariance) +void Omnidirectional3D::validateMotionModel(const StateHistoryElement& state1, const StateHistoryElement& state2, + const fuse_core::Matrix15d& process_noise_covariance) { - try { + try + { state1.validate(); - } catch (const std::runtime_error & ex) { + } + catch (const std::runtime_error& ex) + { throw std::runtime_error("Invalid state #1: " + std::string(ex.what())); } - try { + try + { state2.validate(); - } catch (const std::runtime_error & ex) { + } + catch (const std::runtime_error& ex) + { throw std::runtime_error("Invalid state #2: " + std::string(ex.what())); } - try { + try + { fuse_core::validateCovariance(process_noise_covariance); - } catch (const std::runtime_error & ex) { + } + catch (const std::runtime_error& ex) + { throw std::runtime_error("Invalid process noise covariance: " + std::string(ex.what())); } } -std::ostream & operator<<(std::ostream & stream, const Omnidirectional3D & omnidirectional_3d) +std::ostream& operator<<(std::ostream& stream, const Omnidirectional3D& omnidirectional_3d) { omnidirectional_3d.print(stream); return stream; diff --git a/fuse_models/src/omnidirectional_3d_ignition.cpp b/fuse_models/src/omnidirectional_3d_ignition.cpp index d545007d..55c64a60 100644 --- a/fuse_models/src/omnidirectional_3d_ignition.cpp +++ b/fuse_models/src/omnidirectional_3d_ignition.cpp @@ -67,18 +67,17 @@ namespace fuse_models { Omnidirectional3DIgnition::Omnidirectional3DIgnition() -: fuse_core::AsyncSensorModel(1), - started_(false), - initial_transaction_sent_(false), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")) + : fuse_core::AsyncSensorModel(1) + , started_(false) + , initial_transaction_sent_(false) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) { } void Omnidirectional3DIgnition::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) + fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string& name, + fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -95,52 +94,32 @@ void Omnidirectional3DIgnition::onInit() params_.loadFromROS(interfaces_, name_); // Connect to the reset service - if (!params_.reset_service.empty()) { + if (!params_.reset_service.empty()) + { reset_client_ = rclcpp::create_client( - interfaces_.get_node_base_interface(), - interfaces_.get_node_graph_interface(), - interfaces_.get_node_services_interface(), - params_.reset_service, - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_graph_interface(), + interfaces_.get_node_services_interface(), params_.reset_service, rclcpp::ServicesQoS(), cb_group_); } // Advertise rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind(&Omnidirectional3DIgnition::subscriberCallback, this, std::placeholders::_1), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&Omnidirectional3DIgnition::subscriberCallback, this, std::placeholders::_1), sub_options); set_pose_service_ = rclcpp::create_service( - interfaces_.get_node_base_interface(), - interfaces_.get_node_services_interface(), - fuse_core::joinTopicName( - interfaces_.get_node_base_interface()->get_name(), - params_.set_pose_service), - std::bind( - &Omnidirectional3DIgnition::setPoseServiceCallback, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3), - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_pose_service), + std::bind(&Omnidirectional3DIgnition::setPoseServiceCallback, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), + rclcpp::ServicesQoS(), cb_group_); set_pose_deprecated_service_ = rclcpp::create_service( - interfaces_.get_node_base_interface(), - interfaces_.get_node_services_interface(), - fuse_core::joinTopicName( - interfaces_.get_node_base_interface()->get_name(), - params_.set_pose_deprecated_service), - std::bind( - &Omnidirectional3DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3), - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_pose_deprecated_service), + std::bind(&Omnidirectional3DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3), + rclcpp::ServicesQoS(), cb_group_); } void Omnidirectional3DIgnition::start() @@ -150,7 +129,8 @@ void Omnidirectional3DIgnition::start() // TODO(swilliams) Should this be executed every time optimizer.reset() is called, or only once // ever? I feel like it should be "only once ever". Send an initial state // transaction immediately, if requested - if (params_.publish_on_startup && !initial_transaction_sent_) { + if (params_.publish_on_startup && !initial_transaction_sent_) + { auto pose = geometry_msgs::msg::PoseWithCovarianceStamped(); tf2::Quaternion q; // q.setRPY(params_.initial_state[3], params_.initial_state[4], params_.initial_state[5]); @@ -163,7 +143,8 @@ void Omnidirectional3DIgnition::start() pose.pose.pose.orientation.y = q.y(); pose.pose.pose.orientation.z = q.z(); pose.pose.pose.orientation.w = q.w(); - for (size_t i = 0; i < 6; i++) { + for (size_t i = 0; i < 6; i++) + { pose.pose.covariance[i * 7] = params_.initial_sigma[i] * params_.initial_sigma[i]; } sendPrior(pose); @@ -176,30 +157,32 @@ void Omnidirectional3DIgnition::stop() started_ = false; } -void Omnidirectional3DIgnition::subscriberCallback( - const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +void Omnidirectional3DIgnition::subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) { - try { + try + { process(msg); - } catch (const std::exception & e) { + } + catch (const std::exception& e) + { RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring message."); } } -bool Omnidirectional3DIgnition::setPoseServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPose::Request::SharedPtr req) +bool Omnidirectional3DIgnition::setPoseServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetPose::Request::SharedPtr req) { - try { - process( - req->pose, - [service, request_id]() { - fuse_msgs::srv::SetPose::Response response; - response.success = true; - service->send_response(*request_id, response); - }); - } catch (const std::exception & e) { + try + { + process(req->pose, [service, request_id]() { + fuse_msgs::srv::SetPose::Response response; + response.success = true; + service->send_response(*request_id, response); + }); + } + catch (const std::exception& e) + { fuse_msgs::srv::SetPose::Response response; response.success = false; response.message = e.what(); @@ -210,18 +193,18 @@ bool Omnidirectional3DIgnition::setPoseServiceCallback( } bool Omnidirectional3DIgnition::setPoseDeprecatedServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req) + rclcpp::Service::SharedPtr service, std::shared_ptr request_id, + const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req) { - try { - process( - req->pose, - [service, request_id]() { - fuse_msgs::srv::SetPoseDeprecated::Response response; - service->send_response(*request_id, response); - }); - } catch (const std::exception & e) { + try + { + process(req->pose, [service, request_id]() { + fuse_msgs::srv::SetPoseDeprecated::Response response; + service->send_response(*request_id, response); + }); + } + catch (const std::exception& e) + { fuse_msgs::srv::SetPoseDeprecated::Response response; RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring request."); service->send_response(*request_id, response); @@ -229,33 +212,32 @@ bool Omnidirectional3DIgnition::setPoseDeprecatedServiceCallback( return true; } -void Omnidirectional3DIgnition::process( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, std::function post_process) +void Omnidirectional3DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + std::function post_process) { // Verify we are in the correct state to process set pose requests - if (!started_) { + if (!started_) + { throw std::runtime_error("Attempting to set the pose while the sensor is stopped."); } // Validate the requested pose and covariance before we do anything - if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y) || !std::isfinite(pose.pose.pose.position.z)) { + if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y) || + !std::isfinite(pose.pose.pose.position.z)) + { throw std::invalid_argument( - "Attempting to set the pose to an invalid position (" + - std::to_string(pose.pose.pose.position.x) + ", " + - std::to_string(pose.pose.pose.position.y) + ", " + - std::to_string(pose.pose.pose.position.z) + ")."); + "Attempting to set the pose to an invalid position (" + std::to_string(pose.pose.pose.position.x) + ", " + + std::to_string(pose.pose.pose.position.y) + ", " + std::to_string(pose.pose.pose.position.z) + ")."); } - auto orientation_norm = std::sqrt( - pose.pose.pose.orientation.x * pose.pose.pose.orientation.x + - pose.pose.pose.orientation.y * pose.pose.pose.orientation.y + - pose.pose.pose.orientation.z * pose.pose.pose.orientation.z + - pose.pose.pose.orientation.w * pose.pose.pose.orientation.w); - if (std::abs(orientation_norm - 1.0) > 1.0e-3) { + auto orientation_norm = std::sqrt(pose.pose.pose.orientation.x * pose.pose.pose.orientation.x + + pose.pose.pose.orientation.y * pose.pose.pose.orientation.y + + pose.pose.pose.orientation.z * pose.pose.pose.orientation.z + + pose.pose.pose.orientation.w * pose.pose.pose.orientation.w); + if (std::abs(orientation_norm - 1.0) > 1.0e-3) + { throw std::invalid_argument( - "Attempting to set the pose to an invalid orientation (" + - std::to_string(pose.pose.pose.orientation.x) + ", " + - std::to_string(pose.pose.pose.orientation.y) + ", " + - std::to_string(pose.pose.pose.orientation.z) + ", " + - std::to_string(pose.pose.pose.orientation.w) + ")."); + "Attempting to set the pose to an invalid orientation (" + std::to_string(pose.pose.pose.orientation.x) + ", " + + std::to_string(pose.pose.pose.orientation.y) + ", " + std::to_string(pose.pose.pose.orientation.z) + ", " + + std::to_string(pose.pose.pose.orientation.w) + ")."); } auto position_cov = fuse_core::Matrix3d(); // for (size_t i = 0; i < 3; i++) { @@ -263,18 +245,18 @@ void Omnidirectional3DIgnition::process( // position_cov(i, j) = pose.pose.covariance[i * 6 + j]; // } // } - position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[2], - pose.pose.covariance[6], pose.pose.covariance[7], pose.pose.covariance[8], - pose.pose.covariance[12], pose.pose.covariance[13], pose.pose.covariance[14]; - if (!fuse_core::isSymmetric(position_cov)) { - throw std::invalid_argument( - "Attempting to set the pose with a non-symmetric position covariance matrix\n " + - fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[2], pose.pose.covariance[6], + pose.pose.covariance[7], pose.pose.covariance[8], pose.pose.covariance[12], pose.pose.covariance[13], + pose.pose.covariance[14]; + if (!fuse_core::isSymmetric(position_cov)) + { + throw std::invalid_argument("Attempting to set the pose with a non-symmetric position covariance matrix\n " + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); } - if (!fuse_core::isPositiveDefinite(position_cov)) { - throw std::invalid_argument( - "Attempting to set the pose with a non-positive-definite position covariance matrix\n" + - fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + if (!fuse_core::isPositiveDefinite(position_cov)) + { + throw std::invalid_argument("Attempting to set the pose with a non-positive-definite position covariance matrix\n" + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); } auto orientation_cov = fuse_core::Matrix3d(); // for (size_t i = 0; i < 3; i++) { @@ -283,27 +265,28 @@ void Omnidirectional3DIgnition::process( // } // } orientation_cov << pose.pose.covariance[21], pose.pose.covariance[22], pose.pose.covariance[23], - pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], - pose.pose.covariance[33], pose.pose.covariance[34], pose.pose.covariance[35]; - if (!fuse_core::isSymmetric(orientation_cov)) { - throw std::invalid_argument( - "Attempting to set the pose with a non-symmetric orientation covariance matrix\n " + - fuse_core::to_string(orientation_cov, Eigen::FullPrecision) + "."); + pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], pose.pose.covariance[33], + pose.pose.covariance[34], pose.pose.covariance[35]; + if (!fuse_core::isSymmetric(orientation_cov)) + { + throw std::invalid_argument("Attempting to set the pose with a non-symmetric orientation covariance matrix\n " + + fuse_core::to_string(orientation_cov, Eigen::FullPrecision) + "."); } - if (!fuse_core::isPositiveDefinite(orientation_cov)) { + if (!fuse_core::isPositiveDefinite(orientation_cov)) + { throw std::invalid_argument( - "Attempting to set the pose with a non-positive-definite orientation_cov covariance matrix\n" + - fuse_core::to_string(orientation_cov, Eigen::FullPrecision) + "."); + "Attempting to set the pose with a non-positive-definite orientation_cov covariance matrix\n" + + fuse_core::to_string(orientation_cov, Eigen::FullPrecision) + "."); } // Tell the optimizer to reset before providing the initial state - if (!params_.reset_service.empty()) { + if (!params_.reset_service.empty()) + { // Wait for the reset service while (!reset_client_->wait_for_service(std::chrono::seconds(10)) && - interfaces_.get_node_base_interface()->get_context()->is_valid()) + interfaces_.get_node_base_interface()->get_context()->is_valid()) { - RCLCPP_WARN_STREAM( - logger_, - "Waiting for '" << reset_client_->get_service_name() << "' service to become avaiable."); + RCLCPP_WARN_STREAM(logger_, + "Waiting for '" << reset_client_->get_service_name() << "' service to become avaiable."); } auto srv = std::make_shared(); @@ -311,27 +294,30 @@ void Omnidirectional3DIgnition::process( // It needs to be free to handle the response to this service call. // Have a callback do the rest of the work when a response comes. auto result_future = reset_client_->async_send_request( - srv, - [this, post_process, pose](rclcpp::Client::SharedFuture result) { - (void)result; - // Now that the pose has been validated and the optimizer has been reset, actually send the - // initial state constraints to the optimizer - sendPrior(pose); - if (post_process) { - post_process(); - } - }); - } else { + srv, [this, post_process, pose](rclcpp::Client::SharedFuture result) { + (void)result; + // Now that the pose has been validated and the optimizer has been reset, actually send the + // initial state constraints to the optimizer + sendPrior(pose); + if (post_process) + { + post_process(); + } + }); + } + else + { sendPrior(pose); - if (post_process) { + if (post_process) + { post_process(); } } } -void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped& pose) { - const auto & stamp = pose.header.stamp; + const auto& stamp = pose.header.stamp; // Create variables for the full state. // The initial values of the pose are extracted from the provided PoseWithCovarianceStamped @@ -379,55 +365,42 @@ void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCova // } // } - position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[2], - pose.pose.covariance[6], pose.pose.covariance[7], pose.pose.covariance[8], - pose.pose.covariance[12], pose.pose.covariance[13], pose.pose.covariance[14]; + position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[2], pose.pose.covariance[6], + pose.pose.covariance[7], pose.pose.covariance[8], pose.pose.covariance[12], pose.pose.covariance[13], + pose.pose.covariance[14]; orientation_cov << pose.pose.covariance[21], pose.pose.covariance[22], pose.pose.covariance[23], - pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], - pose.pose.covariance[33], pose.pose.covariance[34], pose.pose.covariance[35]; - - linear_velocity_cov << params_.initial_sigma[6] * params_.initial_sigma[6], 0.0, 0.0, - 0.0, params_.initial_sigma[7] * params_.initial_sigma[7], 0.0, - 0.0, 0.0, params_.initial_sigma[8] * params_.initial_sigma[8]; - - angular_velocity_cov << params_.initial_sigma[9] * params_.initial_sigma[9], 0.0, 0.0, - 0.0, params_.initial_sigma[10] * params_.initial_sigma[10], 0.0, - 0.0, 0.0, params_.initial_sigma[11] * params_.initial_sigma[11]; - - linear_acceleration_cov << params_.initial_sigma[12] * params_.initial_sigma[12], 0.0, 0.0, - 0.0, params_.initial_sigma[13] * params_.initial_sigma[13], 0.0, - 0.0, 0.0, params_.initial_sigma[14] * params_.initial_sigma[14]; + pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], pose.pose.covariance[33], + pose.pose.covariance[34], pose.pose.covariance[35]; + + linear_velocity_cov << params_.initial_sigma[6] * params_.initial_sigma[6], 0.0, 0.0, 0.0, + params_.initial_sigma[7] * params_.initial_sigma[7], 0.0, 0.0, 0.0, + params_.initial_sigma[8] * params_.initial_sigma[8]; + + angular_velocity_cov << params_.initial_sigma[9] * params_.initial_sigma[9], 0.0, 0.0, 0.0, + params_.initial_sigma[10] * params_.initial_sigma[10], 0.0, 0.0, 0.0, + params_.initial_sigma[11] * params_.initial_sigma[11]; + + linear_acceleration_cov << params_.initial_sigma[12] * params_.initial_sigma[12], 0.0, 0.0, 0.0, + params_.initial_sigma[13] * params_.initial_sigma[13], 0.0, 0.0, 0.0, + params_.initial_sigma[14] * params_.initial_sigma[14]; // Create absolute constraints for each variable auto position_constraint = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( - name(), - *position, - fuse_core::Vector3d(position->x(), position->y(), position->z()), - position_cov); - auto orientation_constraint = - fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( - name(), - *orientation, - Eigen::Quaterniond(orientation->w(), orientation->x(), orientation->y(), orientation->z()), - orientation_cov); - auto linear_velocity_constraint = - fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint::make_shared( - name(), - *linear_velocity, - fuse_core::Vector3d(linear_velocity->x(), linear_velocity->y(), linear_velocity->z()), - linear_velocity_cov); - auto angular_velocity_constraint = - fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint::make_shared( - name(), - *angular_velocity, - fuse_core::Vector3d(angular_velocity->roll(), angular_velocity->pitch(), angular_velocity->yaw()), - angular_velocity_cov); - auto linear_acceleration_constraint = - fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint::make_shared( - name(), - *linear_acceleration, - fuse_core::Vector3d(linear_acceleration->x(), linear_acceleration->y(), linear_acceleration->z()), - linear_acceleration_cov); + name(), *position, fuse_core::Vector3d(position->x(), position->y(), position->z()), position_cov); + auto orientation_constraint = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( + name(), *orientation, Eigen::Quaterniond(orientation->w(), orientation->x(), orientation->y(), orientation->z()), + orientation_cov); + auto linear_velocity_constraint = fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint::make_shared( + name(), *linear_velocity, fuse_core::Vector3d(linear_velocity->x(), linear_velocity->y(), linear_velocity->z()), + linear_velocity_cov); + auto angular_velocity_constraint = fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint::make_shared( + name(), *angular_velocity, + fuse_core::Vector3d(angular_velocity->roll(), angular_velocity->pitch(), angular_velocity->yaw()), + angular_velocity_cov); + auto linear_acceleration_constraint = fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint::make_shared( + name(), *linear_acceleration, + fuse_core::Vector3d(linear_acceleration->x(), linear_acceleration->y(), linear_acceleration->z()), + linear_acceleration_cov); // Create the transaction auto transaction = fuse_core::Transaction::make_shared(); @@ -447,13 +420,9 @@ void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCova // Send the transaction to the optimizer. sendTransaction(transaction); - RCLCPP_INFO_STREAM( - logger_, - "Received a set_pose request (stamp: " << rclcpp::Time(stamp).nanoseconds() - << ", x: " << position->x() << ", y: " - << position->y() << ", z: " << position->z() - << ", roll: " << orientation->roll() - << ", pitch: " << orientation->pitch() - << ", yaw: " << orientation->yaw() << ")"); + RCLCPP_INFO_STREAM(logger_, "Received a set_pose request (stamp: " + << rclcpp::Time(stamp).nanoseconds() << ", x: " << position->x() << ", y: " + << position->y() << ", z: " << position->z() << ", roll: " << orientation->roll() + << ", pitch: " << orientation->pitch() << ", yaw: " << orientation->yaw() << ")"); } } // namespace fuse_models diff --git a/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp index 7caf9dc3..7b222f76 100644 --- a/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp +++ b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp @@ -49,36 +49,26 @@ namespace fuse_models { Omnidirectional3DStateKinematicConstraint::Omnidirectional3DStateKinematicConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::VelocityLinear3DStamped & velocity_linear1, - const fuse_variables::VelocityAngular3DStamped & velocity_angular1, - const fuse_variables::AccelerationLinear3DStamped & acceleration_linear1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_variables::VelocityLinear3DStamped & velocity_linear2, - const fuse_variables::VelocityAngular3DStamped & velocity_angular2, - const fuse_variables::AccelerationLinear3DStamped & acceleration_linear2, - const fuse_core::Matrix15d & covariance) -: fuse_core::Constraint( - source, - {position1.uuid(), - orientation1.uuid(), - velocity_linear1.uuid(), - velocity_angular1.uuid(), - acceleration_linear1.uuid(), - position2.uuid(), - orientation2.uuid(), - velocity_linear2.uuid(), - velocity_angular2.uuid(), - acceleration_linear2.uuid()}), // NOLINT - dt_((position2.stamp() - position1.stamp()).seconds()), - sqrt_information_(covariance.inverse().llt().matrixU()) + const std::string& source, const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::VelocityLinear3DStamped& velocity_linear1, + const fuse_variables::VelocityAngular3DStamped& velocity_angular1, + const fuse_variables::AccelerationLinear3DStamped& acceleration_linear1, + const fuse_variables::Position3DStamped& position2, const fuse_variables::Orientation3DStamped& orientation2, + const fuse_variables::VelocityLinear3DStamped& velocity_linear2, + const fuse_variables::VelocityAngular3DStamped& velocity_angular2, + const fuse_variables::AccelerationLinear3DStamped& acceleration_linear2, const fuse_core::Matrix15d& covariance) + : fuse_core::Constraint(source, + { position1.uuid(), orientation1.uuid(), velocity_linear1.uuid(), velocity_angular1.uuid(), + acceleration_linear1.uuid(), position2.uuid(), orientation2.uuid(), velocity_linear2.uuid(), + velocity_angular2.uuid(), acceleration_linear2.uuid() }) + , // NOLINT + dt_((position2.stamp() - position1.stamp()).seconds()) + , sqrt_information_(covariance.inverse().llt().matrixU()) { } -void Omnidirectional3DStateKinematicConstraint::print(std::ostream & stream) const +void Omnidirectional3DStateKinematicConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -97,7 +87,7 @@ void Omnidirectional3DStateKinematicConstraint::print(std::ostream & stream) con << " sqrt_info: " << sqrtInformation() << "\n"; } -ceres::CostFunction * Omnidirectional3DStateKinematicConstraint::costFunction() const +ceres::CostFunction* Omnidirectional3DStateKinematicConstraint::costFunction() const { return new Omnidirectional3DStateCostFunction(dt_, sqrt_information_); // Here we return a cost function that computes the analytic derivatives/jacobians, but we could diff --git a/fuse_models/test/test_omnidirectional_3d.cpp b/fuse_models/test/test_omnidirectional_3d.cpp index f82bb75a..01fbf7f6 100644 --- a/fuse_models/test/test_omnidirectional_3d.cpp +++ b/fuse_models/test/test_omnidirectional_3d.cpp @@ -21,9 +21,9 @@ class Omnidirectional3DModelTest : public fuse_models::Omnidirectional3D { public: - using fuse_models::Omnidirectional3D::updateStateHistoryEstimates; - using fuse_models::Omnidirectional3D::StateHistoryElement; using fuse_models::Omnidirectional3D::StateHistory; + using fuse_models::Omnidirectional3D::StateHistoryElement; + using fuse_models::Omnidirectional3D::updateStateHistoryEstimates; }; TEST(Omnidirectional3D, UpdateStateHistoryEstimates) @@ -33,8 +33,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(1, 0)); auto linear_velocity1 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(1, 0)); auto angular_velocity1 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(1, 0)); - auto linear_acceleration1 = - fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(1, 0)); + auto linear_acceleration1 = fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(1, 0)); position1->x() = 1.1; position1->y() = 2.1; position1->z() = 0.0; @@ -55,8 +54,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto orientation2 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(2, 0)); auto linear_velocity2 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(2, 0)); auto angular_velocity2 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(2, 0)); - auto linear_acceleration2 = - fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(2, 0)); + auto linear_acceleration2 = fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(2, 0)); position2->x() = 1.2; position2->y() = 2.2; position2->z() = 0.0; @@ -77,8 +75,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto orientation3 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(3, 0)); auto linear_velocity3 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(3, 0)); auto angular_velocity3 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(3, 0)); - auto linear_acceleration3 = - fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(3, 0)); + auto linear_acceleration3 = fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(3, 0)); position3->x() = 1.3; position3->y() = 2.3; position3->z() = 0.0; @@ -99,8 +96,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto orientation4 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(4, 0)); auto linear_velocity4 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(4, 0)); auto angular_velocity4 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(4, 0)); - auto linear_acceleration4 = - fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(4, 0)); + auto linear_acceleration4 = fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(4, 0)); position4->x() = 1.4; position4->y() = 2.4; position4->z() = 0.0; @@ -121,8 +117,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto orientation5 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(5, 0)); auto linear_velocity5 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(5, 0)); auto angular_velocity5 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(5, 0)); - auto linear_acceleration5 = - fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(5, 0)); + auto linear_acceleration5 = fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(5, 0)); position5->x() = 1.5; position5->y() = 2.5; position5->z() = 0.0; @@ -156,76 +151,49 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) // Add all of the variables to the state history Omnidirectional3DModelTest::StateHistory state_history; - state_history.emplace( - position1->stamp(), - Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position1->uuid(), - orientation1->uuid(), - linear_velocity1->uuid(), - angular_velocity1->uuid(), - linear_acceleration1->uuid(), - fuse_core::Vector3d(1.0, 0.0, 0.0), - Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position2->stamp(), - Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position2->uuid(), - orientation2->uuid(), - linear_velocity2->uuid(), - angular_velocity2->uuid(), - linear_acceleration2->uuid(), - fuse_core::Vector3d(2.0, 0.0, 0.0), - Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position3->stamp(), - Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position3->uuid(), - orientation3->uuid(), - linear_velocity3->uuid(), - angular_velocity3->uuid(), - linear_acceleration3->uuid(), - fuse_core::Vector3d(3.0, 0.0, 0.0), - Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position4->stamp(), - Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position4->uuid(), - orientation4->uuid(), - linear_velocity4->uuid(), - angular_velocity4->uuid(), - linear_acceleration4->uuid(), - fuse_core::Vector3d(4.0, 0.0, 0.0), - Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position5->stamp(), - Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position5->uuid(), - orientation5->uuid(), - linear_velocity5->uuid(), - angular_velocity5->uuid(), - linear_acceleration5->uuid(), - fuse_core::Vector3d(5.0, 0.0, 0.0), - Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace(position1->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position1->uuid(), orientation1->uuid(), linear_velocity1->uuid(), + angular_velocity1->uuid(), linear_acceleration1->uuid(), fuse_core::Vector3d(1.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position2->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position2->uuid(), orientation2->uuid(), linear_velocity2->uuid(), + angular_velocity2->uuid(), linear_acceleration2->uuid(), fuse_core::Vector3d(2.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position3->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position3->uuid(), orientation3->uuid(), linear_velocity3->uuid(), + angular_velocity3->uuid(), linear_acceleration3->uuid(), fuse_core::Vector3d(3.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position4->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position4->uuid(), orientation4->uuid(), linear_velocity4->uuid(), + angular_velocity4->uuid(), linear_acceleration4->uuid(), fuse_core::Vector3d(4.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position5->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position5->uuid(), orientation5->uuid(), linear_velocity5->uuid(), + angular_velocity5->uuid(), linear_acceleration5->uuid(), fuse_core::Vector3d(5.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0) }); // NOLINT(whitespace/braces) // Update the state history - Omnidirectional3DModelTest::updateStateHistoryEstimates( - graph, state_history, rclcpp::Duration::from_seconds( - 10.0)); + Omnidirectional3DModelTest::updateStateHistoryEstimates(graph, state_history, rclcpp::Duration::from_seconds(10.0)); // Check the state estimates in the state history { @@ -262,7 +230,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) } { // The second entry is included in the graph. It will get updated directly. - auto expected_position = fuse_core::Vector3d(1.2, 2.2, 0.0); // <-- value in the Graph + auto expected_position = fuse_core::Vector3d(1.2, 2.2, 0.0); // <-- value in the Graph auto expected_orientation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); auto actual_position = state_history[rclcpp::Time(2, 0)].position; auto actual_orientation = state_history[rclcpp::Time(2, 0)].orientation; @@ -295,10 +263,9 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) { // The third entry is missing from the graph. It will get predicted from previous state. auto expected_position = fuse_core::Vector3d(1.2, 3.7, 0.0); - auto expected_orientation = - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + auto expected_orientation = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); auto actual_position = state_history[rclcpp::Time(3, 0)].position; auto actual_orientation = state_history[rclcpp::Time(3, 0)].orientation; EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); @@ -309,7 +276,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); - auto expected_linear_velocity = fuse_core::Vector3d(0.0, 2.0, 0.0); + auto expected_linear_velocity = fuse_core::Vector3d(0.0, 2.0, 0.0); auto actual_linear_velocity = state_history[rclcpp::Time(3, 0)].vel_linear; EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); @@ -329,11 +296,10 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) } { // The forth entry is included in the graph. It will get updated directly. - auto expected_position = fuse_core::Vector3d(1.4, 2.4, 0.0); // <-- value in the Graph - auto expected_orientation = - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + auto expected_position = fuse_core::Vector3d(1.4, 2.4, 0.0); // <-- value in the Graph + auto expected_orientation = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); auto actual_position = state_history[rclcpp::Time(4, 0)].position; auto actual_orientation = state_history[rclcpp::Time(4, 0)].orientation; EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); @@ -365,10 +331,9 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) { // The fifth entry is missing from the graph. It will get predicted from previous state. auto expected_position = fuse_core::Vector3d(9.5, 12.0, 0.0); - auto expected_orientation = - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + auto expected_orientation = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); auto actual_position = state_history[rclcpp::Time(5, 0)].position; auto actual_orientation = state_history[rclcpp::Time(5, 0)].orientation; EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); @@ -389,7 +354,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto actual_angular_velocity = state_history[rclcpp::Time(5, 0)].vel_angular; EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); - EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); auto expected_linear_acceleration = fuse_core::Vector3d(7.4, 8.4, 0.0); auto actual_linear_acceleration = state_history[rclcpp::Time(5, 0)].acc_linear; diff --git a/fuse_models/test/test_omnidirectional_3d_predict.cpp b/fuse_models/test/test_omnidirectional_3d_predict.cpp index 230c0b8c..26e97be5 100644 --- a/fuse_models/test/test_omnidirectional_3d_predict.cpp +++ b/fuse_models/test/test_omnidirectional_3d_predict.cpp @@ -43,11 +43,11 @@ TEST(Predict, predictDirectVals) { - fuse_core::Vector3d position1 (0.0, 0.0, 0.0); - Eigen::Quaterniond orientation1 (1.0, 0.0, 0.0, 0.0); - fuse_core::Vector3d vel_linear1 (1.0, 0.0, 0.0); - fuse_core::Vector3d vel_angular1 (0.0, 0.0, 1.570796327); - fuse_core::Vector3d acc_linear1 (1.0, 0.0, 0.0); + fuse_core::Vector3d position1(0.0, 0.0, 0.0); + Eigen::Quaterniond orientation1(1.0, 0.0, 0.0, 0.0); + fuse_core::Vector3d vel_linear1(1.0, 0.0, 0.0); + fuse_core::Vector3d vel_angular1(0.0, 0.0, 1.570796327); + fuse_core::Vector3d acc_linear1(1.0, 0.0, 0.0); double dt = 0.1; fuse_core::Vector3d position2; Eigen::Quaterniond orientation2; @@ -55,22 +55,11 @@ TEST(Predict, predictDirectVals) fuse_core::Vector3d vel_angular2; fuse_core::Vector3d acc_linear2; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); Eigen::Quaterniond q; - q = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + q = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); EXPECT_DOUBLE_EQ(0.105, position2.x()); @@ -91,23 +80,12 @@ TEST(Predict, predictDirectVals) EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); // // Carry on with the output state from last time - show in-place update support - fuse_models::predict( - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - - q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + fuse_models::predict(position2, orientation2, vel_linear2, vel_angular2, acc_linear2, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); + + q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - + EXPECT_DOUBLE_EQ(0.21858415916807189, position2.x()); EXPECT_DOUBLE_EQ(0.017989963481956205, position2.y()); EXPECT_DOUBLE_EQ(0.0, position2.z()); @@ -130,21 +108,10 @@ TEST(Predict, predictDirectVals) vel_angular1.z() = -1.570796327; acc_linear1.y() = -1.0; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - - q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); + + q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); EXPECT_DOUBLE_EQ(0.105, position2.x()); @@ -162,26 +129,15 @@ TEST(Predict, predictDirectVals) EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); // Out of plane motion - position1 = {0.0, 0.0, 0.0}; - orientation1 = {1.0, 0.0, 0.0, 0.0}; - vel_linear1 = {0.0, 0.0, 0.1}; - vel_angular1 = {1.570796327, 0.0, 0.0}; - acc_linear1 = {0.0, 0.0, 1.0}; + position1 = { 0.0, 0.0, 0.0 }; + orientation1 = { 1.0, 0.0, 0.0, 0.0 }; + vel_linear1 = { 0.0, 0.0, 0.1 }; + vel_angular1 = { 1.570796327, 0.0, 0.0 }; + acc_linear1 = { 0.0, 0.0, 1.0 }; dt = 0.1; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2 - ); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(0.0, position2.x()); EXPECT_DOUBLE_EQ(0.0, position2.y()); @@ -201,26 +157,15 @@ TEST(Predict, predictDirectVals) EXPECT_DOUBLE_EQ(1.0, acc_linear2.z()); // General 3D motion (these value are checked against rl predict() equations) - position1 = {0.0, 0.0, 0.0}; - orientation1 = {0.110, -0.003, -0.943, 0.314}; // RPY {-2.490, -0.206, 3.066} - vel_linear1 = {0.1, 0.2, 0.1}; - vel_angular1 = {1.570796327, 1.570796327, -1.570796327}; - acc_linear1 = {-0.5, 1.0, 1.0}; + position1 = { 0.0, 0.0, 0.0 }; + orientation1 = { 0.110, -0.003, -0.943, 0.314 }; // RPY {-2.490, -0.206, 3.066} + vel_linear1 = { 0.1, 0.2, 0.1 }; + vel_angular1 = { 1.570796327, 1.570796327, -1.570796327 }; + acc_linear1 = { -0.5, 1.0, 1.0 }; dt = 0.1; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2 - ); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(-0.012044123300410431, position2.x()); EXPECT_DOUBLE_EQ(0.011755776496514461, position2.y()); @@ -242,11 +187,11 @@ TEST(Predict, predictDirectVals) TEST(Predict, predictFromDoublePointers) { - double position1[3] {0.0, 0.0, 0.0}; - double orientation1[3] {0.0, 0.0, 0.0}; - double vel_linear1[3] {1.0, 0.0, 0.0}; - double vel_angular1[3] {0.0, 0.0, 1.570796327}; - double acc_linear1[3] {1.0, 0.0, 0.0}; + double position1[3]{ 0.0, 0.0, 0.0 }; + double orientation1[3]{ 0.0, 0.0, 0.0 }; + double vel_linear1[3]{ 1.0, 0.0, 0.0 }; + double vel_angular1[3]{ 0.0, 0.0, 1.570796327 }; + double acc_linear1[3]{ 1.0, 0.0, 0.0 }; double dt = 0.1; double position2[3]; double orientation2[3]; @@ -254,60 +199,40 @@ TEST(Predict, predictFromDoublePointers) double vel_angular2[3]; double acc_linear2[3]; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(0.105, position2[0]); - EXPECT_DOUBLE_EQ(0.0, position2[1]); - EXPECT_DOUBLE_EQ(0.0, position2[2]); + EXPECT_DOUBLE_EQ(0.0, position2[1]); + EXPECT_DOUBLE_EQ(0.0, position2[2]); EXPECT_DOUBLE_EQ(0.0, orientation2[0]); EXPECT_DOUBLE_EQ(0.0, orientation2[1]); EXPECT_DOUBLE_EQ(0.1570796327, orientation2[2]); - EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); - EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); - EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[2]); EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); // Carry on with the output state from last time - show in-place update support - fuse_models::predict( - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - - EXPECT_DOUBLE_EQ(0.21858415916807189, position2[0]); + fuse_models::predict(position2, orientation2, vel_linear2, vel_angular2, acc_linear2, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); + + EXPECT_DOUBLE_EQ(0.21858415916807189, position2[0]); EXPECT_DOUBLE_EQ(0.017989963481956205, position2[1]); EXPECT_DOUBLE_EQ(0.0, position2[2]); EXPECT_DOUBLE_EQ(0.0, orientation2[0]); EXPECT_DOUBLE_EQ(0.0, orientation2[1]); EXPECT_DOUBLE_EQ(0.3141592654, orientation2[2]); - EXPECT_DOUBLE_EQ(1.2, vel_linear2[0]); - EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); - EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(1.2, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[2]); EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); @@ -318,35 +243,25 @@ TEST(Predict, predictFromDoublePointers) vel_angular1[2] = -1.570796327; acc_linear1[1] = -1.0; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); + EXPECT_DOUBLE_EQ(0.105, position2[0]); - EXPECT_DOUBLE_EQ(-0.105,position2[1]); - EXPECT_DOUBLE_EQ(0.0, position2[2]); + EXPECT_DOUBLE_EQ(-0.105, position2[1]); + EXPECT_DOUBLE_EQ(0.0, position2[2]); EXPECT_DOUBLE_EQ(0.0, orientation2[0]); EXPECT_DOUBLE_EQ(0.0, orientation2[1]); EXPECT_DOUBLE_EQ(-0.1570796327, orientation2[2]); - EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); - EXPECT_DOUBLE_EQ(-1.1, vel_linear2[1]); - EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); + EXPECT_DOUBLE_EQ(-1.1, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2[2]); - EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); EXPECT_DOUBLE_EQ(-1.0, acc_linear2[1]); EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); - + // Out of plane motion position1[0] = 0.0; position1[1] = 0.0; @@ -365,19 +280,8 @@ TEST(Predict, predictFromDoublePointers) acc_linear1[2] = 1.0; dt = 0.1; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2 - ); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(0.0, position2[0]); EXPECT_DOUBLE_EQ(0.0, position2[1]); @@ -413,26 +317,15 @@ TEST(Predict, predictFromDoublePointers) acc_linear1[2] = 1.0; dt = 0.1; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2 - ); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(-0.012031207341885572, position2[0]); - EXPECT_DOUBLE_EQ(0.011723254405731805, position2[1]); + EXPECT_DOUBLE_EQ(0.011723254405731805, position2[1]); EXPECT_DOUBLE_EQ(-0.024981300126995967, position2[2]); - EXPECT_DOUBLE_EQ(-2.3391131265098766, orientation2[0]); - EXPECT_DOUBLE_EQ(-0.4261584872792554, orientation2[1]); - EXPECT_DOUBLE_EQ(3.0962756133525855, orientation2[2]); + EXPECT_DOUBLE_EQ(-2.3391131265098766, orientation2[0]); + EXPECT_DOUBLE_EQ(-0.4261584872792554, orientation2[1]); + EXPECT_DOUBLE_EQ(3.0962756133525855, orientation2[2]); EXPECT_DOUBLE_EQ(0.05, vel_linear2[0]); EXPECT_DOUBLE_EQ(0.3, vel_linear2[1]); EXPECT_DOUBLE_EQ(0.2, vel_linear2[2]); @@ -448,11 +341,11 @@ TEST(Predict, predictFromJetPointers) { using Jet = ceres::Jet; - Jet position1[3] = {Jet(0.0), Jet(0.0), Jet(0.0)}; - Jet orientation1[3] = {Jet(0.0), Jet(0.0), Jet(0.0)}; - Jet vel_linear1[3] = {Jet(1.0), Jet(0.0), Jet(0.0)}; - Jet vel_angular1[3] = {Jet(0.0), Jet(0.0), Jet(1.570796327)}; - Jet acc_linear1[3] = {Jet(1.0), Jet(0.0), Jet(0.0)}; + Jet position1[3] = { Jet(0.0), Jet(0.0), Jet(0.0) }; + Jet orientation1[3] = { Jet(0.0), Jet(0.0), Jet(0.0) }; + Jet vel_linear1[3] = { Jet(1.0), Jet(0.0), Jet(0.0) }; + Jet vel_angular1[3] = { Jet(0.0), Jet(0.0), Jet(1.570796327) }; + Jet acc_linear1[3] = { Jet(1.0), Jet(0.0), Jet(0.0) }; Jet dt = Jet(0.1); Jet position2[3]; Jet orientation2[3]; @@ -460,100 +353,70 @@ TEST(Predict, predictFromJetPointers) Jet vel_angular2[3]; Jet acc_linear2[3]; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); EXPECT_DOUBLE_EQ(Jet(0.1570796327).a, orientation2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); // Carry on with the output state from last time - show in-place update support - fuse_models::predict( - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - - EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); + fuse_models::predict(position2, orientation2, vel_linear2, vel_angular2, acc_linear2, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); + + EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); EXPECT_DOUBLE_EQ(Jet(0.017989963481956205).a, position2[1].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); EXPECT_DOUBLE_EQ(Jet(0.3141592654).a, orientation2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); // // Use non-zero Y values - vel_linear1[1] = Jet(-1.0); + vel_linear1[1] = Jet(-1.0); vel_angular1[2] = Jet(-1.570796327); - acc_linear1[1] = Jet(-1.0); - - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - + acc_linear1[1] = Jet(-1.0); + + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); + EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); - EXPECT_DOUBLE_EQ(Jet(-0.105).a,position2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(-0.105).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); EXPECT_DOUBLE_EQ(Jet(-0.1570796327).a, orientation2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(-1.0).a, acc_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); -// Out of plane motion + // Out of plane motion position1[0] = Jet(0.0); position1[1] = Jet(0.0); position1[2] = Jet(0.0); @@ -571,19 +434,8 @@ TEST(Predict, predictFromJetPointers) acc_linear1[2] = Jet(1.0); dt = Jet(0.1); - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2 - ); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); @@ -619,26 +471,15 @@ TEST(Predict, predictFromJetPointers) acc_linear1[2] = Jet(1.0); dt = Jet(0.1); - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2 - ); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(Jet(-0.012031207341885572).a, position2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.011723254405731805).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.011723254405731805).a, position2[1].a); EXPECT_DOUBLE_EQ(Jet(-0.024981300126995967).a, position2[2].a); - EXPECT_DOUBLE_EQ(Jet(-2.3391131265098766).a, orientation2[0].a); - EXPECT_DOUBLE_EQ(Jet(-0.4261584872792554).a, orientation2[1].a); - EXPECT_DOUBLE_EQ(Jet(3.0962756133525855).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(-2.3391131265098766).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(-0.4261584872792554).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(3.0962756133525855).a, orientation2[2].a); EXPECT_DOUBLE_EQ(Jet(0.05).a, vel_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(0.3).a, vel_linear2[1].a); EXPECT_DOUBLE_EQ(Jet(0.2).a, vel_linear2[2].a); @@ -648,4 +489,4 @@ TEST(Predict, predictFromJetPointers) EXPECT_DOUBLE_EQ(Jet(-0.5).a, acc_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[1].a); EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[2].a); -} \ No newline at end of file +} diff --git a/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp b/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp index bea153f9..db0b93b6 100644 --- a/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp +++ b/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp @@ -47,49 +47,46 @@ TEST(CostFunction, evaluateCostFunction) { // Create cost function - const double process_noise_diagonal[] = {1e-3, 1e-3, 1e-3, 1e-3, 1e-3, - 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, - 1e-3, 1e-3, 1e-3, 1e-3, 1e-3}; + const double process_noise_diagonal[] = { 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, + 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3 }; const fuse_core::Matrix15d covariance = fuse_core::Vector15d(process_noise_diagonal).asDiagonal(); - const double dt{0.1}; - const fuse_core::Matrix15d sqrt_information{covariance.inverse().llt().matrixU()}; + const double dt{ 0.1 }; + const fuse_core::Matrix15d sqrt_information{ covariance.inverse().llt().matrixU() }; - const fuse_models::Omnidirectional3DStateCostFunction cost_function{dt, sqrt_information}; + const fuse_models::Omnidirectional3DStateCostFunction cost_function{ dt, sqrt_information }; // Evaluate cost function - const double position1[3] = {0.0, 0.0, 0.0}; - const double orientation1[4] = {1.0, 0.0, 0.0, 0.0}; - const double vel_linear1[3] = {1.0, 1.0, 1.0}; - const double vel_angular1[3] = {1.570796327, 1.570796327, 1.570796327}; - const double acc_linear1[3] = {1.0, 1.0, 1.0}; + const double position1[3] = { 0.0, 0.0, 0.0 }; + const double orientation1[4] = { 1.0, 0.0, 0.0, 0.0 }; + const double vel_linear1[3] = { 1.0, 1.0, 1.0 }; + const double vel_angular1[3] = { 1.570796327, 1.570796327, 1.570796327 }; + const double acc_linear1[3] = { 1.0, 1.0, 1.0 }; - const double position2[3] = {0.105, 0.105, 0.105}; + const double position2[3] = { 0.105, 0.105, 0.105 }; Eigen::Quaterniond q2 = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitX()); - const double orientation2[4] = {q2.w(), q2.x(), q2.y(), q2.z()}; - const double vel_linear2[3] = {1.1, 1.1, 1.1}; - const double vel_angular2[3] = {1.570796327, 1.570796327, 1.570796327}; - const double acc_linear2[3] = {1.0, 1.0, 1.0}; + const double orientation2[4] = { q2.w(), q2.x(), q2.y(), q2.z() }; + const double vel_linear2[3] = { 1.1, 1.1, 1.1 }; + const double vel_angular2[3] = { 1.570796327, 1.570796327, 1.570796327 }; + const double acc_linear2[3] = { 1.0, 1.0, 1.0 }; - const double * parameters[10] = - { - position1, orientation1, vel_linear1, vel_angular1, acc_linear1, - position2, orientation2, vel_linear2, vel_angular2, acc_linear2 - }; + const double* parameters[10] = { position1, orientation1, vel_linear1, vel_angular1, acc_linear1, + position2, orientation2, vel_linear2, vel_angular2, acc_linear2 }; fuse_core::Vector15d residuals; - const auto & block_sizes = cost_function.parameter_block_sizes(); + const auto& block_sizes = cost_function.parameter_block_sizes(); const auto num_parameter_blocks = block_sizes.size(); const auto num_residuals = cost_function.num_residuals(); std::vector J(num_parameter_blocks); - std::vector jacobians(num_parameter_blocks); + std::vector jacobians(num_parameter_blocks); - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } @@ -100,11 +97,11 @@ TEST(CostFunction, evaluateCostFunction) // above the residuals are not zero for position2.x = -4.389e-16 and yaw2 = -2.776e-16 EXPECT_MATRIX_NEAR(fuse_core::Vector15d::Zero(), residuals, 1e-15); -// // Check jacobians are correct using a gradient checker + // // Check jacobians are correct using a gradient checker ceres::NumericDiffOptions numeric_diff_options; #if CERES_VERSION_AT_LEAST(2, 1, 0) - std::vector parameterizations; + std::vector parameterizations; ceres::GradientChecker gradient_checker(&cost_function, ¶meterizations, numeric_diff_options); #else ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options); @@ -120,28 +117,27 @@ TEST(CostFunction, evaluateCostFunction) // probe_results.error_log; // Create cost function using automatic differentiation on the cost functor - ceres::AutoDiffCostFunctioncost_function_autodiff(new fuse_models::Omnidirectional3DStateCostFunctor(dt, sqrt_information)); + ceres::AutoDiffCostFunction + cost_function_autodiff(new fuse_models::Omnidirectional3DStateCostFunctor(dt, sqrt_information)); // Evaluate cost function that uses automatic differentiation std::vector J_autodiff(num_parameter_blocks); - std::vector jacobians_autodiff(num_parameter_blocks); + std::vector jacobians_autodiff(num_parameter_blocks); - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J_autodiff[i].resize(num_residuals, cost_function_autodiff.parameter_block_sizes()[i]); jacobians_autodiff[i] = J_autodiff[i].data(); } - EXPECT_TRUE( - cost_function_autodiff.Evaluate( - parameters, residuals.data(), - jacobians_autodiff.data())); + EXPECT_TRUE(cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians_autodiff.data())); - const Eigen::IOFormat HeavyFmt( - Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + const Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], 1e-4) - << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt) - << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt); + << "Autodiff Jacobian[" << i << "] =\n" + << J_autodiff[i].format(HeavyFmt) << "\nAnalytic Jacobian[" << i << "] =\n" + << J[i].format(HeavyFmt); } } diff --git a/fuse_models/test/test_sensor_proc_3d.cpp b/fuse_models/test/test_sensor_proc_3d.cpp index c71d24bf..5c1caaa8 100644 --- a/fuse_models/test/test_sensor_proc_3d.cpp +++ b/fuse_models/test/test_sensor_proc_3d.cpp @@ -13,54 +13,42 @@ namespace fm_common = fuse_models::common; TEST(TestSuite, mergeFullPositionAndFullOrientationIndices) { - const std::vector position_indices{0, 1, 2}; - const std::vector orientation_indices{0, 1, 2}; + const std::vector position_indices{ 0, 1, 2 }; + const std::vector orientation_indices{ 0, 1, 2 }; const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); - EXPECT_THAT( - position_indices, - testing::ElementsAreArray( - merged_indices.begin(), - merged_indices.begin() + position_indices.size())); + EXPECT_THAT(position_indices, + testing::ElementsAreArray(merged_indices.begin(), merged_indices.begin() + position_indices.size())); EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); } TEST(TestSuite, mergeXYPositionAndRollYawOrientationIndices) { - const std::vector position_indices{0, 1}; - const std::vector orientation_indices{0, 2}; + const std::vector position_indices{ 0, 1 }; + const std::vector orientation_indices{ 0, 2 }; const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); - EXPECT_THAT( - position_indices, - testing::ElementsAreArray( - merged_indices.begin(), - merged_indices.begin() + position_indices.size())); + EXPECT_THAT(position_indices, + testing::ElementsAreArray(merged_indices.begin(), merged_indices.begin() + position_indices.size())); EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); } TEST(TestSuite, mergeFullPositionAndEmptyOrientationIndices) { - const std::vector position_indices{0, 1, 2}; + const std::vector position_indices{ 0, 1, 2 }; const std::vector orientation_indices; const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size(), merged_indices.size()); EXPECT_THAT(position_indices, testing::ElementsAreArray(merged_indices)); @@ -69,13 +57,11 @@ TEST(TestSuite, mergeFullPositionAndEmptyOrientationIndices) TEST(TestSuite, mergeEmptyPositionAndFullOrientationIndices) { const std::vector position_indices; - const std::vector orientation_indices{0, 1, 2}; + const std::vector orientation_indices{ 0, 1, 2 }; const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(orientation_indices.size(), merged_indices.size()); EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); @@ -88,9 +74,7 @@ TEST(TestSuite, mergeEmptyPositionAndEmptyOrientationIndices) const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_TRUE(merged_indices.empty()); } @@ -100,44 +84,37 @@ TEST(TestSuite, populatePartialMeasurements) // Test both conversion from quaternion to RPY and partial measurement population // This one is just to generate a random unit quaternion and have the reference in RPY fuse_core::Vector3d rpy = fuse_core::Vector3d::Random(); - Eigen::Quaterniond q = - Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()); - + Eigen::Quaterniond q = Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()); + tf2::Transform tf2_pose; tf2_pose.setOrigin(tf2::Vector3(1.0, 2.0, 3.0)); tf2_pose.setRotation(tf2::Quaternion(q.x(), q.y(), q.z(), q.w())); fuse_core::Vector6d pose_mean_partial; pose_mean_partial.head<3>() << tf2_pose.getOrigin().x(), tf2_pose.getOrigin().y(), tf2_pose.getOrigin().z(); tf2::Matrix3x3(tf2_pose.getRotation()).getRPY(pose_mean_partial(3), pose_mean_partial(4), pose_mean_partial(5)); - + fuse_core::Matrix6d pose_covariance = fuse_core::Matrix6d::Random(); - const std::vector position_indices{0, 1}; - const std::vector orientation_indices{2}; + const std::vector position_indices{ 0, 1 }; + const std::vector orientation_indices{ 2 }; const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); - + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + std::replace_if( - pose_mean_partial.data(), pose_mean_partial.data() + pose_mean_partial.size(), - [&merged_indices, &pose_mean_partial](const double & value) { - return std::find( - merged_indices.begin(), - merged_indices.end(), - &value - pose_mean_partial.data()) == merged_indices.end(); - }, 0.0); + pose_mean_partial.data(), pose_mean_partial.data() + pose_mean_partial.size(), + [&merged_indices, &pose_mean_partial](const double& value) { + return std::find(merged_indices.begin(), merged_indices.end(), &value - pose_mean_partial.data()) == + merged_indices.end(); + }, + 0.0); fuse_core::MatrixXd pose_covariance_partial(3, 3); - fm_common::populatePartialMeasurement( - pose_covariance, - merged_indices, - pose_covariance_partial); + fm_common::populatePartialMeasurement(pose_covariance, merged_indices, pose_covariance_partial); fuse_core::Vector6d expected_pose; expected_pose << 1.0, 2.0, 0.0, 0.0, 0.0, rpy(2);