[Feature request] I want a ROS Param "tf_broadcast" in hdl_graph_slam_nodelet.cpp

[nyxrobotics]

Summary

I would like to perform position estimation by considering the covariance of various sensors and trusting the sensors that output stable values. Specifically, I want to output TFs such as map->odom and odom->base_link using the robot_localization package. At this time, I only want to use nav_msgs::Odometry messages that hdl_graph_slam_nodelet.cpp publishes and disable Broadcast for TF.

Proposal

• Create a ROS Param named tf_broadcast and do not execute the sendTransform functions when False

Reference

• 01 - ROS and Sensor Fusion Tutorial.md
• Consider replacing amcl with hld_graph_slam in the preceding figure.
• amcl has a parameter ~tf_broadcast, which is True by default

[koide3]

Thanks for your feature request. Although I think it's a reasonable feature, I don't have enough time to implement and test it unfortunately. I believe the requested feature can be implemented by modifying a few lines at the following parts, and would be glad if someone would open a PR for this.

https://github.com/koide3/hdl_graph_slam/blob/40fd395bdfdea2ae682ae3d01f7e78d528dcadb9/apps/scan_matching_odometry_nodelet.cpp#L274 https://github.com/koide3/hdl_graph_slam/blob/40fd395bdfdea2ae682ae3d01f7e78d528dcadb9/src/hdl_graph_slam/map2odom_publisher.py#L21

[nyxrobotics]

Thanks for the reply. I am working on it, but I have a new problem. I cannot use ekf because it does not have covariance values in the odometry topic. I would like to get covariance after the following function, do you know anything about an effective solution?

https://github.com/koide3/hdl_graph_slam/blob/40fd395bdfdea2ae682ae3d01f7e78d528dcadb9/apps/scan_matching_odometry_nodelet.cpp#L209

I would like to add something like this. But I don't know which variables to use in the KdTree.

https://github.com/PointCloudLibrary/pcl/blob/4766f4146b792ef833c773d35157fcc9c668110b/registration/include/pcl/registration/gicp.h#L388

Reference #174 Reference https://github.com/koide3/hdl_localization/issues/40

[koide3]

What computeCovariances() in gicp.h computes is not the covariance of a pose but the covariance of a point. Estimating the covariance of a scan matching result is actually difficult to do properly, and this framework uses only a very naive and poor weighting scheme for creating covariance matrices that would not be good for EKF. If you need an accurate pose covariance estimate, I recommend taking a look at the following papers and implementing some covariance estimation algorithms.

Censi, An accurate closed-form estimate of ICP’s covariance, ICRA2007 Landry et al., CELLO-3D: Estimating the Covariance of ICP in the Real World, ICRA2019