3D fuse models by giafranchini · Pull Request #354 · locusrobotics/fuse

giafranchini · 2024-01-16T14:21:41Z

We implemented the following sensor models:

odometry_3d
imu_3d

And the following motion models:

omnidirectional_3d
omnidirectional_3d_ignition

With the associated sensor processing, constraints and tests

…ed_constraint

Co-authored-by: Patrick Roncagliolo <ronca.pat@gmail.com>

…or_pose_3d.hpp Co-authored-by: Patrick Roncagliolo <ronca.pat@gmail.com>

Co-authored-by: Patrick Roncagliolo <ronca.pat@gmail.com>

henrygerardmoore

Found a couple leftovers in omnidirectional_3d.hpp.
Thanks so much for making this PR! I hope soon to contribute a basic tutorial using these in a similar vein to the existing tutorials to help people get up and running with the 3D models, and noticed these while I was working on that.

fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp

fuse_models/include/fuse_models/omnidirectional_3d.hpp

Co-authored-by: Henry Moore <henry.moore@picknik.ai>

giafranchini · 2024-09-25T16:12:44Z

Found a couple leftovers in omnidirectional_3d.hpp. Thanks so much for making this PR! I hope soon to contribute a basic tutorial using these in a similar vein to the existing tutorials to help people get up and running with the 3D models, and noticed these while I was working on that.

Yes, I left these behind during some copy-paste. Thank you for the findings and for the help with the tutorials!!

henrygerardmoore

Found one other thing while working on the 3D tutorial.

fuse_models/include/fuse_models/common/sensor_proc.hpp

Co-authored-by: Henry Moore <henry.moore@picknik.ai>

henrygerardmoore

I think I found an error (found because Eigen will complain when you hit this condition).

fuse_core/include/fuse_core/util.hpp

DavidLocus

I have now been through all of the code in this PR. There's a couple of comments from Stephen that I think are still outstanding, but from what I see, structurally and mathematically, this all looks good.

fuse_optimizers/src/fixed_lag_smoother.cpp

…robotics#366) * Support gcc12 and ceres 2.1.0 * Add support for the Manifold class when using Ceres Solver version 2.1.0 and above * General clean up for Ceres 2.2.0 support * Updated serialization support to be backwards compatible with previously serialized files * Formatting changes required for ROS 2 Rolling / Ubuntu Noble

fuse_models/src/omnidirectional_3d.cpp

Co-authored-by: Henry Moore <henry.moore@picknik.ai>

henrygerardmoore · 2024-10-29T16:47:33Z

fuse_models/src/omnidirectional_3d.cpp

+  state_history_.emplace(beginning_stamp, std::move(state1));
+  state_history_.emplace(ending_stamp, std::move(state2));
+
+  // 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_);
+  }
+
+  // Validate
+  process_noise_covariance *= dt;
+
+  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());
+      return;
+    }
+  }


Suggested change

state_history_.emplace(beginning_stamp, std::move(state1));

state_history_.emplace(ending_stamp, std::move(state2));

// 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_);

}

// Validate

process_noise_covariance *= dt;

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());

return;

}

}

// 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_);

}

// Validate

process_noise_covariance *= dt;

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());

return;

}

}

state_history_.emplace(beginning_stamp, std::move(state1));

state_history_.emplace(ending_stamp, std::move(state2));

I believe state1 and state2 are being used after move here. Does switching the order still work? Seems like it should since none of the switched stuff after relies on state_history_ as far as I can tell.

Yes, it's true that in this case state1 and state2 are being used after move. I basically copied the implementation of generateMotionModel of the Unicycle2D class, where it's done in the same way.
Anyway, after some testing I found that the two object member values are not modified after a call to std::move(), I believe that this is since state1 and state2 are of StateHistoryElement struct type that:

does not implement a custom move constructor

has member types that recall to primitive types such as float, double (I'm not entirely sure about UUIDs which are boost::uuids::uuid but they behave in the same way), which remain unchanged after move

For example, I tried adding another std::string member to StateHistoryElement struct, assign a value to it, and I checked that after move it is left in valid and uninitialized state (an empty string in this case).

About your suggested changes, the only thing that I am not sure of is that in this case, if validateMotionModel fails than the two states are not inserted in the state history, while this happens in the current implementation. I don't know the exact consequences of this.
Maybe @svwilliams can you helps us here? Thank you!

servos · 2024-11-26T13:35:43Z

@svwilliams any update on getting this merged?

henrygerardmoore · 2024-12-13T17:50:51Z

fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp

+    jacobians.data(),
+    jacobian_quat2rpy);
+
+  jacobian << J[0], J[1], J[2], J[3], J[4];


jacobian is a 15x15, right? But isn't this inserting 15 rows and 16 columns (since J[1] is 4 columns)? Sorry I keep coming back with random questions, thank you so much for making this PR!

I ran into this only while using predict while running in debug, where I hit an eigen_assert (Too many coefficients passed to comma initializer (operator<<)) that only happens in debug mode.

henrygerardmoore · 2024-12-13T22:25:46Z

fuse_models/src/odometry_3d_publisher.cpp

+    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;
+    }
+
+    predict(
+      position,
+      orientation,
+      velocity_linear,
+      velocity_angular,
+      acceleration_linear,
+      dt,
+      position,
+      orientation,
+      velocity_linear,
+      velocity_angular,
+      acceleration_linear,
+      jacobian);


should acceleration_linear be initialized to zero here? otherwise predict will use an uninitialized value if you aren't predicting with acceleration.

roncapat · 2025-05-05T15:21:41Z

@svwilliams any updates / roadmap for this to get merged?
Is there a clear list of blocking points?

andreaostuni and others added 30 commits July 21, 2023 10:48

humble branch

6878097

make composable optimizer nodes (dev)

edb7d60

batch and fixed lag optimizer as components

504a1c6

optimizer components refactor

699b9d1

undo optimizer.cpp changes

ce960ad

set precision 1e-4 to validatePartialMeasurement

dce95d2

new branch iron_unicycle3d

f33bc5d

odometry 3d development

ddd06c2

add normal_prior_pose_3d

9927ad0

odometry 3d with partial measurements development

5a3df7b

clean up: removed euler constraints

c065595

3D pose absolute and relative constraints with partial measure support

38cae76

clean up: removed euler constraints plugin from cmake

5f037ce

Add support partial measurements for odometry 3D

d846fff

imu 3D plugin dev

052be96

fix missing type() template specialization for vel 3d

ac1a0f8

add support for partial measurements in absolute_orientation_3d_stamp…

c516176

…ed_constraint

fix: missing tf_timeout param

16f4c53

removed euler constraint headers

5d4dd92

fix: wrong array mapping in unicycle 3d motion model + tests

0c63c45

Update README.md

3fb62bd

Co-authored-by: Patrick Roncagliolo <ronca.pat@gmail.com>

Update typo in fuse_constraints/fuse_plugins.xml

10c0065

Co-authored-by: Patrick Roncagliolo <ronca.pat@gmail.com>

Update typo in fuse_constraints/fuse_plugins.xml

df856ca

Co-authored-by: Patrick Roncagliolo <ronca.pat@gmail.com>

Update typo in fuse_constraints/fuse_plugins.xml

92a3a69

Co-authored-by: Patrick Roncagliolo <ronca.pat@gmail.com>

Update typo in fuse_constraints/fuse_plugins.xml

3fac148

Co-authored-by: Patrick Roncagliolo <ronca.pat@gmail.com>

Update fuse_constraints/fuse_plugins.xml

211e0eb

Co-authored-by: Patrick Roncagliolo <ronca.pat@gmail.com>

Update copyright fuse_constraints/include/fuse_constraints/normal_pri…

89656fb

…or_pose_3d.hpp Co-authored-by: Patrick Roncagliolo <ronca.pat@gmail.com>

Fix: removed double new line

486336b

Co-authored-by: Patrick Roncagliolo <ronca.pat@gmail.com>

Update copyright in fuse_constraints/src/normal_prior_pose_3d.cpp

a09db78

Co-authored-by: Patrick Roncagliolo <ronca.pat@gmail.com>

Cleaning is_angular_3d method in sensor config

f6e78fb

henrygerardmoore reviewed Sep 24, 2024

View reviewed changes

giafranchini and others added 6 commits September 25, 2024 17:43

Update fuse_models/include/fuse_models/omnidirectional_3d.hpp

0376050

Co-authored-by: Henry Moore <henry.moore@picknik.ai>

Update fuse_models/include/fuse_models/omnidirectional_3d.hpp

05bc6a3

Co-authored-by: Henry Moore <henry.moore@picknik.ai>

Update fuse_models/include/fuse_models/omnidirectional_3d.hpp

e729204

Co-authored-by: Henry Moore <henry.moore@picknik.ai>

Update fuse_models/include/fuse_models/omnidirectional_3d.hpp

80aec70

Co-authored-by: Henry Moore <henry.moore@picknik.ai>

fix typos

d26d91f

fix typo: quat instead of rpy

4689930

henrygerardmoore reviewed Sep 26, 2024

View reviewed changes

fuse_models/include/fuse_models/common/sensor_proc.hpp Outdated Show resolved Hide resolved

Fix wrong stride in linear acceleration covariance eigen map

05a35c2

Co-authored-by: Henry Moore <henry.moore@picknik.ai>

henrygerardmoore reviewed Oct 2, 2024

View reviewed changes

fuse_core/include/fuse_core/util.hpp Outdated Show resolved Hide resolved

giafranchini and others added 3 commits October 3, 2024 14:44

fix omnidirectional_3d_ignition orientation init + tests

0e64676

add 3D tutorial (#27)

7475196

Fix wrong quaternion2angleaxis jacobian + test

ec62d42

DavidLocus approved these changes Oct 15, 2024

View reviewed changes

fuse_optimizers/src/fixed_lag_smoother.cpp Outdated Show resolved Hide resolved

remove summary print

491fda4

henrygerardmoore mentioned this pull request Oct 17, 2024

Add upstream changes PickNikRobotics/fuse#6

Merged

svwilliams and others added 4 commits October 18, 2024 12:25

uncrustify

b21c169

test quaternionToAngleAxis for very small rotations

43c5833

Merge branch 'rolling' into models_3d

1dbd922

henrygerardmoore reviewed Oct 28, 2024

View reviewed changes

fuse_models/src/omnidirectional_3d.cpp Outdated Show resolved Hide resolved

fix typo omnidirectional_3d

1383744

Co-authored-by: Henry Moore <henry.moore@picknik.ai>

henrygerardmoore reviewed Oct 29, 2024

View reviewed changes

giafranchini added 2 commits November 2, 2024 16:58

Missing support for Ceres 2.1.0 Manifold in pose_3d constraint tests

568decf

add optional tolerance in ExpectCostFunctionsAreEqual

720eaf0

henrygerardmoore reviewed Dec 13, 2024

View reviewed changes

Conversation

giafranchini commented Jan 16, 2024

Uh oh!

henrygerardmoore left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

giafranchini commented Sep 25, 2024

Uh oh!

henrygerardmoore left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

henrygerardmoore left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

DavidLocus left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

henrygerardmoore Oct 29, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

giafranchini Nov 2, 2024

Choose a reason for hiding this comment

Uh oh!

servos commented Nov 26, 2024

Uh oh!

henrygerardmoore Dec 13, 2024

Choose a reason for hiding this comment

Uh oh!

henrygerardmoore Dec 13, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

roncapat commented May 5, 2025

Uh oh!

Reviewers

Assignees

Labels

Milestone

Development

Uh oh!

9 participants

henrygerardmoore Oct 29, 2024 •

edited

Loading

henrygerardmoore Dec 13, 2024 •

edited

Loading