Skip to content

Commit 46905ee

Browse files
badai-nguyenknzo25pre-commit-ci[bot]
authored
refactor(centerpoint, pointpainting): rearrange parameters for ML models and packages (#6591)
* refactor: lidar_centerpoint Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * refactor: pointpainting Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: fix launch Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: fix launch Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: rearrange params Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: json-schema-check error Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: default param Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * refactor: rename param file Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: align centerpoint param namespace with pointpainting Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix(centerpoint): add schema json Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix(pointpainting): fix schema json typo Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * style(pre-commit): autofix * docs: update pointpainting fusion doc Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * docs: update lidar centerpoint doc Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: change omp param Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix:change twist and variance to model params Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: keep build_only in launch Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: schema check Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: temporary remove schema required Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> --------- Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> Co-authored-by: Kenzo Lobos Tsunekawa <kenzo.lobos@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
1 parent 2809a67 commit 46905ee

21 files changed

+473
-216
lines changed

launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml

+3-2
Original file line numberDiff line numberDiff line change
@@ -82,8 +82,9 @@
8282
<arg name="output/objects" value="objects"/>
8383
<arg name="model_name" value="$(var lidar_detection_model)"/>
8484
<arg name="model_path" value="$(var pointpainting_model_path)"/>
85-
<arg name="model_param_path" value="$(var lidar_model_param_path)/$(var lidar_detection_model).param.yaml"/>
86-
<arg name="class_remapper_param_path" value="$(var lidar_model_param_path)/detection_class_remapper.param.yaml"/>
85+
<arg name="model_param_path" value="$(var lidar_model_param_path)/pointpainting.param.yaml"/>
86+
<arg name="ml_package_param_path" value="$(var model_path)/pointpainting_ml_package.param.yaml"/>
87+
<arg name="class_remapper_param_path" value="$(var model_path)/detection_class_remapper.param.yaml"/>
8788

8889
<arg name="use_pointcloud_container" value="true"/>
8990
<arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>

launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml

+3-2
Original file line numberDiff line numberDiff line change
@@ -18,8 +18,9 @@
1818
<arg name="output/objects" value="objects"/>
1919
<arg name="model_name" value="$(var centerpoint_model_name)"/>
2020
<arg name="model_path" value="$(var centerpoint_model_path)"/>
21-
<arg name="model_param_path" value="$(var lidar_model_param_path)/$(var centerpoint_model_name).param.yaml"/>
22-
<arg name="class_remapper_param_path" value="$(var lidar_model_param_path)/detection_class_remapper.param.yaml"/>
21+
<arg name="model_param_path" value="$(var lidar_model_param_path)/centerpoint.param.yaml"/>
22+
<arg name="ml_package_param_path" value="$(var centerpoint_model_path)/$(var centerpoint_model_name)_ml_package.param.yaml"/>
23+
<arg name="class_remapper_param_path" value="$(var centerpoint_model_path)/detection_class_remapper.param.yaml"/>
2324

2425
<arg name="use_pointcloud_container" value="true"/>
2526
<arg name="pointcloud_container_name" value="$(var pointcloud_container_name)"/>
Original file line numberDiff line numberDiff line change
@@ -1,33 +1,21 @@
11
/**:
22
ros__parameters:
3-
trt_precision: fp16
43
encoder_onnx_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).onnx"
54
encoder_engine_path: "$(var model_path)/pts_voxel_encoder_$(var model_name).engine"
65
head_onnx_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).onnx"
76
head_engine_path: "$(var model_path)/pts_backbone_neck_head_$(var model_name).engine"
8-
9-
model_params:
10-
class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"]
11-
paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"]
12-
point_feature_size: 7 # x, y, z, time-lag and car, pedestrian, bicycle
13-
max_voxel_size: 40000
14-
point_cloud_range: [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0]
15-
voxel_size: [0.32, 0.32, 8.0]
16-
downsample_factor: 1
17-
encoder_in_feature_size: 12
18-
yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0]
19-
has_variance: false
20-
has_twist: false
21-
densification_params:
22-
world_frame_id: "map"
23-
num_past_frames: 0
7+
trt_precision: fp16
248
post_process_params:
259
# post-process params
2610
circle_nms_dist_threshold: 0.3
2711
iou_nms_target_class_names: ["CAR"]
2812
iou_nms_search_distance_2d: 10.0
2913
iou_nms_threshold: 0.1
3014
score_threshold: 0.35
15+
yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0]
16+
densification_params:
17+
world_frame_id: "map"
18+
num_past_frames: 1
3119
omp_params:
3220
# omp params
3321
num_threads: 1
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
/**:
2+
ros__parameters:
3+
model_params:
4+
class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"]
5+
paint_class_names: ["CAR", "BICYCLE", "PEDESTRIAN"]
6+
point_feature_size: 7 # x, y, z, time-lag and car, pedestrian, bicycle
7+
max_voxel_size: 40000
8+
point_cloud_range: [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0]
9+
voxel_size: [0.32, 0.32, 8.0]
10+
downsample_factor: 1
11+
encoder_in_feature_size: 12
12+
has_twist: false
13+
has_variance: false

perception/image_projection_based_fusion/docs/pointpainting-fusion.md

+16-11
Original file line numberDiff line numberDiff line change
@@ -33,17 +33,22 @@ The lidar points are projected onto the output of an image-only 2d object detect
3333

3434
### Core Parameters
3535

36-
| Name | Type | Default Value | Description |
37-
| ------------------------------- | ------ | ------------- | ----------------------------------------------------------- |
38-
| `score_threshold` | float | `0.4` | detected objects with score less than threshold are ignored |
39-
| `densification_world_frame_id` | string | `map` | the world frame id to fuse multi-frame pointcloud |
40-
| `densification_num_past_frames` | int | `0` | the number of past frames to fuse with the current frame |
41-
| `trt_precision` | string | `fp16` | TensorRT inference precision: `fp32` or `fp16` |
42-
| `encoder_onnx_path` | string | `""` | path to VoxelFeatureEncoder ONNX file |
43-
| `encoder_engine_path` | string | `""` | path to VoxelFeatureEncoder TensorRT Engine file |
44-
| `head_onnx_path` | string | `""` | path to DetectionHead ONNX file |
45-
| `head_engine_path` | string | `""` | path to DetectionHead TensorRT Engine file |
46-
| `build_only` | bool | `false` | shutdown the node after TensorRT engine file is built |
36+
| Name | Type | Default Value | Description |
37+
| ------------------------------------------------ | ------------ | ------------------------- | ----------------------------------------------------------- |
38+
| `encoder_onnx_path` | string | `""` | path to VoxelFeatureEncoder ONNX file |
39+
| `encoder_engine_path` | string | `""` | path to VoxelFeatureEncoder TensorRT Engine file |
40+
| `head_onnx_path` | string | `""` | path to DetectionHead ONNX file |
41+
| `head_engine_path` | string | `""` | path to DetectionHead TensorRT Engine file |
42+
| `build_only` | bool | `false` | shutdown the node after TensorRT engine file is built |
43+
| `trt_precision` | string | `fp16` | TensorRT inference precision: `fp32` or `fp16` |
44+
| `post_process_params.score_threshold` | double | `0.4` | detected objects with score less than threshold are ignored |
45+
| `post_process_params.yaw_norm_thresholds` | list[double] | [0.3, 0.3, 0.3, 0.3, 0.0] | An array of distance threshold values of norm of yaw [rad]. |
46+
| `post_process_params.iou_nms_target_class_names` | list[string] | ["CAR"] | An array of class names to be target in NMS. |
47+
| `post_process_params.iou_nms_search_distance_2d` | double | 10.0 | A maximum distance value to search the nearest objects. |
48+
| `post_process_params.iou_nms_threshold` | double | 0.1 | A threshold value of NMS using IoU score. |
49+
| `post_process_params.has_twist` | boolean | false | Indicates whether the model outputs twist value. |
50+
| `densification_params.world_frame_id` | string | `map` | the world frame id to fuse multi-frame pointcloud |
51+
| `densification_params.num_past_frames` | int | `1` | the number of past frames to fuse with the current frame |
4752

4853
## Assumptions / Known limits
4954

perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml

+5-2
Original file line numberDiff line numberDiff line change
@@ -21,8 +21,9 @@
2121
<arg name="data_path" default="$(env HOME)/autoware_data" description="packages data and artifacts directory path"/>
2222
<arg name="model_name" default="pointpainting" description="options: `pointpainting`"/>
2323
<arg name="model_path" default="$(var data_path)/image_projection_based_fusion"/>
24-
<arg name="model_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/$(var model_name).param.yaml"/>
25-
<arg name="class_remapper_param_path" default="$(find-pkg-share lidar_centerpoint)/config/detection_class_remapper.param.yaml"/>
24+
<arg name="model_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/pointpainting.param.yaml"/>
25+
<arg name="ml_package_param_path" default="$(var model_path)/$(var model_name)_ml_package.param.yaml"/>
26+
<arg name="class_remapper_param_path" default="$(var model_path)/detection_class_remapper.param.yaml"/>
2627
<arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
2728
<arg name="build_only" default="false" description="shutdown node after TensorRT engine file is built"/>
2829

@@ -48,6 +49,7 @@
4849
<remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
4950
<remap from="~/output/objects" to="$(var output/objects)"/>
5051
<param from="$(var model_param_path)" allow_substs="true"/>
52+
<param from="$(var ml_package_param_path)" allow_substs="true"/>
5153
<param from="$(var sync_param_path)"/>
5254
<param from="$(var class_remapper_param_path)"/>
5355
<param name="rois_number" value="$(var input/rois_number)"/>
@@ -89,6 +91,7 @@
8991
<remap from="~/input/pointcloud" to="$(var input/pointcloud)"/>
9092
<remap from="~/output/objects" to="$(var output/objects)"/>
9193
<param from="$(var model_param_path)" allow_substs="true"/>
94+
<param from="$(var ml_package_param_path)" allow_substs="true"/>
9295
<param from="$(var sync_param_path)"/>
9396
<param from="$(var class_remapper_param_path)"/>
9497
<param name="rois_number" value="$(var input/rois_number)"/>

perception/image_projection_based_fusion/schema/pointpainting.schema.json

+24-67
Original file line numberDiff line numberDiff line change
@@ -6,71 +6,11 @@
66
"pointpainting": {
77
"type": "object",
88
"properties": {
9-
"model_params": {
10-
"type": "object",
11-
"description": "Parameters for model configuration.",
12-
"properties": {
13-
"class_names": {
14-
"type": "array",
15-
"description": "An array of class names will be predicted.",
16-
"default": ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"],
17-
"uniqueItems": true
18-
},
19-
"paint_class_names": {
20-
"type": "array",
21-
"description": "An array of class names will be painted by PointPainting",
22-
"default": ["CAR", "BICYCLE", "PEDESTRIAN"],
23-
"uniqueItems": true
24-
},
25-
"point_feature_size": {
26-
"type": "integer",
27-
"description": "A number of channels of point feature layer.",
28-
"default": 7
29-
},
30-
"max_voxel_size": {
31-
"type": "integer",
32-
"description": "A maximum size of voxel grid.",
33-
"default": 40000
34-
},
35-
"point_cloud_range": {
36-
"type": "array",
37-
"description": "An array of distance ranges of each class, this must have same length with `class_names`.",
38-
"default": [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0]
39-
},
40-
"voxel_size": {
41-
"type": "array",
42-
"description": "An array of voxel grid sizes for PointPainting, this must have same length with `paint_class_names`.",
43-
"default": [0.32, 0.32, 8.0]
44-
},
45-
"down_sample_factor": {
46-
"type": "integer",
47-
"description": "A scale factor of downsampling points",
48-
"default": 1,
49-
"minimum": 1
50-
},
51-
"encoder_in_feature_size": {
52-
"type": "integer",
53-
"description": "A size of encoder input feature channels.",
54-
"default": 12
55-
},
56-
"yaw_norm_thresholds": {
57-
"type": "array",
58-
"description": "An array of distance threshold values of norm of yaw [rad].",
59-
"default": [0.3, 0.3, 0.3, 0.3, 0.0],
60-
"minimum": 0.0,
61-
"maximum": 1.0
62-
},
63-
"has_variance": {
64-
"type": "boolean",
65-
"description": "Indicates whether the model outputs variance value.",
66-
"default": false
67-
},
68-
"has_twist": {
69-
"type": "boolean",
70-
"description": "Indicates whether the model outputs twist value.",
71-
"default": false
72-
}
73-
}
9+
"trt_precision": {
10+
"type": "string",
11+
"description": "TensorRT inference precision.",
12+
"default": "fp16",
13+
"enum": ["fp32", "fp16"]
7414
},
7515
"densification_params": {
7616
"type": "object",
@@ -112,7 +52,7 @@
11252
"default": ["CAR"],
11353
"uniqueItems": true
11454
},
115-
"iou_search_distance_2d": {
55+
"iou_nms_search_distance_2d": {
11656
"type": "number",
11757
"description": "A maximum distance value to search the nearest objects.",
11858
"default": 10.0,
@@ -124,6 +64,23 @@
12464
"default": 0.1,
12565
"minimum": 0.0,
12666
"maximum": 1.0
67+
},
68+
"yaw_norm_thresholds": {
69+
"type": "array",
70+
"description": "An array of distance threshold values of norm of yaw [rad].",
71+
"default": [0.3, 0.3, 0.3, 0.3, 0.0],
72+
"minimum": 0.0,
73+
"maximum": 1.0
74+
},
75+
"has_variance": {
76+
"type": "boolean",
77+
"description": "Indicates whether the model outputs variance value.",
78+
"default": false
79+
},
80+
"has_twist": {
81+
"type": "boolean",
82+
"description": "Indicates whether the model outputs twist value.",
83+
"default": false
12784
}
12885
}
12986
},
@@ -139,7 +96,7 @@
13996
}
14097
}
14198
},
142-
"required": ["model_params", "densification_params", "post_process_params", "omp_params"]
99+
"required": []
143100
}
144101
},
145102
"properties": {
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
{
2+
"$schema": "http://json-schema.org/draft-07/schema#",
3+
"title": "Parameters for Point Painting ML model",
4+
"type": "object",
5+
"definitions": {
6+
"pointpainting_ml_package": {
7+
"type": "object",
8+
"properties": {
9+
"model_params": {
10+
"type": "object",
11+
"description": "Parameters for model configuration.",
12+
"properties": {
13+
"class_names": {
14+
"type": "array",
15+
"description": "An array of class names will be predicted.",
16+
"default": ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"],
17+
"uniqueItems": true
18+
},
19+
"paint_class_names": {
20+
"type": "array",
21+
"description": "An array of class names will be painted by PointPainting",
22+
"default": ["CAR", "BICYCLE", "PEDESTRIAN"],
23+
"uniqueItems": true
24+
},
25+
"point_feature_size": {
26+
"type": "integer",
27+
"description": "A number of channels of point feature layer.",
28+
"default": 7
29+
},
30+
"max_voxel_size": {
31+
"type": "integer",
32+
"description": "A maximum size of voxel grid.",
33+
"default": 40000
34+
},
35+
"point_cloud_range": {
36+
"type": "array",
37+
"description": "An array of distance ranges of each class, this must have same length with `class_names`.",
38+
"default": [-121.6, -76.8, -3.0, 121.6, 76.8, 5.0]
39+
},
40+
"voxel_size": {
41+
"type": "array",
42+
"description": "An array of voxel grid sizes for PointPainting, this must have same length with `paint_class_names`.",
43+
"default": [0.32, 0.32, 8.0]
44+
},
45+
"down_sample_factor": {
46+
"type": "integer",
47+
"description": "A scale factor of downsampling points",
48+
"default": 1,
49+
"minimum": 1
50+
},
51+
"encoder_in_feature_size": {
52+
"type": "integer",
53+
"description": "A size of encoder input feature channels.",
54+
"default": 12
55+
}
56+
}
57+
}
58+
},
59+
"required": []
60+
}
61+
},
62+
"properties": {
63+
"/**": {
64+
"type": "object",
65+
"properties": {
66+
"ros__parameters": {
67+
"$ref": "#/definitions/pointpainting_ml_package"
68+
}
69+
},
70+
"required": ["ros__parameters"]
71+
}
72+
},
73+
"required": ["/**"]
74+
}

perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -102,7 +102,7 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt
102102
const float circle_nms_dist_threshold = static_cast<float>(
103103
this->declare_parameter<double>("post_process_params.circle_nms_dist_threshold"));
104104
const auto yaw_norm_thresholds =
105-
this->declare_parameter<std::vector<double>>("model_params.yaw_norm_thresholds");
105+
this->declare_parameter<std::vector<double>>("post_process_params.yaw_norm_thresholds");
106106
// densification param
107107
const std::string densification_world_frame_id =
108108
this->declare_parameter<std::string>("densification_params.world_frame_id");
@@ -140,8 +140,8 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt
140140
isClassTable_.erase(cls);
141141
}
142142
}
143-
has_variance_ = this->declare_parameter<bool>("has_variance");
144143
has_twist_ = this->declare_parameter<bool>("model_params.has_twist");
144+
has_variance_ = this->declare_parameter<bool>("model_params.has_variance");
145145
const std::size_t point_feature_size = static_cast<std::size_t>(
146146
this->declare_parameter<std::int64_t>("model_params.point_feature_size"));
147147
const std::size_t max_voxel_size =

0 commit comments

Comments
 (0)