Conversation
…ses msg imu/data_raw and /orca/pose as the dvl info
for more information, see https://pre-commit.ci
for more information, see https://pre-commit.ci
for more information, see https://pre-commit.ci
idk Anders told me to
for more information, see https://pre-commit.ci
…AP_CORE_ONLY to allow eskf to build independently of landmark_egomotion
AntTra
force-pushed
the
feature/eskf-landmark-valve
branch
from
12dc7d0 to
91ec174
Compare
AntTra
force-pushed
the
feature/eskf-landmark-valve
branch
2 times, most recently
from
b395d9b to
752d7f7
Compare
AntTra
force-pushed
the
feature/eskf-landmark-valve
branch
from
752d7f7 to
024dc22
Compare
Codecov Report❌ Patch coverage is Additional details and impacted files@@ Coverage Diff @@
## dev/eskf_fuse_depth #667 +/- ##
=======================================================
- Coverage 36.78% 35.33% -1.45%
=======================================================
Files 86 90 +4
Lines 5755 5991 +236
Branches 1749 1779 +30
=======================================================
Hits 2117 2117
- Misses 3192 3428 +236
Partials 446 446
Flags with carried forward coverage won't be shown. Click here to find out more.
🚀 New features to boost your workflow:
|
…stead of lowest ID
AntTra
force-pushed
the
feature/eskf-landmark-valve
branch
from
7f48bf3 to
6af0de2
Compare
kluge7
left a comment
kluge7
left a comment
There was a problem hiding this comment.
Looks good, can do a more thorough review once we merge it into main
…chor reset warnings, refactor
| nis_gate_pose: 16.812 | ||
| nis_gate_velocity: 11.345 |
There was a problem hiding this comment.
Magic numbers, explaing how you got these numbers
There was a problem hiding this comment.
Might be a good fit for an eskf wiki-page? Like for future reference, instead of adding "value from P(X ≤ chi^2) = 0.99 with 6dof / 3dof" in comments, having a bit more space to explain why this is done.
(And hopefully to avoid having the next N+1 years teams re-implement yet another ESKF 😭)
| #ifdef HAVE_LANDMARK_EGOMOTION | ||
| #include <vortex_msgs/msg/landmark_array.hpp> | ||
| #include "landmark_egomotion/lib/landmark_egomotion.hpp" | ||
| #include "landmark_egomotion/lib/landmark_typedefs.hpp" | ||
| #endif |
There was a problem hiding this comment.
Unsure about this, but probably fine for now
| bool have_last_marker_{false}; | ||
| uint16_t last_marker_id_{0}; | ||
| int vo_rejects_limit_{0}; |
There was a problem hiding this comment.
Should be consistent with either using {} or just setting =, but can fix when merging to main
navigation/eskf/src/ros/eskf_ros.cpp
Outdated
| this->declare_parameter<std::string>("vo.landmarks_topic"); | ||
| this->declare_parameter<std::string>("vo.base_frame"); | ||
| this->declare_parameter<std::string>("vo.camera_frame"); | ||
| this->declare_parameter<bool>("vo.disable_gating", false); |
There was a problem hiding this comment.
Is that a rare default value in the wild? can be removed since it is already defined in config
| vo_cfg.sw_huber_deg = this->get_parameter("vo.huber_deg").as_double(); | ||
| vo_cfg.sw_gate_deg = this->get_parameter("vo.gate_deg").as_double(); | ||
|
|
||
| landmark_eskf_->set_vo_enabled(true); |
| for (const auto& lm : msg->landmarks) { | ||
| if (lm.type.value == vortex_msgs::msg::LandmarkType::ARUCO_MARKER) { | ||
| markers.push_back(&lm); | ||
| } | ||
| } |
There was a problem hiding this comment.
Only handles the LandmarkType being ARUCO_MARKER. should add a default case for handling other types (currently just failing gracefully)
| tf_msg = tf_buffer_->lookupTransform(vo_base_frame_, vo_cam_frame_, | ||
| tf2::TimePointZero); |
There was a problem hiding this comment.
Get from recent message instead of the first?? Unsure, might be fine?? Idk?? msg filter in ros which makes it so that the callback only runs if the transformation is available? more effective but this is probably fine
| LandmarkMeasurement meas; | ||
| meas.pos = t_base; | ||
| meas.quat = q_base; | ||
| meas.stamp = rclcpp::Time(msg->header.stamp).seconds(); | ||
| meas.R = Eigen::Map<const Eigen::Matrix<double, 6, 6, Eigen::RowMajor>>( | ||
| chosen->pose.covariance.data()); |
There was a problem hiding this comment.
Unsure about this, if you should either pass in the different measurements and then create a LandmarkMeasurement in the function, or as it is now.
AntTra
force-pushed
the
feature/eskf-landmark-valve
branch
from
db9668f to
d3b0b14
Compare
…s from ESKF directly. Fixed quaternion error to use right perturbation
AntTra
force-pushed
the
feature/eskf-landmark-valve
branch
from
d3b0b14 to
dc82fb7
Compare
…state machine, in polling. updated compute target
Fix/landmark server
AntTra
force-pushed
the
feature/eskf-landmark-valve
branch
from
cdb6a8d to
bbb983c
Compare
8 participants
Visual egomotion feature in ESKF
The current ESKF struggles with drift over time during the mission, especially when the DVL loses bottom lock when the AUV is pitched to look at the bottom valve with the front camera. This project aims to use landmark-based visual odometry as an additional measurement to be fused to the ESKF as 6-dof pose and derived velocity measurements alongside the IMU and DVL.
It works by placing an anchor on the first valid landmark detection and calculates the relative positioning, computing a transform from the VO frame to the navigation frame. All subsequent marker measurements are mapped through this anchor to produce position and orientation in the navigation frame. Velocity is derived from consecutive position measurements. NIS gating on both pose and velocity updates are based on chi-squared thresholds.
Features:
For testing:
colcon build --symlink-install --packages-select eskf aruco_detector source install/setup.bashIn one terminal run
and in another
together with the simulator and direct the AUV towards an AruCo-marker.
Test anchor reset on marker dropout, when the marker ID gets switched, and the performance during movement.
Note: Another important test would be to test the performance of the sliding window as the ESKF already acts as a filter for outliers
TODO: Change weighted quaternion averaging method for estimation. Replace Huber weighting with ref(?): (https://ntrs.nasa.gov/api/citations/20070017872/downloads/20070017872.pdf). Filter buffer with gating before passing to average_quaternions