Localization Pose Offset Issue in lidar_localization_ros2

[karthist]

I have used the lidar_slam_ros2 package to map my environment using my custom robot and lidar setup and successfully saved my PCD map. However, I am experiencing an issue when using the lidar_localization_ros2 package to localize against the map. Specifically, my /pcl_pose is consistently reported to be above the ground, causing localization inaccuracies.
My initial pose set correctly in relation to the ground level in
initial_pose_x: 0.0
initial_pose_y: 0.0
initial_pose_z: 0.0
initial_pose_qx: 0.0
initial_pose_qy: 0.0
initial_pose_qz: 0.0
initial_pose_qw: 1.0

Current /pcl_pose:
header:
stamp:
sec: 1354
nanosec: 401000000
frame_id: map
pose:
pose:
position:
x: 0.0054741911590099335
y: -0.003969608340412378
z: 1.0869423151016235

I would appreciate any questions that you have to point me in the right direction.

@rsasaki0109

[rsasaki0109]

Is my understanding correct that the z-coordinate of the ground in the point cloud map is zero, and the lidar is placed on the ground (z=0)?
I can't tell from this image, but are the ground point clouds in the map and the ground scan point clouds misaligned?

[karthist]

Thank you for your response. The Lidar is mounted above the base link at a height of 0.84 meters. however transforms in the setup are defined to account for this elevation. The base link is defined at (0,0,0). The same setup is used to map the environment as well, the red points are the map, and the white points are the live point cloud. The map and the the live scan points are aligned. (Red arrow is pcl_pose, and green arrow is initial_pose in the image.)

Now, when I load this map to localize using the corresponding package, my robot is placed above the ground upon loading and I set the initialpose using the 2DPoseEstimate,

ros2 topic echo /initialpose
header:
stamp:
sec: 1723228739
nanosec: 600797296
frame_id: map
pose:
pose:
position:
x: 0.0016092951409518719
y: 0.025223005563020706
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0022016431995647893
w: 0.999997576380674.

But when I echo my /pcl_pose it'z value is non-zero.
ros2 topic echo /pcl_pose
header:
stamp:
sec: 368
nanosec: 700000000
frame_id: map
pose:
pose:
position:
x: -0.0008895088685676455
y: 0.01030069962143898
z: 0.9628825783729553
orientation:
x: 0.0022107157934049314
y: 0.00022940006925932106
z: -0.0007276802659441157
w: 0.9999972581825816

And, because of this, my robot's base_link is above the ground and therefore the map point cloud and the scan points are misaligned, yes.

When I do a ros2 run tf2_ros tf2_echo map base_link, this is my output.
[INFO] [1723235814.154896184] [tf2_echo]: Waiting for transform map -> base_link: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
At time 1154.700000000

• Translation: [-0.001, 0.011, 0.959]
• Rotation: in Quaternion [0.002, 0.000, -0.001, 1.000]
• Rotation: in RPY (radian) [0.005, 0.000, -0.001]
• Rotation: in RPY (degree) [0.262, 0.011, -0.085]
• Matrix:
  1.000 0.001 0.000 -0.001
  -0.001 1.000 -0.005 0.011
  -0.000 0.005 1.000 0.959
  0.000 0.000 0.000 1.000
  At time 1155.600000000

Does this answer your question? Let me know if you need any other info.

[rsasaki0109]

It might not be good to place the lidar horizontally in that environment. Tilting the lidar from the mapping phase might improve localization as well.

[karthist]

The lidar isn't placed horizontally, I am not sure what you mean by that. I guess what I am trying to understand is what could introduce the height in the pcl_pose when my initial pose doesn't. I am a beginner in this, so I would appreciate any possible questions you might ask that can help me solve this problem. I have all my transforms right, when I don't see this height issue in the mapping, why is this occurring during localization?

[rsasaki0109]

It's not positioned horizontally? It might be better to tilt it further so it can scan the ceiling.
In the environment where you're performing localization, the features in the z-direction might be similar, leading to incorrect matching. By tilting the lidar and utilizing the ceiling's point clouds as well, there's a possibility to more accurately estimate the z-values.

[karthist]

I understand what you mean by placing it horizontally now, yes, it is placed flat on the surface without any tilt. Thank you for the suggestion! I understand the reasoning behind tilting the LiDAR to capture more features in the z-direction, especially if there are overhead structures that could improve localization accuracy. However, in my current environment, there doesn't seem to be anything above the scanned points.

Given this, could it be possible that I’m making a mistake with the code or logic in my implementation? I’d appreciate any insights you might have on potential issues or areas I should double-check. Thanks again for your help!

[rsasaki0109]

Since this issue hasn't been reported before, I believe there are no problems with the code or the implementation logic; it might be the features of the environment that are the issue... It could be worth trying in other environments as well.

[karthist]

Thank you for the response. I tried testing it in a different environment, and the issue still persists. I understand that there might not be an issue in the code/repo itself, my question was meant to ask if you think I am making errors in the implementation of the package itself, as I am adapting the package for my custom robot and environment. I would be happy to provide you with more information on areas that you suspect the issue could arise from.

One other thing that I notice as I drive the robot around is that the localization is incorrect. The robot seems to be moving in a completely different direction by 90 degrees as seen in the video I am moving forward, but the robot is moving to the left.

Screencast.from.08-12-2024.01-21-06.PM.1.mp4

On further debugging, I wrote a script to check the z values in the pcd map, and this is what they look like.
Min z-value: -0.77902734
Max z-value: 2.3179598
Mean z-value: 0.5371676269014424

[rsasaki0109]

Could the issue possibly be in the following code?
Maybe changing transform_stamped.child_frame_id = "lidar frame id" would resolve the problem?
https://github.com/rsasaki0109/lidar_localization_ros2/blob/main/src/lidar_localization_component.cpp#L482-L490

Sorry, I had forgotten that the following issues exist with this package.
#25

[karthist]

When I change the transform_stamped.child_frame_id = "lidar frame id", it solves the issue of point cloud misalignment, however it breaks all other transforms.


I am now certain that the issue is due to transforms, but I can't quite tell what is missing. Because when I have the child_fram_id = "base_link_frame_", my transform tree looks right. map->base_link->lidar

[rsasaki0109]

It seems that the tf from 'map' to 'velodyne' and from 'base_link' to 'velodyne' might not work.
Therefore, it seems necessary for lidar_localization_ros2 to send the tf from 'map' to 'base_link'.

It might be necessary to port the function 'transform_sensor_measurement' found below.
https://github.com/autowarefoundation/autoware.universe/blob/main/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp#L358-L360

[karthist]

Hey, thank you I tried the same, I added this code to my cloudReceievd function

\\ Transform the point cloud from velodyne frame to base_link frame
  sensor_msgs::msg::PointCloud2 transformed_cloud;
  try {
    tfbuffer_.transform(*msg, transformed_cloud, base_frame_id_, tf2::TimePointZero, msg->header.frame_id);
  } catch (tf2::TransformException &ex) {
    RCLCPP_ERROR(get_logger(), "Transform failed: %s", ex.what());
    return;
  }`

and it seems to have fixed my issue. Thanks a lot for the help.