Help needed in matching a PCD map created within a simulator with a Lanelet2 data created in real-world

[sglee-morai]

Checklist

• [X] I've read the contribution guidelines.
• [X] #1498
• [X] I've agreed with the maintainers that I can plan this task.

Description

Basically, help from any member who is familiar with matching PCD data to lanelet2 data is needed to solve the following problem

A PCD map for ITRI Campus is created using MORAI SIM: Drive (Actually using AWSIM from TierIV as well).

I put the PCD map along with the original lanelet2 data in the same path.

When launching the Autoware using the command below, it is shown in the rviz that PCD data doesn't match the lanelet2 data. (It only shows PCD data. Lanelet2 data cannot be found)

ros2 launch autoware_launch autoware.launch.xml map_path:={the_path_including_pcd_map_and_lanelet2_data} vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit

The picture below shows that the PCD data is not displayed on top of the lanelet2 data. (Note the PCD map here is only a partial one by the way)

When using PCD data created in the real-world instead, the PCD map and lanelet2 data match altogether correctly.

Purpose

This task is required to have an E2E simulator (either MORAI SIM: Drive or AWSIM) prepared for bus ODD development & testing.

Related Issues in the Simulation WG

• Integration of ITRI campus with ITRI bus and Autoware - MORAI SIM
• Integration of ITRI campus with ITRI bus and Autoware - AWSIM

Possible approaches

I'm guessing there must be a way to specify how to convert the PCD data onto the global coordinate system so that it can be matched onto the lanelet2 map.

I'm currently looking into map_loader package, but I cannot find one yet.

I know how to match this PCD data with the lanelet2 data theoretically, but I don't know how to let Autoware do that.

Here, the PCD data is described w.r.t. simulation map coordinate system whereas lanelet2 data is done so w.r.t. the global coordinate system. Therefore I believe the problem is simply specifying the projection for the map coordinate system.

FYI: Matching the ITRI map coordinate system (simulation) with the global coordinate system

Definition of done

When launching the Autoware using the command below, rviz should show PCD data on top of lanele2 data.

ros2 launch autoware_launch autoware.launch.xml map_path:={the_path_including_pcd_map_and_lanelet2_data} vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit

[mitsudome-r]

@sglee-morai If you would like to move pointcloud you can either use pcl_tools or CloudCompare

• pcl_tools: install sudo apt install pcl-tools and run pcl_transform_point_cloud
• CloudCompare: https://www.danielgm.net/cc/

[VRichardJP]

@sglee-morai If you would like to move pointcloud you can either use pcl_tools or CloudCompare

• pcl_tools: install sudo apt install pcl-tools and run pcl_transform_point_cloud
• CloudCompare: https://www.danielgm.net/cc/

I second pcl-tools. In the past I had a similar issue: I created a lanelet2 map from JOSM (georeferenced) and had to match it on the PCD (which was not). You can use the following principle:

1. Identify 2 points (or more) that are both on your lanelet and pcd. For each, find its global (latlon/mgrs/utm) and local (pcd) coordinates
2. compute global and local coordinate of the center point
3. compute average required rotation angle around the center point (stddev should be low). For example, something like:

rot = []
for p in pts:
  local_diff = p["local"] - center["local"]
  lobal_angle = atan2(local_diff.y, local_diff.x)
  global_diff = p["global"] - center["global"]
  global_angle = atan2(global_diff.y, global_diff.x)
  angle_diff = fmod((global_angle - local_angle) + pi, 2*pi) - pi
  rot.append(angle_diff)
avg_rot = np.mean(rot)
stddev_rot = np.std(rot)

5. then run something like:

# change pcd origin to center point
pcl_transform_point_cloud src dst -trans -center_p["local"].x,-center_p["local"].y,0
# rotate pcd around the center point
pcl_transform_point_cloud dst dst -axisangle 0,0,1,avg_rot
# translate pcd back to match global coordinates
pcl_transform_point_cloud dst dst -trans center_p["global"].x,center_p["global].y,0
# eventually you may have to change the format to binary. You can do this with some python:
# pcl.save(pcl.load(input), output, binary=True)

Eventually double check that each point you used as referenced have been moved at the expected coordinate.

[sglee-morai]

@mitsudome-r @VRichardJP Thanks a lot for providing highly detailed guidelines!

PCD map sample from the simulator & ITRI Campus Lanelet2 Map captured in the real-world campus

Here is a download link for the initial sample PCD map, created using MORAI SIM: Drive, that you might take a look at if you'd like.

Btw, the real-world lanelet2 data that I'm trying to match the PCD map to is the ITRI campus lanelet2 map uploaded in the CI/CD pipeline. (I believe I'm not able to share it publicly.)

Matching Result (Work-in-progress)

I'm currently trying it out and it seems like I'm almost there.

Due to some other issues, I wasn't able to follow the way that @VRichardJP has suggested yet, so far I just give a few numbers that look okay for now. That's why the matching doesn't seem accurate enough yet, which I'm going to work on this week and later.

Since I can create another PCD map, which looks much more similar to the real-world one than the current PCD map, I would like to create the map once again and then try matching them.

(The result so far 1): the real-world PCD map (blue) + PCD map from MORAI SIM: Drive, after roughly matching (red)

(The result so far 2): the PCD map from MORAI SIM (after roughly matched) + real-world lanelet2 data

[sglee-morai]

[22-08-16] Update

Trying to test, but stuck at another issue (lidar driver)

I created some PCD maps and tried to do some testing before closing this issue.

For testing, I'm trying to run MORAI SIM: Drive with Autwoare to see whether localization is working fine, but I'm struggling in starting a lidar driver for Autoware.Universe

• Velodyne models and Ouster models of the MORAI SIM: Drive send just like real-world sensors (Same UDP package)
• I'm looking for how to run Velodyne driver and Ouster driver correctly for Autoware.Universe (FYI, for Autoware.Auto, the Velodyne driver was working fine)

Another Quick Question

Is it possible for a lanelet2 map for Autoware can have an arbitrary position (lat, lon) as the origin of the local map coordinate system? (NOT the origin of an MGRS system)

In other words, is there any way to specify georeference information as an arbitrary proj4 string (or something like UTM + offset value)?

[xmfcx]

https://github.com/tier4/velodyne_vls is being used in the Isuzu bus for velodyne driver.

[mitsudome-r]

Is it possible for a lanelet2 map for Autoware can have an arbitrary position (lat, lon) as the origin of the local map coordinate system? (NOT the origin of an MGRS system)

Yes. You should be able to provide it as UTM origin by giving as a parameter to the loader with something similar to https://github.com/autowarefoundation/autoware.universe/blob/main/map/map_loader/config/lanelet2_map_loader.param.yaml

[sglee-morai]

@xmfcx @mitsudome-r Thanks a lot for sharing!

I was following through how Autoware.Universe launches velodyne drivers, and I found this one: sample_sensor_kit_launch/launch/lidar.launch.xml

Is this the configuration file for lidar drivers when launching the Autoware using the autoware_launch/launch/autoware.launch.xml and sample_sensor_kit (the default value) is passed for sensor_model argument?