fix CI

.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
 

fuse.repos

@@ -6,4 +6,4 @@ repositories:
   covariance_geometry_ros:
     type: git
     url: https://github.com/giafranchini/covariance_geometry_ros.git
-    version: iron
+    version: iron

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 <boost/serialization/base_object.hpp>
 #include <boost/serialization/export.hpp>
 
-
 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<size_t> & 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<size_t>& 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<class Archive>
-  void serialize(Archive & archive, const unsigned int /* version */)
+  template <class Archive>
+  void serialize(Archive& archive, const unsigned int /* version */)
   {
-    archive & boost::serialization::base_object<fuse_core::Constraint>(*this);
-    archive & mean_;
-    archive & sqrt_information_;
+    archive& boost::serialization::base_object<fuse_core::Constraint>(*this);
+    archive& mean_;
+    archive& sqrt_information_;
   }
 };
 

fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp

@@ -41,7 +41,6 @@
 #include <fuse_core/fuse_macros.hpp>
 #include <fuse_core/util.hpp>
 
-
 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<typename T>
-  bool operator()(
-    const T * const position1,
-    const T * const orientation1,
-    const T * const position2,
-    const T * const orientation2,
-    T * residual) const;
+  template <typename T>
+  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<typename T>
-bool NormalDeltaPose3DEulerCostFunctor::operator()(
-  const T * const position1,
-  const T * const orientation1,
-  const T * const position2,
-  const T * const orientation2,
-  T * residual) const
+template <typename T>
+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_
+#endif  // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_

fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp

@@ -38,7 +38,6 @@
 
 #include <fuse_core/eigen.hpp>
 
-
 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

fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp

@@ -39,7 +39,6 @@
 
 #include <fuse_core/eigen.hpp>
 
-
 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

fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp

@@ -38,7 +38,6 @@
 
 #include <fuse_core/eigen.hpp>
 
-
 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<ceres::DYNAMIC, 3
    *              order (x, y, z, roll, pitch, yaw)
    * @param[in] b The pose measurement or prior in order (x, y, z, roll, pitch, yaw)
    */
-  NormalPriorPose3DEuler(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b);
+  NormalPriorPose3DEuler(const fuse_core::MatrixXd& A, const fuse_core::Vector6d& b);
 
   /**
    * @brief Destructor
@@ -90,10 +89,7 @@ class NormalPriorPose3DEuler : public ceres::SizedCostFunction<ceres::DYNAMIC, 3
    * @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::MatrixXd A_;  //!< The residual weighting matrix, most likely the square root