From ebc65661bb1d39a99221ccffb0fe73b1317af660 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 13 Dec 2024 16:11:30 -0700 Subject: [PATCH] fix uninitialized acceleration and overwritten jacobian --- .../include/fuse_models/omnidirectional_3d_predict.hpp | 4 +++- fuse_models/src/odometry_3d_publisher.cpp | 4 ++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp index ab64cd35..856882e6 100644 --- 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 index 980defaf..e4935942 100644 --- 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);