Multiple Camera support for transform_sensor - #24
Merged
Conversation
rlpratt12
requested changes
Mar 18, 2025
rlpratt12
left a comment
Collaborator
There was a problem hiding this comment.
Generally looks good! The only minor change I'd suggest is making pose_covariance a vector (of vectors) to match estimation_frames so that we can pass in a different covariance for each (camera) source.
Collaborator
|
Also, I think we need to fix the blowing up orientation covariance issue. I confirmed this only happens in this branch, and not on main: |
Author
Good point, I'll do that. |
Author
Will address this in a separate PR for that workspace |
henrygerardmoore
force-pushed
the
multiple_cameras
branch
from
March 18, 2025 20:34
e7224be to
12c2040
Compare
Author
|
Tested on HW and this works well, so merging it |
bkanator
added a commit
that referenced
this pull request
Jul 22, 2026
The forward prediction delta was computed as std::min(to_predict_to - stamp, 0.0), which is <= 0 in normal forward operation, forcing dt == 0 and silently disabling predict_to_current_time: the publisher emitted the stale latest-optimized pose stamped as "now". This manifested as a turn-rate-proportional yaw lag (~150 ms; up to ~4.4 deg at 30 deg/s on a mecanum base) that no configuration could fix. Regressed in #24. Clamp the delta to be non-negative (std::max, matching the 2D publisher) so forward prediction is preserved while backward prediction is still prevented. Extract the delta into detail::forwardPredictionDt() and add a regression test that would have caught this. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
bkanator
added a commit
that referenced
this pull request
Jul 22, 2026
The forward prediction delta was computed as std::min(to_predict_to - stamp, 0.0), which is <= 0 in normal forward operation, forcing dt == 0 and silently disabling predict_to_current_time: the publisher emitted the stale latest-optimized pose stamped as "now". This manifested as a turn-rate-proportional yaw lag (~150 ms; up to ~4.4 deg at 30 deg/s on a mecanum base) that no configuration could fix. Regressed in #24. Clamp the delta to be non-negative (std::max, matching the 2D publisher) so forward prediction is preserved while backward prediction is still prevented. Extract the delta into detail::forwardPredictionDt() and add a regression test that would have caught this. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
bkanator
added a commit
that referenced
this pull request
Jul 23, 2026
The forward prediction delta was computed as std::min(to_predict_to - stamp, 0.0), which is <= 0 in normal forward operation, forcing dt == 0 and silently disabling predict_to_current_time: the publisher emitted the stale latest-optimized pose stamped as "now". This manifested as a turn-rate-proportional yaw lag (~150 ms; up to ~4.4 deg at 30 deg/s on a mecanum base) that no configuration could fix. Regressed in #24. Clamp the delta to be non-negative (std::max, matching the 2D publisher) so forward prediction is preserved while backward prediction is still prevented. Extract the delta into detail::forwardPredictionDt() and add a regression test that would have caught this. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
bkanator
added a commit
that referenced
this pull request
Jul 23, 2026
The forward prediction delta was computed as std::min(to_predict_to - stamp, 0.0), which is <= 0 in normal forward operation, forcing dt == 0 and silently disabling predict_to_current_time: the publisher emitted the stale latest-optimized pose stamped as "now". This manifested as a turn-rate-proportional yaw lag (~150 ms; up to ~4.4 deg at 30 deg/s on a mecanum base) that no configuration could fix. Regressed in #24. Clamp the delta to be non-negative (std::max, matching the 2D publisher) so forward prediction is preserved while backward prediction is still prevented. Extract the delta into detail::forwardPredictionDt() and add a regression test that would have caught this. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
griswaldbrooks
pushed a commit
that referenced
this pull request
Jul 23, 2026
…3D jacobian (#35) * fix(odometry_3d): restore predict_to_current_time forward extrapolation The forward prediction delta was computed as std::min(to_predict_to - stamp, 0.0), which is <= 0 in normal forward operation, forcing dt == 0 and silently disabling predict_to_current_time: the publisher emitted the stale latest-optimized pose stamped as "now". This manifested as a turn-rate-proportional yaw lag (~150 ms; up to ~4.4 deg at 30 deg/s on a mecanum base) that no configuration could fix. Regressed in #24. Clamp the delta to be non-negative (std::max, matching the 2D publisher) so forward prediction is preserved while backward prediction is still prevented. Extract the delta into detail::forwardPredictionDt() and add a regression test that would have caught this. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com> * fix(omnidirectional_3d): compute prediction jacobian in RPY space The assembled state jacobian placed J[1] (d(state)/d(quaternion), 15x4) directly into the orientation columns and truncated the acceleration block (J[4].block<15,2>), leaving Ceres an incorrect orientation gradient during 3D rotation. Convert J[1] to RPY space (15x3) via the pseudo-inverse of the quat->rpy jacobian (chain rule). A rank-revealing CompleteOrthogonalDecomposition is used so it degrades gracefully at gimbal lock, where quaternion2rpy zeros rows of the quat->rpy jacobian and an explicit (A*A^T)^-1 would NaN. Add a predictJacobians test that compares the analytic 15x15 jacobian against ceres::Jet autodiff; it fails against the prior truncated assembly and passes with this fix. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com> --------- Co-authored-by: Claude Opus 4.8 <noreply@anthropic.com>
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
This adds the ability to use multiple cameras and relevant documentation.
Also fixes the update rate not listening to configuration by changing the default tf timeout for the publisher to be inline with the other values of that parameter.