Adding grid coordinate information in load_partial_pointcloud_map interface in pointcloud_map_loader

[kminoda]

Issue

I developed dynamic map loading feature for ndt_scan_matcher as well as a new interface in pointcloud_map_loader, which is a ROS 2 service that enables partial pointcloud map loading. Please refer to the Readme in map_loader and the Readme in autoware_map_msgs for detail. In short, the GetPartialPointCloudMap.srv and GetDifferentialPointCloudMap.srv sends a vector of pointcloud map (in a grid) and its ID.

However, it turned out that there is some use cases where we need to have an additional information: x&y coordinates at which the grid is divided.

Mathematically, a pointcloud map $M$ is divided using two sequences $\lbrace x_0, x_1, ..., x_n \rbrace$ and $\lbrace y_0, y_1, ..., y_n \rbrace$ as follows:

$M_{ij} = \lbrace p | x_i < p_x < x_{i+1} \land y_j < p_y < y_{j+1} \land p \in M \rbrace$

And the point here is, we need to add the above grid coordinate information ($x_i, x_{i+1}, y_j, y_{j+1}$) additionally to GetPartialPointCloudMap.srv and GetDifferentialPointCloudMap.srv when receiving a divided map $M_{ij}$.

Possible solutions

Prepare grid coordinate information for pointcloud map file(s)

A: Prepare ${x_n}$ and ${y_n}$ beforehand (e.g. in YAML file, in a name of each pcd file) and load it in pointcloud_map_loader

In this design, we prepare an additional information for each (set of) pointcloud file(s) that contains the above two sequences ${x_n}$ and ${y_n}$. Then, in response to the ROS 2 service request from other nodes, pointcloud_map_loader sends $M_{ij}$ as well as its grid coordinate information.

• Pros:
  • Easy for users to understand the errors when dividing the pointcloud map (since users will use offline tools)
• Cons:
  • Modification in Autoware interface (additional supplemental information required)

B: Implement pointcloud dividing algorithm in pointcloud_map_loader

In this design, we first concatenate all the pointcloud files into one file in pointcloud_map_loader, and then divide it into several grids (we are going to have ${x_n}$ and ${y_n}$ information when dividing the pointcloud map). Then, in response to the ROS 2 service request from other nodes, pointcloud_map_loader sends $M_{ij}$ as well as its grid coordinate information.

• Pros:
  • No modification in pointcloud map interface (still we will have to modify a bit of GetPartialPointCloudMap.srv and GetDifferentialPointCloudMap.srv)
  • Provide flexibility towards the dividing resolution
• Cons:
  • Difficult for users to understand the errors when dividing the pointcloud map
  • Machine specs may cap the size of the pointcloud map

Send the grid coordinate information in load_partial_pointcloud_map and load_differential_pointcloud_map interfaces

This can be done by simply modifying https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs, e.g. by editing PointCloudMapCellWithID.msg as follows:

# Pointcloud data with ID
string cell_id
sensor_msgs/PointCloud2 pointcloud
double min_x
double min_y
double max_x
double max_y

(which denotes that the grid is within x1<x<x2 and y1<y<y2)

[badai-nguyen]

@kminoda Thank your for your open the discussion,
I agree that it's better to include the x&y coordinates of map grids into map_loader service's response.
As I'm working on compare_map filter and it can be precisely defined which map grid that pointcloud belong to as well as no need to re-compute boundary points.

[mayacarla2070]

@kminoda @badai-nguyen Could you please help, how to enable partial map loading in autoware universe.
Which are the files we need to change and what changes we need to make in order for it to work.

[mayacarla2070]

@kminoda @badai-nguyen Thanks a lot for the help. Autoware is able to merge the split .pcd files as a complete map in Rviz.
I have another query is it possible to load the split .pcd files as per need as shown here.(https://autowarefoundation.github.io/autoware.universe/main/localization/ndt_scan_matcher/#dynamic-map-loading).

Also, here https://autowarefoundation.github.io/autoware.universe/main/map/map_loader/ it refers to some .srv files.

[kminoda]

Yes, ndt_scan_matcher will automatically load the split map dynamically if the map is already split?

[mayacarla2070]

@kminoda Yes the map is already split. but it loads full map. I am not able to get the partial map loading to work, could there be anything I might be missing?

[kminoda]

@mayacarla2070 How did you confirm that ndt_scan_matcher loads the full map?

[kminoda]

BTW it may be better to create another discussion, as this topic is a bit irrelevant from this discussion. It would also be more helpful when someone else has similar issue in the future.

[kminoda]

IMHO proposal A sounds good. The yaml file would look like this:

x_coord: x_0, x_1, x_2, ..., x_m
y_coord: y_0, y_1, y_2, ..., y_n
A.pcd: [0, 0]   # -> x_0 < x < x_1, y_0 < y < y_1
B.pcd: [0, 1]   # -> x_0 < x < x_1, y_1 < y < y_2
C.pcd: [0, 5]   # -> x_0 < x < x_1, y_5 < y < y_6
D.pcd: [1, 1]   # -> x_1 < x < x_2, y_1 < y < y_2
...

What do you think? @yukkysaito

[kminoda]

I created several options and their pros & cons. I think III is the most flexible while keeping the users from providing invalid inputs.

I. List dividing coordinates for each pcd file

# metadata.yaml
# name: [x_min, x_max, y_min, y_max]
A.pcd: [x_a_min, x_a_max, y_a_min, y_a_max]
B.pcd: [x_b_min, x_b_max, y_b_min, y_b_max]
C.pcd: [x_c_min, x_c_max, y_c_min, y_c_max]
D.pcd: [x_d_min, x_d_max, y_d_min, y_d_max]
...

Pros

• Simple expression and easy to understand

Cons

• Too sufficient information

II. List dividing coordinates for each pcd file

# metadata.yaml
x_resolution: 100.0
y_resolution: 150.0
A.pcd: [1200, 2500]   # -> 1200 < x < 1300, 2500 < y < 2650
B.pcd: [1300, 2500]   # -> 1300 < x < 1400, 2500 < y < 2650
C.pcd: [1200, 2650]   # -> 1200 < x < 1300, 2650 < y < 2800
D.pcd: [1400, 2650]   # -> 1400 < x < 1500, 2650 < y < 2800
...

Pros

• Also simple expression and easy to understand
• Avoid users setting uneven resolution (which can also be cons)

Cons

• Too sufficient information (same as I.)

III. List dividing coordinates for x and y in an yaml file.

# metadata.yaml
x_0: 1200.0
y_0: 2500.0
resolution_x: 100.0
resolution_y: 150.0
A.pcd: [i, j]   # -> x_0 + i * res_x < x < x_0 + (i + 1) * res_x, y_0 + j * res_y < y < y_0 + (j + 1) * res_y
B.pcd: [0, 1]
C.pcd: [0, 5] 
D.pcd: [1, 1]
...

Pros

• Contains necessary and sufficient information for grid coordinates
  • For example, we can avoid users providing A.pcd and B.pcd with some overlap to Autoware (while I and II cannot)

Cons

• Bit more tricky to understand compared to I?

IV. Use name convention

$ ls pointcloud_map/
100_120_900_920.pcd
120_140_900_920.pcd
140_160_900_920.pcd
100_120_920_940.pcd
...

Pros

• No additional metafile required

Cons

• May conflict with other naming conventions (such as a requirement from Vector Map Builder or Fleet Management System)

[mitsudome-r]

The first option seems to be the simplest and intuitive

[yukkysaito]

With proposal I, there is too much flexibility and the size of each grid may be different. In that case, index access may not be possible on the map server side.
I would suggest II for simplicity and index access.

[kminoda]

OK, thank you for the comments.
If you are all OK, I would go for II.
( @YamatoAndo @mitsudome-r @yukkysaito )

[kminoda]

Addressed here: autowarefoundation/autoware.universe#3205