Dynamic Pose Covariance for ndt_pose

asked 2021-11-01 23:57:57 -0600

Muhd Hizam
1 ●1 ●2 ●5

updated 2021-11-02 22:57:37 -0600

I am trying to fuse autoware.AI's ndt_pose into the robot_localization package with other sensor data. However I will need to add covariance to ndt_pose to convert the message type from geometry_msgs/PoseStamped to geometry_msgs/PoseWithCovarianceStamped.

Is there a good way to generate the ndt_pose covariance dynamically (perhaps based on the Fitness Score)? I am currently using static covariance but I feel that dynamic covariance will be better for the localisation performance.

Edited

I've decided to calculate simple moving average (SMA) and exponential moving average (EMA) to dynamically derive the covariance. I'm more inclined to use the covariance derived from the exponential moving average and I chose a time period which is close to the rate of publishing of the /ndt_pose topic.

Nonetheless, are there other better ways to dynamically generate the ndt_pose covariance?

SMA and EMA formulas = https://en.wikipedia.org/wiki/Moving_...

answered 2021-11-02 14:52:52 -0600

xmfcx
96 ●1 ●6 ●6

updated 2021-11-02 14:53:18 -0600

Having worked with the NDT localization package of the Autoware.AI for quite a while with various car sized applications and I can say that fitness score is not a reliable source of matched-ness. And it couldn't reliably report its health state.

It could be very low or high in various parts of the point cloud map and it could tell it's doing fine when it is floating all around the map.

I wouldn't attempt to directly calculate the covariance with the current state of the algorithm.

I suggest you use a combination of some wheel odometry, imu, vehicle model and decide that the localization is broken when the pose coming from NDT Matching algorithm disagrees with these sources more than some threshold.

edit flag offensive delete link

Comments

Thanks so much! Can I just clarify the following:

1) Is it still a good idea to add a static covariance to the ndt_pose (i'm only doing this to fuse it to the robot localisation package)? If yes, do you have any recommendations on the static covariance values to use?

2) With reference to your last para, do you also imply that I should not add the ndt_pose into the EKF (i.e. to generate the ndt_pose output and ekf_output separately and then compare the pose)?

I understand that the autoware.ai package also provides the ekf_localizer package which can take in ndt_pose without covariance. But I have not tried and read the full details of it yet .

Muhd Hizam ( 2021-11-02 22:16:45 -0600 )edit

1) Static covariance means, you assign a constant distrust ratio to a all of the readings. Covariance matrix in pose is a 6x6 matrix. The diagonal (top left to bottom right) can be made from variances in order of x, y, z, r, p, y. Variance is the square of the RMSE values of each reading. So if your robot is small, moving slow, it could be small. If your robot is large and moving fast it could be larger. You could keep a constant covariance, it would be simpler in case of NDT in autoware.ai.

2) If you are planning to add ndt poses to the ekf localizer, you need to fully understand the ekf localizer node and be able to reject non-fit ndt inputs. It's a lot of work though, good luck!

xmfcx ( 2021-11-03 03:12:30 -0600 )edit

Thanks alot! These are really useful.

Muhd Hizam ( 2021-11-03 10:52:27 -0600 )edit

add a comment

Dynamic Pose Covariance for ndt_pose

1 Answer

Comments

Question Tools

Stats

Related questions

Dynamic Pose Covariance for ndt_pose edit

1 Answer

Comments

Question Tools

Stats

Related questions

Dynamic Pose Covariance for ndt_pose