fix uninitialized acceleration and overwritten jacobian

PickNikRobotics · Dec 13, 2024 · ebc6566 · ebc6566
1 parent fe83249
commit ebc6566
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 1 deletion.
diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp
@@ -493,7 +493,9 @@ inline void predict(const fuse_core::Vector3d& position1, const Eigen::Quaternio
           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];
+  // TODO(henrygerardmoore): figure out how to fix this
+  // see https://github.com/locusrobotics/fuse/pull/354#discussion_r1884288806
+  jacobian << J[0], J[1], J[2], J[3], J[4].block<15, 2>(0, 0);
 
   // Convert back to quaternion
   orientation2 = Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ()) *

diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp
@@ -465,6 +465,10 @@ void Odometry3DPublisher::predict(tf2::Transform& pose, nav_msgs::msg::Odometry&
     acceleration_linear << acceleration_output.accel.accel.linear.x, acceleration_output.accel.accel.linear.y,
         acceleration_output.accel.accel.linear.z;
   }
+  else
+  {
+    acceleration_linear = fuse_core::Vector3d::Zero();
+  }
 
   ::fuse_models::predict(position, orientation, velocity_linear, velocity_angular, acceleration_linear, dt, position,
                          orientation, velocity_linear, velocity_angular, acceleration_linear, jacobian);