Add upstream changes

fuse_core/include/fuse_core/util.hpp

@@ -317,9 +317,9 @@ static inline void quaternionToAngleAxis(const double* q, double* angle_axis, do
     else
     {
       jacobian_map.setZero();
-      jacobian_map(0, 0) = 2.0;
-      jacobian_map(1, 1) = 2.0;
-      jacobian_map(2, 2) = 2.0;
+      jacobian_map(0, 1) = 2.0;
+      jacobian_map(1, 2) = 2.0;
+      jacobian_map(2, 3) = 2.0;
     }
   }
 }

fuse_core/test/test_util.cpp

@@ -2,6 +2,7 @@
  * Software License Agreement (BSD License)
  *
  *  Copyright (c) 2020, Clearpath Robotics
+ *  Copyright (c) 2024, Giacomo Franchini
  *  All rights reserved.
  *
  *  Redistribution and use in source and binary forms, with or without
@@ -214,25 +215,50 @@ TEST(Util, quaternionProduct)
 
 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() };
+  // Test correct quaternion to angle-axis function jacobian, for quaternions representing non-zero rotation
+  {
+    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 J_autodiff[12];
+    double J_analytic[12];
+    double J_autodiff[12];
 
-  fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic);
+    fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic);
 
-  double* jacobians[1] = { J_autodiff };
-  const double* parameters[1] = { q };
+    double* jacobians[1] = { J_autodiff };
+    const double* parameters[1] = { q };
 
-  ceres::CostFunction* quat2angle_axis_cf = Quat2AngleAxis::Create();
-  double angle_axis_autodiff[3];
-  quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians);
+    ceres::CostFunction* quat2angle_axis_cf = Quat2AngleAxis::Create();
+    double angle_axis_autodiff[3];
+    quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians);
 
-  Eigen::Map<fuse_core::Matrix<double, 3, 4>> J_autodiff_map(jacobians[0]);
-  Eigen::Map<fuse_core::Matrix<double, 3, 4>> J_analytic_map(J_analytic);
+    Eigen::Map<fuse_core::Matrix<double, 3, 4>> J_autodiff_map(jacobians[0]);
+    Eigen::Map<fuse_core::Matrix<double, 3, 4>> J_analytic_map(J_analytic);
 
-  EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map));
+    EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map));
+  }
+
+  // Test correct quaternion to angle-axis function jacobian, for quaternions representing zero rotation
+  {
+    double angle_axis[3];
+    double q[4]{ 1.0, 0.0, 0.0, 0.0 };
+
+    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 };
+
+    ceres::CostFunction* quat2angle_axis_cf = Quat2AngleAxis::Create();
+    double angle_axis_autodiff[3];
+    quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians);
+
+    Eigen::Map<fuse_core::Matrix<double, 3, 4>> J_autodiff_map(jacobians[0]);
+    Eigen::Map<fuse_core::Matrix<double, 3, 4>> J_analytic_map(J_analytic);
+
+    EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map));
+  }
 }

fuse_models/include/fuse_models/omnidirectional_3d.hpp

@@ -55,7 +55,7 @@ namespace fuse_models
  * @brief A fuse_models 3D kinematic model that generates kinematic constraints between provided
  *        time stamps, and adds those constraints to the fuse graph.
  *
- * This class uses an omnidirectional kinematic model for the robot. It is equivalent to the motion model
+ * This class uses a omnidirectional kinematic model for the robot. It is equivalent to the motion model
  * in the robot_localization state estimation nodes.
  *
  * Parameters:

fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp

@@ -31,8 +31,8 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef FUSE_MODELS__UNICYCLE_3D_IGNITION_HPP_
-#define FUSE_MODELS__UNICYCLE_3D_IGNITION_HPP_
+#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_IGNITION_HPP_
+#define FUSE_MODELS__OMNIDIRECTIONAL_3D_IGNITION_HPP_
 
 #include <atomic>
 #include <memory>
@@ -57,7 +57,7 @@ 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, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel,
+ * omnidirectional 3D motion model (x, y, z, roll, pitch, yaw, 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
@@ -205,4 +205,4 @@ class Omnidirectional3DIgnition : public fuse_core::AsyncSensorModel
 
 }  // namespace fuse_models
 
-#endif  // FUSE_MODELS__UNICYCLE_3D_IGNITION_HPP_
+#endif  // FUSE_MODELS__OMNIDIRECTIONAL_3D_IGNITION_HPP_

fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp

@@ -31,8 +31,8 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_
-#define FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_
+#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_PREDICT_HPP_
+#define FUSE_MODELS__OMNIDIRECTIONAL_3D_PREDICT_HPP_
 
 #include <Eigen/Core>
 
@@ -503,4 +503,4 @@ inline void predict(const fuse_core::Vector3d& position1, const Eigen::Quaternio
 
 }  // namespace fuse_models
 
-#endif  // FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_
+#endif  // FUSE_MODELS__OMNIDIRECTIONAL_3D_PREDICT_HPP_

fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp

@@ -31,8 +31,8 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTION_HPP_
-#define FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTION_HPP_
+#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTION_HPP_
+#define FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTION_HPP_
 
 #include <ceres/sized_cost_function.h>
 
@@ -278,4 +278,4 @@ Omnidirectional3DStateCostFunction::Omnidirectional3DStateCostFunction(const dou
 
 }  // namespace fuse_models
 
-#endif  // FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTION_HPP_
+#endif  // FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTION_HPP_

fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp

@@ -31,8 +31,8 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_
-#define FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_
+#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTOR_HPP_
+#define FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTOR_HPP_
 
 #include <fuse_models/omnidirectional_3d_predict.hpp>
 
@@ -185,4 +185,4 @@ bool Omnidirectional3DStateCostFunctor::operator()(const T* const position1, con
 
 }  // namespace fuse_models
 
-#endif  // FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_
+#endif  // FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTOR_HPP_

fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp

@@ -31,8 +31,8 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef FUSE_MODELS__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_
-#define FUSE_MODELS__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_
+#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_KINEMATIC_CONSTRAINT_HPP_
+#define FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_KINEMATIC_CONSTRAINT_HPP_
 
 #include <ostream>
 #include <string>
@@ -189,4 +189,4 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint
 
 BOOST_CLASS_EXPORT_KEY(fuse_models::Omnidirectional3DStateKinematicConstraint);
 
-#endif  // FUSE_MODELS__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_
+#endif  // FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_KINEMATIC_CONSTRAINT_HPP_

fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp

@@ -31,8 +31,8 @@
  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  *  POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef FUSE_MODELS__PARAMETERS__UNICYCLE_3D_IGNITION_PARAMS_HPP_
-#define FUSE_MODELS__PARAMETERS__UNICYCLE_3D_IGNITION_PARAMS_HPP_
+#ifndef FUSE_MODELS__PARAMETERS__OMNIDIRECTIONAL_3D_IGNITION_PARAMS_HPP_
+#define FUSE_MODELS__PARAMETERS__OMNIDIRECTIONAL_3D_IGNITION_PARAMS_HPP_
 
 #include <algorithm>
 #include <stdexcept>
@@ -175,4 +175,4 @@ struct Omnidirectional3DIgnitionParams : public ParameterBase
 
 }  // namespace fuse_models
 
-#endif  // FUSE_MODELS__PARAMETERS__UNICYCLE_3D_IGNITION_PARAMS_HPP_
+#endif  // FUSE_MODELS__PARAMETERS__OMNIDIRECTIONAL_3D_IGNITION_PARAMS_HPP_

fuse_models/src/omnidirectional_3d_ignition.cpp

@@ -133,8 +133,7 @@ void Omnidirectional3DIgnition::start()
   {
     auto pose = geometry_msgs::msg::PoseWithCovarianceStamped();
     tf2::Quaternion q;
-    // q.setRPY(params_.initial_state[3], params_.initial_state[4], params_.initial_state[5]);
-    q.setEuler(params_.initial_state[5], params_.initial_state[4], params_.initial_state[3]);
+    q.setRPY(params_.initial_state[3], params_.initial_state[4], params_.initial_state[5]);
     pose.header.stamp = clock_->now();
     pose.pose.pose.position.x = params_.initial_state[0];
     pose.pose.pose.position.y = params_.initial_state[1];

fuse_models/test/CMakeLists.txt

@@ -8,7 +8,8 @@ set(TEST_TARGETS
     test_unicycle_2d_ignition
     test_omnidirectional_3d
     test_omnidirectional_3d_predict
-    test_omnidirectional_3d_state_cost_function)
+    test_omnidirectional_3d_state_cost_function
+    test_omnidirectional_3d_ignition)
 
 foreach(test_name ${TEST_TARGETS})
   ament_add_gtest("${test_name}" "${test_name}.cpp")