Aligning the Cloud_Map and the road of the Vector_Map

The issue

An frequent issue you might encounter is that the Cloud_Map and Vector_Map aren't properly aligned when launching a simulation and visualing it with Rviz.

---

First Solution : Manually Moving the PointCloud Map (Recommended)

This method is the recommended one as it doesn't require any code modification. The objective here is to use an external open-source software (CloudCompare) to visualize and adjust the point cloud map's position, aligning it with the vector map, and then saving it.

A - Install CloudCompare

We will install the CloudCompare software, first ensure that snapd is installed on your computer (this will make installing CloudCompare easier):

sudo apt update
sudo apt install snapd

Then, install CloudCompare with snap: sudo snap install cloudcompare (version 2.11.1 is recommended).

B - Change Orientation of the PCD Map

• First we will align the orientation of maps :

  • Open your PCD file in CloudCompare (either drag and drop or select it). You should see the map in the 3D space.

  

  • In the "DB Tree" section on the left, select your cloud.

  

  • Press Ctrl + T to open the Transformation Window.
  • In the window, go to the "Axis,Angle" page.

  

  • Here, you can modify the rotation angle in degrees to align it with the vector map.

  • Apply the transformation.

  • Save it by pressing Ctrl + S as a "Point Cloud library cloud" file.

  • To test it, you can visualize it in RVIZ when launching your simulation.

C - Change Position of the PCD Map

• Once the rotation is properly aligned :

  • Do the same but to apply the desired translation on the X, Y, and Z axes.

(Don't forget to clear the rotation values before applying the translation, otherwise it will keep changing the orientation of your map.)

---

Second Solution : Moving the PointCloud map in the code with Static transform

First Step - How to align them

This solution involves loading the cloud map frame into a specific new frame (to be used as an intermediate) and then using a node with the ros_tf functionality to modify the position and rotation parameters of the point cloud map. This new frame is then provided to the points map loader instead of the initial frame.

You can solve it by adding the “static_transform_publisher” node in your manager.launch file

<node
    pkg="tf"
    type="static_transform_publisher"
    name="link1_broadcaster"
    args="23 58 0 0 0 -0.679 0.734 map map_temp 100"
/>

• The arguments (“args”) are the parameters you need to change to move the point cloud map under vector_map

  • The three first one are X Y Z axis (in Meters)

  • The four next one are the rotation (so : X Y Z W) (in Quaternion)

  • map is the parent frame

  • map_temp is the new child frame

  • and 100 is the publishing rate (in milliseconds)

• Also , you need to change the “points_map_loader.cpp” file in autoware to get (at line 351):

  • pcd.header.frame_id = "map_temp"; instead of pcd.header.frame_id = "map";

• And recompile Autoware with :

  • AUTOWARE_COMPILE_WITH_CUDA=1 colcon build --continue-on-error --cmake-args -DCMAKE_BUILD_TYPE=Release

• And see your modification of the "args" by launching the simulation with Rivz :

  • roslaunch adeye manager_performance_testing.launch

  • roslaunch adeye my_rviz.launch

---

Second Step : How to make the car found its position

A - The car localization

• To enable the car to localize in this new frame, a few modifications are needed.

  • Initially, the Tf tree had two frames: "World" and "map", with everything referenced to "map". We added a new frame, "map_temp," where the point cloud map is now referenced, while the vector map remains referenced to "map." The issue is that the car still localizes relative to "map."

• We need to change this to connect the car ("base_link") to "map_temp". To do this, modify all references from "map" or "/map" to "map_temp" and "/map_temp" in the ndt_matching.cpp file in Autoware (lines 112, 435, 443, 444, 451, 454, 612, 613, 642, ... to 1374).

After these changes, "base_link" should be connected to "map_temp."

B - The use_local_transform parameter

• Finally, in the localization.launch file, set the use_local_transform parameter to true when launching the ndt_matching node:

  <arg name="use_local_transform" value="true" />

  > how does it works:

  • The use_local_transform parameter applies a local transformation to the map and pose data, improving localization by aligning the map frame and the vehicle's local frame:

    • use_local_transform set to true: Applies a the same transformation made between the original world frame and the defined frame, aligning them.
    • use_local_transform set to false: Assumes the frames are already aligned, using pose data directly.

  > problematic:

  • If a local transformation is applied but use_local_transform is not set to true, the point cloud map may revert to its initial position when replaying a ROS bag or the car could start drifting from the position it should be.

  > Solution:

  • Set use_local_transform to true to consistently apply the local transformation when aligning maps.

---

Third Solution : Research of a New Solution (UNFINISHED)

The goal was to find a solution that avoids creating a new frame, thus preventing modifications to the ndt_matching.cpp file and potentially other parts of Autoware.

• We utilized the existing World to Map static transformers in the map.launch file to apply the desired translation and rotation directly to the map frame, eliminating the need for a temporary frame.

  • Keep the frame-id reference as "map" in the point_map_loader.cpp.
  • Set the use_local_transform argument to true in the ndt_matching node within the localization.launch file.

This approach allows the point cloud to move according to the given arguments while enabling the car to localize itself within the map frame without modifying ndt_matching.cpp.

The main issue with this solution is that the vector map moves along with the point cloud map, causing misalignment. Attempts to resolve this by modifying the frame-id in vector_map_loader.cpp and vector_map.cpp have been unsuccessful.

To finalize this solution, we need to find a way to load the vector map in a different frame than the transformed map frame.

---

Usefull Websites

About Ros_TF : https://wiki.ros.org/tf#static_transform_publisher

About Ros_Frame : https://www.ros.org/reps/rep-0105.html

For rotation conversion https://quaternions.online/