Open stevemartinov opened 4 years ago
Good morning :)
I do not perform point cloud registration in the lidar frame.
Instead I do it in the map frame and then apply the ICP corrections on top of:
new_base_link_to_map_transformation = icp_corrections_transformation * last_odom_to_map_transformation * base_link_to_odom_transformation
This is equivalent to perform the ICP registration on the sensor frame while providing an initial alignment / guess:
https://github.com/PointCloudLibrary/pcl/blob/master/registration/include/pcl/registration/registration.h#L425
using the transformation:
alignment_guess_sensor_to_map_transformation = last_odom_to_map_transformation * base_link_to_odom_transformation * sensor_to_base_link_transformation
This way ICP will only correct small errors of the odometry that occur since the last lidar scan that was processed, allowing it to converge much faster.
After having the new_base_link_to_map_transformation
, I compute the TF [odom -> map] using pose_to_tf_publisher.
new_odom_to_map_transformation = new_base_link_to_map_transformation * odom_to_base_link_transformation
last_odom_to_map_transformation
is from the pose estimation performed using the last lidar scan
base_link_to_odom_transformation
uses the timestamp of the lidar sensor dataI usually provide the robot pose in the base_link or base_footprint frame: https://www.ros.org/reps/rep-0105.html http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF
In drl, the main steps that involve transforming the sensor data are:
transform_odom_to_map = transform_base_link_to_map * transform_odom_to_base_link
Have a nice day :)
Whenvever you find the TF from the lidar frame using the ICP:
icp.getFinalTransformation()
, getting the matrix and converting to ROS TF, how do you then proceed on publishing the Map to Odom TF? Is it like this?final_transformation_as_tf = final_transformation_as_tf (the one you got from the matrix) * map_to_base_tf2 (from listening the TF);
relative_tf2 = sensor_to_base_TF.inverseTimes(odom_to_base_TF)
final_transformation_as_tf = final_transformation_as_tf * relative_tf2;
Is that correct?