diff --git a/.gitignore b/.gitignore index 7fa512d79..c68149a2d 100644 --- a/.gitignore +++ b/.gitignore @@ -13,6 +13,7 @@ docs/_site meters_to_gnss_degrees.py data/maps/grand_loop/carlaMapAdjust3.xodr data/maps/grand_loop/grand_loop_medium.pcd +complex_yolov4_mse_loss.pth *.bt *.pyc trace* diff --git a/Dockerfile b/Dockerfile index f2f0dffa6..bab848927 100755 --- a/Dockerfile +++ b/Dockerfile @@ -179,7 +179,7 @@ RUN pip3 install \ # Scientific Computing - used widely scipy \ # - shapely==2.0.0 \ + shapely==2.0.2 \ # simple-pid \ # diff --git a/data/navigator_default.rviz b/data/navigator_default.rviz index d47a3cc90..191344da2 100644 --- a/data/navigator_default.rviz +++ b/data/navigator_default.rviz @@ -526,6 +526,36 @@ Visualization Manager: Reliability Policy: Reliable Value: /radar/radar_viz Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Detected + Namespaces: + object_bounding_box: true + object_tag: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /viz/detected/objects3d + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Tracked + Namespaces: + object_bounding_box: true + object_tag: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /viz/tracked/objects3d + Value: false + Enabled: true + Name: 3D Objects - Class: rviz_default_plugins/Marker Enabled: false Name: Planning Goal @@ -716,9 +746,9 @@ Visualization Manager: Swap Stereo Eyes: false Value: false Focal Point: - X: -1.3190577030181885 - Y: 0.0719301849603653 - Z: 0 + X: 5.467041015625 + Y: -0.561253547668457 + Z: 5.697730541229248 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false diff --git a/data/perception/configs/complex_yolov4.cfg b/data/perception/configs/complex_yolov4.cfg new file mode 100644 index 000000000..132e6a4e9 --- /dev/null +++ b/data/perception/configs/complex_yolov4.cfg @@ -0,0 +1,1160 @@ +[net] +batch=64 +subdivisions=8 +# Training +#width=512 +#height=512 +width=608 +height=608 +channels=3 +momentum=0.949 +decay=0.0005 +angle=0 +saturation = 1.5 +exposure = 1.5 +hue=.1 + +learning_rate=0.0013 +burn_in=1000 +max_batches = 500500 +policy=steps +steps=400000,450000 +scales=.1,.1 + +#cutmix=1 +mosaic=1 + +#:104x104 54:52x52 85:26x26 104:13x13 for 416 + +[convolutional] +batch_normalize=1 +filters=32 +size=3 +stride=1 +pad=1 +activation=mish + +# Downsample + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=2 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -2 + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=32 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -1,-7 + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +# Downsample + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=2 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -2 + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=64 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=64 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -1,-10 + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +# Downsample + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=2 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -2 + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=128 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -1,-28 + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +# Downsample + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=2 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -2 + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=256 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -1,-28 + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +# Downsample + +[convolutional] +batch_normalize=1 +filters=1024 +size=3 +stride=2 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -2 + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +[convolutional] +batch_normalize=1 +filters=512 +size=3 +stride=1 +pad=1 +activation=mish + +[shortcut] +from=-3 +activation=linear + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=mish + +[route] +layers = -1,-16 + +[convolutional] +batch_normalize=1 +filters=1024 +size=1 +stride=1 +pad=1 +activation=mish + +########################## + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +### SPP ### +[maxpool] +stride=1 +size=5 + +[route] +layers=-2 + +[maxpool] +stride=1 +size=9 + +[route] +layers=-4 + +[maxpool] +stride=1 +size=13 + +[route] +layers=-1,-3,-5,-6 +### End SPP ### + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[upsample] +stride=2 + +[route] +layers = 85 + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[route] +layers = -1, -3 + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=512 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=512 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[upsample] +stride=2 + +[route] +layers = 54 + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[route] +layers = -1, -3 + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=256 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=256 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=128 +size=1 +stride=1 +pad=1 +activation=leaky + +########################## + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=256 +activation=leaky + +[convolutional] +size=1 +stride=1 +pad=1 +filters=30 +activation=linear + + +[yolo] +mask = 0,1,2 +#anchors = 11,14,-3.14, 11,14,0, 11,14,3.14, 11,25,-3.14, 11,25,0, 11,25,3.14, 23,51,-3.14, 23,51,0, 23,51,3.14 +anchors = 11, 15, 0, 10, 24, 0, 11, 25, 0, 23, 49, 0, 23, 55, 0, 24, 53, 0, 24, 60, 0, 27, 63, 0, 29, 74, 0 +classes=3 +num=9 +jitter=.3 +ignore_thresh = .7 +truth_thresh = 1 +scale_x_y = 1.2 +iou_thresh=0.213 +cls_normalizer=1.0 +iou_normalizer=0.07 +iou_loss=ciou +nms_kind=greedynms +beta_nms=0.6 +max_delta=5 + + +[route] +layers = -4 + +[convolutional] +batch_normalize=1 +size=3 +stride=2 +pad=1 +filters=256 +activation=leaky + +[route] +layers = -1, -16 + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=512 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=512 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=256 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=512 +activation=leaky + +[convolutional] +size=1 +stride=1 +pad=1 +filters=30 +activation=linear + + +[yolo] +mask = 3,4,5 +#anchors = 11,14,-3.14, 11,14,0, 11,14,3.14, 11,25,-3.14, 11,25,0, 11,25,3.14, 23,51,-3.14, 23,51,0, 23,51,3.14 +anchors = 11, 15, 0, 10, 24, 0, 11, 25, 0, 23, 49, 0, 23, 55, 0, 24, 53, 0, 24, 60, 0, 27, 63, 0, 29, 74, 0 +classes=3 +num=9 +jitter=.3 +ignore_thresh = .7 +truth_thresh = 1 +scale_x_y = 1.1 +iou_thresh=0.213 +cls_normalizer=1.0 +iou_normalizer=0.07 +iou_loss=ciou +nms_kind=greedynms +beta_nms=0.6 +max_delta=5 + + +[route] +layers = -4 + +[convolutional] +batch_normalize=1 +size=3 +stride=2 +pad=1 +filters=512 +activation=leaky + +[route] +layers = -1, -37 + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[convolutional] +batch_normalize=1 +filters=512 +size=1 +stride=1 +pad=1 +activation=leaky + +[convolutional] +batch_normalize=1 +size=3 +stride=1 +pad=1 +filters=1024 +activation=leaky + +[convolutional] +size=1 +stride=1 +pad=1 +filters=30 +activation=linear + + +[yolo] +mask = 6,7,8 +#anchors = 11,14,-3.14, 11,14,0, 11,14,3.14, 11,25,-3.14, 11,25,0, 11,25,3.14, 23,51,-3.14, 23,51,0, 23,51,3.14 +anchors = 11, 15, 0, 10, 24, 0, 11, 25, 0, 23, 49, 0, 23, 55, 0, 24, 53, 0, 24, 60, 0, 27, 63, 0, 29, 74, 0 +classes=3 +num=9 +jitter=.3 +ignore_thresh = .7 +truth_thresh = 1 +random=1 +scale_x_y = 1.05 +iou_thresh=0.213 +cls_normalizer=1.0 +iou_normalizer=0.07 +iou_loss=ciou +nms_kind=greedynms +beta_nms=0.6 +max_delta=5 diff --git a/data/perception/checkpoints/hv_pointpillars_secfpn_6x8_160e_kitti-3d-3class_20220301_150306-37dc2420.pth b/data/perception/models/hv_pointpillars_secfpn_6x8_160e_kitti-3d-3class_20220301_150306-37dc2420.pth similarity index 100% rename from data/perception/checkpoints/hv_pointpillars_secfpn_6x8_160e_kitti-3d-3class_20220301_150306-37dc2420.pth rename to data/perception/models/hv_pointpillars_secfpn_6x8_160e_kitti-3d-3class_20220301_150306-37dc2420.pth diff --git a/docs/Perception/lidar_detection.md b/docs/Perception/lidar_detection.md new file mode 100644 index 000000000..bc3bcf281 --- /dev/null +++ b/docs/Perception/lidar_detection.md @@ -0,0 +1,77 @@ +--- +layout: default +title: LiDAR Detection Node +nav_order: 6 +parent: Perception +--- + +# Lidar 3D Object Detector Node +{: .no_toc } + +*Maintained by Gueren Sanford and Ragib Arnab* + +## Overview +The LiDAR Detection Node performs 3D object detection on LiDAR pointclouds. The detection must be done using raw pointcloud data to optimize the detection rate. The node has two different methods for detection: + +### Complex YOLOv4 +The `complex_yolov4_model` turns pointcloud data into a birdseye view map of the points. The model uses these 2D images to form 3D detections. The implementation was adpated from [this](https://github.com/maudzung/Complex-YOLOv4-Pytorch) GitHub repo. Look at `complex_yolov4_model` for an example of how to integrate + organize a custom neural network into navigator. More information: +- Average Hz: + - 20.5 +- Optimal confidence threshold: + - 0.9 +- Pros: + - Higher accuracy + - Many detections +- Cons: + - More hallucinations + +### MMDetection3D +The `mmdetection3d_model` uses pointcloud data to form 3D detections. This implementation uses the [MMDetection3D](https://mmdetection3d.readthedocs.io/en/latest/) API for its model intialization and inferencing. The API supports a variety of LiDAR vehicle detection models, which can be switched by changing the `config_path` and `checkpoint_path`. More information: +- Average Hz: + - 23.5 +- Optimal confidence threshold: + - 0.4 +- Pros: + - More reliable + - Interchangable model files +- Cons: + - Low confidence scores + +--- + +### Parameters: + +- **device** *str* + - The device the model will run on. Choices: + - "cuda:0" (DEFAULT) + - "cuda:{NUMBER}" +- **model** *str* + - The type of model making the detections. Choices: + - "mmdetection3d" (DEFAULT) + - "complex_yolo" +- **conf_thresh** *float* + - The mininum confidence value accepted for bounding boxes. Choices: + - 0.7 (DEFAULT) + - 0.0 to 1.0 +- **nms_thresh** *float* + - The maximum accepted intersection accepted for bounding boxes. Choices: + - 0.2 (DEFAULT) + - 0.0 to 1.0 + +--- + +### In: + +- **/lidar** [*PointCloud2*](https://docs.ros2.org/latest/api/sensor_msgs/msg/PointCloud.html) + - Receives raw LiDAR point cloud data. + + +### Out: + +- **/detected/objects3d** [*Object3DArray*](../messages.md#object3darray) + - Publishes an array of 3D objects. + +--- + +### lidar_callback(lidar_msg: PointCloud2) +Uses the lidar msg and a 3D object deteciton model to form 3D bouding boxes around objects. Then, publishes the boxes to a 'detected' topic. diff --git a/docs/Perception/lidar.md b/docs/Perception/lidar_processor.md similarity index 100% rename from docs/Perception/lidar.md rename to docs/Perception/lidar_processor.md diff --git a/docs/Perception/object_detector_3d.md b/docs/Perception/object_detector_3d.md deleted file mode 100644 index 2000f9b5d..000000000 --- a/docs/Perception/object_detector_3d.md +++ /dev/null @@ -1,34 +0,0 @@ ---- -layout: default -title: 3D Object Detector -nav_order: 6 -parent: Perception ---- - -# Lidar 3D Object Detector Node -{: .no_toc } - -*Maintained by Ragib Arnab* - -## Overview -A ROS node which performs 3D object detection on LiDAR data using [MMDetection3D](https://github.com/open-mmlab/mmdetection3d) API. - -Any LiDAR object detection models supported by that package can be used by this node. To use a model, specify the path to the config and checkpoint in the parameters file `/navigator/param/perception/lidar_objdet3d_params` and use that when launching this node. In this project, configs and checkpoints are stored under `/navigator/data/perception` in their respective directories. - ---- - -### In: - -- **/lidar** [*PointCloud2*](https://docs.ros2.org/latest/api/sensor_msgs/msg/PointCloud.html) - - Receives raw LiDAR point cloud data. - - -### Out: - -- **/objdet3d_raw** [*Object3DArray*](../messages.md#object3darray) - - Publishes an array of 3D objects. - ---- - -### lidar_callback(lidar_msg: PointCloud2) -Takes in LiDAR input, performs 3D object detection using [MMDetection3D](https://github.com/open-mmlab/mmdetection3d) API, and publishes the results as an `Object3DArray` message. diff --git a/docs/Perception/static_occupancy_node.md b/docs/Perception/static_occupancy_node.md index c088c070e..67bae30ba 100644 --- a/docs/Perception/static_occupancy_node.md +++ b/docs/Perception/static_occupancy_node.md @@ -12,11 +12,11 @@ parent: Perception ## Overview The `StaticOccupancyNode` is responsible for generating static occupancy and mass grids based on ground-segmented pointclouds provided by the -[`ground_segmentation_node`](https://github.com/jpahm/navigator/blob/documentation/docs/Perception/ground_segmentation_node.md). +[`ground_segmentation_node`](https://nova-utd.github.io/navigator/Perception/ground_segmentation.html). These grids are then consumed by the -[`junction_manager`](https://github.com/jpahm/navigator/blob/documentation/docs/Planning/junction_manager.md) +[`junction_manager`](https://nova-utd.github.io/navigator/Planning/junction_manager.html) and the -[`grid_summation_node`](https://github.com/jpahm/navigator/blob/documentation/docs/Planning/grid_summation_node.md) +[`grid_summation_node`](https://nova-utd.github.io/navigator/Planning/grid_summation.html) for planning purposes. The **occupancy grid** is generated using diff --git a/launches/launch.object3d.py b/launches/launch.object3d.py new file mode 100644 index 000000000..1df95e140 --- /dev/null +++ b/launches/launch.object3d.py @@ -0,0 +1,71 @@ +from os import path + +from launch import LaunchDescription +from launch_ros.actions import Node + + +launch_path = path.realpath(__file__) +launch_dir = path.join(path.dirname(launch_path), '..') +print(launch_dir) + +def generate_launch_description(): + + launch_path = path.realpath(__file__) + launch_dir = path.dirname(launch_path) + param_dir = path.join(launch_dir, "param") + data_dir = path.join(launch_dir, "data") + + complex_yolo_model = Node( + package = 'lidar_detection', + executable = 'lidar_detection_node', + name = 'lidar_detection_node', + parameters=[ + {'model': 'complex_yolo'}, + {'device': 'cuda:0'}, + {'conf_thresh': 0.9} + ], + ) + + mmdetection3d_model = Node( + package = 'lidar_detection', + executable = 'lidar_detection_node', + name = 'lidar_detection_node', + parameters=[ + {'model': 'mmdetection3d'}, + {'device': 'cuda:0'}, + {'conf_thresh': 0.4} + ], + ) + + object_viz_deteced_node = Node( + package='visualizer', + executable='object_visualizer_node', + name='object_visualizer_node', + parameters=[ + {'topic': '/detected/objects3d'} + ] + ) + + multi_object_tracker_3d_node = Node( + package = 'multi_object_tracker_3d', + executable = 'multi_object_tracker_3d_node', + name = 'multi_object_tracker_3d_node', + parameters=[], + ) + + object_viz_tracked_node = Node( + package='visualizer', + executable='object_visualizer_node', + name='object_visualizer_node', + parameters=[ + {'topic': '/tracked/objects3d'} + ] + ) + + return LaunchDescription([ + complex_yolo_model, + # mmdetection3d_model, + object_viz_deteced_node, + multi_object_tracker_3d_node, + object_viz_tracked_node, + ]) diff --git a/launches/launch.perception.py b/launches/launch.perception.py index 2c642d35f..0bf94a7e4 100644 --- a/launches/launch.perception.py +++ b/launches/launch.perception.py @@ -7,17 +7,30 @@ sys.path.append(path.abspath("/navigator/")) from launches.launch_node_definitions import * - +from launches.utils import ( + err_fatal, + try_get_launch_description_from_include, + IncludeError, +) def generate_launch_description(): - return LaunchDescription( - [ - # image_segmentation, - # semantic_projection, - ground_seg, - static_grid, - # traffic_light_detector, - # prednet_inference, - # driveable_area, - ] - ) + # Include 3D object detection launch file and extract launch entities for reuse. + try: + object3d_launch_description = try_get_launch_description_from_include( + "launches/launch.object3d.py" + ) + except IncludeError as e: + err_fatal(e) + + object3d_launch_entities = object3d_launch_description.describe_sub_entities() + + return LaunchDescription([ + # image_segmentation, + # semantic_projection, + ground_seg, + static_grid, + # *object3d_launch_entities, + # traffic_light_detector, + # prednet_inference, + # driveable_area, + ]) diff --git a/launches/object_detection_tracking.launch.py b/launches/object_detection_tracking.launch.py deleted file mode 100644 index d4afb989c..000000000 --- a/launches/object_detection_tracking.launch.py +++ /dev/null @@ -1,56 +0,0 @@ -from os import path - -from launch import LaunchDescription -from launch_ros.actions import Node - - -launch_path = path.realpath(__file__) -launch_dir = path.join(path.dirname(launch_path), '..') -print(launch_dir) - -def generate_launch_description(): - - launch_path = path.realpath(__file__) - launch_dir = path.dirname(launch_path) - param_dir = path.join(launch_dir, "param") - data_dir = path.join(launch_dir, "data") - - lidar_object_detection_3d_node = Node( - package = 'object_detector_3d', - executable = 'lidar_object_detector_3d_node', - name = 'lidar_object_detector_3d_node', - parameters=[ - (path.join(param_dir, "perception/lidar_objdet3d_params.yaml")), - ], - ) - - multi_object_tracker_3d_node = Node( - package = 'multi_object_tracker_3d', - executable = 'multi_object_tracker_3d_node', - name = 'multi_object_tracker_3d_node', - parameters=[ - (path.join(param_dir, "perception/mot3d_params.yaml")), - ], - ) - - object_visualizer_node = Node( - package='visualizer', - executable='object_visualizer_node', - name='object_visualizer_node', - ) - - rviz = Node( - package='rviz2', - namespace='', - executable='rviz2', - name='rviz2', - arguments=['-d' + path.join(data_dir, 'object_detection_tracking.rviz')], - respawn=True - ) - - return LaunchDescription([ - lidar_object_detection_3d_node, - multi_object_tracker_3d_node, - object_visualizer_node, - rviz - ]) diff --git a/param/perception/lidar_objdet3d_params.yaml b/param/perception/lidar_objdet3d_params.yaml deleted file mode 100644 index 83033d625..000000000 --- a/param/perception/lidar_objdet3d_params.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - config_path: "/navigator/data/perception/configs/pointpillars_hv_secfpn_8xb6-160e_kitti-3d-3class.py" - checkpoint_path: "/navigator/data/perception/checkpoints/hv_pointpillars_secfpn_6x8_160e_kitti-3d-3class_20220301_150306-37dc2420.pth" - device: "cuda:0" diff --git a/src/perception/object_detector_3d/object_detector_3d/__init__.py b/src/perception/lidar_detection/lidar_detection/__init__.py similarity index 100% rename from src/perception/object_detector_3d/object_detector_3d/__init__.py rename to src/perception/lidar_detection/lidar_detection/__init__.py diff --git a/src/perception/lidar_detection/lidar_detection/complex_yolov4/README.md b/src/perception/lidar_detection/lidar_detection/complex_yolov4/README.md new file mode 100644 index 000000000..da52fff3d --- /dev/null +++ b/src/perception/lidar_detection/lidar_detection/complex_yolov4/README.md @@ -0,0 +1,11 @@ +# Complex YOLOv4 + +The PyTorch Implementation based on YOLOv4 of the paper: [Complex-YOLO: Real-time 3D Object Detection on Point Clouds](https://arxiv.org/pdf/1803.06199.pdf) + +Most files are taken (some modified) from the GitHub Repo: [https://github.com/maudzung/Complex-YOLOv4-Pytorch/tree/master](https://github.com/maudzung/Complex-YOLOv4-Pytorch/tree/master) + +## Installation and Setup + +Follow the instrucitons below to ensure the model is setup properly: +1. Click [here](https://drive.google.com/drive/folders/16zuyjh0c7iiWRSNKQY7CnzXotecYN5vc) and download the `complex_yolov4_mse_loss.pth` file +2. Move `complex_yolov4_mse_loss.pth` into the directory `navigator/data/perception/models/complex_yolov4_mse_loss.pth` \ No newline at end of file diff --git a/src/perception/object_detector_3d/resource/object_detector_3d b/src/perception/lidar_detection/lidar_detection/complex_yolov4/__init__.py similarity index 100% rename from src/perception/object_detector_3d/resource/object_detector_3d rename to src/perception/lidar_detection/lidar_detection/complex_yolov4/__init__.py diff --git a/src/perception/lidar_detection/lidar_detection/complex_yolov4/model/__init__.py b/src/perception/lidar_detection/lidar_detection/complex_yolov4/model/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/perception/lidar_detection/lidar_detection/complex_yolov4/model/darknet2pytorch.py b/src/perception/lidar_detection/lidar_detection/complex_yolov4/model/darknet2pytorch.py new file mode 100644 index 000000000..662e572b1 --- /dev/null +++ b/src/perception/lidar_detection/lidar_detection/complex_yolov4/model/darknet2pytorch.py @@ -0,0 +1,451 @@ +""" +# -*- coding: utf-8 -*- +----------------------------------------------------------------------------------- +# Refer: https://github.com/Tianxiaomo/pytorch-YOLOv4 +""" + +import sys +import math + +import torch +import torch.nn as nn +import torch.nn.functional as F +import numpy as np + +sys.path.append('../') + +from lidar_detection.complex_yolov4.model.yolo_layer import YoloLayer +from lidar_detection.complex_yolov4.model.darknet_utils import parse_cfg, print_cfg, load_fc, load_conv_bn, load_conv +from lidar_detection.complex_yolov4.utils.torch_utils import to_cpu + + +class Mish(nn.Module): + def __init__(self): + super().__init__() + + def forward(self, x): + x = x * (torch.tanh(F.softplus(x))) + return x + + +class MaxPoolDark(nn.Module): + def __init__(self, size=2, stride=1): + super(MaxPoolDark, self).__init__() + self.size = size + self.stride = stride + + def forward(self, x): + ''' + darknet output_size = (input_size + p - k) / s +1 + p : padding = k - 1 + k : size + s : stride + torch output_size = (input_size + 2*p -k) / s +1 + p : padding = k//2 + ''' + p = self.size // 2 + if ((x.shape[2] - 1) // self.stride) != ((x.shape[2] + 2 * p - self.size) // self.stride): + padding1 = (self.size - 1) // 2 + padding2 = padding1 + 1 + else: + padding1 = (self.size - 1) // 2 + padding2 = padding1 + if ((x.shape[3] - 1) // self.stride) != ((x.shape[3] + 2 * p - self.size) // self.stride): + padding3 = (self.size - 1) // 2 + padding4 = padding3 + 1 + else: + padding3 = (self.size - 1) // 2 + padding4 = padding3 + x = F.max_pool2d(F.pad(x, (padding3, padding4, padding1, padding2), mode='replicate'), + self.size, stride=self.stride) + return x + + +class Upsample_expand(nn.Module): + def __init__(self, stride=2): + super(Upsample_expand, self).__init__() + self.stride = stride + + def forward(self, x): + stride = self.stride + assert (x.data.dim() == 4) + B = x.data.size(0) + C = x.data.size(1) + H = x.data.size(2) + W = x.data.size(3) + ws = stride + hs = stride + x = x.view(B, C, H, 1, W, 1).expand(B, C, H, stride, W, stride).contiguous().view(B, C, H * stride, W * stride) + return x + + +class Upsample_interpolate(nn.Module): + def __init__(self, stride): + super(Upsample_interpolate, self).__init__() + self.stride = stride + + def forward(self, x): + x_numpy = x.cpu().detach().numpy() + H = x_numpy.shape[2] + W = x_numpy.shape[3] + + H = H * self.stride + W = W * self.stride + + out = F.interpolate(x, size=(H, W), mode='nearest') + return out + + +class Reorg(nn.Module): + def __init__(self, stride=2): + super(Reorg, self).__init__() + self.stride = stride + + def forward(self, x): + stride = self.stride + assert (x.data.dim() == 4) + B = x.data.size(0) + C = x.data.size(1) + H = x.data.size(2) + W = x.data.size(3) + assert (H % stride == 0) + assert (W % stride == 0) + ws = stride + hs = stride + x = x.view(B, C, H / hs, hs, W / ws, ws).transpose(3, 4).contiguous() + x = x.view(B, C, H / hs * W / ws, hs * ws).transpose(2, 3).contiguous() + x = x.view(B, C, hs * ws, H / hs, W / ws).transpose(1, 2).contiguous() + x = x.view(B, hs * ws * C, H / hs, W / ws) + return x + + +class GlobalAvgPool2d(nn.Module): + def __init__(self): + super(GlobalAvgPool2d, self).__init__() + + def forward(self, x): + N = x.data.size(0) + C = x.data.size(1) + H = x.data.size(2) + W = x.data.size(3) + x = F.avg_pool2d(x, (H, W)) + x = x.view(N, C) + return x + + +# for route and shortcut +class EmptyModule(nn.Module): + def __init__(self): + super(EmptyModule, self).__init__() + + def forward(self, x): + return x + + +# support route shortcut and reorg +class Darknet(nn.Module): + def __init__(self, cfgfile, use_giou_loss): + super(Darknet, self).__init__() + self.use_giou_loss = use_giou_loss + self.blocks = parse_cfg(cfgfile) + self.width = int(self.blocks[0]['width']) + self.height = int(self.blocks[0]['height']) + + self.models = self.create_network(self.blocks) # merge conv, bn,leaky + self.yolo_layers = [layer for layer in self.models if layer.__class__.__name__ == 'YoloLayer'] + + self.loss = self.models[len(self.models) - 1] + + self.header = torch.IntTensor([0, 0, 0, 0]) + self.seen = 0 + + def forward(self, x, targets=None): + # batch_size, c, h, w + img_size = x.size(2) + ind = -2 + self.loss = None + outputs = dict() + loss = 0. + yolo_outputs = [] + for block in self.blocks: + ind = ind + 1 + # if ind > 0: + # return x + + if block['type'] == 'net': + continue + elif block['type'] in ['convolutional', 'maxpool', 'reorg', 'upsample', 'avgpool', 'softmax', 'connected']: + x = self.models[ind](x) + outputs[ind] = x + elif block['type'] == 'route': + layers = block['layers'].split(',') + layers = [int(i) if int(i) > 0 else int(i) + ind for i in layers] + if len(layers) == 1: + if 'groups' not in block.keys() or int(block['groups']) == 1: + x = outputs[layers[0]] + outputs[ind] = x + else: + groups = int(block['groups']) + group_id = int(block['group_id']) + _, b, _, _ = outputs[layers[0]].shape + x = outputs[layers[0]][:, b // groups * group_id:b // groups * (group_id + 1)] + outputs[ind] = x + elif len(layers) == 2: + x1 = outputs[layers[0]] + x2 = outputs[layers[1]] + x = torch.cat((x1, x2), 1) + outputs[ind] = x + elif len(layers) == 4: + x1 = outputs[layers[0]] + x2 = outputs[layers[1]] + x3 = outputs[layers[2]] + x4 = outputs[layers[3]] + x = torch.cat((x1, x2, x3, x4), 1) + outputs[ind] = x + else: + print("rounte number > 2 ,is {}".format(len(layers))) + + elif block['type'] == 'shortcut': + from_layer = int(block['from']) + activation = block['activation'] + from_layer = from_layer if from_layer > 0 else from_layer + ind + x1 = outputs[from_layer] + x2 = outputs[ind - 1] + x = x1 + x2 + if activation == 'leaky': + x = F.leaky_relu(x, 0.1, inplace=True) + elif activation == 'relu': + x = F.relu(x, inplace=True) + outputs[ind] = x + elif block['type'] == 'yolo': + x, layer_loss = self.models[ind](x, targets, img_size, self.use_giou_loss) + loss += layer_loss + yolo_outputs.append(x) + elif block['type'] == 'cost': + continue + else: + print('unknown type %s' % (block['type'])) + yolo_outputs = to_cpu(torch.cat(yolo_outputs, 1)) + + return yolo_outputs if targets is None else (loss, yolo_outputs) + + def print_network(self): + print_cfg(self.blocks) + + def create_network(self, blocks): + models = nn.ModuleList() + + prev_filters = 3 + out_filters = [] + prev_stride = 1 + out_strides = [] + conv_id = 0 + for block in blocks: + if block['type'] == 'net': + prev_filters = int(block['channels']) + continue + elif block['type'] == 'convolutional': + conv_id = conv_id + 1 + batch_normalize = int(block['batch_normalize']) + filters = int(block['filters']) + kernel_size = int(block['size']) + stride = int(block['stride']) + is_pad = int(block['pad']) + pad = (kernel_size - 1) // 2 if is_pad else 0 + activation = block['activation'] + model = nn.Sequential() + if batch_normalize: + model.add_module('conv{0}'.format(conv_id), + nn.Conv2d(prev_filters, filters, kernel_size, stride, pad, bias=False)) + model.add_module('bn{0}'.format(conv_id), nn.BatchNorm2d(filters)) + # model.add_module('bn{0}'.format(conv_id), BN2d(filters)) + else: + model.add_module('conv{0}'.format(conv_id), + nn.Conv2d(prev_filters, filters, kernel_size, stride, pad)) + if activation == 'leaky': + model.add_module('leaky{0}'.format(conv_id), nn.LeakyReLU(0.1, inplace=True)) + elif activation == 'relu': + model.add_module('relu{0}'.format(conv_id), nn.ReLU(inplace=True)) + elif activation == 'mish': + model.add_module('mish{0}'.format(conv_id), Mish()) + else: + print("[INFO] No error, the convolution haven't activate {}".format(activation)) + + prev_filters = filters + out_filters.append(prev_filters) + prev_stride = stride * prev_stride + out_strides.append(prev_stride) + models.append(model) + elif block['type'] == 'maxpool': + pool_size = int(block['size']) + stride = int(block['stride']) + if stride == 1 and pool_size % 2: + # You can use Maxpooldark instead, here is convenient to convert onnx. + # Example: [maxpool] size=3 stride=1 + model = nn.MaxPool2d(kernel_size=pool_size, stride=stride, padding=pool_size // 2) + elif stride == pool_size: + # You can use Maxpooldark instead, here is convenient to convert onnx. + # Example: [maxpool] size=2 stride=2 + model = nn.MaxPool2d(kernel_size=pool_size, stride=stride, padding=0) + else: + model = MaxPoolDark(pool_size, stride) + out_filters.append(prev_filters) + prev_stride = stride * prev_stride + out_strides.append(prev_stride) + models.append(model) + elif block['type'] == 'avgpool': + model = GlobalAvgPool2d() + out_filters.append(prev_filters) + models.append(model) + elif block['type'] == 'softmax': + model = nn.Softmax() + out_strides.append(prev_stride) + out_filters.append(prev_filters) + models.append(model) + elif block['type'] == 'cost': + if block['_type'] == 'sse': + model = nn.MSELoss(size_average=True) + elif block['_type'] == 'L1': + model = nn.L1Loss(size_average=True) + elif block['_type'] == 'smooth': + model = nn.SmoothL1Loss(size_average=True) + out_filters.append(1) + out_strides.append(prev_stride) + models.append(model) + elif block['type'] == 'reorg': + stride = int(block['stride']) + prev_filters = stride * stride * prev_filters + out_filters.append(prev_filters) + prev_stride = prev_stride * stride + out_strides.append(prev_stride) + models.append(Reorg(stride)) + elif block['type'] == 'upsample': + stride = int(block['stride']) + out_filters.append(prev_filters) + prev_stride = prev_stride // stride + out_strides.append(prev_stride) + + models.append(Upsample_expand(stride)) + # models.append(Upsample_interpolate(stride)) + + elif block['type'] == 'route': + layers = block['layers'].split(',') + ind = len(models) + layers = [int(i) if int(i) > 0 else int(i) + ind for i in layers] + if len(layers) == 1: + if 'groups' not in block.keys() or int(block['groups']) == 1: + prev_filters = out_filters[layers[0]] + prev_stride = out_strides[layers[0]] + else: + prev_filters = out_filters[layers[0]] // int(block['groups']) + prev_stride = out_strides[layers[0]] // int(block['groups']) + elif len(layers) == 2: + assert (layers[0] == ind - 1 or layers[1] == ind - 1) + prev_filters = out_filters[layers[0]] + out_filters[layers[1]] + prev_stride = out_strides[layers[0]] + elif len(layers) == 4: + assert (layers[0] == ind - 1) + prev_filters = out_filters[layers[0]] + out_filters[layers[1]] + out_filters[layers[2]] + \ + out_filters[layers[3]] + prev_stride = out_strides[layers[0]] + else: + print("route error!!!") + + out_filters.append(prev_filters) + out_strides.append(prev_stride) + models.append(EmptyModule()) + elif block['type'] == 'shortcut': + ind = len(models) + prev_filters = out_filters[ind - 1] + out_filters.append(prev_filters) + prev_stride = out_strides[ind - 1] + out_strides.append(prev_stride) + models.append(EmptyModule()) + elif block['type'] == 'connected': + filters = int(block['output']) + if block['activation'] == 'linear': + model = nn.Linear(prev_filters, filters) + elif block['activation'] == 'leaky': + model = nn.Sequential( + nn.Linear(prev_filters, filters), + nn.LeakyReLU(0.1, inplace=True)) + elif block['activation'] == 'relu': + model = nn.Sequential( + nn.Linear(prev_filters, filters), + nn.ReLU(inplace=True)) + prev_filters = filters + out_filters.append(prev_filters) + out_strides.append(prev_stride) + models.append(model) + elif block['type'] == 'yolo': + anchor_masks = [int(i) for i in block['mask'].split(',')] + anchors = [float(i) for i in block['anchors'].split(',')] + anchors = [(anchors[i], anchors[i + 1], math.sin(anchors[i + 2]), math.cos(anchors[i + 2])) for i in + range(0, len(anchors), 3)] + anchors = [anchors[i] for i in anchor_masks] + + num_classes = int(block['classes']) + self.num_classes = num_classes + scale_x_y = float(block['scale_x_y']) + ignore_thresh = float(block['ignore_thresh']) + + yolo_layer = YoloLayer(num_classes=num_classes, anchors=anchors, stride=prev_stride, + scale_x_y=scale_x_y, ignore_thresh=ignore_thresh) + + out_filters.append(prev_filters) + out_strides.append(prev_stride) + models.append(yolo_layer) + else: + print('unknown type %s' % (block['type'])) + + return models + + def load_weights(self, weightfile): + fp = open(weightfile, 'rb') + header = np.fromfile(fp, count=5, dtype=np.int32) + self.header = torch.from_numpy(header) + self.seen = self.header[3] + buf = np.fromfile(fp, dtype=np.float32) + fp.close() + + start = 0 + ind = -2 + for block in self.blocks: + if start >= buf.size: + break + ind = ind + 1 + if block['type'] == 'net': + continue + elif block['type'] == 'convolutional': + model = self.models[ind] + batch_normalize = int(block['batch_normalize']) + if batch_normalize: + start = load_conv_bn(buf, start, model[0], model[1]) + else: + start = load_conv(buf, start, model[0]) + elif block['type'] == 'connected': + model = self.models[ind] + if block['activation'] != 'linear': + start = load_fc(buf, start, model[0]) + else: + start = load_fc(buf, start, model) + elif block['type'] == 'maxpool': + pass + elif block['type'] == 'reorg': + pass + elif block['type'] == 'upsample': + pass + elif block['type'] == 'route': + pass + elif block['type'] == 'shortcut': + pass + elif block['type'] == 'yolo': + pass + elif block['type'] == 'avgpool': + pass + elif block['type'] == 'softmax': + pass + elif block['type'] == 'cost': + pass + else: + print('unknown type %s' % (block['type'])) diff --git a/src/perception/lidar_detection/lidar_detection/complex_yolov4/model/darknet_utils.py b/src/perception/lidar_detection/lidar_detection/complex_yolov4/model/darknet_utils.py new file mode 100644 index 000000000..63715a8ff --- /dev/null +++ b/src/perception/lidar_detection/lidar_detection/complex_yolov4/model/darknet_utils.py @@ -0,0 +1,270 @@ +""" +# -*- coding: utf-8 -*- +----------------------------------------------------------------------------------- +# Refer: https://github.com/Tianxiaomo/pytorch-YOLOv4 +""" + +import sys + +import torch + +sys.path.append('../') +from lidar_detection.complex_yolov4.utils.torch_utils import convert2cpu + +__all__ = ['parse_cfg', 'print_cfg', 'load_conv', 'load_conv_bn', 'save_conv', 'save_conv_bn', 'load_fc', 'save_fc'] + + +def parse_cfg(cfgfile): + blocks = [] + fp = open(cfgfile, 'r') + block = None + line = fp.readline() + while line != '': + line = line.rstrip() + if line == '' or line[0] == '#': + line = fp.readline() + continue + elif line[0] == '[': + if block: + blocks.append(block) + block = dict() + block['type'] = line.lstrip('[').rstrip(']') + # set default value + if block['type'] == 'convolutional': + block['batch_normalize'] = 0 + else: + key, value = line.split('=') + key = key.strip() + if key == 'type': + key = '_type' + value = value.strip() + block[key] = value + line = fp.readline() + + if block: + blocks.append(block) + fp.close() + return blocks + + +def print_cfg(blocks): + print('layer filters size input output') + prev_width = 416 + prev_height = 416 + prev_filters = 3 + out_filters = [] + out_widths = [] + out_heights = [] + ind = -2 + for block in blocks: + ind = ind + 1 + if block['type'] == 'net': + prev_width = int(block['width']) + prev_height = int(block['height']) + continue + elif block['type'] == 'convolutional': + filters = int(block['filters']) + kernel_size = int(block['size']) + stride = int(block['stride']) + is_pad = int(block['pad']) + pad = (kernel_size - 1) // 2 if is_pad else 0 + width = (prev_width + 2 * pad - kernel_size) // stride + 1 + height = (prev_height + 2 * pad - kernel_size) // stride + 1 + print('%5d %-6s %4d %d x %d / %d %3d x %3d x%4d -> %3d x %3d x%4d' % ( + ind, 'conv', filters, kernel_size, kernel_size, stride, prev_width, prev_height, prev_filters, width, + height, filters)) + prev_width = width + prev_height = height + prev_filters = filters + out_widths.append(prev_width) + out_heights.append(prev_height) + out_filters.append(prev_filters) + elif block['type'] == 'maxpool': + pool_size = int(block['size']) + stride = int(block['stride']) + width = prev_width // stride + height = prev_height // stride + print('%5d %-6s %d x %d / %d %3d x %3d x%4d -> %3d x %3d x%4d' % ( + ind, 'max', pool_size, pool_size, stride, prev_width, prev_height, prev_filters, width, height, + filters)) + prev_width = width + prev_height = height + prev_filters = filters + out_widths.append(prev_width) + out_heights.append(prev_height) + out_filters.append(prev_filters) + elif block['type'] == 'avgpool': + width = 1 + height = 1 + print('%5d %-6s %3d x %3d x%4d -> %3d' % ( + ind, 'avg', prev_width, prev_height, prev_filters, prev_filters)) + prev_width = width + prev_height = height + prev_filters = filters + out_widths.append(prev_width) + out_heights.append(prev_height) + out_filters.append(prev_filters) + elif block['type'] == 'softmax': + print('%5d %-6s -> %3d' % (ind, 'softmax', prev_filters)) + out_widths.append(prev_width) + out_heights.append(prev_height) + out_filters.append(prev_filters) + elif block['type'] == 'cost': + print('%5d %-6s -> %3d' % (ind, 'cost', prev_filters)) + out_widths.append(prev_width) + out_heights.append(prev_height) + out_filters.append(prev_filters) + elif block['type'] == 'reorg': + stride = int(block['stride']) + filters = stride * stride * prev_filters + width = prev_width // stride + height = prev_height // stride + print('%5d %-6s / %d %3d x %3d x%4d -> %3d x %3d x%4d' % ( + ind, 'reorg', stride, prev_width, prev_height, prev_filters, width, height, filters)) + prev_width = width + prev_height = height + prev_filters = filters + out_widths.append(prev_width) + out_heights.append(prev_height) + out_filters.append(prev_filters) + elif block['type'] == 'upsample': + stride = int(block['stride']) + filters = prev_filters + width = prev_width * stride + height = prev_height * stride + print('%5d %-6s * %d %3d x %3d x%4d -> %3d x %3d x%4d' % ( + ind, 'upsample', stride, prev_width, prev_height, prev_filters, width, height, filters)) + prev_width = width + prev_height = height + prev_filters = filters + out_widths.append(prev_width) + out_heights.append(prev_height) + out_filters.append(prev_filters) + elif block['type'] == 'route': + layers = block['layers'].split(',') + layers = [int(i) if int(i) > 0 else int(i) + ind for i in layers] + if len(layers) == 1: + print('%5d %-6s %d' % (ind, 'route', layers[0])) + prev_width = out_widths[layers[0]] + prev_height = out_heights[layers[0]] + prev_filters = out_filters[layers[0]] + elif len(layers) == 2: + print('%5d %-6s %d %d' % (ind, 'route', layers[0], layers[1])) + prev_width = out_widths[layers[0]] + prev_height = out_heights[layers[0]] + assert (prev_width == out_widths[layers[1]]) + assert (prev_height == out_heights[layers[1]]) + prev_filters = out_filters[layers[0]] + out_filters[layers[1]] + elif len(layers) == 4: + print('%5d %-6s %d %d %d %d' % (ind, 'route', layers[0], layers[1], layers[2], layers[3])) + prev_width = out_widths[layers[0]] + prev_height = out_heights[layers[0]] + assert (prev_width == out_widths[layers[1]] == out_widths[layers[2]] == out_widths[layers[3]]) + assert (prev_height == out_heights[layers[1]] == out_heights[layers[2]] == out_heights[layers[3]]) + prev_filters = out_filters[layers[0]] + out_filters[layers[1]] + out_filters[layers[2]] + out_filters[ + layers[3]] + else: + print("route error !!! {} {} {}".format(sys._getframe().f_code.co_filename, + sys._getframe().f_code.co_name, sys._getframe().f_lineno)) + + out_widths.append(prev_width) + out_heights.append(prev_height) + out_filters.append(prev_filters) + elif block['type'] in ['region', 'yolo']: + print('%5d %-6s' % (ind, 'detection')) + out_widths.append(prev_width) + out_heights.append(prev_height) + out_filters.append(prev_filters) + elif block['type'] == 'shortcut': + from_id = int(block['from']) + from_id = from_id if from_id > 0 else from_id + ind + print('%5d %-6s %d' % (ind, 'shortcut', from_id)) + prev_width = out_widths[from_id] + prev_height = out_heights[from_id] + prev_filters = out_filters[from_id] + out_widths.append(prev_width) + out_heights.append(prev_height) + out_filters.append(prev_filters) + elif block['type'] == 'connected': + filters = int(block['output']) + print('%5d %-6s %d -> %3d' % (ind, 'connected', prev_filters, filters)) + prev_filters = filters + out_widths.append(1) + out_heights.append(1) + out_filters.append(prev_filters) + else: + print('unknown type %s' % (block['type'])) + + +def load_conv(buf, start, conv_model): + num_w = conv_model.weight.numel() + num_b = conv_model.bias.numel() + conv_model.bias.data.copy_(torch.from_numpy(buf[start:start + num_b])) + start = start + num_b + conv_model.weight.data.copy_(torch.from_numpy(buf[start:start + num_w]).reshape(conv_model.weight.data.shape)) + start = start + num_w + return start + + +def save_conv(fp, conv_model): + if conv_model.bias.is_cuda: + convert2cpu(conv_model.bias.data).numpy().tofile(fp) + convert2cpu(conv_model.weight.data).numpy().tofile(fp) + else: + conv_model.bias.data.numpy().tofile(fp) + conv_model.weight.data.numpy().tofile(fp) + + +def load_conv_bn(buf, start, conv_model, bn_model): + num_w = conv_model.weight.numel() + num_b = bn_model.bias.numel() + bn_model.bias.data.copy_(torch.from_numpy(buf[start:start + num_b])) + start = start + num_b + bn_model.weight.data.copy_(torch.from_numpy(buf[start:start + num_b])) + start = start + num_b + bn_model.running_mean.copy_(torch.from_numpy(buf[start:start + num_b])) + start = start + num_b + bn_model.running_var.copy_(torch.from_numpy(buf[start:start + num_b])) + start = start + num_b + conv_model.weight.data.copy_(torch.from_numpy(buf[start:start + num_w]).reshape(conv_model.weight.data.shape)) + start = start + num_w + return start + + +def save_conv_bn(fp, conv_model, bn_model): + if bn_model.bias.is_cuda: + convert2cpu(bn_model.bias.data).numpy().tofile(fp) + convert2cpu(bn_model.weight.data).numpy().tofile(fp) + convert2cpu(bn_model.running_mean).numpy().tofile(fp) + convert2cpu(bn_model.running_var).numpy().tofile(fp) + convert2cpu(conv_model.weight.data).numpy().tofile(fp) + else: + bn_model.bias.data.numpy().tofile(fp) + bn_model.weight.data.numpy().tofile(fp) + bn_model.running_mean.numpy().tofile(fp) + bn_model.running_var.numpy().tofile(fp) + conv_model.weight.data.numpy().tofile(fp) + + +def load_fc(buf, start, fc_model): + num_w = fc_model.weight.numel() + num_b = fc_model.bias.numel() + fc_model.bias.data.copy_(torch.from_numpy(buf[start:start + num_b])) + start = start + num_b + fc_model.weight.data.copy_(torch.from_numpy(buf[start:start + num_w])) + start = start + num_w + return start + + +def save_fc(fp, fc_model): + fc_model.bias.data.numpy().tofile(fp) + fc_model.weight.data.numpy().tofile(fp) + + +if __name__ == '__main__': + import sys + + blocks = parse_cfg('cfg/yolo.cfg') + if len(sys.argv) == 2: + blocks = parse_cfg(sys.argv[1]) + print_cfg(blocks) diff --git a/src/perception/lidar_detection/lidar_detection/complex_yolov4/model/model_utils.py b/src/perception/lidar_detection/lidar_detection/complex_yolov4/model/model_utils.py new file mode 100644 index 000000000..15a55f261 --- /dev/null +++ b/src/perception/lidar_detection/lidar_detection/complex_yolov4/model/model_utils.py @@ -0,0 +1,219 @@ +""" +# -*- coding: utf-8 -*- +----------------------------------------------------------------------------------- +# Author: Nguyen Mau Dung +# DoC: 2020.07.20 +# email: nguyenmaudung93.kstn@gmail.com +----------------------------------------------------------------------------------- +# Description: This script for iou calculation of rotated boxes (on GPU) + +""" + +import sys + +import torch +from shapely.geometry import Polygon +from scipy.spatial import ConvexHull + +sys.path.append('../') + +class Line: + # ax + by + c = 0 + def __init__(self, p1, p2): + """ + + Args: + p1: (x, y) + p2: (x, y) + """ + self.a = p2[1] - p1[1] + self.b = p1[0] - p2[0] + self.c = p2[0] * p1[1] - p2[1] * p1[0] # cross + self.device = p1.device + + def cal_values(self, pts): + return self.a * pts[:, 0] + self.b * pts[:, 1] + self.c + + def find_intersection(self, other): + # See e.g. https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Using_homogeneous_coordinates + if not isinstance(other, Line): + return NotImplemented + w = self.a * other.b - self.b * other.a + return torch.tensor([(self.b * other.c - self.c * other.b) / w, (self.c * other.a - self.a * other.c) / w], + device=self.device) + +def PolyArea2D(pts): + roll_pts = torch.roll(pts, -1, dims=0) + area = (pts[:, 0] * roll_pts[:, 1] - pts[:, 1] * roll_pts[:, 0]).sum().abs() * 0.5 + return area + +def intersection_area(rect1, rect2): + """Calculate the inter + + Args: + rect1: vertices of the rectangles (4, 2) + rect2: vertices of the rectangles (4, 2) + + Returns: + + """ + + # Use the vertices of the first rectangle as, starting vertices of the intersection polygon. + intersection = rect1 + + # Loop over the edges of the second rectangle + roll_rect2 = torch.roll(rect2, -1, dims=0) + for p, q in zip(rect2, roll_rect2): + if len(intersection) <= 2: + break # No intersection + + line = Line(p, q) + + # Any point p with line(p) <= 0 is on the "inside" (or on the boundary), + # any point p with line(p) > 0 is on the "outside". + # Loop over the edges of the intersection polygon, + # and determine which part is inside and which is outside. + new_intersection = [] + line_values = line.cal_values(intersection) + roll_intersection = torch.roll(intersection, -1, dims=0) + roll_line_values = torch.roll(line_values, -1, dims=0) + for s, t, s_value, t_value in zip(intersection, roll_intersection, line_values, roll_line_values): + if s_value <= 0: + new_intersection.append(s) + if s_value * t_value < 0: + # Points are on opposite sides. + # Add the intersection of the lines to new_intersection. + intersection_point = line.find_intersection(Line(s, t)) + new_intersection.append(intersection_point) + + if len(new_intersection) > 0: + intersection = torch.stack(new_intersection) + else: + break + + # Calculate area + if len(intersection) <= 2: + return 0. + + return PolyArea2D(intersection) + + +def cvt_box_2_polygon(box): + """ + :param array: an array of shape [num_conners, 2] + :return: a shapely.geometry.Polygon object + """ + # use .buffer(0) to fix a line polygon + # more infor: https://stackoverflow.com/questions/13062334/polygon-intersection-error-in-shapely-shapely-geos-topologicalerror-the-opera + return Polygon([(box[i, 0], box[i, 1]) for i in range(len(box))]).buffer(0) + + +def get_corners_vectorize(x, y, w, l, yaw): + """bev image coordinates format - vectorization + + :param x, y, w, l, yaw: [num_boxes,] + :return: num_boxes x (x,y) of 4 conners + """ + device = x.device + bbox2 = torch.zeros((x.size(0), 4, 2), device=device, dtype=torch.float) + cos_yaw = torch.cos(yaw) + sin_yaw = torch.sin(yaw) + + # front left + bbox2[:, 0, 0] = x - w / 2 * cos_yaw - l / 2 * sin_yaw + bbox2[:, 0, 1] = y - w / 2 * sin_yaw + l / 2 * cos_yaw + + # rear left + bbox2[:, 1, 0] = x - w / 2 * cos_yaw + l / 2 * sin_yaw + bbox2[:, 1, 1] = y - w / 2 * sin_yaw - l / 2 * cos_yaw + + # rear right + bbox2[:, 2, 0] = x + w / 2 * cos_yaw + l / 2 * sin_yaw + bbox2[:, 2, 1] = y + w / 2 * sin_yaw - l / 2 * cos_yaw + + # front right + bbox2[:, 3, 0] = x + w / 2 * cos_yaw - l / 2 * sin_yaw + bbox2[:, 3, 1] = y + w / 2 * sin_yaw + l / 2 * cos_yaw + + return bbox2 + + +def get_polygons_areas_fix_xy(boxes, fix_xy=100.): + """ + Args: + box: (num_boxes, 4) --> w, l, im, re + """ + device = boxes.device + n_boxes = boxes.size(0) + x = torch.full(size=(n_boxes,), fill_value=fix_xy, device=device, dtype=torch.float) + y = torch.full(size=(n_boxes,), fill_value=fix_xy, device=device, dtype=torch.float) + w, l, im, re = boxes.t() + yaw = torch.atan2(im, re) + boxes_conners = get_corners_vectorize(x, y, w, l, yaw) + boxes_polygons = [cvt_box_2_polygon(box_) for box_ in boxes_conners] + boxes_areas = w * l + + return boxes_polygons, boxes_areas + + +def iou_rotated_boxes_targets_vs_anchors(anchors_polygons, anchors_areas, targets_polygons, targets_areas): + device = anchors_areas.device + num_anchors = len(anchors_areas) + num_targets_boxes = len(targets_areas) + + ious = torch.zeros(size=(num_anchors, num_targets_boxes), device=device, dtype=torch.float) + + for a_idx in range(num_anchors): + for tg_idx in range(num_targets_boxes): + intersection = anchors_polygons[a_idx].intersection(targets_polygons[tg_idx]).area + iou = intersection / (anchors_areas[a_idx] + targets_areas[tg_idx] - intersection + 1e-16) + ious[a_idx, tg_idx] = iou + + return ious + + +def iou_pred_vs_target_boxes(pred_boxes, target_boxes, GIoU=False, DIoU=False, CIoU=False): + assert pred_boxes.size() == target_boxes.size(), "Unmatch size of pred_boxes and target_boxes" + device = pred_boxes.device + n_boxes = pred_boxes.size(0) + + t_x, t_y, t_w, t_l, t_im, t_re = target_boxes.t() + t_yaw = torch.atan2(t_im, t_re) + t_conners = get_corners_vectorize(t_x, t_y, t_w, t_l, t_yaw) + t_areas = t_w * t_l + + p_x, p_y, p_w, p_l, p_im, p_re = pred_boxes.t() + p_yaw = torch.atan2(p_im, p_re) + p_conners = get_corners_vectorize(p_x, p_y, p_w, p_l, p_yaw) + p_areas = p_w * p_l + + ious = [] + giou_loss = torch.tensor([0.], device=device, dtype=torch.float) + # Thinking to apply vectorization this step + for box_idx in range(n_boxes): + p_cons, t_cons = p_conners[box_idx], t_conners[box_idx] + if not GIoU: + p_poly, t_poly = cvt_box_2_polygon(p_cons), cvt_box_2_polygon(t_cons) + intersection = p_poly.intersection(t_poly).area + else: + intersection = intersection_area(p_cons, t_cons) + + p_area, t_area = p_areas[box_idx], t_areas[box_idx] + union = p_area + t_area - intersection + iou = intersection / (union + 1e-16) + + if GIoU: + convex_conners = torch.cat((p_cons, t_cons), dim=0) + hull = ConvexHull(convex_conners.clone().detach().cpu().numpy()) # done on cpu, just need indices output + convex_conners = convex_conners[hull.vertices] + convex_area = PolyArea2D(convex_conners) + giou_loss += 1. - (iou - (convex_area - union) / (convex_area + 1e-16)) + else: + giou_loss += 1. - iou + + if DIoU or CIoU: + raise NotImplementedError + + ious.append(iou) + + return torch.tensor(ious, device=device, dtype=torch.float), giou_loss \ No newline at end of file diff --git a/src/perception/lidar_detection/lidar_detection/complex_yolov4/model/yolo_layer.py b/src/perception/lidar_detection/lidar_detection/complex_yolov4/model/yolo_layer.py new file mode 100644 index 000000000..82cd969ea --- /dev/null +++ b/src/perception/lidar_detection/lidar_detection/complex_yolov4/model/yolo_layer.py @@ -0,0 +1,253 @@ +""" +# -*- coding: utf-8 -*- +----------------------------------------------------------------------------------- +# Author: Nguyen Mau Dung +# DoC: 2020.07.05 +# email: nguyenmaudung93.kstn@gmail.com +----------------------------------------------------------------------------------- +# Description: This script for the yolo layer + +# Refer: https://github.com/Tianxiaomo/pytorch-YOLOv4 +# Refer: https://github.com/VCasecnikovs/Yet-Another-YOLOv4-Pytorch +""" + +import sys + +import torch +import torch.nn as nn +import torch.nn.functional as F + +sys.path.append('../') + +from lidar_detection.complex_yolov4.utils.torch_utils import to_cpu +from lidar_detection.complex_yolov4.model.model_utils import iou_pred_vs_target_boxes, iou_rotated_boxes_targets_vs_anchors, \ + get_polygons_areas_fix_xy + + +class YoloLayer(nn.Module): + """Yolo layer""" + + def __init__(self, num_classes, anchors, stride, scale_x_y, ignore_thresh): + super(YoloLayer, self).__init__() + # Update the attributions when parsing the cfg during create the darknet + self.num_classes = num_classes + self.anchors = anchors + self.num_anchors = len(anchors) + self.stride = stride + self.scale_x_y = scale_x_y + self.ignore_thresh = ignore_thresh + + self.noobj_scale = 100 + self.obj_scale = 1 + self.lgiou_scale = 3.54 + self.leular_scale = 3.54 + self.lobj_scale = 64.3 + self.lcls_scale = 37.4 + + self.seen = 0 + # Initialize dummy variables + self.grid_size = 0 + self.img_size = 0 + self.metrics = {} + + def compute_grid_offsets(self, grid_size): + self.grid_size = grid_size + g = self.grid_size + self.stride = self.img_size / self.grid_size + # Calculate offsets for each grid + self.grid_x = torch.arange(g, device=self.device, dtype=torch.float).repeat(g, 1).view([1, 1, g, g]) + self.grid_y = torch.arange(g, device=self.device, dtype=torch.float).repeat(g, 1).t().view([1, 1, g, g]) + self.scaled_anchors = torch.tensor( + [(a_w / self.stride, a_h / self.stride, im, re) for a_w, a_h, im, re in self.anchors], device=self.device, + dtype=torch.float) + self.anchor_w = self.scaled_anchors[:, 0:1].view((1, self.num_anchors, 1, 1)) + self.anchor_h = self.scaled_anchors[:, 1:2].view((1, self.num_anchors, 1, 1)) + + # Pre compute polygons and areas of anchors + self.scaled_anchors_polygons, self.scaled_anchors_areas = get_polygons_areas_fix_xy(self.scaled_anchors) + + def build_targets(self, pred_boxes, pred_cls, target, anchors): + """ Built yolo targets to compute loss + :param out_boxes: [num_samples or batch, num_anchors, grid_size, grid_size, 6] + :param pred_cls: [num_samples or batch, num_anchors, grid_size, grid_size, num_classes] + :param target: [num_boxes, 8] + :param anchors: [num_anchors, 4] + :return: + """ + nB, nA, nG, _, nC = pred_cls.size() + n_target_boxes = target.size(0) + + # Create output tensors on "device" + obj_mask = torch.full(size=(nB, nA, nG, nG), fill_value=0, device=self.device, dtype=torch.uint8) + noobj_mask = torch.full(size=(nB, nA, nG, nG), fill_value=1, device=self.device, dtype=torch.uint8) + class_mask = torch.full(size=(nB, nA, nG, nG), fill_value=0, device=self.device, dtype=torch.float) + iou_scores = torch.full(size=(nB, nA, nG, nG), fill_value=0, device=self.device, dtype=torch.float) + tx = torch.full(size=(nB, nA, nG, nG), fill_value=0, device=self.device, dtype=torch.float) + ty = torch.full(size=(nB, nA, nG, nG), fill_value=0, device=self.device, dtype=torch.float) + tw = torch.full(size=(nB, nA, nG, nG), fill_value=0, device=self.device, dtype=torch.float) + th = torch.full(size=(nB, nA, nG, nG), fill_value=0, device=self.device, dtype=torch.float) + tim = torch.full(size=(nB, nA, nG, nG), fill_value=0, device=self.device, dtype=torch.float) + tre = torch.full(size=(nB, nA, nG, nG), fill_value=0, device=self.device, dtype=torch.float) + tcls = torch.full(size=(nB, nA, nG, nG, nC), fill_value=0, device=self.device, dtype=torch.float) + tconf = obj_mask.float() + giou_loss = torch.tensor([0.], device=self.device, dtype=torch.float) + + if n_target_boxes > 0: # Make sure that there is at least 1 box + b, target_labels = target[:, :2].long().t() + target_boxes = torch.cat((target[:, 2:6] * nG, target[:, 6:8]), dim=-1) # scale up x, y, w, h + + gxy = target_boxes[:, :2] + gwh = target_boxes[:, 2:4] + gimre = target_boxes[:, 4:6] + + targets_polygons, targets_areas = get_polygons_areas_fix_xy(target_boxes[:, 2:6]) + # Get anchors with best iou + ious = iou_rotated_boxes_targets_vs_anchors(self.scaled_anchors_polygons, self.scaled_anchors_areas, + targets_polygons, targets_areas) + best_ious, best_n = ious.max(0) + + gx, gy = gxy.t() + gw, gh = gwh.t() + gim, gre = gimre.t() + gi, gj = gxy.long().t() + # Set masks + obj_mask[b, best_n, gj, gi] = 1 + noobj_mask[b, best_n, gj, gi] = 0 + + # Set noobj mask to zero where iou exceeds ignore threshold + for i, anchor_ious in enumerate(ious.t()): + noobj_mask[b[i], anchor_ious > self.ignore_thresh, gj[i], gi[i]] = 0 + + # Coordinates + tx[b, best_n, gj, gi] = gx - gx.floor() + ty[b, best_n, gj, gi] = gy - gy.floor() + # Width and height + tw[b, best_n, gj, gi] = torch.log(gw / anchors[best_n][:, 0] + 1e-16) + th[b, best_n, gj, gi] = torch.log(gh / anchors[best_n][:, 1] + 1e-16) + # Im and real part + tim[b, best_n, gj, gi] = gim + tre[b, best_n, gj, gi] = gre + + # One-hot encoding of label + tcls[b, best_n, gj, gi, target_labels] = 1 + class_mask[b, best_n, gj, gi] = (pred_cls[b, best_n, gj, gi].argmax(-1) == target_labels).float() + ious, giou_loss = iou_pred_vs_target_boxes(pred_boxes[b, best_n, gj, gi], target_boxes, + GIoU=self.use_giou_loss) + iou_scores[b, best_n, gj, gi] = ious + if self.reduction == 'mean': + giou_loss /= n_target_boxes + tconf = obj_mask.float() + + return iou_scores, giou_loss, class_mask, obj_mask.type(torch.bool), noobj_mask.type(torch.bool), \ + tx, ty, tw, th, tim, tre, tcls, tconf + + def forward(self, x, targets=None, img_size=608, use_giou_loss=False): + """ + :param x: [num_samples or batch, num_anchors * (6 + 1 + num_classes), grid_size, grid_size] + :param targets: [num boxes, 8] (box_idx, class, x, y, w, l, sin(yaw), cos(yaw)) + :param img_size: default 608 + :return: + """ + self.img_size = img_size + self.use_giou_loss = use_giou_loss + self.device = x.device + num_samples, _, _, grid_size = x.size() + + prediction = x.view(num_samples, self.num_anchors, self.num_classes + 7, grid_size, grid_size) + prediction = prediction.permute(0, 1, 3, 4, 2).contiguous() + # prediction size: [num_samples, num_anchors, grid_size, grid_size, num_classes + 7] + + # Get outputs + pred_x = torch.sigmoid(prediction[..., 0]) + pred_y = torch.sigmoid(prediction[..., 1]) + pred_w = prediction[..., 2] # Width + pred_h = prediction[..., 3] # Height + pred_im = prediction[..., 4] # angle imaginary part + pred_re = prediction[..., 5] # angle real part + pred_conf = torch.sigmoid(prediction[..., 6]) # Conf + pred_cls = torch.sigmoid(prediction[..., 7:]) # Cls pred. + + # If grid size does not match current we compute new offsets + if grid_size != self.grid_size: + self.compute_grid_offsets(grid_size) + + # Add offset and scale with anchors + # pred_boxes size: [num_samples, num_anchors, grid_size, grid_size, 6] + pred_boxes = torch.empty(prediction[..., :6].shape, device=self.device, dtype=torch.float) + pred_boxes[..., 0] = pred_x + self.grid_x + pred_boxes[..., 1] = pred_y + self.grid_y + pred_boxes[..., 2] = torch.exp(pred_w).clamp(max=1E3) * self.anchor_w + pred_boxes[..., 3] = torch.exp(pred_h).clamp(max=1E3) * self.anchor_h + pred_boxes[..., 4] = pred_im + pred_boxes[..., 5] = pred_re + + output = torch.cat(( + pred_boxes[..., :4].view(num_samples, -1, 4) * self.stride, + pred_boxes[..., 4:6].view(num_samples, -1, 2), + pred_conf.view(num_samples, -1, 1), + pred_cls.view(num_samples, -1, self.num_classes), + ), dim=-1) + # output size: [num_samples, num boxes, 7 + num_classes] + + if targets is None: + return output, 0 + else: + self.reduction = 'mean' + iou_scores, giou_loss, class_mask, obj_mask, noobj_mask, tx, ty, tw, th, tim, tre, tcls, tconf = self.build_targets( + pred_boxes=pred_boxes, pred_cls=pred_cls, target=targets, anchors=self.scaled_anchors) + + loss_x = F.mse_loss(pred_x[obj_mask], tx[obj_mask], reduction=self.reduction) + loss_y = F.mse_loss(pred_y[obj_mask], ty[obj_mask], reduction=self.reduction) + loss_w = F.mse_loss(pred_w[obj_mask], tw[obj_mask], reduction=self.reduction) + loss_h = F.mse_loss(pred_h[obj_mask], th[obj_mask], reduction=self.reduction) + loss_im = F.mse_loss(pred_im[obj_mask], tim[obj_mask], reduction=self.reduction) + loss_re = F.mse_loss(pred_re[obj_mask], tre[obj_mask], reduction=self.reduction) + loss_im_re = (1. - torch.sqrt(pred_im[obj_mask] ** 2 + pred_re[obj_mask] ** 2)) ** 2 # as tim^2 + tre^2 = 1 + loss_im_re_red = loss_im_re.sum() if self.reduction == 'sum' else loss_im_re.mean() + loss_eular = loss_im + loss_re + loss_im_re_red + + loss_conf_obj = F.binary_cross_entropy(pred_conf[obj_mask], tconf[obj_mask], reduction=self.reduction) + loss_conf_noobj = F.binary_cross_entropy(pred_conf[noobj_mask], tconf[noobj_mask], reduction=self.reduction) + loss_cls = F.binary_cross_entropy(pred_cls[obj_mask], tcls[obj_mask], reduction=self.reduction) + + if self.use_giou_loss: + loss_obj = loss_conf_obj + loss_conf_noobj + total_loss = giou_loss * self.lgiou_scale + loss_eular * self.leular_scale + loss_obj * self.lobj_scale + loss_cls * self.lcls_scale + else: + loss_obj = self.obj_scale * loss_conf_obj + self.noobj_scale * loss_conf_noobj + total_loss = loss_x + loss_y + loss_w + loss_h + loss_eular + loss_obj + loss_cls + + # Metrics (store loss values using tensorboard) + cls_acc = 100 * class_mask[obj_mask].mean() + conf_obj = pred_conf[obj_mask].mean() + conf_noobj = pred_conf[noobj_mask].mean() + conf50 = (pred_conf > 0.5).float() + iou50 = (iou_scores > 0.5).float() + iou75 = (iou_scores > 0.75).float() + detected_mask = conf50 * class_mask * tconf + precision = torch.sum(iou50 * detected_mask) / (conf50.sum() + 1e-16) + recall50 = torch.sum(iou50 * detected_mask) / (obj_mask.sum() + 1e-16) + recall75 = torch.sum(iou75 * detected_mask) / (obj_mask.sum() + 1e-16) + + self.metrics = { + "loss": to_cpu(total_loss).item(), + "iou_score": to_cpu(iou_scores[obj_mask].mean()).item(), + 'giou_loss': to_cpu(giou_loss).item(), + 'loss_x': to_cpu(loss_x).item(), + 'loss_y': to_cpu(loss_y).item(), + 'loss_w': to_cpu(loss_w).item(), + 'loss_h': to_cpu(loss_h).item(), + 'loss_eular': to_cpu(loss_eular).item(), + 'loss_im': to_cpu(loss_im).item(), + 'loss_re': to_cpu(loss_re).item(), + "loss_obj": to_cpu(loss_obj).item(), + "loss_cls": to_cpu(loss_cls).item(), + "cls_acc": to_cpu(cls_acc).item(), + "recall50": to_cpu(recall50).item(), + "recall75": to_cpu(recall75).item(), + "precision": to_cpu(precision).item(), + "conf_obj": to_cpu(conf_obj).item(), + "conf_noobj": to_cpu(conf_noobj).item() + } + + return output, total_loss diff --git a/src/perception/lidar_detection/lidar_detection/complex_yolov4/utils/__init__.py b/src/perception/lidar_detection/lidar_detection/complex_yolov4/utils/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/perception/lidar_detection/lidar_detection/complex_yolov4/utils/evaluation_utils.py b/src/perception/lidar_detection/lidar_detection/complex_yolov4/utils/evaluation_utils.py new file mode 100644 index 000000000..2980295ef --- /dev/null +++ b/src/perception/lidar_detection/lidar_detection/complex_yolov4/utils/evaluation_utils.py @@ -0,0 +1,114 @@ +import sys +import torch +import numpy as np +from shapely.geometry import Polygon + +sys.path.append('../') + +from lidar_detection.complex_yolov4.utils.kitti_bev_utils import get_corners + + +def cvt_box_2_polygon(box): + """ + :param box: an array of shape [4, 2] + :return: a shapely.geometry.Polygon object + """ + # use .buffer(0) to fix a line polygon + # more infor: https://stackoverflow.com/questions/13062334/polygon-intersection-error-in-shapely-shapely-geos-topologicalerror-the-opera + return Polygon([(box[i, 0], box[i, 1]) for i in range(len(box))]).buffer(0) + + +def iou_rotated_single_vs_multi_boxes_cpu(single_box, multi_boxes): + """ + :param pred_box: Numpy array + :param target_boxes: Numpy array + :return: + """ + + s_x, s_y, s_w, s_l, s_im, s_re = single_box + s_area = s_w * s_l + s_yaw = np.arctan2(s_im, s_re) + s_conners = get_corners(s_x, s_y, s_w, s_l, s_yaw) + s_polygon = cvt_box_2_polygon(s_conners) + + m_x, m_y, m_w, m_l, m_im, m_re = multi_boxes.transpose(1, 0) + targets_areas = m_w * m_l + m_yaw = np.arctan2(m_im, m_re) + m_boxes_conners = get_corners_vectorize(m_x, m_y, m_w, m_l, m_yaw) + m_boxes_polygons = [cvt_box_2_polygon(box_) for box_ in m_boxes_conners] + + ious = [] + for m_idx in range(multi_boxes.shape[0]): + intersection = s_polygon.intersection(m_boxes_polygons[m_idx]).area + iou_ = intersection / (s_area + targets_areas[m_idx] - intersection + 1e-16) + ious.append(iou_) + + return torch.tensor(ious, dtype=torch.float) + + +def get_corners_vectorize(x, y, w, l, yaw): + """bev image coordinates format - vectorization + + :param x, y, w, l, yaw: [num_boxes,] + :return: num_boxes x (x,y) of 4 conners + """ + bbox2 = np.zeros((x.shape[0], 4, 2), dtype=np.float32) + cos_yaw = np.cos(yaw) + sin_yaw = np.sin(yaw) + + # front left + bbox2[:, 0, 0] = x - w / 2 * cos_yaw - l / 2 * sin_yaw + bbox2[:, 0, 1] = y - w / 2 * sin_yaw + l / 2 * cos_yaw + + # rear left + bbox2[:, 1, 0] = x - w / 2 * cos_yaw + l / 2 * sin_yaw + bbox2[:, 1, 1] = y - w / 2 * sin_yaw - l / 2 * cos_yaw + + # rear right + bbox2[:, 2, 0] = x + w / 2 * cos_yaw + l / 2 * sin_yaw + bbox2[:, 2, 1] = y + w / 2 * sin_yaw - l / 2 * cos_yaw + + # front right + bbox2[:, 3, 0] = x + w / 2 * cos_yaw - l / 2 * sin_yaw + bbox2[:, 3, 1] = y + w / 2 * sin_yaw + l / 2 * cos_yaw + + return bbox2 + + +def post_processing_v2(prediction, conf_thresh=0.95, nms_thresh=0.4): + """ + Removes detections with lower object confidence score than 'conf_thres' and performs + Non-Maximum Suppression to further filter detections. + Returns detections with shape: + (x, y, w, l, im, re, object_conf, class_score, class_pred) + """ + output = [None for _ in range(len(prediction))] + for image_i, image_pred in enumerate(prediction): + # Filter out confidence scores below threshold + image_pred = image_pred[image_pred[:, 6] >= conf_thresh] + # If none are remaining => process next image + if not image_pred.size(0): + continue + # Object confidence times class confidence + score = image_pred[:, 6] * image_pred[:, 7:].max(dim=1)[0] + # Sort by it + image_pred = image_pred[(-score).argsort()] + class_confs, class_preds = image_pred[:, 7:].max(dim=1, keepdim=True) + detections = torch.cat((image_pred[:, :7].float(), class_confs.float(), class_preds.float()), dim=1) + # Perform non-maximum suppression + keep_boxes = [] + while detections.size(0): + # large_overlap = rotated_bbox_iou(detections[0, :6].unsqueeze(0), detections[:, :6], 1.0, False) > nms_thres # not working + large_overlap = iou_rotated_single_vs_multi_boxes_cpu(detections[0, :6], detections[:, :6]) > nms_thresh + label_match = detections[0, -1] == detections[:, -1] + # Indices of boxes with lower confidence scores, large IOUs and matching labels + invalid = large_overlap & label_match + weights = detections[invalid, 6:7] + # Merge overlapping bboxes by order of confidence + detections[0, :6] = (weights * detections[invalid, :6]).sum(0) / weights.sum() + keep_boxes += [detections[0]] + detections = detections[~invalid] + if len(keep_boxes) > 0: + output[image_i] = torch.stack(keep_boxes) + + return output diff --git a/src/perception/lidar_detection/lidar_detection/complex_yolov4/utils/kitti_bev_utils.py b/src/perception/lidar_detection/lidar_detection/complex_yolov4/utils/kitti_bev_utils.py new file mode 100644 index 000000000..82f6aaca1 --- /dev/null +++ b/src/perception/lidar_detection/lidar_detection/complex_yolov4/utils/kitti_bev_utils.py @@ -0,0 +1,148 @@ +""" +# -*- coding: utf-8 -*- +----------------------------------------------------------------------------------- +# Refer: https://github.com/ghimiredhikura/Complex-YOLOv3 +""" + +import sys +import numpy as np + +sys.path.append('../') + +import lidar_detection.complex_yolov4.utils.kitti_config as cnf + + +def removePoints(PointCloud, BoundaryCond): + # Boundary condition + minX = BoundaryCond['minX'] + maxX = BoundaryCond['maxX'] + minY = BoundaryCond['minY'] + maxY = BoundaryCond['maxY'] + minZ = BoundaryCond['minZ'] + maxZ = BoundaryCond['maxZ'] + + # Remove the point out of range x,y,z + mask = np.where((PointCloud[:, 0] >= minX) & (PointCloud[:, 0] <= maxX) & (PointCloud[:, 1] >= minY) & ( + PointCloud[:, 1] <= maxY) & (PointCloud[:, 2] >= minZ) & (PointCloud[:, 2] <= maxZ)) + PointCloud = PointCloud[mask] + + PointCloud[:, 2] = PointCloud[:, 2] - minZ + + return PointCloud + + +def makeBVFeature(PointCloud_, Discretization, bc): + Height = cnf.BEV_HEIGHT + 1 + Width = cnf.BEV_WIDTH + 1 + + # Discretize Feature Map + PointCloud = np.copy(PointCloud_) + PointCloud[:, 0] = np.int_(np.floor(PointCloud[:, 0] / Discretization)) + PointCloud[:, 1] = np.int_(np.floor(PointCloud[:, 1] / Discretization) + Width / 2) + + # sort-3times + indices = np.lexsort((-PointCloud[:, 2], PointCloud[:, 1], PointCloud[:, 0])) + PointCloud = PointCloud[indices] + + # Height Map + heightMap = np.zeros((Height, Width)) + + _, indices = np.unique(PointCloud[:, 0:2], axis=0, return_index=True) + PointCloud_frac = PointCloud[indices] + # some important problem is image coordinate is (y,x), not (x,y) + max_height = float(np.abs(bc['maxZ'] - bc['minZ'])) + heightMap[np.int_(PointCloud_frac[:, 0]), np.int_(PointCloud_frac[:, 1])] = PointCloud_frac[:, 2] / max_height + + # Intensity Map & DensityMap + intensityMap = np.zeros((Height, Width)) + densityMap = np.zeros((Height, Width)) + + _, indices, counts = np.unique(PointCloud[:, 0:2], axis=0, return_index=True, return_counts=True) + PointCloud_top = PointCloud[indices] + + normalizedCounts = np.minimum(1.0, np.log(counts + 1) / np.log(64)) + + intensityMap[np.int_(PointCloud_top[:, 0]), np.int_(PointCloud_top[:, 1])] = PointCloud_top[:, 3] + densityMap[np.int_(PointCloud_top[:, 0]), np.int_(PointCloud_top[:, 1])] = normalizedCounts + + RGB_Map = np.zeros((3, Height - 1, Width - 1)) + RGB_Map[2, :, :] = densityMap[:cnf.BEV_HEIGHT, :cnf.BEV_WIDTH] # r_map + RGB_Map[1, :, :] = heightMap[:cnf.BEV_HEIGHT, :cnf.BEV_WIDTH] # g_map + RGB_Map[0, :, :] = intensityMap[:cnf.BEV_HEIGHT, :cnf.BEV_WIDTH] # b_map + + return RGB_Map + + +# bev image coordinates format +def get_corners(x, y, w, l, yaw): + bev_corners = np.zeros((4, 2), dtype=np.float32) + cos_yaw = np.cos(yaw) + sin_yaw = np.sin(yaw) + # front left + bev_corners[0, 0] = x - w / 2 * cos_yaw - l / 2 * sin_yaw + bev_corners[0, 1] = y - w / 2 * sin_yaw + l / 2 * cos_yaw + + # rear left + bev_corners[1, 0] = x - w / 2 * cos_yaw + l / 2 * sin_yaw + bev_corners[1, 1] = y - w / 2 * sin_yaw - l / 2 * cos_yaw + + # rear right + bev_corners[2, 0] = x + w / 2 * cos_yaw + l / 2 * sin_yaw + bev_corners[2, 1] = y + w / 2 * sin_yaw - l / 2 * cos_yaw + + # front right + bev_corners[3, 0] = x + w / 2 * cos_yaw - l / 2 * sin_yaw + bev_corners[3, 1] = y + w / 2 * sin_yaw + l / 2 * cos_yaw + + return bev_corners + +# From Nova, converts ComplexYolo 2D boxes into BoundingBox3D ros2 msg +def get_corners_3d(x, y, z, w, l, h, yaw): + bev_corners = np.zeros((8, 3), dtype=np.float64) + cos_yaw = np.cos(yaw) + sin_yaw = np.sin(yaw) + + # front right + bev_corners[[0,1], 0] = x + w / 2 * cos_yaw - l / 2 * sin_yaw + bev_corners[[0,1], 1] = y + w / 2 * sin_yaw + l / 2 * cos_yaw + + # front left + bev_corners[[2,3], 0] = x - w / 2 * cos_yaw - l / 2 * sin_yaw + bev_corners[[2,3], 1] = y - w / 2 * sin_yaw + l / 2 * cos_yaw + + # rear right + bev_corners[[4,5], 0] = x + w / 2 * cos_yaw + l / 2 * sin_yaw + bev_corners[[4,5], 1] = y + w / 2 * sin_yaw - l / 2 * cos_yaw + + # rear left + bev_corners[[6,7], 0] = x - w / 2 * cos_yaw + l / 2 * sin_yaw + bev_corners[[6,7], 1] = y - w / 2 * sin_yaw - l / 2 * cos_yaw + + # top corners + bev_corners[[1,2,5,6], 2] = z + h + + # bottom corners + bev_corners[[0,3,4,7], 2] = z + + return bev_corners + + +def inverse_yolo_target(targets, img_size, bc): + labels = [] + for t in targets: + y, x, l, w, im, re, c_conf, *_, c = t # swaps x and y + z, h = 0.0, 1.5 + if c == 1: + h = 1.8 + elif c == 2: + h = 1.4 + + y = (y / img_size) * (bc["maxY"] - bc["minY"]) + bc["minY"] + x = (x / img_size) * (bc["maxX"] - bc["minX"]) + bc["minX"] + w = (w / img_size) * (bc["maxY"] - bc["minY"]) + l = (l / img_size) * (bc["maxX"] - bc["minX"]) + w -= 0.3 + l -= 0.3 + labels.append([c, c_conf, x, y, z, w, l, h, - np.arctan2(im, re) - 2 * np.pi]) + + return np.array(labels) diff --git a/src/perception/lidar_detection/lidar_detection/complex_yolov4/utils/kitti_config.py b/src/perception/lidar_detection/lidar_detection/complex_yolov4/utils/kitti_config.py new file mode 100644 index 000000000..37b24b29a --- /dev/null +++ b/src/perception/lidar_detection/lidar_detection/complex_yolov4/utils/kitti_config.py @@ -0,0 +1,36 @@ +class_list = ["Car", "Pedestrian", "Cyclist"] + +CLASS_NAME_TO_ID = { + 'Car': 0, + 'Pedestrian': 1, + 'Cyclist': 2, + 'Van': 0, + 'Person_sitting': 1, +} + +# Front side (of vehicle) Point Cloud boundary for BEV +boundary = { + "minX": 0, + "maxX": 50, + "minY": -25, + "maxY": 25, + "minZ": -1.23, #-0.73, #-2.73, # -(height to ground + 1) + "maxZ": 2.77 #3.27 #1.27 +} + +# Back back (of vehicle) Point Cloud boundary for BEV +boundary_back = { + "minX": -50, + "maxX": 0, + "minY": -25, + "maxY": 25, + "minZ": -2.73, + "maxZ": 1.27 +} + +BEV_WIDTH = 608 # across y axis -25m ~ 25m +BEV_HEIGHT = 608 # across x axis 0m ~ 50m + +DISCRETIZATION = (boundary["maxX"] - boundary["minX"]) / BEV_HEIGHT + +colors = [[0, 255, 255], [0, 0, 255], [255, 0, 0]] diff --git a/src/perception/lidar_detection/lidar_detection/complex_yolov4/utils/torch_utils.py b/src/perception/lidar_detection/lidar_detection/complex_yolov4/utils/torch_utils.py new file mode 100644 index 000000000..b5e6684b9 --- /dev/null +++ b/src/perception/lidar_detection/lidar_detection/complex_yolov4/utils/torch_utils.py @@ -0,0 +1,27 @@ +""" +# -*- coding: utf-8 -*- +----------------------------------------------------------------------------------- +# Author: Nguyen Mau Dung +# DoC: 2020.07.05 +# email: nguyenmaudung93.kstn@gmail.com +----------------------------------------------------------------------------------- +# Description: some utilities of torch (conversion) +----------------------------------------------------------------------------------- +# Refer: https://github.com/Tianxiaomo/pytorch-YOLOv4 +""" + +import torch + +__all__ = ['convert2cpu', 'convert2cpu_long', 'to_cpu'] + + +def convert2cpu(gpu_matrix): + return torch.FloatTensor(gpu_matrix.size()).copy_(gpu_matrix) + + +def convert2cpu_long(gpu_matrix): + return torch.LongTensor(gpu_matrix.size()).copy_(gpu_matrix) + + +def to_cpu(tensor): + return tensor.detach().cpu() diff --git a/src/perception/lidar_detection/lidar_detection/complex_yolov4_model.py b/src/perception/lidar_detection/lidar_detection/complex_yolov4_model.py new file mode 100644 index 000000000..d13426839 --- /dev/null +++ b/src/perception/lidar_detection/lidar_detection/complex_yolov4_model.py @@ -0,0 +1,147 @@ +""" +Package: lidar_detection +Filename: complex_yolov4_model.py +Author: Gueren Sanford +Email: guerensanford@gmail.com +Copyright: 2021, Nova UTD +License: MIT License +Use the ComplexYOLOv4Model class to make object detections with LiDAR data in the format +[x,y,z,intensity]. We use the model Complex Yolo to make the +predictions, which requires a birds eye view map of the LiDAR data. +With the predictions, feed it into the postprocess to filter out and +format the results. +""" + +# Python Imports +import random +import numpy as np +import ros2_numpy as rnp +import torch + +# Model Imports +from lidar_detection.complex_yolov4.model.darknet2pytorch import Darknet +from lidar_detection.complex_yolov4.utils import kitti_bev_utils +from lidar_detection.complex_yolov4.utils.evaluation_utils import post_processing_v2 +import lidar_detection.complex_yolov4.utils.kitti_config as cnf + +# Message Imports +from geometry_msgs.msg import Point +from sensor_msgs.msg import PointCloud2 +from navigator_msgs.msg import BoundingBox3D, Object3D, Object3DArray + +# Paths to pretrained model and config +config_path = './data/perception/configs/complex_yolov4.cfg' +checkpoint_path = './data/perception/models/complex_yolov4_mse_loss.pth' + +class ComplexYOLOv4Model(): + def __init__(self, device: torch.device): + """! Initializes the node. + @param device[torch.device] The device the model will run on + """ + self.img_size = 608 # Determined by the birds eye view setup + self.device = device + + # Setup for the Complex Yolo model + self.model = Darknet(cfgfile=config_path, + use_giou_loss=True) # generalized intersection over union + self.model.load_state_dict(torch.load(checkpoint_path, + map_location=self.device)) + self.model.to(device=self.device) + + def preprocess(self, lidar_msg: PointCloud2): + """! Converts ROS LiDAR message into the bird's eye view input + tensor for the model. + @param lidar_msg[PointCloud2] The ros2 lidar message data + formatted as a PointCloud2 message. + @return torch.Tensor A 608 x 608 birds eye view image of the + point cloud data. Red = Intensity, Green = Height, Blue = + Density + """ + + # Point cloud byte stream -> np array with all fields and dtypes + lidar_np = rnp.numpify(lidar_msg) + # Reduces the vertical res from 128 to 64 + lidar_np = lidar_np[::2] # matches Kitti dataset + # Combines necessary lidar data into an array [n, n, n, n]] + lidar_pcd = np.array([ # Avoids np array dtype issues + lidar_np['x'].flatten(), + lidar_np['y'].flatten(), + lidar_np['z'].flatten(), + lidar_np['reflectivity'].flatten()]) + # Tranforms array to shape [n, (x y z r)] + lidar_pcd = np.transpose(lidar_pcd) + lidar_pcd[:, 3] /= 255.0 # Normalizes refl. + + # Removes the points outside birds eye view (bev) map + reduced_lidar = kitti_bev_utils.removePoints(lidar_pcd, + cnf.boundary) + # Turns the point cloud into a bev rgb map + rgb_map = kitti_bev_utils.makeBVFeature(reduced_lidar, + cnf.DISCRETIZATION, cnf.boundary) + # Turned into a tensor for the model + inputs = torch.tensor(rgb_map, device=self.device).float() \ + .unsqueeze(0) + + return inputs + + def predict(self, inputs: torch.Tensor): + """! Uses the tensor from the preprocess fn to return the + bounding boxes for the objects. + @param inputs[torch.Tensor] The result of the preprocess + function. + @return torch.Tensor The output of the model in the shape + [1, boxes, x y w l classes], where the classes are Car, + Pedestrian, Cyclist, Van, Person_sitting. + """ + return self.model(inputs) + + def postprocess(self, predictions: torch.Tensor, conf_thresh: int, + nms_thresh: int): + """! Converts the model's predictions into a ROS2 message, and filters + out boxes below the confidence threshold and above the intersection + over union threshold. + @param predictions[torch.Tensor] The output of the model in + the shape [1, boxes, x y w l classes], where the classes + are Car, Pedestrian, Cyclist, Van, Person_sitting. + @param conf_thresh[float] The mininum confidence value accepted + for bounding boxes. + @param nms_thresh[float] The maximum accepted intersection over + union value for bounding boxes. + @return Object3DArray A ros2 message ready to be published. + Before publishing, the header needs to be attached. + """ + # Filters boxes under conf_thresh and over nms_thresh + detections = post_processing_v2(predictions, conf_thresh=conf_thresh, + nms_thresh=nms_thresh)[0] # Returns: (x, y, w, l, im, re, object_conf, class_score, class_pred) + + # Skip if no detections made + if detections is None: + return None + + # Returns shape [c, c_conf, x, y, z, w, l, h, yaw] + predictions = kitti_bev_utils.inverse_yolo_target(detections, self.img_size, cnf.boundary) + + # Turns the predictions into array of Object3D msg types + object_3d_array = Object3DArray() + for prediction in predictions: + # Defines the custom ros2 msgs + bounding_box = BoundingBox3D() + object_3d = Object3D() + + # Need x, y, z, x_size, y_size, z_size, yaw + bounding_box.coordinates = prediction[2:] + + # Returns the corners in the order specified by BoundingBox3D msg + corners_3d = kitti_bev_utils.get_corners_3d(*bounding_box.coordinates) + # Fills the Point corners array + for i, corner_3d in enumerate(corners_3d): + bounding_box.corners[i] = Point(x=corner_3d[0], y=corner_3d[1], z=corner_3d[2]) + + object_3d.label = (int(prediction[0]) - 1) % 3 # changes label to match format of message + object_3d.id = random.randint(0, 2**16-1) + object_3d.confidence_score = prediction[1] + object_3d.bounding_box = bounding_box + + object_3d_array.objects.append(object_3d) + + return object_3d_array \ No newline at end of file diff --git a/src/perception/lidar_detection/lidar_detection/lidar_detection_node.py b/src/perception/lidar_detection/lidar_detection/lidar_detection_node.py new file mode 100644 index 000000000..9396f862a --- /dev/null +++ b/src/perception/lidar_detection/lidar_detection/lidar_detection_node.py @@ -0,0 +1,142 @@ +""" +Package: lidar_detection +Filename: lidar_detection_node.py +Author: Gueren Sanford +Email: guerensanford@gmail.com +Copyright: 2021, Nova UTD +License: MIT License +Subscribes to raw lidar pointcloud data and publishes an array 3D object predictions. The +model for the predictions can be chosen using the model parameter. The predictions can be +filtered using the node's prediction confidence threshold and the non-maximum supression +threshold (limits the intersection of boxes). +""" + +# Python Imports +import torch + +# Local Import +from lidar_detection.complex_yolov4_model import ComplexYOLOv4Model +from lidar_detection.mmdetection3d_model import MMDetection3DModel + +# Ros Imports +import rclpy +from rclpy.node import Node + +# Message Imports +from rosgraph_msgs.msg import Clock +from builtin_interfaces.msg import Time +from sensor_msgs.msg import PointCloud2 +from navigator_msgs.msg import Object3DArray + +class LidarDetectionNode(Node): + + def __init__(self): + """! Initializes the node. + @param device[str] The device the model will run on. Choices: + 'cpu' (DEFAULT) | 'cuda:{NUMBER}' + @param model[str] The type of model making the detections. Choices: + 'mmdetection3d' (DEFAULT) | 'complex_yolo' + @param conf_thresh[float] The mininum confidence value accepted + for bounding boxes. Choices: 0.7 (DEFAULT) | 0.0 - 1.0 + @param nms_thresh[float] The maximum accepted intersection accepted + for bounding boxes. Choices 0.2 (DEFAULT) | 0.0 - 1.0 + """ + super().__init__("lidar_detection_node") + + # Declare default ROS2 node parameters + self.declare_parameter('device', 'cuda:0') + self.declare_parameter('model', 'mmdetection3d') + self.declare_parameter('conf_thresh', 0.7) + self.declare_parameter('nms_thresh', 0.2) + self.declare_parameter('config_path', # Paths to pretrained model and config + './data/perception/configs/pointpillars_hv_secfpn_8xb6-160e_kitti-3d-3class.py') + self.declare_parameter('checkpoint_path', + './data/perception/models/hv_pointpillars_secfpn_6x8_160e_kitti-3d-3class_20220301_150306-37dc2420.pth') + + # Get ROS2 parameters + self.device = torch.device(self.get_parameter('device') \ + .get_parameter_value().string_value) + self.conf_thresh = self.get_parameter('conf_thresh') \ + .get_parameter_value().double_value + self.nms_thresh = self.get_parameter('nms_thresh') \ + .get_parameter_value().double_value + model_type = self.get_parameter('model') \ + .get_parameter_value().string_value + config_path = self.get_parameter('config_path') \ + .get_parameter_value().string_value + checkpoint_path = self.get_parameter('checkpoint_path') \ + .get_parameter_value().string_value + + # Declared for publishing msgs + self.stamp = Time() + + # Instantiates the model type + if model_type == 'complex_yolo': + self.model = ComplexYOLOv4Model(self.device) + else: + self.model = MMDetection3DModel(config_path, checkpoint_path, str(self.device)) + + # Subcribes to raw lidar data + self.lidar_sub = self.create_subscription( + PointCloud2, '/lidar', self.lidar_callback, 10) + # Subscribes to clock for headers + self.clock_sub = self.create_subscription( + Clock, '/clock', self.clock_cb, 10) + + # Publishes array of 3D objects + self.objects3d_pub = self.create_publisher( + Object3DArray, 'detected/objects3d', 10) + + def clock_cb(self, msg): + """! + Updates the clock for message headers. + @param msg[Clock] The clock message. + """ + + self.stamp.sec = msg.clock.sec + self.stamp.nanosec = msg.clock.nanosec + + def lidar_callback(self, lidar_msg: PointCloud2): + """! Uses the lidar msg and a 3D object deteciton model to + form 3D bouding boxes around objects. Then, publishes + the boxes to a 'detected' topic. + @param lidar_msg[PointCloud2] The raw lidar point cloud. + """ + + # Values determined by model + inputs = self.model.preprocess(lidar_msg) + predictions = self.model.predict(inputs) + + # Object3DArray ROS2 msg w/ bounding box data + objecst3d_array = self.model.postprocess(predictions, + self.conf_thresh, self.nms_thresh) + + # Return if no detections made + if objecst3d_array is None: + objecst3d_array = Object3DArray() + + # Attaches the header to the message + objecst3d_array.header.stamp = self.stamp + objecst3d_array.header.frame_id = lidar_msg.header.frame_id + + # Publishes the Object3DArray msg + self.objects3d_pub.publish(objecst3d_array) + +def main(args=None): + rclpy.init(args=args) + + lidar_detection_node = LidarDetectionNode() + + rclpy.spin(lidar_detection_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + lidar_detection_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() + + + diff --git a/src/perception/lidar_detection/lidar_detection/mmdetection3d_model.py b/src/perception/lidar_detection/lidar_detection/mmdetection3d_model.py new file mode 100644 index 000000000..788233b5d --- /dev/null +++ b/src/perception/lidar_detection/lidar_detection/mmdetection3d_model.py @@ -0,0 +1,154 @@ +""" +Package: lidar_detection +Filename: mmdetection3d_model.py +Author: Gueren Sanford, Ragib Arnab +Email: guerensanford@gmail.com +Copyright: 2021, Nova UTD +License: MIT License +Use the MMDetection3D class to make object detections with LiDAR data in the format +[x,y,z,intensity]. We use the model MMDetection3D with PointPillars to make the +predictions. With the predictions, feed it into the postprocess to filter out and +format the results. +""" + +# Python Imports +import random +import numpy as np +import ros2_numpy as rnp + +# Model Imports +from mmdet3d.apis import init_model, inference_detector +from mmdet3d.structures.det3d_data_sample import Det3DDataSample +from mmcv.ops import nms_rotated + +# Message Imports +from geometry_msgs.msg import Point +from sensor_msgs.msg import PointCloud2 +from navigator_msgs.msg import BoundingBox3D, Object3D, Object3DArray + +# dict of class/label association +CLASS2LABEL = { 'Pedestrian': 0, 'Cyclist': 1, 'Car': 2 } + +class MMDetection3DModel(): + def __init__(self, config_path: str, checkpoint_path: str, device: str): + """! Initializes the node. + @param config_path[str] The config file for MMDetection3D, must use + the config for the specific checkpoint. + @param checkpoint_path[str] The checkpoint file for MMDetection3D. + Check their api for the list of checkpoint files. + @param device[str] The device the model will run on. + """ + + # Declare model attributes + self.translate_height = -1.5 + self.device = device + + # Setup for the Complex Yolo model + self.model = init_model(config=config_path, checkpoint=checkpoint_path, device=device) + + def preprocess(self, lidar_msg: PointCloud2): + """! Converts ROS LiDAR message into a numpy for the model. + @param lidar_msg[PointCloud2] The ros2 lidar message data + formatted as a PointCloud2 message. + @return numpy.ndarray An array of point cloud data in the shape + [n, (x, y, z, refl)] + """ + + # Point cloud byte stream -> np array with all fields and dtypes + lidar_np = rnp.numpify(lidar_msg) + + # Reduces the vertical res from 128 to 64 + lidar_np = lidar_np[::2] # matches Kitti dataset + + # Combines necessary lidar data into an array [n, n, n, n]] + lidar_pcd = np.array([ # Avoids np array dtype issues + lidar_np['x'].flatten(), + lidar_np['y'].flatten(), + lidar_np['z'].flatten(), + lidar_np['reflectivity'].flatten()]) + + # Tranforms array to shape [n, (x y z r)] + lidar_pcd = np.transpose(lidar_pcd) + lidar_pcd[:, 3] /= 255.0 # Normalizes refl. + lidar_pcd[:, 2] += self.translate_height + + return lidar_pcd + + def predict(self, inputs: np.ndarray): + """! Uses the tensor from the preprocess fn to return the + bounding boxes for the objects. + @param inputs[Det3DDataSample] The result of the preprocess + function. + @return Det3DDataSample You will see a list of Det3DDataSample, + and the predictions are in the pred_instances_3d, indicating + the detected bounding boxes, labels, and scores. The classes + are Car, Pedestrian, Cyclist, Van, Person_sitting. + """ + # Outputs predictions in format of Det3DDataSample class + predictions, _ = inference_detector(model=self.model, pcds=inputs) + + return predictions + + def postprocess(self, predictions: Det3DDataSample, conf_thresh: int, + nms_thresh: int): + """! Converts the model's predictions into a ROS2 message, and filters + out boxes below the confidence threshold and above the intersection + over union threshold. + @param predictions[Det3DDataSample] The output from the ppredict + function. + @param conf_thresh[float] The mininum confidence value accepted + for bounding boxes. + @param nms_thresh[float] The maximum accepted intersection over + union value for bounding boxes. + @return Object3DArray A ros2 message ready to be published. + Before publishing, the header needs to be attached. + """ + + bounding_boxes = predictions.pred_instances_3d.bboxes_3d + bbox_corners = predictions.pred_instances_3d.bboxes_3d.corners + labels = predictions.pred_instances_3d.labels_3d + scores = predictions.pred_instances_3d.scores_3d + detections = [bounding_boxes.tensor.double(), bbox_corners, labels, scores] + + # Filters out scores less than the conficdence threshold + indices = scores >= conf_thresh + detections = [tensor[indices] for tensor in detections] + + # Skip if no detections made + if len(detections[0]) == 0: + return None + + # Filters out boxes overlapping above nms threshold + bounding_boxes2d = detections[0][:,[0,1,3,4,6]] # [center_x, center_y, width, height, rotation] + _, indices = nms_rotated(bounding_boxes2d, detections[3], iou_threshold=nms_thresh) + detections = [tensor[indices] for tensor in detections] + + # Turns the detections into array of Object3D msg types + object_3d_array = Object3DArray() + for bounding_box, corners, label, score in zip(*detections): + # Defines the custom ros2 msgs + bounding_box_3d = BoundingBox3D() + object_3d = Object3D() + + # Shifts height of boxe down + bounding_box[2] -= self.translate_height + corners[:,2] -= self.translate_height + + # Coordinates needs x, y, z, x_size, y_size, z_size, yaw + bounding_box_3d.coordinates = bounding_box.double().cpu().numpy() + + # Converts detection corners into BoundingBox3D msg format + index = 3 + for corner in corners: + bounding_box_3d.corners[index % 8] = Point( + x=float(corner[0]), y=float(corner[1]), z=float(corner[2])) + index -= 1 + + object_3d.label = int(label) + object_3d.confidence_score = float(score) + object_3d.id = random.randint(0, 2**16-1) + object_3d.bounding_box = bounding_box_3d + + object_3d_array.objects.append(object_3d) + + return object_3d_array \ No newline at end of file diff --git a/src/perception/object_detector_3d/package.xml b/src/perception/lidar_detection/package.xml similarity index 82% rename from src/perception/object_detector_3d/package.xml rename to src/perception/lidar_detection/package.xml index 39567d1e9..8f202c1d2 100644 --- a/src/perception/object_detector_3d/package.xml +++ b/src/perception/lidar_detection/package.xml @@ -1,15 +1,17 @@ - object_detector_3d - 0.0.0 + lidar_detection + 1.0.0 TODO: Package description + Gueren Sanford Ragib Arnab TODO: License declaration rclpy ros2_numpy sensor_msgs + torch geometry_msgs navigator_msgs diff --git a/src/perception/lidar_detection/resource/lidar_detection b/src/perception/lidar_detection/resource/lidar_detection new file mode 100644 index 000000000..e69de29bb diff --git a/src/perception/lidar_detection/setup.cfg b/src/perception/lidar_detection/setup.cfg new file mode 100644 index 000000000..24db5ee2e --- /dev/null +++ b/src/perception/lidar_detection/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/lidar_detection +[install] +install_scripts=$base/lib/lidar_detection diff --git a/src/perception/object_detector_3d/setup.py b/src/perception/lidar_detection/setup.py similarity index 70% rename from src/perception/object_detector_3d/setup.py rename to src/perception/lidar_detection/setup.py index 32005a8ac..f5cef5b41 100644 --- a/src/perception/object_detector_3d/setup.py +++ b/src/perception/lidar_detection/setup.py @@ -1,6 +1,6 @@ from setuptools import find_packages, setup -package_name = 'object_detector_3d' +package_name = 'lidar_detection' setup( name=package_name, @@ -13,14 +13,14 @@ ], install_requires=['setuptools'], zip_safe=True, - maintainer='Ragib Arnab', - maintainer_email='ragib.arnab@gmail.com', + maintainer=['Gueren Sanford', 'Ragib Arnab'], + maintainer_email=['guerensanford@gmail.com', 'ragib.arnab@gmail.com'], description='TODO: Package description', license='TODO: License declaration', tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'lidar_object_detector_3d_node = object_detector_3d.lidar_objdet3d_node:main' + 'lidar_detection_node = lidar_detection.lidar_detection_node:main', ], }, ) diff --git a/src/perception/multi_object_tracker_3d/multi_object_tracker_3d/mot3d_node.py b/src/perception/multi_object_tracker_3d/multi_object_tracker_3d/mot3d_node.py index d47b37ad7..527b003a7 100644 --- a/src/perception/multi_object_tracker_3d/multi_object_tracker_3d/mot3d_node.py +++ b/src/perception/multi_object_tracker_3d/multi_object_tracker_3d/mot3d_node.py @@ -78,14 +78,14 @@ def __init__(self): self.objects_subscription = self.create_subscription( msg_type = Object3DArray, - topic = 'objdet3d_raw', + topic = '/detected/objects3d', callback = self.detection_callback, qos_profile = 10 ) self.tracked_objects_publisher = self.create_publisher( msg_type = Object3DArray, - topic = 'objdet3d_tracked', + topic = '/tracked/objects3d', qos_profile = 10 ) diff --git a/src/perception/object_detector_3d/object_detector_3d/lidar_objdet3d_node.py b/src/perception/object_detector_3d/object_detector_3d/lidar_objdet3d_node.py deleted file mode 100644 index c5c6daac8..000000000 --- a/src/perception/object_detector_3d/object_detector_3d/lidar_objdet3d_node.py +++ /dev/null @@ -1,109 +0,0 @@ -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import PointCloud2 -import ros2_numpy as rnp -import numpy as np -from mmdet3d.apis import init_model, inference_detector -from navigator_msgs.msg import Object3D, Object3DArray -from geometry_msgs.msg import Point -import random - - -CLASS2LABEL = { - 'Pedestrian': 0, - 'Cyclist': 1, - 'Car': 2 -} - - -class LidarObjectDetector3DNode(Node): - - def __init__(self): - super().__init__('lidar_object_detector_3d_node') - - # declare parameters - self.declare_parameter("config_path", "/navigator/data/perception/configs/pointpillars_hv_secfpn_8xb6-160e_kitti-3d-3class.py") - self.declare_parameter("checkpoint_path", "/navigator/data/perception/checkpoints/hv_pointpillars_secfpn_6x8_160e_kitti-3d-3class_20220301_150306-37dc2420.pth") - self.declare_parameter("device", "cuda:0") - self.declare_parameter("translate_height", -1.5) - - # get parameters - config_path = self.get_parameter("config_path").get_parameter_value().string_value - checkpoint_path = self.get_parameter("checkpoint_path").get_parameter_value().string_value - device = self.get_parameter("device").get_parameter_value().string_value - self.translate_height = self.get_parameter("translate_height").get_parameter_value().double_value - - self.model = init_model(config=config_path, checkpoint=checkpoint_path, device=device) - - self.lidar_subscription = self.create_subscription( - msg_type = PointCloud2, - topic = 'lidar', - callback = self.lidar_callback, - qos_profile = 10 - ) - - self.objects_publisher = self.create_publisher( - Object3DArray, - 'objdet3d_raw', - 10 - ) - - def lidar_callback(self, lidar_msg: PointCloud2): - pcd = rnp.numpify(lidar_msg) - pcd = pcd[::2] # this reduces the rings to 64, it appears to work when running against the parade bag - pcd = np.array([ pcd['x'].flatten(), pcd['y'].flatten(), pcd['z'].flatten(), pcd['reflectivity'].flatten()]).T - pcd[:, 3] /= 255.0 - pcd[:, 2] += self.translate_height - - results, _ = inference_detector(model=self.model, pcds=pcd) - bounding_boxes = results.pred_instances_3d.bboxes_3d - bbox_corners = results.pred_instances_3d.bboxes_3d.corners.cpu().numpy() - labels = results.pred_instances_3d.labels_3d - scores = results.pred_instances_3d.scores_3d - - object_instance_array = Object3DArray() - for bounding_box, corners, label, score in zip(bounding_boxes, bbox_corners, labels, scores): - object_instance = Object3D() - - x, y, z, x_size, y_size, z_size, yaw = bounding_box - object_instance.bounding_box.coordinates[0] = float(x) - object_instance.bounding_box.coordinates[1] = float(y) - object_instance.bounding_box.coordinates[2] = float(z) - self.translate_height - object_instance.bounding_box.coordinates[3] = float(x_size) - object_instance.bounding_box.coordinates[4] = float(y_size) - object_instance.bounding_box.coordinates[5] = float(z_size) - object_instance.bounding_box.coordinates[6] = float(yaw) - - corners[:,2] -= self.translate_height - x0y0z0, x0y0z1, x0y1z1, x0y1z0, x1y0z0, x1y0z1, x1y1z1, x1y1z0 = corners - object_instance.bounding_box.corners[3] = Point(x=float(x0y0z0[0]), y=float(x0y0z0[1]), z=float(x0y0z0[2])) - object_instance.bounding_box.corners[2] = Point(x=float(x0y0z1[0]), y=float(x0y0z1[1]), z=float(x0y0z1[2])) - object_instance.bounding_box.corners[1] = Point(x=float(x0y1z1[0]), y=float(x0y1z1[1]), z=float(x0y1z1[2])) - object_instance.bounding_box.corners[0] = Point(x=float(x0y1z0[0]), y=float(x0y1z0[1]), z=float(x0y1z0[2])) - object_instance.bounding_box.corners[7] = Point(x=float(x1y0z0[0]), y=float(x1y0z0[1]), z=float(x1y0z0[2])) - object_instance.bounding_box.corners[6] = Point(x=float(x1y0z1[0]), y=float(x1y0z1[1]), z=float(x1y0z1[2])) - object_instance.bounding_box.corners[5] = Point(x=float(x1y1z1[0]), y=float(x1y1z1[1]), z=float(x1y1z1[2])) - object_instance.bounding_box.corners[4] = Point(x=float(x1y1z0[0]), y=float(x1y1z0[1]), z=float(x1y1z0[2])) - - object_instance.label = CLASS2LABEL[self.model.dataset_meta['classes'][label]] - object_instance.confidence_score = float(score) - object_instance.id = random.randint(0, 2**16-1) - object_instance_array.objects.append(object_instance) - - object_instance_array.header.stamp = lidar_msg.header.stamp - object_instance_array.header.frame_id = lidar_msg.header.frame_id - self.objects_publisher.publish(object_instance_array) - - -def main(args=None): - rclpy.init(args=args) - - lidar_objdet3d_node = LidarObjectDetector3DNode() - rclpy.spin(lidar_objdet3d_node) - - lidar_objdet3d_node.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/perception/object_detector_3d/setup.cfg b/src/perception/object_detector_3d/setup.cfg deleted file mode 100644 index 653e0b48a..000000000 --- a/src/perception/object_detector_3d/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/object_detector_3d -[install] -install_scripts=$base/lib/object_detector_3d diff --git a/src/perception/object_detector_3d/test/test_copyright.py b/src/perception/object_detector_3d/test/test_copyright.py deleted file mode 100644 index 97a39196e..000000000 --- a/src/perception/object_detector_3d/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/src/perception/object_detector_3d/test/test_flake8.py b/src/perception/object_detector_3d/test/test_flake8.py deleted file mode 100644 index 27ee1078f..000000000 --- a/src/perception/object_detector_3d/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/src/perception/object_detector_3d/test/test_pep257.py b/src/perception/object_detector_3d/test/test_pep257.py deleted file mode 100644 index b234a3840..000000000 --- a/src/perception/object_detector_3d/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/src/perception/visualizer/visualizer/object_visualizer_node.py b/src/perception/visualizer/visualizer/object_visualizer_node.py index 1e1aefd1a..810af988a 100644 --- a/src/perception/visualizer/visualizer/object_visualizer_node.py +++ b/src/perception/visualizer/visualizer/object_visualizer_node.py @@ -16,59 +16,66 @@ class ObjectVisualizerNode(Node): def __init__(self): super().__init__('object_visualizer_node') + # Declare ROS2 parameters + self.declare_parameter('topic', '/tracked/objects3d') # or /detected/objects3d + sub_topic = self.get_parameter('topic').get_parameter_value().string_value + pub_topic = sub_topic if sub_topic[0] != '/' else sub_topic[1:] + self.subscription = self.create_subscription( msg_type = Object3DArray, - topic = 'objdet3d_tracked', + topic = sub_topic, callback = self.visualize_objects, qos_profile = 1 ) - self.visualization_publisher = self.create_publisher(MarkerArray, 'objdet3d_viz', 10) + self.visualization_publisher = self.create_publisher(MarkerArray, f"viz/{pub_topic}", 10) def visualize_objects(self, msg: Object3DArray): - #self.get_logger().info(f"{msg.header}") + # Creates an array of bounding boxes for rviz marker_array = MarkerArray() for object in msg.objects: - + # Creates bounding box for object box = Marker() box.header.frame_id = msg.header.frame_id box.header.stamp = self.get_clock().now().to_msg() box.id = object.id - box.type = 5 + box.type = Marker.LINE_LIST box.color.r, box.color.g, box.color.b = LABEL_TO_COLOR[object.label] box.color.a = 1.0 - box.scale.x = 0.10 + box.scale.x = 0.10 # width of the line box.lifetime = Duration(seconds=0.2).to_msg() # should be removed when object.id exists box.ns = "object_bounding_box" + # Creates 12 lines for the bounding box for i in range(4): # this should do 0-1, 1-2, 2-3, 3-4 src = object.bounding_box.corners[i] dst = object.bounding_box.corners[(i+1) % 4] box.points.append(src) - box.points.append(dst) + box.points.append(dst) # needs 2 points per line # this should do 4-5, 5-6, 6-7, 7-4 src = object.bounding_box.corners[i+4] dst = object.bounding_box.corners[((i+1) % 4) + 4] box.points.append(src) - box.points.append(dst) + box.points.append(dst) # needs 2 points per line # this should do 0-4, 1-5, 2-6, 3-7 src = object.bounding_box.corners[i] dst = object.bounding_box.corners[i+4] box.points.append(src) - box.points.append(dst) + box.points.append(dst) # needs 2 points per line marker_array.markers.append(box) + # Creates the label for box tag = Marker() tag.header.frame_id = msg.header.frame_id tag.header.stamp = self.get_clock().now().to_msg() tag.id = object.id - tag.type = 9 + tag.type = Marker.TEXT_VIEW_FACING tag.color.r, tag.color.g, tag.color.b = (1.0, 1.0, 1.0) tag.color.a = 1.0 tag.scale.z = 0.5 @@ -84,7 +91,6 @@ def visualize_objects(self, msg: Object3DArray): self.visualization_publisher.publish(marker_array) - def main(args=None): rclpy.init(args=args)