search

ARK-Electronics / PX4-Isaac-VIO-bridge

Bridge between Isaac ROS VSLAM and PX4 via DDS
9 stars 3 forks source link

Odometry variance #1

Open fp018 opened 4 months ago

fp018 commented 4 months ago

Hi,

Problem description

I am using this code with little modifications for forwarding the odometry provided by a D435i + rtabmap to PX4 using the uxrce_dds. However, the estimation is not stable (QGC mainly declares velocity estimate error).

While trying to figure out the problem(s) with my setup, I noticed that some covariance values were negative, this should be due to a wrong covariance frame conversion.

Relevant code

position_variance = tf2::quatRotate(rotation, position_variance); orientation_variance = tf2::quatRotate(rotation, orientation_variance); velocity_variance = tf2::quatRotate(rotation, velocity_variance);

Solution

Refer also to mavros, odom.cpp for the transformation.

Thanks for making your code freely available. Best!

dakejahl commented 4 months ago

Would you mind submitting a PR to fix? I also realized after the fact I should be using the static transform publisher rather than hard coding the rotation