├── .gitignore ├── LICENSE ├── README.md ├── cam_lidar_fusion ├── CMakeLists.txt ├── config │ ├── cloud_cluster.yaml │ ├── cluster.rviz │ ├── image_lidar.yaml │ ├── jtcx.yaml │ ├── rviz.rviz │ └── simulation.yaml ├── include │ ├── cloud_cluster.h │ ├── ground_filter.h │ ├── lshaped_fitting.h │ └── vis_bbox.h ├── launch │ ├── cluster.launch │ ├── ground_filter.launch │ └── test.launch ├── package.xml ├── src │ ├── cloud_cluster.cpp │ ├── ground_filter.cpp │ ├── image_shower.cpp │ ├── lshaped_fitting.cpp │ └── vis_bbox.cpp └── utils │ ├── image_publisher.cpp │ └── lidar_publisher.cpp ├── colored_pointcloud ├── CMakeLists.txt ├── config │ └── calib_result.yaml ├── include │ └── colored_pointcloud │ │ └── colored_pointcloud.h ├── launch │ └── colored_pointcloud_node.launch ├── package.xml ├── rviz │ └── colored_pointcloud161.rviz └── src │ └── colored_pointcloud_node161.cpp ├── gazebo_plugin ├── CMakeLists.txt ├── include │ ├── GetBBox.hh │ └── MovingObj.hh ├── launch │ └── environment.launch ├── lib │ ├── libGetBBox.so │ └── libMovingObj.so ├── models │ └── pickup │ │ ├── materials │ │ └── textures │ │ │ ├── pickup_diffuse.jpg │ │ │ └── wheels2.jpg │ │ ├── meshes │ │ └── pickup.dae │ │ ├── model.config │ │ └── model.sdf ├── msg │ └── bbox.msg ├── package.xml ├── src │ ├── GetBBox.cc │ ├── MovingObj.cc │ └── test_detection.cpp ├── srv │ └── bboxPerFrame.srv └── worlds │ ├── movingCar.world │ └── test_detection.world ├── package.json ├── resource ├── YOLOV5.png ├── calibration.json ├── cam_lidar_fusion.png ├── cloud_project.png ├── demo.gif ├── detect.png ├── final.png ├── ground_filter.png ├── l_shape_fitter.png ├── project_img.png ├── rviz2.gif └── world.gif └── yolo_ros ├── CMakeLists.txt ├── config └── config.yaml ├── launch └── detect.launch ├── msg ├── BoundingBox.msg ├── BoundingBoxes.msg └── ObjectCount.msg ├── package.xml ├── scripts ├── models │ ├── __init__.py │ ├── common.py │ ├── experimental.py │ ├── export.py │ ├── hub │ │ ├── yolov3-spp.yaml │ │ ├── yolov5-fpn.yaml │ │ └── yolov5-panet.yaml │ ├── yolo.py │ ├── yolov5l.yaml │ ├── yolov5m.yaml │ ├── yolov5s.pt │ ├── yolov5s.yaml │ └── yolov5x.yaml ├── utils │ ├── __init__.py │ ├── activations.py │ ├── datasets.py │ ├── evolve.sh │ ├── general.py │ ├── google_app_engine │ │ ├── Dockerfile │ │ ├── additional_requirements.txt │ │ └── app.yaml │ ├── google_utils.py │ └── torch_utils.py ├── yolo_bridge.py ├── yolo_bridge_srv.py ├── yolo_node.py ├── yolo_node_srv.py └── yolov5s.pt └── srv └── yolo_srv.srv /.gitignore: -------------------------------------------------------------------------------- 1 | /yolo_ros/scripts/utils/__pycache__ 2 | /yolo_ros/scripts/models/__pycache__ 3 | /yolo_ros/scripts/__pycache__ 4 | .vscode 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 吕瑞晨 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 3d-object-detection 2 | this is a reproduction of my senior's graduation project. It main use yolo to detect object in an image, and cluster the lidar points which are projected to the image. Thus, we can get the 3D boundingbox of the object. 3 | 4 | --- 5 | ## TODO: 6 | - [x] camera intrinsic calibration 7 | - [ ] camera lidar calibration 8 | - [x] yolo object detection 9 | - [x] cloud identification 10 | - [x] cloud clustering 11 | - [x] the scoring method(distance number iou) 12 | - [x] L shape fitter 13 | 14 | ## Environments: 15 | - PyTorch 1.10 16 | - Cuda 11.3 17 | - device NVIDIA RTX 3050ti 18 | ## YOLOv5 detection 19 | change the topic in ```yolo_ros/config/config.yaml``` or you can run by default using laptop camera 20 | ``` 21 | roslaunch usb_cam usb_cam-test.launch 22 | roslaunch yolo_ros demo.launch 23 | ``` 24 | ![](./resource/YOLOV5.png) 25 | ## lidar camera fusion 26 | change the lidar and camera topic in ```colored_pointcloud/config/calib_results.yaml```. This package will project the cloud to image and color the cloud. 27 | ``` 28 | roslaunch colored_pointcloud colored_poincloud_node.launch 29 | ``` 30 | ![](./resource/cam_lidar_fusion.png) 31 | ## Iterative lidar ground filter 32 | ![](./resource/ground_filter.png) 33 | ``` 34 | roslaunch cam_lidar_fusion ground_filter.launch 35 | ``` 36 | ## Project the lidar cloud to yolo img 37 | Since we have detected the car or person using yolo, and then we project the corresponding lidar points of each object. Different object are colored invidually. 38 | ![](./resource/project_img.png) 39 | 40 | ## Get the object cloud in rviz 41 | We do this supposing we have already known the cameram intrinsis and extrinsic and you can change it in the ```cam_lidar_fusion/config/cloud_cluster.yaml ``` 42 | ![](./resource/cloud_project.png) 43 | ## L-shape-fitter 44 | We want to get the yaw angle of the cluster of a car, so i naively use a minimum rectangle to encircle the clouds projected to xy plane, thus i get the yaw angle. 45 | ![](./resource/l_shape_fitter.png) 46 | ## Final results 47 | I just take one frame of kitti dataset for example 48 | ![](./resource/final.png) 49 | ![](./resource/demo.gif) 50 | 51 | ## Test in simulation world 52 | I build a simple dynamic world to check my algorithm 53 | ![](./resource/world.gif) 54 | the detect results are as followed, we can see that this algorithm distingguish the front view and background validly. 55 | ![](./resource/rviz2.gif) 56 | Here is the detection precision in gazebo world, hopefully, the precision is 80% in average. 57 | ![](./resource/detect.png) 58 | Many thanks to my friend Ouyang and my senior Caoming for their code. 59 | -------------------------------------------------------------------------------- /cam_lidar_fusion/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cam_lidar_fusion) 3 | 4 | # # Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++14) 6 | 7 | set(CMAKE_BUILD_TYPE "Debug") 8 | 9 | # # Find catkin macros and libraries 10 | # # if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 11 | # # is used, also find other catkin packages 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | rospy 15 | std_msgs 16 | cv_bridge 17 | tf 18 | tf_conversions 19 | yolo_ros 20 | ) 21 | find_package(OpenCV REQUIRED) 22 | find_package(PCL 1.10 REQUIRED) 23 | find_package(Eigen3) 24 | find_package(yaml-cpp) 25 | 26 | # # System dependencies are found with CMake's conventions 27 | find_package(Boost REQUIRED COMPONENTS system) 28 | MESSAGE(STATUS "OpenCV version: " ${OpenCV_VERSION}) 29 | 30 | # # Generate messages in the 'msg' folder 31 | # add_message_files( 32 | # FILES 33 | # Message1.msg 34 | # Message2.msg 35 | # ) 36 | 37 | # # Generate services in the 'srv' folder 38 | # add_service_files( 39 | # FILES 40 | # Service1.srv 41 | # Service2.srv 42 | # ) 43 | 44 | # # Generate actions in the 'action' folder 45 | # add_action_files( 46 | # FILES 47 | # Action1.action 48 | # Action2.action 49 | # ) 50 | 51 | # # Generate added messages and services with any dependencies listed here 52 | # generate_messages( 53 | # DEPENDENCIES 54 | # std_msgs 55 | # ) 56 | include_directories( 57 | include 58 | ${catkin_INCLUDE_DIRS} 59 | ${PCL_INCLUDE_DIRS} 60 | ${YAML_CPP_INCLUDE_DIR} 61 | ) 62 | link_directories(${PCL_LIBRARY_DIRS}) 63 | 64 | catkin_package() 65 | 66 | add_executable(image_publisher utils/image_publisher.cpp) 67 | target_link_libraries(image_publisher 68 | ${catkin_LIBRARIES} 69 | ${OpenCV_LIBRARIES}) 70 | 71 | add_executable(lidar_publisher utils/lidar_publisher.cpp) 72 | target_link_libraries(lidar_publisher 73 | ${catkin_LIBRARIES} 74 | ${PCL_LIBRARIES} 75 | yaml-cpp) 76 | 77 | add_executable(ground_filter src/ground_filter.cpp) 78 | target_link_libraries(ground_filter 79 | ${catkin_LIBRARIES} 80 | ${PCL_LIBRARIES}) 81 | 82 | add_library(lshaped_fitting src/lshaped_fitting.cpp) 83 | target_link_libraries(lshaped_fitting ${OpenCV_LIBRARIES}) 84 | 85 | add_library(vis_bbox src/vis_bbox.cpp) 86 | target_link_libraries(vis_bbox 87 | ${catkin_LIBRARIES} 88 | ${OpenCV_LIBRARIES} 89 | ${PCL_LIBRARIES}) 90 | 91 | add_executable(cloud_cluster src/cloud_cluster.cpp) 92 | target_link_libraries(cloud_cluster 93 | ${catkin_LIBRARIES} 94 | ${PCL_LIBRARIES} 95 | ${OpenCV_LIBRARIES} 96 | lshaped_fitting) 97 | 98 | add_executable(image_shower src/image_shower.cpp) 99 | target_link_libraries(image_shower 100 | ${catkin_LIBRARIES} 101 | ${OpenCV_LIBRARIES}) 102 | -------------------------------------------------------------------------------- /cam_lidar_fusion/config/cloud_cluster.yaml: -------------------------------------------------------------------------------- 1 | cam_intrinsic: 2 | [721.538, 0, 609.559, 3 | 0, 721.538, 172.854, 4 | 0, 0, 1] 5 | velo_to_cam: 6 | [ 0.00491538, -0.999863, -0.0157715, -0.00445048, 7 | 0.0137532, 0.0158378, -0.99978, -0.0759921, 8 | 0.999893, 0.00469739, 0.0138291, -0.271865, 9 | 0, 0, 0, 1] 10 | image_topic: "/kitti/camera_color_left/image_raw" 11 | lidar_topic: "/point_no_ground" 12 | bbox_topic: "/bounding_boxes" 13 | w1: 10 14 | w2: 5 15 | w3: 20 -------------------------------------------------------------------------------- /cam_lidar_fusion/config/cluster.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | - /PointCloud21/Status1 11 | - /BoundingBoxArray1 12 | - /PointCloud22 13 | Splitter Ratio: 0.5 14 | Tree Height: 406 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: PointCloud2 34 | Preferences: 35 | PromptSaveOnExit: true 36 | Toolbars: 37 | toolButtonStyle: 2 38 | Visualization Manager: 39 | Class: "" 40 | Displays: 41 | - Alpha: 0.5 42 | Cell Size: 1 43 | Class: rviz/Grid 44 | Color: 160; 160; 164 45 | Enabled: true 46 | Line Style: 47 | Line Width: 0.029999999329447746 48 | Value: Lines 49 | Name: Grid 50 | Normal Cell Count: 0 51 | Offset: 52 | X: 0 53 | Y: 0 54 | Z: 0 55 | Plane: XY 56 | Plane Cell Count: 10 57 | Reference Frame: 58 | Value: true 59 | - Alpha: 1 60 | Class: rviz/Axes 61 | Enabled: true 62 | Length: 1 63 | Name: Axes 64 | Radius: 0.10000000149011612 65 | Reference Frame: 66 | Show Trail: false 67 | Value: true 68 | - Alpha: 1 69 | Autocompute Intensity Bounds: true 70 | Autocompute Value Bounds: 71 | Max Value: 4.339521408081055 72 | Min Value: 0.49235206842422485 73 | Value: true 74 | Axis: Z 75 | Channel Name: intensity 76 | Class: rviz/PointCloud2 77 | Color: 255; 255; 255 78 | Color Transformer: AxisColor 79 | Decay Time: 0 80 | Enabled: true 81 | Invert Rainbow: false 82 | Max Color: 255; 255; 255 83 | Min Color: 0; 0; 0 84 | Name: PointCloud2 85 | Position Transformer: XYZ 86 | Queue Size: 10 87 | Selectable: true 88 | Size (Pixels): 3 89 | Size (m): 0.10000000149011612 90 | Style: Flat Squares 91 | Topic: /detect_result_cloud 92 | Unreliable: false 93 | Use Fixed Frame: true 94 | Use rainbow: true 95 | Value: true 96 | - Class: jsk_rviz_plugin/BoundingBoxArray 97 | Enabled: true 98 | Name: BoundingBoxArray 99 | Queue Size: 10 100 | Topic: /bounding_boxes_lidar 101 | Unreliable: false 102 | Value: true 103 | alpha: 0.800000011920929 104 | color: 25; 255; 0 105 | coloring: "" 106 | line width: 0.004999999888241291 107 | only edge: false 108 | show coords: false 109 | - Class: rviz/Image 110 | Enabled: true 111 | Image Topic: /yolo_result 112 | Max Value: 1 113 | Median window: 5 114 | Min Value: 0 115 | Name: Image 116 | Normalize Range: true 117 | Queue Size: 2 118 | Transport Hint: raw 119 | Unreliable: false 120 | Value: true 121 | - Alpha: 1 122 | Autocompute Intensity Bounds: true 123 | Autocompute Value Bounds: 124 | Max Value: 10 125 | Min Value: -10 126 | Value: true 127 | Axis: Z 128 | Channel Name: intensity 129 | Class: rviz/PointCloud2 130 | Color: 255; 255; 255 131 | Color Transformer: Intensity 132 | Decay Time: 0 133 | Enabled: false 134 | Invert Rainbow: false 135 | Max Color: 255; 255; 255 136 | Min Color: 0; 0; 0 137 | Name: PointCloud2 138 | Position Transformer: XYZ 139 | Queue Size: 10 140 | Selectable: true 141 | Size (Pixels): 3 142 | Size (m): 0.05000000074505806 143 | Style: Flat Squares 144 | Topic: /kitti/velo/pointcloud 145 | Unreliable: false 146 | Use Fixed Frame: true 147 | Use rainbow: true 148 | Value: false 149 | Enabled: true 150 | Global Options: 151 | Background Color: 48; 48; 48 152 | Default Light: true 153 | Fixed Frame: sim/base_footprint 154 | Frame Rate: 30 155 | Name: root 156 | Tools: 157 | - Class: rviz/Interact 158 | Hide Inactive Objects: true 159 | - Class: rviz/MoveCamera 160 | - Class: rviz/Select 161 | - Class: rviz/FocusCamera 162 | - Class: rviz/Measure 163 | - Class: rviz/SetInitialPose 164 | Theta std deviation: 0.2617993950843811 165 | Topic: /initialpose 166 | X std deviation: 0.5 167 | Y std deviation: 0.5 168 | - Class: rviz/SetGoal 169 | Topic: /move_base_simple/goal 170 | - Class: rviz/PublishPoint 171 | Single click: true 172 | Topic: /clicked_point 173 | Value: true 174 | Views: 175 | Current: 176 | Class: rviz/Orbit 177 | Distance: 65.86319732666016 178 | Enable Stereo Rendering: 179 | Stereo Eye Separation: 0.05999999865889549 180 | Stereo Focal Distance: 1 181 | Swap Stereo Eyes: false 182 | Value: false 183 | Field of View: 0.7853981852531433 184 | Focal Point: 185 | X: -0.7532031536102295 186 | Y: -3.2958569526672363 187 | Z: 12.467116355895996 188 | Focal Shape Fixed Size: true 189 | Focal Shape Size: 0.05000000074505806 190 | Invert Z Axis: false 191 | Name: Current View 192 | Near Clip Distance: 0.009999999776482582 193 | Pitch: 0.9852014780044556 194 | Target Frame: 195 | Yaw: 3.16853666305542 196 | Saved: ~ 197 | Window Geometry: 198 | Displays: 199 | collapsed: false 200 | Height: 1016 201 | Hide Left Dock: false 202 | Hide Right Dock: false 203 | Image: 204 | collapsed: false 205 | QMainWindow State: 000000ff00000000fd0000000400000000000002880000035afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000221000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000264000001330000001600ffffff000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000041a0000003efc0100000002fb0000000800540069006d006501000000000000041a000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000000770000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 206 | Selection: 207 | collapsed: false 208 | Time: 209 | collapsed: false 210 | Tool Properties: 211 | collapsed: false 212 | Views: 213 | collapsed: false 214 | Width: 1050 215 | X: 2002 216 | Y: 302 217 | -------------------------------------------------------------------------------- /cam_lidar_fusion/config/image_lidar.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | # image_path: /home/eric/data/kitti_raw/2011_09_26_drive_0001_sync/2011_09_26/2011_09_26_drive_0001_sync/image_00/data/0000000005.png 3 | # yolo has to input a three channel RGB image 4 | image_path: /home/eric/data/kitti_raw/2011_09_26_drive_0001_sync/2011_09_26/2011_09_26_drive_0001_sync/image_02/data/0000000040.png 5 | lidar_path: /home/eric/data/kitti_raw/2011_09_26_drive_0001_sync/2011_09_26/2011_09_26_drive_0001_sync/velodyne_points/data/0000000040.bin 6 | pub_xyzi: true -------------------------------------------------------------------------------- /cam_lidar_fusion/config/jtcx.yaml: -------------------------------------------------------------------------------- 1 | cam_intrinsic: 2 | [772.061,0,614.467, 3 | 0,580.638,378.459, 4 | 0,0,1] 5 | velo_to_cam: 6 | [ -0.99923709, 0.00179877, 0.04076136, -0.03284734, 7 | -0.03865524, 0.27415632, -0.96103018, -0.51552788 8 | -0.01288896, -0.96178298, -0.27386127, -0.60707708, 9 | 0., 0., 0., 1. ] 10 | image_topic: "/realsense/image_raw" #修改成九天的消息名 11 | lidar_topic: "/point_no_ground" 12 | bbox_topic: "/bounding_boxes" 13 | w1: 10 14 | w2: 5 15 | w3: 20 -------------------------------------------------------------------------------- /cam_lidar_fusion/config/rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud21 10 | - /PointCloud22 11 | Splitter Ratio: 0.5 12 | Tree Height: 728 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: PointCloud2 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 10 55 | Reference Frame: 56 | Value: true 57 | - Alpha: 1 58 | Autocompute Intensity Bounds: true 59 | Autocompute Value Bounds: 60 | Max Value: 2.872999906539917 61 | Min Value: -4.9710001945495605 62 | Value: true 63 | Axis: Z 64 | Channel Name: intensity 65 | Class: rviz/PointCloud2 66 | Color: 255; 255; 255 67 | Color Transformer: FlatColor 68 | Decay Time: 0 69 | Enabled: true 70 | Invert Rainbow: false 71 | Max Color: 255; 255; 255 72 | Min Color: 0; 0; 0 73 | Name: PointCloud2 74 | Position Transformer: XYZ 75 | Queue Size: 10 76 | Selectable: true 77 | Size (Pixels): 3 78 | Size (m): 0.009999999776482582 79 | Style: Flat Squares 80 | Topic: /lidar_points 81 | Unreliable: false 82 | Use Fixed Frame: true 83 | Use rainbow: true 84 | Value: true 85 | - Alpha: 1 86 | Autocompute Intensity Bounds: true 87 | Autocompute Value Bounds: 88 | Max Value: 2.872999906539917 89 | Min Value: -3.069999933242798 90 | Value: true 91 | Axis: Z 92 | Channel Name: intensity 93 | Class: rviz/PointCloud2 94 | Color: 255; 255; 255 95 | Color Transformer: AxisColor 96 | Decay Time: 0 97 | Enabled: true 98 | Invert Rainbow: false 99 | Max Color: 255; 255; 255 100 | Min Color: 0; 0; 0 101 | Name: PointCloud2 102 | Position Transformer: XYZ 103 | Queue Size: 10 104 | Selectable: true 105 | Size (Pixels): 3 106 | Size (m): 0.10000000149011612 107 | Style: Flat Squares 108 | Topic: /point_no_ground 109 | Unreliable: false 110 | Use Fixed Frame: true 111 | Use rainbow: true 112 | Value: true 113 | Enabled: true 114 | Global Options: 115 | Background Color: 48; 48; 48 116 | Default Light: true 117 | Fixed Frame: lidar_link 118 | Frame Rate: 30 119 | Name: root 120 | Tools: 121 | - Class: rviz/Interact 122 | Hide Inactive Objects: true 123 | - Class: rviz/MoveCamera 124 | - Class: rviz/Select 125 | - Class: rviz/FocusCamera 126 | - Class: rviz/Measure 127 | - Class: rviz/SetInitialPose 128 | Theta std deviation: 0.2617993950843811 129 | Topic: /initialpose 130 | X std deviation: 0.5 131 | Y std deviation: 0.5 132 | - Class: rviz/SetGoal 133 | Topic: /move_base_simple/goal 134 | - Class: rviz/PublishPoint 135 | Single click: true 136 | Topic: /clicked_point 137 | Value: true 138 | Views: 139 | Current: 140 | Class: rviz/Orbit 141 | Distance: 104.66547393798828 142 | Enable Stereo Rendering: 143 | Stereo Eye Separation: 0.05999999865889549 144 | Stereo Focal Distance: 1 145 | Swap Stereo Eyes: false 146 | Value: false 147 | Focal Point: 148 | X: 0 149 | Y: 0 150 | Z: 0 151 | Focal Shape Fixed Size: true 152 | Focal Shape Size: 0.05000000074505806 153 | Invert Z Axis: false 154 | Name: Current View 155 | Near Clip Distance: 0.009999999776482582 156 | Pitch: 0.4253990650177002 157 | Target Frame: 158 | Value: Orbit (rviz) 159 | Yaw: 0.24540340900421143 160 | Saved: ~ 161 | Window Geometry: 162 | Displays: 163 | collapsed: false 164 | Height: 1025 165 | Hide Left Dock: false 166 | Hide Right Dock: false 167 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004cc0000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 168 | Selection: 169 | collapsed: false 170 | Time: 171 | collapsed: false 172 | Tool Properties: 173 | collapsed: false 174 | Views: 175 | collapsed: false 176 | Width: 1853 177 | X: 67 178 | Y: 27 179 | -------------------------------------------------------------------------------- /cam_lidar_fusion/config/simulation.yaml: -------------------------------------------------------------------------------- 1 | cam_intrinsic: 2 | [ 762.7249337622711, 0.0, 640.5, 3 | 0.0, 762.7249337622711,360.5, 4 | 0.0, 0.0, 1.0] 5 | velo_to_cam: 6 | [ 0, -1, 0, 0, 7 | 0, 0, -1, -0.678, 8 | 1, 0, 0, -0.443, 9 | 0, 0, 0, 1] 10 | image_topic: "/usb_cam_front/image_raw" 11 | lidar_topic: "/point_no_ground" 12 | bbox_topic: "/bounding_boxes" 13 | w1: 10 14 | w2: 5 15 | w3: 20 -------------------------------------------------------------------------------- /cam_lidar_fusion/include/cloud_cluster.h: -------------------------------------------------------------------------------- 1 | #ifndef CLOUD_CLUSTER_H_ 2 | #define CLOUD_CLUSTER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include "lshaped_fitting.h" 34 | #include "yolo_ros/BoundingBox.h" 35 | #include "yolo_ros/BoundingBoxes.h" 36 | #include "yolo_ros/ObjectCount.h" 37 | using namespace std; 38 | 39 | class CloudCluster 40 | { 41 | public: 42 | CloudCluster(ros::NodeHandle& nh, ros::NodeHandle& nh_local); 43 | ~CloudCluster(){}; 44 | struct Detected_object 45 | { 46 | pcl::PointXYZ min_point_; 47 | pcl::PointXYZ max_point_; 48 | pcl::PointXYZ centroid_; 49 | string category_; 50 | cv::RotatedRect rr; 51 | vector vertices; 52 | jsk_recognition_msgs::BoundingBox bounding_box_; 53 | }; 54 | 55 | private: 56 | void readParam(); 57 | void callback(const sensor_msgs::ImageConstPtr& img_raw, 58 | const sensor_msgs::PointCloud2ConstPtr& cropped_cloud, 59 | const yolo_ros::BoundingBoxesConstPtr& bd_boxes); 60 | bool kd_cluster(pcl::PointCloud::Ptr in_pc, 61 | yolo_ros::BoundingBox& bbox_, Detected_object& obj_info_); 62 | void drawCube(vector& obj_vec_); 63 | double judgeScore(pcl::PointCloud::Ptr cloud_in, 64 | yolo_ros::BoundingBox& bbox_, int total_cloud_num); 65 | double distScore(pcl::PointCloud::Ptr cloud_in); 66 | double numSocre(pcl::PointCloud::Ptr, int total_num); 67 | double iouScore(pcl::PointCloud::Ptr cloud_in, 68 | yolo_ros::BoundingBox& bbox_); 69 | 70 | void computeBox(pcl::PointCloud::Ptr); 71 | // bool filterBboxByArea();//以后有时间再改进 72 | 73 | LShapedFIT lshape_fitter; 74 | 75 | ros::NodeHandle nh_, nh_local_; 76 | message_filters::Subscriber image_sub_; 77 | message_filters::Subscriber lidar_sub_; 78 | message_filters::Subscriber bd_box_sub_; 79 | 80 | ros::Publisher detect_result_cloud_pub_; 81 | ros::Publisher enhanced_yolo_img_pub_; 82 | ros::Publisher bounding_box_pub_; 83 | 84 | typedef message_filters::sync_policies::ApproximateTime< 85 | sensor_msgs::Image, sensor_msgs::PointCloud2, yolo_ros::BoundingBoxes> 86 | enhanced_yolo_policy; 87 | typedef message_filters::Synchronizer Sync; 88 | boost::shared_ptr sync; 89 | 90 | Eigen::Matrix3d cam_intrinsic_; 91 | Eigen::Matrix4d velo_to_cam_; // look in camera frame 92 | Eigen::Matrix4d cam_to_velo_; // look in lidar frame 93 | string image_topic; 94 | string lidar_topic; 95 | string bbox_topic; 96 | std_msgs::Header cloud_heander_; 97 | map> area_thres_; 98 | double w1 = 10; 99 | double w2 = 5; 100 | double w3 = 20; 101 | int color[21][3] = {{255, 0, 0}, {255, 69, 0}, {255, 99, 71}, 102 | {255, 140, 0}, {255, 165, 0}, {238, 173, 14}, 103 | {255, 193, 37}, {255, 255, 0}, {255, 236, 139}, 104 | {202, 255, 112}, {0, 255, 0}, {84, 255, 159}, 105 | {127, 255, 212}, {0, 229, 238}, {152, 245, 255}, 106 | {178, 223, 238}, {126, 192, 238}, {28, 134, 238}, 107 | {0, 0, 255}, {72, 118, 255}, {122, 103, 238}}; 108 | }; 109 | 110 | #endif -------------------------------------------------------------------------------- /cam_lidar_fusion/include/ground_filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Note: Remove ground from the points cloud. 3 | * Algrothm proposed by 4 | * 《Fast segmentation of 3D point clouds: A paradigm on LiDAR data for 5 | * autonomous vehicle applications》 6 | */ 7 | #ifndef GROUND_FILTER_H_ 8 | #define GROUND_FILTER_H_ 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | class GroundPlaneFilter 24 | { 25 | public: 26 | /** \brief constructor 27 | */ 28 | GroundPlaneFilter(ros::NodeHandle& nh, ros::NodeHandle& nh_local); 29 | ~GroundPlaneFilter(); 30 | 31 | private: 32 | typedef pcl::console::TicToc TicToc; 33 | 34 | void updateParams(); 35 | 36 | void callBack(const sensor_msgs::PointCloud2ConstPtr fused_cloud); 37 | 38 | void extractInitialSeeds(pcl::PointCloud& in_cloud); 39 | 40 | void estimatePlane(); 41 | 42 | inline void allClear() 43 | { 44 | } 45 | 46 | void paperMethod(pcl::PointCloud& in_cloud); 47 | 48 | void backupMethod(pcl::PointCloud& in_cloud, 49 | double ground_height); 50 | 51 | ros::NodeHandle nh_; 52 | ros::NodeHandle nh_local_; 53 | ros::Subscriber sub_; 54 | ros::Publisher point_no_ground_pub_; 55 | ros::Publisher point_ground_pub_; 56 | ros::Publisher ground_height_pub_; 57 | Eigen::Vector3f norm_vector_; // norm vector that describes the plane 58 | double d_; // d that describe the plane. n'x + d = 0 59 | pcl::PointCloud point_ground_; 60 | pcl::PointCloud point_no_ground_; 61 | TicToc tt_; 62 | std::string topic_str_; 63 | int n_iter_; // number of iteration 64 | int n_lpr_; // number of points used to estimate the LPR 65 | double thres_seeds_; // threshold for points to be considered initial 66 | // seeds. 67 | double thres_dist_; // threshold distance from the plane 68 | }; 69 | 70 | #endif -------------------------------------------------------------------------------- /cam_lidar_fusion/include/lshaped_fitting.h: -------------------------------------------------------------------------------- 1 | #ifndef _L_SHAPED_FITTING_H 2 | #define _L_SHAPED_FITTING_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | class LShapedFIT 9 | { 10 | public: 11 | LShapedFIT(); 12 | 13 | ~LShapedFIT(); 14 | 15 | // For Each Cluster. 16 | cv::RotatedRect FitBox(std::vector* pointcloud_ptr); 17 | 18 | std::vector getRectVertex(); 19 | 20 | private: 21 | // Different Criterion For Cluster BBox. 22 | double calc_area_criterion(const cv::Mat& c1, const cv::Mat& c2); 23 | 24 | double calc_nearest_criterion(const cv::Mat& c1, const cv::Mat& c2); 25 | 26 | double calc_variances_criterion(const cv::Mat& c1, const cv::Mat& c2); 27 | 28 | double calc_var(const std::vector& v); 29 | 30 | void calc_cross_point(const double a0, const double a1, const double b0, 31 | const double b1, const double c0, const double c1, 32 | double& x, double& y); 33 | 34 | cv::RotatedRect calc_rect_contour(); 35 | 36 | public: 37 | enum Criterion 38 | { 39 | AREA, 40 | NEAREST, 41 | VARIANCE 42 | }; 43 | 44 | private: 45 | double min_dist_of_nearest_crit_; 46 | double dtheta_deg_for_search_; 47 | 48 | Criterion criterion_; 49 | 50 | std::vector a_; 51 | std::vector b_; 52 | std::vector c_; 53 | 54 | std::vector vertex_pts_; 55 | cv::Point2f hot_pt_; 56 | 57 | }; // class LShapedFIT 58 | 59 | #endif -------------------------------------------------------------------------------- /cam_lidar_fusion/include/vis_bbox.h: -------------------------------------------------------------------------------- 1 | #ifndef _VIS_BBOX_H_ 2 | #define _VIS_BBOX_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include "tf/transform_datatypes.h" 17 | using namespace std; 18 | using namespace Eigen; 19 | namespace vis_utils 20 | { 21 | struct rosTraj 22 | { 23 | visualization_msgs::Marker line; 24 | 25 | rosTraj(string frame_id, ros::Time stamp) 26 | { 27 | std_msgs::Header hd; 28 | hd.frame_id = frame_id; 29 | hd.stamp = stamp; 30 | 31 | line.header = hd; 32 | line.ns = "traj"; 33 | line.action = visualization_msgs::Marker::ADD; 34 | line.pose.orientation.w = 1.0; 35 | 36 | line.id = 0; 37 | line.type = visualization_msgs::Marker::LINE_STRIP; 38 | line.scale.x = 0.05; 39 | line.color.r = 1.0; 40 | line.color.g = 1.0; 41 | line.color.b = 0.0; 42 | line.color.a = 1.0; 43 | } 44 | }; 45 | 46 | visualization_msgs::Marker getText(const string& frame_id, const string& msg, 47 | const vector& color); 48 | 49 | void make_visualize_traj(int traj_id, ros::Time stamp, 50 | const vector& traj, rosTraj& rT); 51 | 52 | visualization_msgs::Marker getBBox( 53 | int box_id, ros::Time stamp, string frame_id, const vector& color, 54 | const vector>& bBoxes); 55 | 56 | visualization_msgs::Marker getPoint(string frame_id, int id, 57 | vector& color, 58 | const vector>& trackPoints); 59 | 60 | visualization_msgs::Marker getArrow(string frame_id, int id, 61 | vector& color, 62 | const vector& startPoint, double v, 63 | double yaw); 64 | 65 | // visualization_msgs::MarkerArray 66 | // getLabels(const object_tracking::resboxes& tbox); 67 | 68 | } // namespace vis_utils 69 | 70 | #endif -------------------------------------------------------------------------------- /cam_lidar_fusion/launch/cluster.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /cam_lidar_fusion/launch/ground_filter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /cam_lidar_fusion/launch/test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /cam_lidar_fusion/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cam_lidar_fusion 4 | 0.0.0 5 | The cam_lidar_fusion package 6 | 7 | 8 | 9 | 10 | eric 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | yolo_ros 56 | roscpp 57 | rospy 58 | std_msgs 59 | yolo_ros 60 | roscpp 61 | rospy 62 | std_msgs 63 | yolo_ros 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /cam_lidar_fusion/src/ground_filter.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "ground_filter.h" 3 | 4 | using namespace std; 5 | 6 | GroundPlaneFilter::GroundPlaneFilter(ros::NodeHandle& nh, 7 | ros::NodeHandle& nh_local) 8 | : nh_(nh), nh_local_(nh_local) 9 | { 10 | updateParams(); 11 | sub_ = nh_.subscribe(topic_str_, 1, &GroundPlaneFilter::callBack, this); 12 | point_ground_pub_ = 13 | nh_.advertise("/point_ground", 10); 14 | point_no_ground_pub_ = 15 | nh_.advertise("/point_no_ground", 10); 16 | ground_height_pub_ = nh_.advertise("ground_height", 10); 17 | } 18 | 19 | GroundPlaneFilter::~GroundPlaneFilter() 20 | { 21 | nh_local_.deleteParam("n_iter"); 22 | nh_local_.deleteParam("n_lpr"); 23 | nh_local_.deleteParam("thres_seeds"); 24 | nh_local_.deleteParam("thres_dist"); 25 | nh_local_.deleteParam("topic_str"); 26 | } 27 | 28 | void GroundPlaneFilter::updateParams() 29 | { 30 | nh_local_.param("n_iter", n_iter_, 3); 31 | nh_local_.param("n_lpr", n_lpr_, 100); 32 | nh_local_.param("thres_seeds", thres_seeds_, 0.5); 33 | nh_local_.param("thres_dist", thres_dist_, 0.3); 34 | nh_local_.param("topic_str", topic_str_, "/lidar_points"); 35 | 36 | assert(n_lpr_ > 0); 37 | } 38 | 39 | void GroundPlaneFilter::callBack( 40 | const sensor_msgs::PointCloud2ConstPtr fused_cloud) 41 | { 42 | pcl::PointCloud in_cloud; 43 | if (fused_cloud->fields.at(3).name != "intensity") 44 | { 45 | pcl::PointCloud cloud_xyz; 46 | pcl::fromROSMsg(*fused_cloud, cloud_xyz); 47 | pcl::copyPointCloud(cloud_xyz, in_cloud); 48 | } 49 | else 50 | { 51 | pcl::fromROSMsg(*fused_cloud, in_cloud); 52 | } 53 | 54 | in_cloud.header.frame_id = fused_cloud->header.frame_id; 55 | in_cloud.header.stamp = fused_cloud->header.stamp.nsec; 56 | 57 | pcl::PointCloud in_valid_cloud; 58 | // for (int i = 0; i < in_cloud.size(); i++) 59 | // { 60 | // if (isfinite(in_cloud[i].x)) 61 | // in_valid_cloud.push_back(in_cloud[i]); 62 | // } 63 | allClear(); 64 | paperMethod(in_cloud); 65 | 66 | sensor_msgs::PointCloud2 ros_point_no_ground; 67 | pcl::toROSMsg(point_no_ground_, ros_point_no_ground); 68 | ros_point_no_ground.header.stamp = fused_cloud->header.stamp; 69 | ros_point_no_ground.header.frame_id = fused_cloud->header.frame_id; 70 | point_no_ground_pub_.publish(ros_point_no_ground); 71 | } 72 | 73 | void GroundPlaneFilter::extractInitialSeeds( 74 | pcl::PointCloud& in_cloud) 75 | { 76 | sort(in_cloud.points.begin(), in_cloud.points.end(), 77 | [](const pcl::PointXYZI& lhs, const pcl::PointXYZI& rhs) { 78 | return lhs.z < rhs.z; 79 | }); 80 | // get mean z value of LPR 81 | double LPR_height = 0; // Lowest Point Representation 82 | for (int i = 0; i < n_lpr_; i++) 83 | { 84 | LPR_height += in_cloud.points[i].z; 85 | } 86 | LPR_height /= n_lpr_; 87 | 88 | for (int i = 0; i < in_cloud.points.size(); i++) 89 | { 90 | if (isnan(in_cloud.points[i].x) || 91 | isnan(in_cloud.points[i].y || isnan(in_cloud.points[i].z))) 92 | { 93 | continue; 94 | } 95 | 96 | if (in_cloud.points[i].z < LPR_height + thres_seeds_) 97 | { 98 | point_ground_.push_back(in_cloud.points[i]); 99 | } 100 | } 101 | } 102 | 103 | void GroundPlaneFilter::estimatePlane() 104 | { 105 | Eigen::Matrix3f cov; 106 | Eigen::Vector4f mean; 107 | pcl::computeMeanAndCovarianceMatrix(point_ground_, cov, mean); 108 | Eigen::JacobiSVD svd( 109 | cov, Eigen::DecompositionOptions::ComputeFullU); 110 | norm_vector_ = svd.matrixU().col(2); 111 | auto mean_point = mean.head<3>(); 112 | d_ = -norm_vector_.transpose() * mean_point; 113 | std_msgs::Float32 ground_height_msg; 114 | 115 | ground_height_msg.data = d_; 116 | ground_height_pub_.publish(ground_height_msg); 117 | } 118 | 119 | void GroundPlaneFilter::paperMethod(pcl::PointCloud& in_cloud) 120 | { 121 | tt_.tic(); 122 | extractInitialSeeds(in_cloud); 123 | // cout << "Elapsed: " << tt_.toc() << "ms" << endl; 124 | for (int i = 0; i < n_iter_; i++) 125 | { 126 | estimatePlane(); 127 | point_ground_.clear(); 128 | point_no_ground_.clear(); 129 | Eigen::Vector3f pt_vec; 130 | for (auto pt : in_cloud.points) 131 | { 132 | if (isnan(pt.x) || isnan(pt.y) || isnan(pt.z)) 133 | continue; 134 | pt_vec.x() = pt.x; 135 | pt_vec.y() = pt.y; 136 | pt_vec.z() = pt.z; 137 | if (norm_vector_.transpose() * pt_vec + d_ > thres_dist_) 138 | point_no_ground_.points.push_back(pt); 139 | else 140 | point_ground_.points.push_back(pt); 141 | } 142 | } 143 | // when the method is down, then use the plane method to filter the ground 144 | backupMethod(in_cloud, d_); 145 | } 146 | 147 | void GroundPlaneFilter::backupMethod(pcl::PointCloud& in_cloud, 148 | double ground_height) 149 | { 150 | if (ground_height < 2) 151 | return; 152 | else 153 | { 154 | cout << "the paper method is down, use plane fitter method" << endl; 155 | point_ground_.clear(); 156 | point_no_ground_.clear(); 157 | pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); 158 | pcl::ModelCoefficients coefficients; 159 | pcl::SACSegmentation segmentation; 160 | segmentation.setModelType(pcl::SACMODEL_PLANE); 161 | segmentation.setMethodType(pcl::SAC_RANSAC); 162 | segmentation.setMaxIterations(500); 163 | segmentation.setDistanceThreshold(0.2); 164 | segmentation.setInputCloud(in_cloud.makeShared()); 165 | segmentation.segment(*inliers, coefficients); 166 | 167 | pcl::ExtractIndices extract; 168 | extract.setInputCloud(in_cloud.makeShared()); 169 | extract.setIndices(inliers); 170 | extract.setNegative(true); 171 | extract.filter(point_no_ground_); 172 | } 173 | } 174 | 175 | int main(int argc, char** argv) 176 | { 177 | ros::init(argc, argv, "grounf_filter_node"); 178 | ros::NodeHandle nh(""); 179 | ros::NodeHandle nh_local("~"); 180 | 181 | if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, 182 | ros::console::levels::Debug)) 183 | { 184 | ros::console::notifyLoggerLevelsChanged(); 185 | } 186 | 187 | GroundPlaneFilter gpf(nh, nh_local); 188 | ros::spin(); 189 | return 0; 190 | } 191 | -------------------------------------------------------------------------------- /cam_lidar_fusion/src/image_shower.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | void call_back(const sensor_msgs::Image::ConstPtr& img_in) { 7 | cv::Mat img_to_show; 8 | cv_bridge::CvImagePtr cv_ptr; 9 | cv_ptr = cv_bridge::toCvCopy(img_in, sensor_msgs::image_encodings::BGR8); 10 | img_to_show = cv_ptr->image; 11 | cv::circle(img_to_show, cv::Point2d(50, 50), 3, cv::Scalar(255, 0, 0), 3); 12 | cv::namedWindow("image"); 13 | cv::imshow("image", img_to_show); 14 | cv::waitKey(3); 15 | } 16 | int main(int argc, char** argv) { 17 | ros::init(argc, argv, "image_shower_node"); 18 | ros::NodeHandle nh; 19 | ros::Subscriber sub = nh.subscribe("/image_raw", 1, call_back); 20 | ros::spin(); 21 | return 0; 22 | } 23 | -------------------------------------------------------------------------------- /cam_lidar_fusion/src/lshaped_fitting.cpp: -------------------------------------------------------------------------------- 1 | #include "lshaped_fitting.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | LShapedFIT::LShapedFIT() 8 | { 9 | min_dist_of_nearest_crit_ = 0.01; 10 | dtheta_deg_for_search_ = 1.0; 11 | 12 | criterion_ = LShapedFIT::VARIANCE; 13 | 14 | vertex_pts_.clear(); 15 | } 16 | 17 | LShapedFIT::~LShapedFIT() 18 | { 19 | } 20 | 21 | cv::RotatedRect LShapedFIT::FitBox(std::vector* pointcloud_ptr) 22 | { 23 | std::vector& points = *pointcloud_ptr; 24 | 25 | if (points.size() < 3) 26 | return cv::RotatedRect(); 27 | 28 | // Initialize Contour Points Matrix. 29 | cv::Mat Matrix_pts = cv::Mat::zeros(points.size(), 2, CV_64FC1); 30 | 31 | for (size_t i = 0; i < points.size(); ++i) 32 | { 33 | Matrix_pts.at(i, 0) = points[i].x; 34 | Matrix_pts.at(i, 1) = points[i].y; 35 | } 36 | 37 | double dtheta = 38 | dtheta_deg_for_search_ * M_PI / 180; // np.deg2rad(x) --> x * pi/180 39 | 40 | double minimal_cost = (-1.0) * std::numeric_limits::max(); 41 | double best_theta = std::numeric_limits::max(); 42 | 43 | // Search This Best Direction For ENUM. 44 | int loop_number = ceil((M_PI / 2.0 - dtheta) / dtheta); 45 | 46 | cv::Mat e1 = cv::Mat::zeros(1, 2, CV_64FC1); 47 | cv::Mat e2 = cv::Mat::zeros(1, 2, CV_64FC1); 48 | 49 | for (int k = 0; k < loop_number; ++k) 50 | { 51 | double theta = k * dtheta; 52 | double cost = std::numeric_limits::min(); 53 | // Be Sure Yaw Is In Range. 54 | if (theta < (M_PI / 2.0 - dtheta)) 55 | { 56 | e1.at(0, 0) = cos(theta); 57 | e1.at(0, 1) = sin(theta); 58 | e2.at(0, 0) = -sin(theta); 59 | e2.at(0, 1) = cos(theta); 60 | 61 | cv::Mat c1 = Matrix_pts * e1.t(); 62 | cv::Mat c2 = Matrix_pts * e2.t(); 63 | 64 | if (criterion_ == Criterion::AREA) 65 | { 66 | cost = calc_area_criterion(c1, c2); 67 | } 68 | else if (criterion_ == Criterion::NEAREST) 69 | { 70 | cost = calc_nearest_criterion(c1, c2); 71 | } 72 | else if (criterion_ == Criterion::VARIANCE) 73 | { 74 | cost = calc_variances_criterion(c1, c2); 75 | } 76 | else 77 | { 78 | std::cout << "L-Shaped Algorithm Criterion Is Not Supported." 79 | << std::endl; 80 | break; 81 | } 82 | 83 | if (minimal_cost < cost) 84 | { 85 | minimal_cost = cost; 86 | best_theta = theta; 87 | } 88 | } 89 | else 90 | { 91 | break; 92 | } 93 | } 94 | 95 | if (minimal_cost > (-1.0) * std::numeric_limits::max() && 96 | best_theta < std::numeric_limits::max()) 97 | { 98 | ; // Do Nothing, Continue Run As Follows. 99 | } 100 | else 101 | { 102 | std::cout << "RotatedRect Fit Failed." << std::endl; 103 | return cv::RotatedRect(); 104 | } 105 | 106 | double sin_s = sin(best_theta); 107 | double cos_s = cos(best_theta); 108 | 109 | cv::Mat e1_s = cv::Mat::zeros(1, 2, CV_64FC1); 110 | e1_s.at(0, 0) = cos_s; 111 | e1_s.at(0, 1) = sin_s; 112 | 113 | cv::Mat e2_s = cv::Mat::zeros(1, 2, CV_64FC1); 114 | e2_s.at(0, 0) = -sin_s; 115 | e2_s.at(0, 1) = cos_s; 116 | 117 | cv::Mat c1_s = Matrix_pts * e1_s.t(); 118 | cv::Mat c2_s = Matrix_pts * e2_s.t(); 119 | 120 | double min_c1_s = std::numeric_limits::max(); 121 | double max_c1_s = (-1.0) * std::numeric_limits::max(); 122 | double min_c2_s = std::numeric_limits::max(); 123 | double max_c2_s = (-1.0) * std::numeric_limits::max(); 124 | 125 | cv::minMaxLoc(c1_s, &min_c1_s, &max_c1_s, NULL, NULL); 126 | cv::minMaxLoc(c2_s, &min_c2_s, &max_c2_s, NULL, NULL); 127 | 128 | a_.clear(); 129 | b_.clear(); 130 | c_.clear(); 131 | 132 | if (min_c1_s < std::numeric_limits::max() && 133 | min_c2_s < std::numeric_limits::max() && 134 | max_c1_s > (-1.0) * std::numeric_limits::max() && 135 | max_c2_s > (-1.0) * std::numeric_limits::max()) 136 | { 137 | a_.push_back(cos_s); 138 | b_.push_back(sin_s); 139 | c_.push_back(min_c1_s); 140 | 141 | a_.push_back(-sin_s); 142 | b_.push_back(cos_s); 143 | c_.push_back(min_c2_s); 144 | 145 | a_.push_back(cos_s); 146 | b_.push_back(sin_s); 147 | c_.push_back(max_c1_s); 148 | 149 | a_.push_back(-sin_s); 150 | b_.push_back(cos_s); 151 | c_.push_back(max_c2_s); 152 | 153 | return calc_rect_contour(); 154 | } 155 | else 156 | { 157 | return cv::RotatedRect(); 158 | } 159 | } 160 | 161 | double LShapedFIT::calc_area_criterion(const cv::Mat& c1, const cv::Mat& c2) 162 | { 163 | std::vector c1_deep; 164 | std::vector c2_deep; 165 | 166 | for (int i = 0; i < c1.rows; i++) 167 | { 168 | for (int j = 0; j < c1.cols; j++) 169 | { 170 | c1_deep.push_back(c1.at(i, j)); 171 | c2_deep.push_back(c2.at(i, j)); 172 | } 173 | } 174 | 175 | // sort vector from min to max. 176 | sort(c1_deep.begin(), c1_deep.end()); 177 | sort(c2_deep.begin(), c2_deep.end()); 178 | 179 | int n_c1 = c1_deep.size(); 180 | int n_c2 = c2_deep.size(); 181 | 182 | double c1_min = c1_deep[0]; 183 | double c2_min = c2_deep[0]; 184 | 185 | double c1_max = c1_deep[n_c1 - 1]; 186 | double c2_max = c2_deep[n_c2 - 1]; 187 | 188 | double alpha = -(c1_max - c1_min) * (c2_max - c2_min); 189 | 190 | return alpha; 191 | } 192 | 193 | double LShapedFIT::calc_nearest_criterion(const cv::Mat& c1, const cv::Mat& c2) 194 | { 195 | std::vector c1_deep; 196 | std::vector c2_deep; 197 | 198 | for (int i = 0; i < c1.rows; i++) 199 | { 200 | for (int j = 0; j < c1.cols; j++) 201 | { 202 | c1_deep.push_back(c1.at(i, j)); 203 | c2_deep.push_back(c2.at(i, j)); 204 | } 205 | } 206 | 207 | // sort vector from min to max. 208 | sort(c1_deep.begin(), c1_deep.end()); 209 | sort(c2_deep.begin(), c2_deep.end()); 210 | 211 | int n_c1 = c1_deep.size(); 212 | int n_c2 = c2_deep.size(); 213 | 214 | double c1_min = c1_deep[0]; 215 | double c2_min = c2_deep[0]; 216 | 217 | double c1_max = c1_deep[n_c1 - 1]; 218 | double c2_max = c2_deep[n_c2 - 1]; 219 | 220 | std::vector d1; 221 | std::vector d2; 222 | 223 | for (int i = 0; i < n_c1; i++) 224 | { 225 | double temp = std::min(sqrt(pow((c1_max - c1_deep[i]), 2)), 226 | sqrt(pow((c1_deep[i] - c1_min), 2))); 227 | d1.push_back(temp); 228 | } 229 | 230 | for (int i = 0; i < n_c2; i++) 231 | { 232 | double temp = std::min(sqrt(pow((c2_max - c2_deep[i]), 2)), 233 | sqrt(pow((c2_deep[i] - c2_min), 2))); 234 | d2.push_back(temp); 235 | } 236 | 237 | double beta = 0; 238 | 239 | for (size_t i = 0; i < d1.size(); i++) 240 | { 241 | double d = std::max(std::min(d1[i], d2[i]), min_dist_of_nearest_crit_); 242 | beta += (1.0 / d); 243 | } 244 | 245 | return beta; 246 | } 247 | 248 | double LShapedFIT::calc_variances_criterion(const cv::Mat& c1, 249 | const cv::Mat& c2) 250 | { 251 | std::vector c1_deep; 252 | std::vector c2_deep; 253 | 254 | for (int i = 0; i < c1.rows; i++) 255 | { 256 | for (int j = 0; j < c1.cols; j++) 257 | { 258 | c1_deep.push_back(c1.at(i, j)); 259 | c2_deep.push_back(c2.at(i, j)); 260 | } 261 | } 262 | 263 | // sort vector from min to max. 264 | sort(c1_deep.begin(), c1_deep.end()); 265 | sort(c2_deep.begin(), c2_deep.end()); 266 | 267 | int n_c1 = c1_deep.size(); 268 | int n_c2 = c2_deep.size(); 269 | 270 | double c1_min = c1_deep[0]; 271 | double c2_min = c2_deep[0]; 272 | 273 | double c1_max = c1_deep[n_c1 - 1]; 274 | double c2_max = c2_deep[n_c2 - 1]; 275 | 276 | std::vector d1; 277 | std::vector d2; 278 | 279 | // D1 = [ min( [np.linalg.norm(c1_max - ic1), np.linalg.norm(ic1 - c1_min)] 280 | // ) for ic1 in c1 ] 281 | for (int i = 0; i < n_c1; i++) 282 | { 283 | double temp = std::min(sqrt(pow((c1_max - c1_deep[i]), 2)), 284 | sqrt(pow((c1_deep[i] - c1_min), 2))); 285 | d1.push_back(temp); 286 | } 287 | 288 | for (int i = 0; i < n_c2; i++) 289 | { 290 | double temp = std::min(sqrt(pow((c2_max - c2_deep[i]), 2)), 291 | sqrt(pow((c2_deep[i] - c2_min), 2))); 292 | d2.push_back(temp); 293 | } 294 | 295 | std::vector e1; 296 | std::vector e2; 297 | 298 | assert(d1.size() == d2.size()); 299 | 300 | // d1.size() || d2.size() Is equals. 301 | for (size_t i = 0; i < d1.size(); i++) 302 | { 303 | if (d1[i] < d2[i]) 304 | { 305 | e1.push_back(d1[i]); 306 | } 307 | else 308 | { 309 | e2.push_back(d2[i]); 310 | } 311 | } 312 | 313 | double v1 = 0.0; 314 | if (!e1.empty()) 315 | { 316 | v1 = (-1.0) * calc_var(e1); 317 | } 318 | 319 | double v2 = 0.0; 320 | if (!e2.empty()) 321 | { 322 | v2 = (-1.0) * calc_var(e2); 323 | } 324 | 325 | double gamma = v1 + v2; 326 | 327 | return gamma; 328 | } 329 | 330 | double LShapedFIT::calc_var(const std::vector& v) 331 | { 332 | double sum = std::accumulate(std::begin(v), std::end(v), 0.0); 333 | double mean = sum / v.size(); 334 | 335 | double acc_var_num = 0.0; 336 | 337 | std::for_each(std::begin(v), std::end(v), [&](const double d) { 338 | acc_var_num += (d - mean) * (d - mean); 339 | }); 340 | 341 | double var = sqrt(acc_var_num / (v.size() - 1)); 342 | 343 | return var; 344 | } 345 | 346 | void LShapedFIT::calc_cross_point(const double a0, const double a1, 347 | const double b0, const double b1, 348 | const double c0, const double c1, double& x, 349 | double& y) 350 | { 351 | x = (b0 * (-c1) - b1 * (-c0)) / (a0 * b1 - a1 * b0); 352 | y = (a1 * (-c0) - a0 * (-c1)) / (a0 * b1 - a1 * b0); 353 | } 354 | 355 | cv::RotatedRect LShapedFIT::calc_rect_contour() 356 | { 357 | // std::vector vertex_pts_; 358 | vertex_pts_.clear(); 359 | 360 | double top_left_x = 0.0, top_left_y = 0.0; 361 | calc_cross_point(a_[0], a_[1], b_[0], b_[1], c_[0], c_[1], top_left_x, 362 | top_left_y); 363 | vertex_pts_.push_back(cv::Point2f(top_left_x, top_left_y)); 364 | 365 | double top_right_x = 0.0, top_right_y = 0.0; 366 | calc_cross_point(a_[1], a_[2], b_[1], b_[2], c_[1], c_[2], top_right_x, 367 | top_right_y); 368 | vertex_pts_.push_back(cv::Point2f(top_right_x, top_right_y)); 369 | 370 | double bottom_left_x = 0.0, bottom_left_y = 0.0; 371 | calc_cross_point(a_[2], a_[3], b_[2], b_[3], c_[2], c_[3], bottom_left_x, 372 | bottom_left_y); 373 | vertex_pts_.push_back(cv::Point2f(bottom_left_x, bottom_left_y)); 374 | 375 | double bottom_right_x = 0.0, bottom_right_y = 0.0; 376 | calc_cross_point(a_[3], a_[0], b_[3], b_[0], c_[3], c_[0], bottom_right_x, 377 | bottom_right_y); 378 | vertex_pts_.push_back(cv::Point2f(bottom_right_x, bottom_right_y)); 379 | 380 | return cv::minAreaRect(vertex_pts_); 381 | } 382 | 383 | std::vector LShapedFIT::getRectVertex() 384 | { 385 | return vertex_pts_; 386 | } 387 | -------------------------------------------------------------------------------- /cam_lidar_fusion/src/vis_bbox.cpp: -------------------------------------------------------------------------------- 1 | #include "vis_bbox.h" 2 | 3 | namespace vis_utils 4 | { 5 | void make_visualize_traj(int traj_id, ros::Time stamp, 6 | const vector& traj, rosTraj& rT) 7 | { 8 | rT.line.id = traj_id; 9 | rT.line.lifetime = ros::Duration(0.5); 10 | rT.line.header.stamp = stamp; 11 | rT.line.points.clear(); 12 | for (int i = 0; i < traj.size(); i++) 13 | { 14 | geometry_msgs::Point p; 15 | p.x = traj[i](0); 16 | p.y = traj[i](1); 17 | p.z = traj[i](2); 18 | 19 | rT.line.points.emplace_back(p); 20 | } 21 | } 22 | 23 | visualization_msgs::Marker getBBox( 24 | int box_id, ros::Time stamp, string frame_id, const vector& color, 25 | const vector>& bBoxes) 26 | { 27 | assert(color.size() == 3); 28 | visualization_msgs::Marker line_list; 29 | 30 | line_list.header.frame_id = 31 | frame_id; // 定义frame_id (rviz需要设置世界坐标系为velo_link) 32 | line_list.header.stamp = stamp; 33 | line_list.ns = "boxes"; 34 | line_list.action = visualization_msgs::Marker::ADD; 35 | line_list.pose.orientation.w = 1.0; 36 | line_list.id = box_id; 37 | line_list.type = 38 | visualization_msgs::Marker::LINE_LIST; //线条序列 type设置类型 39 | 40 | // LINE_LIST markers use only the x component of scale, for the line width 41 | // 仅将比例的x分量用于线宽 42 | line_list.scale.x = 0.1; 43 | // Points are green 44 | line_list.color.r = color[0]; 45 | line_list.color.g = color[1]; // 边框绿色 46 | line_list.color.b = color[2]; 47 | line_list.color.a = 1.0; 48 | 49 | line_list.lifetime = ros::Duration(0.5); 50 | 51 | int id = 0; 52 | string ids; 53 | for (int objectI = 0; objectI < bBoxes.size(); objectI++) 54 | { // 多少个边界框,循环几次 55 | for (int pointI = 0; pointI < 4; pointI++) 56 | { //内循环4次?? 57 | assert((pointI + 1) % 4 < bBoxes[objectI].size()); 58 | assert((pointI + 4) < bBoxes[objectI].size()); 59 | assert((pointI + 1) % 4 + 4 < bBoxes[objectI].size()); 60 | 61 | id++; 62 | ids = to_string(id); 63 | 64 | geometry_msgs::Point p; // 定义p 65 | p.x = bBoxes[objectI][pointI].x; 66 | p.y = bBoxes[objectI][pointI].y; 67 | p.z = bBoxes[objectI][pointI].z; 68 | line_list.points.push_back(p); // 给line_lists添加点!!!! 69 | p.x = bBoxes[objectI][(pointI + 1) % 4].x; // 取余4 70 | p.y = bBoxes[objectI][(pointI + 1) % 4].y; 71 | p.z = bBoxes[objectI][(pointI + 1) % 4].z; 72 | line_list.points.push_back(p); 73 | 74 | p.x = bBoxes[objectI][pointI].x; 75 | p.y = bBoxes[objectI][pointI].y; 76 | p.z = bBoxes[objectI][pointI].z; 77 | line_list.points.push_back(p); 78 | p.x = bBoxes[objectI][pointI + 4].x; // 加4? 79 | p.y = bBoxes[objectI][pointI + 4].y; 80 | p.z = bBoxes[objectI][pointI + 4].z; 81 | line_list.points.push_back(p); 82 | 83 | p.x = bBoxes[objectI][pointI + 4].x; 84 | p.y = bBoxes[objectI][pointI + 4].y; 85 | p.z = bBoxes[objectI][pointI + 4].z; 86 | line_list.points.push_back(p); 87 | p.x = bBoxes[objectI][(pointI + 1) % 4 + 4].x; 88 | p.y = bBoxes[objectI][(pointI + 1) % 4 + 4].y; 89 | p.z = bBoxes[objectI][(pointI + 1) % 4 + 4].z; 90 | line_list.points.push_back(p); 91 | } 92 | } 93 | return line_list; 94 | } 95 | 96 | visualization_msgs::Marker getPoint(string frame_id, int id, 97 | vector& color, 98 | const vector>& trackPoints) 99 | { 100 | visualization_msgs::Marker points; 101 | 102 | points.header.frame_id = frame_id; 103 | points.header.stamp = ros::Time::now(); 104 | points.ns = "points"; 105 | 106 | points.action = visualization_msgs::Marker::ADD; 107 | points.pose.orientation.w = 1.0; 108 | 109 | points.id = id; 110 | points.type = visualization_msgs::Marker::POINTS; 111 | 112 | // POINTS markers use x and y scale for width/height respectively 113 | points.scale.x = 0.1; 114 | points.scale.y = 0.1; 115 | 116 | points.color.r = color[0]; 117 | points.color.g = color[1]; // 边框绿色 118 | points.color.b = color[2]; 119 | points.color.a = 1.0; 120 | 121 | // cout << "targetPoints.size() is --=------" << targetPoints.size() 122 | // <& color, 141 | const vector& startPoint, double v, 142 | double yaw) 143 | { 144 | visualization_msgs::Marker arrowsG; 145 | arrowsG.lifetime = ros::Duration(0.2); 146 | arrowsG.header.frame_id = frame_id; 147 | 148 | arrowsG.header.stamp = ros::Time::now(); 149 | arrowsG.ns = "arrows"; 150 | arrowsG.action = visualization_msgs::Marker::ADD; 151 | arrowsG.type = visualization_msgs::Marker::ARROW; 152 | 153 | // green 设置颜色 154 | arrowsG.color.g = color[0]; 155 | arrowsG.color.r = color[1]; // 红色 156 | arrowsG.color.b = color[2]; // 红色 157 | arrowsG.color.a = 1.0; 158 | arrowsG.id = id; 159 | 160 | double tv = v; 161 | double tyaw = yaw; 162 | 163 | // Set the pose of the marker. This is a full 6DOF pose relative to the 164 | // frame/time specified in the header 165 | arrowsG.pose.position.x = startPoint[0]; 166 | arrowsG.pose.position.y = startPoint[1]; 167 | arrowsG.pose.position.z = startPoint[2]; 168 | 169 | // convert from 3 angles to quartenion 170 | tf::Matrix3x3 obs_mat; 171 | obs_mat.setEulerYPR(tyaw, 0, 0); // yaw, pitch, roll 172 | tf::Quaternion q_tf; 173 | obs_mat.getRotation(q_tf); 174 | arrowsG.pose.orientation.x = q_tf.getX(); 175 | arrowsG.pose.orientation.y = q_tf.getY(); 176 | arrowsG.pose.orientation.z = q_tf.getZ(); 177 | arrowsG.pose.orientation.w = q_tf.getW(); 178 | 179 | // Set the scale of the arrowsG -- 1x1x1 here means 1m on a side 180 | // arrowsG.scale.x = tv; 181 | arrowsG.scale.x = 3; 182 | arrowsG.scale.y = 0.3; 183 | arrowsG.scale.z = 0.3; 184 | 185 | return arrowsG; 186 | } 187 | 188 | visualization_msgs::Marker getText(const string& frame_id, const string& msg, 189 | const vector& color) 190 | { 191 | visualization_msgs::Marker label_marker; 192 | label_marker.lifetime = ros::Duration(0.5); 193 | label_marker.header.stamp = ros::Time::now(); 194 | label_marker.header.frame_id = frame_id; 195 | label_marker.ns = "text"; 196 | label_marker.action = visualization_msgs::Marker::ADD; 197 | label_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; 198 | label_marker.scale.x = 2.0; 199 | label_marker.scale.y = 2.0; 200 | label_marker.scale.z = 2.0; 201 | 202 | label_marker.color.r = color[0]; 203 | label_marker.color.g = color[1]; 204 | label_marker.color.b = color[2]; 205 | label_marker.color.a = 1.0; 206 | 207 | label_marker.id = -1; 208 | label_marker.text = msg; 209 | 210 | label_marker.pose.position.x = 0.0; 211 | label_marker.pose.position.y = 0.0; 212 | label_marker.pose.position.z = 2.0; 213 | label_marker.scale.z = 2.0; 214 | 215 | return label_marker; 216 | } 217 | 218 | // visualization_msgs::MarkerArray getLabels(const object_tracking::resboxes& 219 | // tbox) 220 | // { 221 | // visualization_msgs::MarkerArray label_markers; 222 | // for (auto const &e: tbox.rb) 223 | // { 224 | 225 | // visualization_msgs::Marker label_marker; 226 | // label_marker.lifetime = ros::Duration(0.5); 227 | // label_marker.header = tbox.header; 228 | // label_marker.ns = "label_markers"; 229 | // label_marker.action = visualization_msgs::Marker::ADD; 230 | // label_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; 231 | // label_marker.scale.x = 1.5; 232 | // label_marker.scale.y = 1.5; 233 | // label_marker.scale.z = 1.5; 234 | 235 | // label_marker.color.r = 1.0; 236 | // label_marker.color.g = 1.0; 237 | // label_marker.color.b = 1.0; 238 | // label_marker.color.a = 1.0; 239 | 240 | // label_marker.id = e.id; 241 | 242 | // string text_format = "Id: %d\nVel: %.2fm/s\nYaw: %.3f"; 243 | // char text[64]; 244 | // sprintf(text, text_format.c_str(), e.id, e.vel, e.yaw); 245 | // label_marker.text = string(text); 246 | // //if(!object.label.empty() && object.label != "unknown") 247 | // //label_marker.text = object.label + " "; //Object Class if available 248 | 249 | // //std::stringstream distance_stream; 250 | // //distance_stream << std::fixed << std::setprecision(1) 251 | // //<< sqrt((object.pose.position.x * 252 | // object.pose.position.x) + 253 | // //(object.pose.position.y * 254 | // object.pose.position.y)); 255 | // //std::string distance_str = distance_stream.str() + " m"; 256 | // //label_marker.text += distance_str; 257 | 258 | // //if (object.velocity_reliable) 259 | // //{ 260 | // //double velocity = object.velocity.linear.x; 261 | // //if (velocity < -0.1) 262 | // //{ 263 | // //velocity *= -1; 264 | // //} 265 | 266 | // //if (abs(velocity) < object_speed_threshold_) 267 | // //{ 268 | // //velocity = 0.0; 269 | // //} 270 | 271 | // //tf::Quaternion q(object.pose.orientation.x, 272 | // object.pose.orientation.y, 273 | // //object.pose.orientation.z, 274 | // object.pose.orientation.w); 275 | 276 | // //double roll, pitch, yaw; 277 | // //tf::Matrix3x3(q).getRPY(roll, pitch, yaw); 278 | 279 | // //// convert m/s to km/h 280 | // //std::stringstream kmh_velocity_stream; 281 | // //kmh_velocity_stream << std::fixed << std::setprecision(1) << 282 | // (velocity * 3.6); 283 | // //std::string text = "\n<" + std::to_string(object.id) + "> " + 284 | // kmh_velocity_stream.str() + " km/h"; 285 | // //label_marker.text += text; 286 | // //} 287 | 288 | // label_marker.pose.position.x = e.traj_x.at(0); 289 | // label_marker.pose.position.y = e.traj_y.at(0); 290 | // label_marker.pose.position.z = 1.0; 291 | // label_marker.scale.z = 1.0; 292 | // if (!label_marker.text.empty()) 293 | // label_markers.markers.push_back(std::move(label_marker)); 294 | // } // end in_objects.objects loop 295 | // return label_markers; 296 | // }//ObjectsToLabels 297 | 298 | } // namespace vis_utils -------------------------------------------------------------------------------- /cam_lidar_fusion/utils/image_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | int main(int argc, char **argv) { 8 | ros::init(argc, argv, "image_publisher_node"); 9 | ros::NodeHandle nh; 10 | ros::Publisher pub = nh.advertise("/kitti/camera_color_left/image_raw", 1); 11 | std::string config_path = "/home/eric/a_ros_ws/object_detection_ws/src/3d-object-detection/cam_lidar_fusion/config"; 12 | std::string file_name = "image_lidar.yaml"; 13 | std::string config_file_name = config_path + "/" + file_name; 14 | cv::FileStorage fs_reader(config_file_name, cv::FileStorage::READ); 15 | std::string image_path; 16 | fs_reader["image_path"] >> image_path; 17 | cv::Mat image = cv::imread(image_path); 18 | cv::namedWindow("image"); 19 | cv::imshow("image", image); 20 | cv::waitKey(1000); 21 | cv::destroyWindow("image"); 22 | sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg(); 23 | ros::Rate loop_rate(30); 24 | while (ros::ok()) { 25 | msg->header.stamp = ros::Time::now(); 26 | msg->header.frame_id = "camera_link"; 27 | pub.publish(msg); 28 | ros::spinOnce(); 29 | // ROS_INFO("send once"); 30 | loop_rate.sleep(); 31 | } 32 | 33 | return 0; 34 | } 35 | -------------------------------------------------------------------------------- /cam_lidar_fusion/utils/lidar_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | using namespace std; 10 | 11 | typedef pcl::PointXYZ PointT; 12 | typedef pcl::PointXYZI PointI; 13 | typedef pcl::PointCloud PointCloudT; 14 | typedef pcl::PointCloud PointCloudI; 15 | 16 | void read_from_bin(PointCloudT& pc_, string file_path) 17 | { 18 | pc_.clear(); 19 | fstream input(file_path.c_str(), ios::in | ios::binary); 20 | if (!input.good()) 21 | { 22 | cerr << "Couldn't read file: " << file_path << endl; 23 | exit(EXIT_FAILURE); 24 | } 25 | input.seekg(0, ios::beg); 26 | float inten; 27 | for (int i = 0; input.good() && !input.eof(); i++) 28 | { 29 | PointT point; 30 | input.read((char*)&point.x, 3 * sizeof(float)); 31 | input.read((char*)&inten, sizeof(float)); 32 | pc_.push_back(point); 33 | } 34 | input.close(); 35 | } 36 | 37 | void read_from_bin(PointCloudI& pc_, string file_path) 38 | { 39 | pc_.clear(); 40 | fstream input(file_path.c_str(), ios::in | ios::binary); 41 | if (!input.good()) 42 | { 43 | cerr << "Couldn't read file: " << file_path << endl; 44 | exit(EXIT_FAILURE); 45 | } 46 | input.seekg(0, ios::beg); 47 | float inten; 48 | for (int i = 0; input.good() && !input.eof(); i++) 49 | { 50 | PointI point; 51 | input.read((char*)&point.x, 4 * sizeof(float)); 52 | pc_.push_back(point); 53 | } 54 | input.close(); 55 | } 56 | void read_from_pcd() 57 | { 58 | } 59 | 60 | int main(int argc, char** argv) 61 | { 62 | ros::init(argc, argv, "lidar_publisher_node"); 63 | ros::NodeHandle nh; 64 | YAML::Node config = YAML::LoadFile("/home/eric/a_ros_ws/" 65 | "object_detection_ws/src/" 66 | "3d-object-detection/cam_lidar_fusion/" 67 | "config/image_lidar.yaml"); 68 | cout << "lidar_path: " << config["lidar_path"].as() << endl; 69 | string lidar_path = config["lidar_path"].as(); 70 | ros::Publisher lidar_pub = 71 | nh.advertise("/kitti/velo/pointcloud", 1); 72 | PointCloudT point_cloud; 73 | PointCloudI point_cloud_i; 74 | sensor_msgs::PointCloud2 velodyne_points; 75 | string suffixStr = lidar_path.substr(lidar_path.find_last_of('.') + 1); 76 | cout << "the suffix is " << suffixStr << endl; 77 | bool is_bin = suffixStr == "bin" ? true : false; 78 | if (config["pub_xyzi"]) 79 | { 80 | read_from_bin(point_cloud_i, lidar_path); 81 | pcl::toROSMsg(point_cloud_i, velodyne_points); 82 | } 83 | else 84 | { 85 | read_from_bin(point_cloud, lidar_path); 86 | pcl::toROSMsg(point_cloud, velodyne_points); 87 | } 88 | ros::Rate loop_rate(10); 89 | 90 | velodyne_points.header.frame_id = "velo_link"; 91 | while (ros::ok()) 92 | { 93 | ros::Time now = ros::Time::now(); 94 | velodyne_points.header.stamp = now; 95 | lidar_pub.publish(velodyne_points); 96 | loop_rate.sleep(); 97 | } 98 | 99 | return 0; 100 | } 101 | -------------------------------------------------------------------------------- /colored_pointcloud/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.3) 2 | project(colored_pointcloud) 3 | add_compile_options(-std=c++14) 4 | set(CMAKE_BUILD_TYPE Release) 5 | 6 | find_package(catkin REQUIRED COMPONENTS 7 | cv_bridge 8 | image_transport 9 | message_filters 10 | roscpp 11 | rospy 12 | std_msgs 13 | tf 14 | ) 15 | 16 | ## Generate messages in the 'msg' folder 17 | # add_message_files( 18 | # FILES 19 | # Message1.msg 20 | # Message2.msg 21 | # ) 22 | 23 | ## Generate services in the 'srv' folder 24 | # add_service_files( 25 | # FILES 26 | # Service1.srv 27 | # Service2.srv 28 | # ) 29 | 30 | ## Generate actions in the 'action' folder 31 | # add_action_files( 32 | # FILES 33 | # Action1.action 34 | # Action2.action 35 | # ) 36 | 37 | ## Generate added messages and services with any dependencies listed here 38 | # generate_messages( 39 | # DEPENDENCIES 40 | # std_msgs 41 | # ) 42 | 43 | ################################### 44 | ## catkin specific configuration ## 45 | ################################### 46 | ## The catkin_package macro generates cmake config files for your package 47 | ## Declare things to be passed to dependent projects 48 | ## INCLUDE_DIRS: uncomment this if your package contains header files 49 | ## LIBRARIES: libraries you create in this project that dependent projects also need 50 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 51 | ## DEPENDS: system dependencies of this project that dependent projects also need 52 | 53 | 54 | ########### 55 | ## Build ## 56 | ########### 57 | 58 | ## Specify additional locations of header files 59 | ## Your package locations should be listed before other locations 60 | include_directories( 61 | include 62 | ${catkin_INCLUDE_DIRS} 63 | ) 64 | 65 | # opencv 66 | find_package(OpenCV REQUIRED) 67 | include_directories(${OpenCV_INCLUDE_DIRS}) 68 | 69 | # pcl 70 | find_package(PCL 1.10 REQUIRED) 71 | include_directories(${PCL_INCLUDE_DIRS}) 72 | add_definitions(${PCL_DEFINITIONS}) 73 | link_directories(${PCL_LIBRARY_DIRS}) 74 | 75 | # boost 76 | find_package(Boost REQUIRED) 77 | include_directories(${Boost_INCLUDE_DIRS}) 78 | 79 | catkin_package( 80 | # INCLUDE_DIRS include 81 | # LIBRARIES colored_pointcloud 82 | # CATKIN_DEPENDS cv_bridge image_transport message_filters roscpp rospy std_msgs 83 | # DEPENDS system_lib 84 | ) 85 | 86 | ## Declare a C++ executable 87 | ## With catkin_make all packages are built within a single CMake context 88 | ## The recommended prefix ensures that target names across packages don't collide 89 | add_executable(colored_pointcloud_node161 src/colored_pointcloud_node161.cpp) 90 | 91 | ## Specify libraries to link a library or executable target against 92 | target_link_libraries(colored_pointcloud_node161 ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES} ${Boost_LIBRARIES}) 93 | -------------------------------------------------------------------------------- /colored_pointcloud/config/calib_result.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | CameraMat: !!opencv-matrix 4 | rows: 3 5 | cols: 3 6 | dt: d 7 | data: [381.36246688113556, 0.0, 320.5, 8 | 0.0, 381.36246688113556, 240.5, 9 | 0.0, 0.0, 1.0] 10 | 11 | DistCoeff: !!opencv-matrix 12 | rows: 1 13 | cols: 5 14 | dt: d 15 | data: [0.0, 0.0, 0.0, 0.0, 0.0] 16 | 17 | ImageSize: [ 640, 480 ] 18 | CameraExtrinsicMat: !!opencv-matrix 19 | rows: 4 20 | cols: 4 21 | dt: d 22 | data: [ 0, -1, 0, -0.4, 23 | 0, 0, -1, 0, 24 | 1, 0, 0, 0.1, 25 | 0, 0, 0, 1] 26 | Pose: [ 0, 0, 27 | 0, 0, 28 | 0, 0 ] 29 | UseTruth: 0 30 | -------------------------------------------------------------------------------- /colored_pointcloud/include/colored_pointcloud/colored_pointcloud.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | struct EIGEN_ALIGN16 PointXYZRGBI 4 | { 5 | PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) 6 | PCL_ADD_RGB; 7 | float i; //intensity 8 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 9 | }; 10 | 11 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZRGBI, 12 | (float,x,x) 13 | (float,y,y) 14 | (float,z,z) 15 | (uint8_t,r,r) 16 | (uint8_t,g,g) 17 | (uint8_t,b,b) 18 | (float,i,i) 19 | ) 20 | 21 | -------------------------------------------------------------------------------- /colored_pointcloud/launch/colored_pointcloud_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /colored_pointcloud/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | colored_pointcloud 4 | 0.0.0 5 | The colored_pointcloud package 6 | 7 | 8 | 9 | 10 | imrs 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | cv_bridge 53 | image_transport 54 | message_filters 55 | roscpp 56 | rospy 57 | std_msgs 58 | cv_bridge 59 | image_transport 60 | message_filters 61 | roscpp 62 | rospy 63 | std_msgs 64 | cv_bridge 65 | image_transport 66 | message_filters 67 | roscpp 68 | rospy 69 | std_msgs 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | -------------------------------------------------------------------------------- /colored_pointcloud/rviz/colored_pointcloud161.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /ColoredPoints1 9 | - /OriginalPoints1 10 | Splitter Ratio: 0.5 11 | Tree Height: 332 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: Image 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 255; 255; 255 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 16 54 | Reference Frame: 55 | Value: true 56 | - Class: rviz/Axes 57 | Enabled: true 58 | Length: 1 59 | Name: Axes 60 | Radius: 0.10000000149011612 61 | Reference Frame: 62 | Value: true 63 | - Class: rviz/Image 64 | Enabled: true 65 | Image Topic: /fused_image 66 | Max Value: 1 67 | Median window: 5 68 | Min Value: 0 69 | Name: Image 70 | Normalize Range: true 71 | Queue Size: 2 72 | Transport Hint: raw 73 | Unreliable: false 74 | Value: true 75 | - Alpha: 1 76 | Autocompute Intensity Bounds: true 77 | Autocompute Value Bounds: 78 | Max Value: 10 79 | Min Value: -10 80 | Value: true 81 | Axis: Z 82 | Channel Name: intensity 83 | Class: rviz/PointCloud2 84 | Color: 255; 255; 255 85 | Color Transformer: RGB8 86 | Decay Time: 0 87 | Enabled: true 88 | Invert Rainbow: false 89 | Max Color: 255; 255; 255 90 | Min Color: 0; 0; 0 91 | Name: ColoredPoints 92 | Position Transformer: XYZ 93 | Queue Size: 10 94 | Selectable: true 95 | Size (Pixels): 3 96 | Size (m): 0.10000000149011612 97 | Style: Flat Squares 98 | Topic: /colored_cloud_toshow 99 | Unreliable: false 100 | Use Fixed Frame: true 101 | Use rainbow: true 102 | Value: true 103 | - Alpha: 1 104 | Autocompute Intensity Bounds: true 105 | Autocompute Value Bounds: 106 | Max Value: 10 107 | Min Value: -10 108 | Value: true 109 | Axis: Z 110 | Channel Name: intensity 111 | Class: rviz/PointCloud2 112 | Color: 255; 255; 255 113 | Color Transformer: Intensity 114 | Decay Time: 0 115 | Enabled: true 116 | Invert Rainbow: false 117 | Max Color: 255; 255; 255 118 | Min Color: 0; 0; 0 119 | Name: OriginalPoints 120 | Position Transformer: XYZ 121 | Queue Size: 10 122 | Selectable: true 123 | Size (Pixels): 3 124 | Size (m): 0.10000000149011612 125 | Style: Flat Squares 126 | Topic: /velodyne_points 127 | Unreliable: false 128 | Use Fixed Frame: true 129 | Use rainbow: true 130 | Value: true 131 | Enabled: true 132 | Global Options: 133 | Background Color: 48; 48; 48 134 | Default Light: true 135 | Fixed Frame: base_footprint 136 | Frame Rate: 30 137 | Name: root 138 | Tools: 139 | - Class: rviz/Interact 140 | Hide Inactive Objects: true 141 | - Class: rviz/MoveCamera 142 | - Class: rviz/Select 143 | - Class: rviz/FocusCamera 144 | - Class: rviz/Measure 145 | - Class: rviz/SetInitialPose 146 | Theta std deviation: 0.2617993950843811 147 | Topic: /initialpose 148 | X std deviation: 0.5 149 | Y std deviation: 0.5 150 | - Class: rviz/SetGoal 151 | Topic: /move_base_simple/goal 152 | - Class: rviz/PublishPoint 153 | Single click: true 154 | Topic: /clicked_point 155 | Value: true 156 | Views: 157 | Current: 158 | Class: rviz/Orbit 159 | Distance: 29.304489135742188 160 | Enable Stereo Rendering: 161 | Stereo Eye Separation: 0.05999999865889549 162 | Stereo Focal Distance: 1 163 | Swap Stereo Eyes: false 164 | Value: false 165 | Focal Point: 166 | X: 4.691612720489502 167 | Y: -0.2594977021217346 168 | Z: 0.44143861532211304 169 | Focal Shape Fixed Size: true 170 | Focal Shape Size: 0.05000000074505806 171 | Invert Z Axis: false 172 | Name: Current View 173 | Near Clip Distance: 0.009999999776482582 174 | Pitch: 0.539797306060791 175 | Target Frame: 176 | Value: Orbit (rviz) 177 | Yaw: 3.0204129219055176 178 | Saved: ~ 179 | Window Geometry: 180 | Displays: 181 | collapsed: false 182 | Height: 1025 183 | Hide Left Dock: false 184 | Hide Right Dock: false 185 | Image: 186 | collapsed: false 187 | QMainWindow State: 000000ff00000000fd00000004000000000000033800000361fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000189000000c900fffffffb00000018006d00610072006b00650072005f0069006d006100670065000000024d0000013e0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001cc000001d20000001600ffffff000000010000010f00000369fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000369000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d00000040fc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003ff0000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 188 | Selection: 189 | collapsed: false 190 | Time: 191 | collapsed: false 192 | Tool Properties: 193 | collapsed: false 194 | Views: 195 | collapsed: false 196 | Width: 1853 197 | X: 67 198 | Y: 27 199 | -------------------------------------------------------------------------------- /gazebo_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(gazebo_plugin) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++14) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | message_generation 15 | yolo_ros 16 | ) 17 | find_package(Eigen3) 18 | find_package(OpenCV REQUIRED) 19 | find_package(gazebo REQUIRED) 20 | link_directories(${GAZEBO_LIBRARIES_DIRS}) 21 | list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}") 22 | ## Generate messages in the 'msg' folder 23 | add_message_files( 24 | FILES 25 | bbox.msg 26 | ) 27 | 28 | ## Generate services in the 'srv' folder 29 | add_service_files( 30 | FILES 31 | bboxPerFrame.srv 32 | ) 33 | 34 | ## Generate actions in the 'action' folder 35 | # add_action_files( 36 | # FILES 37 | # Action1.action 38 | # Action2.action 39 | # ) 40 | 41 | ## Generate added messages and services with any dependencies listed here 42 | generate_messages( 43 | DEPENDENCIES 44 | std_msgs 45 | ) 46 | 47 | catkin_package( 48 | # INCLUDE_DIRS include 49 | # LIBRARIES gazebo_plugin 50 | CATKIN_DEPENDS roscpp rospy std_msgs message_runtime message_generation 51 | # DEPENDS system_lib 52 | ) 53 | 54 | ########### 55 | ## Build ## 56 | ########### 57 | 58 | ## Specify additional locations of header files 59 | ## Your package locations should be listed before other locations 60 | include_directories( 61 | include 62 | ${GAZEBO_INCLUDE_DIRS} 63 | ${catkin_INCLUDE_DIRS} 64 | ) 65 | 66 | add_library(MovingObj SHARED src/MovingObj.cc) 67 | add_library(GetBBox SHARED src/GetBBox.cc) 68 | 69 | set_target_properties(MovingObj PROPERTIES 70 | LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) 71 | set_target_properties(GetBBox PROPERTIES 72 | LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) 73 | 74 | target_link_libraries(MovingObj ${GAZEBO_LIBRARIES}) 75 | target_link_libraries(GetBBox ${GAZEBO_LIBRARIES}) 76 | 77 | add_executable(test_detection src/test_detection.cpp) 78 | target_link_libraries(test_detection ${catkin_LIBRARIES} 79 | ${OpenCV_LIBRARIES}) -------------------------------------------------------------------------------- /gazebo_plugin/include/GetBBox.hh: -------------------------------------------------------------------------------- 1 | #ifndef GAZEBO_GETBBOX_HH_ 2 | #define GAZEBO_GETBBOX_HH_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "gazebo/common/Plugin.hh" 22 | #include "gazebo/physics/physics.hh" 23 | #include "gazebo/util/system.hh" 24 | 25 | using namespace std; 26 | namespace gazebo 27 | { 28 | class GetBBox : public ModelPlugin 29 | { 30 | public: 31 | GetBBox(); 32 | 33 | public: 34 | virtual ~GetBBox(); 35 | 36 | public: 37 | void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); 38 | 39 | protected: 40 | virtual void UpdateChild(); 41 | 42 | private: 43 | physics::WorldPtr world_; 44 | 45 | private: 46 | rendering::ScenePtr scene_; 47 | 48 | private: 49 | ros::NodeHandle* rosnode_; 50 | 51 | private: 52 | boost::mutex lock_; 53 | 54 | private: 55 | std::string robot_namespace_; 56 | 57 | private: 58 | bool ServiceCallback(gazebo_plugin::bboxPerFrame::Request& req, 59 | gazebo_plugin::bboxPerFrame::Response& res); 60 | 61 | private: 62 | ros::ServiceServer srv_; 63 | 64 | private: 65 | std::string service_name_; 66 | 67 | private: 68 | ros::CallbackQueue imu_queue_; 69 | 70 | private: 71 | void QueueThread(); 72 | 73 | private: 74 | boost::thread callback_queue_thread_; 75 | 76 | // Pointer to the update event connection 77 | private: 78 | event::ConnectionPtr update_connection_; 79 | 80 | // deferred load in case ros is blocking 81 | private: 82 | sdf::ElementPtr sdf; 83 | 84 | private: 85 | void LoadThread(); 86 | 87 | private: 88 | boost::thread deferred_load_thread_; 89 | }; 90 | } // namespace gazebo 91 | 92 | #endif -------------------------------------------------------------------------------- /gazebo_plugin/include/MovingObj.hh: -------------------------------------------------------------------------------- 1 | #ifndef GAZEBO_PLUGINS_MOVINGObj_HH_ 2 | #define GAZEBO_PLUGINS_MOVINGObj_HH_ 3 | 4 | #include 5 | #include 6 | 7 | #include "gazebo/common/Plugin.hh" 8 | #include "gazebo/physics/physics.hh" 9 | #include "gazebo/util/system.hh" 10 | 11 | namespace gazebo 12 | { 13 | class GAZEBO_VISIBLE MovingObj : public ModelPlugin 14 | { 15 | /// \brief Constructor 16 | public: 17 | MovingObj(); 18 | 19 | /// \brief Load the actor plugin. 20 | /// \param[in] _model Pointer to the parent model. 21 | /// \param[in] _sdf Pointer to the plugin's SDF elements. 22 | public: 23 | virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 24 | 25 | // Documentation Inherited. 26 | public: 27 | virtual void Reset(); 28 | 29 | private: 30 | void OnUpdate(); 31 | 32 | private: 33 | physics::ModelPtr model; 34 | 35 | private: 36 | physics::WorldPtr world; 37 | 38 | private: 39 | sdf::ElementPtr sdf; 40 | 41 | private: 42 | ignition::math::Vector3d velocity; 43 | 44 | private: 45 | std::vector connections; 46 | 47 | private: 48 | std::vector waypoints_vec; 49 | 50 | private: 51 | ignition::math::Vector3d way_point; 52 | 53 | private: 54 | bool forth_flag; 55 | 56 | private: 57 | common::Time lastUpdate; 58 | 59 | private: 60 | double animationFactor = 1.0; 61 | 62 | private: 63 | int target_idx = 0; 64 | }; 65 | } // namespace gazebo 66 | #endif -------------------------------------------------------------------------------- /gazebo_plugin/launch/environment.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /gazebo_plugin/lib/libGetBBox.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/gazebo_plugin/lib/libGetBBox.so -------------------------------------------------------------------------------- /gazebo_plugin/lib/libMovingObj.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/gazebo_plugin/lib/libMovingObj.so -------------------------------------------------------------------------------- /gazebo_plugin/models/pickup/materials/textures/pickup_diffuse.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/gazebo_plugin/models/pickup/materials/textures/pickup_diffuse.jpg -------------------------------------------------------------------------------- /gazebo_plugin/models/pickup/materials/textures/wheels2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/gazebo_plugin/models/pickup/materials/textures/wheels2.jpg -------------------------------------------------------------------------------- /gazebo_plugin/models/pickup/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Pickup 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | A pickup truck. 15 | 16 | 17 | -------------------------------------------------------------------------------- /gazebo_plugin/models/pickup/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 0 0 0 0 0 -1.57079632679 8 | 9 | 10 | model://pickup/meshes/pickup.dae 11 | 12 | 13 | 14 | 15 | 0 0 0 0 0 -1.57079632679 16 | 17 | 18 | model://pickup/meshes/pickup.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /gazebo_plugin/msg/bbox.msg: -------------------------------------------------------------------------------- 1 | uint8 id 2 | float32[] xyz 3 | float32[] pos 4 | float32 yaw -------------------------------------------------------------------------------- /gazebo_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | gazebo_plugin 4 | 0.0.0 5 | The gazebo_plugin package 6 | 7 | 8 | 9 | 10 | eric 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | message_generation 56 | roscpp 57 | rospy 58 | std_msgs 59 | roscpp 60 | rospy 61 | std_msgs 62 | message_generation 63 | message_runtime 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /gazebo_plugin/src/GetBBox.cc: -------------------------------------------------------------------------------- 1 | #include "GetBBox.hh" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | // for version 9 | #if GAZEBO_MAJOR_VERSION >= 11 10 | using ign_Box = ignition::math::v6::AxisAlignedBox; 11 | #elif GAZEBO_MAJOR_VERSION >= 9 12 | using ign_Box = ignition::math::v4::Box; 13 | #else 14 | #error GAZEBO VERSION SHOULD BE GREATER THAN 11 OR 9 15 | #endif 16 | vector Car = {"pickup", "ambulance", "hatchback", "hatchback_blue", 17 | "suv"}; 18 | vector Car_yaw_offset = {0, -M_PI_2, 0, 0, 0}; 19 | vector Truck = {"bus", "truck"}; 20 | vector Truck_yaw_offset = {-M_PI_2, -M_PI_2}; 21 | vector Pedestrian = {"Scrubs", "walking", "FemaleVisitor", 22 | "MaleVisitorOnPhone"}; 23 | vector Ped_yaw_offset = {0, -M_PI, 0, 0}; 24 | 25 | void toLowerString(string& str) 26 | { 27 | for (size_t i = 0; i < str.size(); i++) 28 | { 29 | if (isalpha(str[i])) 30 | str[i] = tolower(str[i]); 31 | } 32 | } 33 | 34 | int getClassType(string str) 35 | { 36 | for (size_t i = 0; i < Car.size(); i++) 37 | { 38 | if (str.find(Car[i]) != str.npos) 39 | return i; 40 | } 41 | for (size_t i = 0; i < Truck.size(); i++) 42 | { 43 | if (str.find(Truck[i]) != str.npos) 44 | return Car.size() + i; 45 | } 46 | for (size_t i = 0; i < Pedestrian.size(); i++) 47 | { 48 | if (str.find(Pedestrian[i]) != str.npos) 49 | return Car.size() + Truck.size() + i; 50 | } 51 | return -1; 52 | } 53 | 54 | namespace gazebo 55 | { 56 | // Register this plugin with the simulator 57 | GZ_REGISTER_MODEL_PLUGIN(GetBBox) 58 | // Constructor 59 | GetBBox::GetBBox() 60 | { 61 | } 62 | 63 | // Destructor 64 | GetBBox::~GetBBox() 65 | { 66 | this->update_connection_.reset(); 67 | // Finalize the controller 68 | this->rosnode_->shutdown(); 69 | this->callback_queue_thread_.join(); 70 | delete this->rosnode_; 71 | } 72 | 73 | // Load the controller 74 | void GetBBox::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) 75 | { 76 | // save pointers 77 | this->world_ = _parent->GetWorld(); 78 | this->sdf = _sdf; 79 | 80 | // this->scene_ = gazebo::rendering::get_scene(); 81 | 82 | if (!this->scene_) 83 | { 84 | ROS_WARN("no camera or other rendering object in this world.\n" 85 | "try to add model://camera at origin by default"); 86 | this->world_->InsertModelFile("model://camera"); 87 | } 88 | 89 | // ros callback queue for processing subscription 90 | this->deferred_load_thread_ = 91 | boost::thread(boost::bind(&GetBBox::LoadThread, this)); 92 | } 93 | // Load the controller 94 | void GetBBox::LoadThread() 95 | { 96 | // load parameters 97 | this->robot_namespace_ = "getbbox_in_gazebo"; 98 | if (this->sdf->HasElement("robotNamespace")) 99 | this->robot_namespace_ = 100 | this->sdf->Get("robotNamespace") + "/"; 101 | 102 | if (!this->sdf->HasElement("serviceName")) 103 | { 104 | this->service_name_ = "/getbbox"; 105 | } 106 | else 107 | this->service_name_ = this->sdf->Get("serviceName"); 108 | 109 | // Make sure the ROS node for Gazebo has already been initialized 110 | if (!ros::isInitialized()) 111 | { 112 | ROS_ERROR("ros node for gazebo is not initialed!!"); 113 | return; 114 | } 115 | ROS_WARN("ros node for gazebo is initialized!!"); 116 | this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); 117 | 118 | // New Mechanism for Updating every World Cycle 119 | // Listen to the update event. This event is broadcast every 120 | // simulation iteration. 121 | this->update_connection_ = event::Events::ConnectWorldUpdateBegin( 122 | boost::bind(&GetBBox::UpdateChild, this)); 123 | } 124 | 125 | bool GetBBox::ServiceCallback(gazebo_plugin::bboxPerFrame::Request& req, 126 | gazebo_plugin::bboxPerFrame::Response& res) 127 | { 128 | res.box_num = 0; 129 | if (req.msg == "bbox") 130 | { 131 | for (size_t i = 0; i < this->world_->ModelCount(); i++) 132 | { 133 | physics::ModelPtr model = this->world_->ModelByIndex(i); 134 | string model_name = model->GetName(); 135 | 136 | int idx = getClassType(model_name); 137 | 138 | if (idx >= 0) 139 | { 140 | // std::cout << "model class is: " << model_name << std::endl; 141 | gazebo_plugin::bbox bbox; 142 | // this->scene_ = rendering::get_scene(); 143 | if (!this->scene_ || !this->scene_->Initialized()) 144 | return false; 145 | 146 | rendering::VisualPtr vis = this->scene_->GetVisual(model_name); 147 | ign_Box tmp_bbox = vis->BoundingBox(); 148 | float yaw_offset; 149 | 150 | if (idx < Car.size()) 151 | { 152 | bbox.id = 1; 153 | yaw_offset = Car_yaw_offset[idx]; 154 | } 155 | else if (idx < Car.size() + Truck.size()) 156 | { 157 | bbox.id = 2; 158 | idx -= Car.size(); 159 | yaw_offset = Truck_yaw_offset[idx]; 160 | } 161 | else 162 | { 163 | bbox.id = 3; 164 | idx -= (Car.size() + Truck.size()); 165 | yaw_offset = Ped_yaw_offset[idx]; 166 | } 167 | 168 | // ROS_INFO("(x, y, z) = (%f, %f, %f)", tmp_bbox.Center().X(), 169 | // tmp_bbox.Center().Y(), tmp_bbox.Center().Z()); 170 | bbox.xyz.push_back(tmp_bbox.XLength()); 171 | bbox.xyz.push_back(tmp_bbox.YLength()); 172 | bbox.xyz.push_back(tmp_bbox.ZLength()); 173 | 174 | double yaw = vis->Pose().Rot().Yaw(); 175 | 176 | double cx_in_world = cos(yaw) * tmp_bbox.Center().X() - 177 | sin(yaw) * tmp_bbox.Center().Y(); 178 | double cy_in_world = sin(yaw) * tmp_bbox.Center().X() - 179 | cos(yaw) * tmp_bbox.Center().Y(); 180 | 181 | bbox.yaw = model->WorldPose().Rot().Yaw() + yaw_offset; 182 | 183 | bbox.pos.push_back(vis->Pose().Pos().X() + cx_in_world); 184 | bbox.pos.push_back(vis->Pose().Pos().Y() + cy_in_world); 185 | bbox.pos.push_back(vis->Pose().Pos().Z() + 186 | tmp_bbox.Center().Z()); 187 | 188 | res.box_num++; 189 | 190 | res.bboxes.push_back(bbox); 191 | } 192 | } 193 | } 194 | return true; 195 | } 196 | 197 | // Update the controller 198 | void GetBBox::UpdateChild() 199 | { 200 | // this->scene_ = gazebo::rendering::get_scene(); 201 | if (this->scene_ == NULL) 202 | { 203 | // ROS_WARN("rendering has been created. try to get scene handle once 204 | // again ..."); 205 | this->scene_ = gazebo::rendering::get_scene(); 206 | if (this->scene_) 207 | { 208 | ROS_WARN("get scene handle! begin to search bbox"); 209 | // advertise services on the custom queue 210 | ros::AdvertiseServiceOptions aso = 211 | ros::AdvertiseServiceOptions::create< 212 | gazebo_plugin::bboxPerFrame>( 213 | this->service_name_, 214 | boost::bind(&GetBBox::ServiceCallback, this, _1, _2), 215 | ros::VoidPtr(), &this->imu_queue_); 216 | this->srv_ = this->rosnode_->advertiseService(aso); 217 | // start custom queue for imu 218 | this->callback_queue_thread_ = 219 | boost::thread(boost::bind(&GetBBox::QueueThread, this)); 220 | } 221 | else 222 | { 223 | return; 224 | } 225 | } 226 | } 227 | 228 | // Put laser data to the interface 229 | void GetBBox::QueueThread() 230 | { 231 | static const double timeout = 0.01; 232 | 233 | while (this->rosnode_->ok()) 234 | { 235 | this->imu_queue_.callAvailable(ros::WallDuration(timeout)); 236 | } 237 | } 238 | 239 | } // namespace gazebo -------------------------------------------------------------------------------- /gazebo_plugin/src/MovingObj.cc: -------------------------------------------------------------------------------- 1 | #include "MovingObj.hh" 2 | 3 | #include 4 | #include 5 | 6 | #include "gazebo/physics/physics.hh" 7 | 8 | using namespace gazebo; 9 | GZ_REGISTER_MODEL_PLUGIN(MovingObj) 10 | 11 | double PI = 3.1415; 12 | // some global variables 13 | ///////////////////////////////////////////////// 14 | MovingObj::MovingObj() 15 | { 16 | } 17 | 18 | ///////////////////////////////////////////////// 19 | void MovingObj::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) 20 | { 21 | this->sdf = _sdf; 22 | this->model = _model; 23 | this->world = this->model->GetWorld(); 24 | this->connections.push_back(event::Events::ConnectWorldUpdateEnd( 25 | std::bind(&MovingObj::OnUpdate, this))); 26 | this->Reset(); 27 | } 28 | 29 | void MovingObj::Reset() 30 | { 31 | this->lastUpdate = 0; 32 | if (this->sdf && this->sdf->HasElement("velocity")) 33 | this->velocity = this->sdf->Get("velocity"); 34 | else 35 | this->velocity = 1.0; 36 | if (this->sdf && this->sdf->HasElement("waypoint")) 37 | { 38 | sdf::ElementPtr waypointPtr = sdf->GetElement("waypoint"); 39 | while (waypointPtr) 40 | { 41 | this->waypoints_vec.push_back( 42 | waypointPtr->Get()); 43 | waypointPtr = waypointPtr->GetNextElement("waypoint"); 44 | } 45 | } 46 | } 47 | 48 | ///////////////////////////////////////////////// 49 | void MovingObj::OnUpdate() 50 | { 51 | // delta time 52 | double dt = (this->world->SimTime() - this->lastUpdate).Double(); 53 | ignition::math::Pose3d current_pose = this->model->WorldPose(); 54 | ignition::math::Pose3d target_pose; 55 | ignition::math::Vector3d current_rpy, tmp_pos; 56 | current_rpy = current_pose.Rot().Euler(); 57 | tmp_pos = this->waypoints_vec[target_idx] - current_pose.Pos(); 58 | double distance = tmp_pos.Length(); 59 | if (distance < 0.1) 60 | target_idx += 1; 61 | if (target_idx >= waypoints_vec.size()) 62 | target_idx = 0; 63 | tmp_pos = tmp_pos.Normalize(); 64 | ignition::math::Angle yaw = 65 | atan2(tmp_pos.Y(), tmp_pos.X()) - current_rpy.Z(); 66 | while (yaw > PI) 67 | { 68 | yaw -= 2 * PI; 69 | } 70 | while (yaw < -1 * PI) 71 | { 72 | yaw += 2 * PI; 73 | } 74 | if (yaw.Radian() > IGN_DTOR(10)) 75 | { 76 | target_pose.Rot() = ignition::math::Quaterniond( 77 | 0, 0, 78 | current_rpy.Z() + std::max(yaw.Radian() * 0.001, IGN_DTOR(0.1))); 79 | target_pose.Pos() = current_pose.Pos(); 80 | } 81 | else if (yaw.Radian() < IGN_DTOR(-10)) 82 | { 83 | target_pose.Rot() = ignition::math::Quaterniond( 84 | 0, 0, 85 | current_rpy.Z() + std::min(yaw.Radian() * 0.001, IGN_DTOR(-0.1))); 86 | target_pose.Pos() = current_pose.Pos(); 87 | } 88 | else 89 | { 90 | target_pose.Pos() = current_pose.Pos() + tmp_pos * this->velocity * dt; 91 | target_pose.Rot() = 92 | ignition::math::Quaterniond(0, 0, current_rpy.Z() + yaw.Radian()); 93 | } 94 | this->model->SetWorldPose(target_pose, false, true); 95 | this->lastUpdate = this->world->SimTime(); 96 | } -------------------------------------------------------------------------------- /gazebo_plugin/src/test_detection.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "gazebo_plugin/bboxPerFrame.h" 12 | #include "yolo_ros/BoundingBox.h" 13 | #include "yolo_ros/BoundingBoxes.h" 14 | #include "yolo_ros/ObjectCount.h" 15 | using namespace std; 16 | class TestDetection 17 | { 18 | public: 19 | TestDetection(ros::NodeHandle& nh); 20 | ~TestDetection(){}; 21 | 22 | private: 23 | void call_back( 24 | const jsk_recognition_msgs::BoundingBoxArray::ConstPtr bbox_array); 25 | ros::NodeHandle nh_; 26 | ros::Subscriber detection_result_sub_; 27 | ros::Publisher accuration_pub_; 28 | ros::ServiceClient client; 29 | }; 30 | 31 | TestDetection::TestDetection(ros::NodeHandle& nh) : nh_(nh) 32 | { 33 | detection_result_sub_ = nh_.subscribe("bounding_boxes_lidar", 10, 34 | &TestDetection::call_back, this); 35 | accuration_pub_ = 36 | nh_.advertise("/detection_accuration", 10); 37 | client = nh_.serviceClient("getbbox"); 38 | ros::service::waitForService("getbbox"); 39 | } 40 | 41 | double getDistance(cv::Point3d pointO, cv::Point3d pointA) 42 | 43 | { 44 | double distance; 45 | pointO.x -= 0.258; 46 | pointO.z -= 1.216; 47 | distance = powf((pointO.x - pointA.x), 2) + powf((pointO.y - pointA.y), 2); 48 | 49 | distance = sqrtf(distance); 50 | 51 | return distance; 52 | } 53 | 54 | void TestDetection::call_back( 55 | const jsk_recognition_msgs::BoundingBoxArray::ConstPtr bbox_array) 56 | { 57 | int num = bbox_array->boxes.size(); 58 | vector detect_centers; 59 | for (int i = 0; i < num; i++) 60 | { 61 | cv::Point3d tmp_center(bbox_array->boxes[i].pose.position.x, 62 | bbox_array->boxes[i].pose.position.y, 63 | bbox_array->boxes[i].pose.position.z); 64 | detect_centers.push_back(tmp_center); 65 | } 66 | cout << "detected " << num << " objetcs" << endl; 67 | gazebo_plugin::bboxPerFrame true_box; 68 | true_box.request.msg = "bbox"; 69 | bool flag = client.call(true_box); 70 | vector true_centers; 71 | if (flag) 72 | { 73 | // ROS_INFO("Has received the true box"); 74 | int true_num = true_box.response.box_num; 75 | for (int i = 0; i < true_num; i++) 76 | { 77 | double pos_x = true_box.response.bboxes[i].pos[0] + 0.223; 78 | double pos_y = true_box.response.bboxes[i].pos[1]; 79 | double pos_z = true_box.response.bboxes[i].pos[2] + 1.212; 80 | cv::Point3d true_center(pos_x, pos_y, pos_z); 81 | true_centers.push_back(true_center); 82 | } 83 | } 84 | int success = 0; 85 | int fail = 0; 86 | if (!detect_centers.empty() && !true_centers.empty()) 87 | { 88 | // start compute the accuration 89 | 90 | for (int i = 0; i < detect_centers.size(); i++) 91 | { 92 | for (int j = 0; j < true_centers.size(); j++) 93 | { 94 | double distance = 95 | getDistance(detect_centers[i], true_centers[j]); 96 | if (distance < 2) 97 | success++; 98 | } 99 | } 100 | } 101 | float accuracy = float(success) / float(detect_centers.size()); 102 | std_msgs::Float32 accu_msg; 103 | accu_msg.data = accuracy; 104 | ROS_INFO("the accuracy is: %d, %d", success, detect_centers.size()); 105 | accuration_pub_.publish(accu_msg); 106 | } 107 | 108 | int main(int argc, char** argv) 109 | { 110 | ros::init(argc, argv, "test_detection"); 111 | ros::NodeHandle nh(""); 112 | TestDetection test_detect(nh); 113 | ros::spin(); 114 | return 0; 115 | } -------------------------------------------------------------------------------- /gazebo_plugin/srv/bboxPerFrame.srv: -------------------------------------------------------------------------------- 1 | string msg 2 | --- 3 | uint16 box_num 4 | bbox[] bboxes -------------------------------------------------------------------------------- /gazebo_plugin/worlds/movingCar.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 10 0 -0 0 6 | 0.8 0.8 0.8 1 7 | 0.2 0.2 0.2 1 8 | 9 | 1000 10 | 0.9 11 | 0.01 12 | 0.001 13 | 14 | -0.5 0.1 -0.9 15 | 16 | 17 | 1 18 | 19 | 20 | 21 | 22 | 0 0 1 23 | 100 100 24 | 25 | 26 | 27 | 28 | 65535 29 | 30 | 31 | 32 | 33 | 100 34 | 50 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 10 43 | 44 | 45 | 0 46 | 47 | 48 | 0 0 1 49 | 100 100 50 | 51 | 52 | 53 | 57 | 58 | 59 | 0 60 | 0 61 | 0 62 | 63 | 64 | 0 0 -9.8 65 | 6e-06 2.3e-05 -4.2e-05 66 | 67 | 68 | 0.001 69 | 1 70 | 1000 71 | 72 | 73 | 0.4 0.4 0.4 1 74 | 0.7 0.7 0.7 1 75 | 1 76 | 77 | 78 | 79 | EARTH_WGS84 80 | 0 81 | 0 82 | 0 83 | 0 84 | 85 | 86 | 87 | true 88 | 89 | 90 | 0 0 0 0 0 -1.57079632679 91 | 92 | 93 | model://pickup/meshes/pickup.dae 94 | 95 | 96 | 97 | 0 98 | 0 99 | 0 100 | 101 | 102 | 10 0 0 103 | 10 10 0 104 | 0 10 0 105 | 0 0 0 106 | 4.0 107 | 108 | 109 | 110 | 111 | 0 0 112 | 0 0 113 | 0 0 114 | 646878 115 | 116 | 0 0 0 0 -0 0 117 | 1 1 1 118 | 119 | 0 0 0 0 -0 0 120 | 0 0 0 0 -0 0 121 | 0 0 0 0 -0 0 122 | 0 0 0 0 -0 0 123 | 124 | 125 | 126 | 127 | 0 0 10 0 -0 0 128 | 129 | 130 | 131 | 132 | 133 | -49.8445 -3.19733 94.0873 -0 1.03564 -0.307807 134 | orbit 135 | perspective 136 | 137 | 138 | 139 | 140 | -------------------------------------------------------------------------------- /package.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "3d-object-detection", 3 | "version": "1.0.0", 4 | "description": "this is a reproduction of my senior's graduation project. It main use yolo to detect object in an image, and cluster the lidar points which are projected to the image. Thus, we can get the 3D boundingbox of the object.", 5 | "main": "index.js", 6 | "scripts": { 7 | "test": "echo \"Error: no test specified\" && exit 1" 8 | }, 9 | "repository": { 10 | "type": "git", 11 | "url": "git+https://github.com/lvruichen/3d-object-detection.git" 12 | }, 13 | "keywords": [], 14 | "author": "", 15 | "license": "ISC", 16 | "bugs": { 17 | "url": "https://github.com/lvruichen/3d-object-detection/issues" 18 | }, 19 | "homepage": "https://github.com/lvruichen/3d-object-detection#readme" 20 | } 21 | -------------------------------------------------------------------------------- /resource/YOLOV5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/resource/YOLOV5.png -------------------------------------------------------------------------------- /resource/calibration.json: -------------------------------------------------------------------------------- 1 | { 2 | "BackCameraDist": [ 3 | 0.0846, 4 | -0.1151, 5 | 0.0014, 6 | 0.0063, 7 | 0.0431 8 | ], 9 | "BackCameraImageSize": [ 10 | 1280, 11 | 720 12 | ], 13 | "BackCameraMtx": [ 14 | 368.355, 15 | 0, 16 | 304.205, 17 | 0, 18 | 368.158, 19 | 234.410, 20 | 0, 21 | 0, 22 | 1 23 | ], 24 | "FrontCameraDist": [ 25 | 0.0689, 26 | -0.0501, 27 | 0.0130, 28 | 0.0025, 29 | 0.0287 30 | ], 31 | "FrontCameraImageSize": [ 32 | 1280, 33 | 720 34 | ], 35 | "FrontCameraMtx": [ 36 | 772.061, 37 | 0, 38 | 614.467, 39 | 0, 40 | 580.638, 41 | 378.459, 42 | 0, 43 | 0, 44 | 1 45 | ], 46 | "FrontLidarToTopLidar": [ 47 | 0.0499, 48 | -0.9987, 49 | 0, 50 | -0.0299, 51 | 0.0499, 52 | 0.0024, 53 | -0.9987, 54 | -0.3699, 55 | 0.9975, 56 | 0.0499, 57 | 0.0499, 58 | -0.5999, 59 | 0, 60 | 0, 61 | 0, 62 | 1 63 | ], 64 | "PerceptionLidarToBackCamera": [ 65 | -0.0018, 66 | -0.2741, 67 | 0.9616, 68 | 0.4424, 69 | -0.9991, 70 | -0.0387, 71 | -0.0129, 72 | -0.0606, 73 | 0.0407, 74 | -0.9608, 75 | -0.2738, 76 | 0.5398, 77 | 0, 78 | 0, 79 | 0, 80 | 1 81 | ], 82 | "FrontCameraToPerceptionLidar": [ 83 | -0.0018, 84 | -0.2741, 85 | 0.9616, 86 | 0.4424, 87 | -0.9991, 88 | -0.0387, 89 | -0.0129, 90 | -0.0606, 91 | 0.0407, 92 | -0.9608, 93 | -0.2738, 94 | 0.5398, 95 | 0, 96 | 0, 97 | 0, 98 | 1 99 | ], 100 | "TopLidarToPerceptionLidar": [ 101 | 0, 102 | -1, 103 | 0, 104 | 0, 105 | 1, 106 | 0, 107 | 0, 108 | 0, 109 | 0, 110 | 0, 111 | 1, 112 | 1.2, 113 | 0, 114 | 0, 115 | 0, 116 | 1 117 | ] 118 | } -------------------------------------------------------------------------------- /resource/cam_lidar_fusion.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/resource/cam_lidar_fusion.png -------------------------------------------------------------------------------- /resource/cloud_project.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/resource/cloud_project.png -------------------------------------------------------------------------------- /resource/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/resource/demo.gif -------------------------------------------------------------------------------- /resource/detect.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/resource/detect.png -------------------------------------------------------------------------------- /resource/final.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/resource/final.png -------------------------------------------------------------------------------- /resource/ground_filter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/resource/ground_filter.png -------------------------------------------------------------------------------- /resource/l_shape_fitter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/resource/l_shape_fitter.png -------------------------------------------------------------------------------- /resource/project_img.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/resource/project_img.png -------------------------------------------------------------------------------- /resource/rviz2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/resource/rviz2.gif -------------------------------------------------------------------------------- /resource/world.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/resource/world.gif -------------------------------------------------------------------------------- /yolo_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(yolo_ros) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | message_generation 15 | ) 16 | 17 | add_message_files( 18 | FILES 19 | BoundingBox.msg 20 | BoundingBoxes.msg 21 | ObjectCount.msg 22 | ) 23 | 24 | add_service_files( 25 | FILES 26 | yolo_srv.srv 27 | ) 28 | 29 | generate_messages( 30 | DEPENDENCIES 31 | std_msgs # Or other packages containing msgs 32 | ) 33 | 34 | catkin_package( 35 | # INCLUDE_DIRS include 36 | # LIBRARIES yolo_ros 37 | # CATKIN_DEPENDS roscpp rospy std_msgs 38 | # DEPENDS system_lib 39 | ) 40 | 41 | 42 | include_directories( 43 | # include 44 | ${catkin_INCLUDE_DIRS} 45 | ) 46 | 47 | ## Declare a C++ library 48 | # add_library(${PROJECT_NAME} 49 | # src/${PROJECT_NAME}/yolo_ros.cpp 50 | # ) 51 | 52 | ## Add cmake target dependencies of the library 53 | ## as an example, code may need to be generated before libraries 54 | ## either from message generation or dynamic reconfigure 55 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 56 | 57 | ## Declare a C++ executable 58 | ## With catkin_make all packages are built within a single CMake context 59 | ## The recommended prefix ensures that target names across packages don't collide 60 | # add_executable(${PROJECT_NAME}_node src/yolo_ros_node.cpp) 61 | 62 | ## Rename C++ executable without prefix 63 | ## The above recommended prefix causes long target names, the following renames the 64 | ## target back to the shorter version for ease of user use 65 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 66 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 67 | 68 | ## Add cmake target dependencies of the executable 69 | ## same as for the library above 70 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 71 | 72 | ## Specify libraries to link a library or executable target against 73 | # target_link_libraries(${PROJECT_NAME}_node 74 | # ${catkin_LIBRARIES} 75 | # ) 76 | 77 | ############# 78 | ## Install ## 79 | ############# 80 | 81 | # all install targets should use catkin DESTINATION variables 82 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 83 | 84 | ## Mark executable scripts (Python etc.) for installation 85 | ## in contrast to setup.py, you can choose the destination 86 | # catkin_install_python(PROGRAMS 87 | # scripts/my_python_script 88 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 89 | # ) 90 | 91 | ## Mark executables for installation 92 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 93 | # install(TARGETS ${PROJECT_NAME}_node 94 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 95 | # ) 96 | 97 | ## Mark libraries for installation 98 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 99 | # install(TARGETS ${PROJECT_NAME} 100 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 101 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 102 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 103 | # ) 104 | 105 | ## Mark cpp header files for installation 106 | # install(DIRECTORY include/${PROJECT_NAME}/ 107 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 108 | # FILES_MATCHING PATTERN "*.h" 109 | # PATTERN ".svn" EXCLUDE 110 | # ) 111 | 112 | ## Mark other files for installation (e.g. launch and bag files, etc.) 113 | # install(FILES 114 | # # myfile1 115 | # # myfile2 116 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 117 | # ) 118 | 119 | ############# 120 | ## Testing ## 121 | ############# 122 | 123 | ## Add gtest based cpp test target and link libraries 124 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_yolo_ros.cpp) 125 | # if(TARGET ${PROJECT_NAME}-test) 126 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 127 | # endif() 128 | 129 | ## Add folders to be run by python nosetests 130 | # catkin_add_nosetests(test) 131 | -------------------------------------------------------------------------------- /yolo_ros/config/config.yaml: -------------------------------------------------------------------------------- 1 | yolov5: 2 | path: /home/eric/a_ros_ws/object_detection_ws/src/3d-object-detection/yolo_ros 3 | weight: /home/eric/a_ros_ws/object_detection_ws/src/3d-object-detection/yolo_ros/scripts/yolov5s.pt 4 | device: gpu 5 | image_topic: /usb_cam_front/image_raw 6 | # image_topic: /kitti/camera_color_left/image_raw 7 | img_size: 1280 8 | -------------------------------------------------------------------------------- /yolo_ros/launch/detect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /yolo_ros/msg/BoundingBox.msg: -------------------------------------------------------------------------------- 1 | float64 probability 2 | int64 xmin 3 | int64 ymin 4 | int64 xmax 5 | int64 ymax 6 | int16 id 7 | string Class -------------------------------------------------------------------------------- /yolo_ros/msg/BoundingBoxes.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | BoundingBox[] bounding_boxes -------------------------------------------------------------------------------- /yolo_ros/msg/ObjectCount.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int8 count -------------------------------------------------------------------------------- /yolo_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | yolo_ros 4 | 0.0.0 5 | The yolo_ros package 6 | 7 | 8 | 9 | 10 | eric 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | message_generation 56 | roscpp 57 | rospy 58 | std_msgs 59 | message_generation 60 | roscpp 61 | rospy 62 | std_msgs 63 | message_runtime 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /yolo_ros/scripts/models/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/yolo_ros/scripts/models/__init__.py -------------------------------------------------------------------------------- /yolo_ros/scripts/models/common.py: -------------------------------------------------------------------------------- 1 | # This file contains modules common to various models 2 | 3 | import math 4 | import numpy as np 5 | import torch 6 | import torch.nn as nn 7 | 8 | from utils.datasets import letterbox 9 | from utils.general import non_max_suppression, make_divisible, scale_coords 10 | 11 | 12 | def autopad(k, p=None): # kernel, padding 13 | # Pad to 'same' 14 | if p is None: 15 | p = k // 2 if isinstance(k, int) else [x // 2 for x in k] # auto-pad 16 | return p 17 | 18 | 19 | def DWConv(c1, c2, k=1, s=1, act=True): 20 | # Depthwise convolution 21 | return Conv(c1, c2, k, s, g=math.gcd(c1, c2), act=act) 22 | 23 | 24 | class Conv(nn.Module): 25 | # Standard convolution 26 | def __init__(self, c1, c2, k=1, s=1, p=None, g=1, act=True): # ch_in, ch_out, kernel, stride, padding, groups 27 | super(Conv, self).__init__() 28 | self.conv = nn.Conv2d(c1, c2, k, s, autopad(k, p), groups=g, bias=False) 29 | self.bn = nn.BatchNorm2d(c2) 30 | self.act = nn.Hardswish() if act else nn.Identity() 31 | 32 | def forward(self, x): 33 | return self.act(self.bn(self.conv(x))) 34 | 35 | def fuseforward(self, x): 36 | return self.act(self.conv(x)) 37 | 38 | 39 | class Bottleneck(nn.Module): 40 | # Standard bottleneck 41 | def __init__(self, c1, c2, shortcut=True, g=1, e=0.5): # ch_in, ch_out, shortcut, groups, expansion 42 | super(Bottleneck, self).__init__() 43 | c_ = int(c2 * e) # hidden channels 44 | self.cv1 = Conv(c1, c_, 1, 1) 45 | self.cv2 = Conv(c_, c2, 3, 1, g=g) 46 | self.add = shortcut and c1 == c2 47 | 48 | def forward(self, x): 49 | return x + self.cv2(self.cv1(x)) if self.add else self.cv2(self.cv1(x)) 50 | 51 | 52 | class BottleneckCSP(nn.Module): 53 | # CSP Bottleneck https://github.com/WongKinYiu/CrossStagePartialNetworks 54 | def __init__(self, c1, c2, n=1, shortcut=True, g=1, e=0.5): # ch_in, ch_out, number, shortcut, groups, expansion 55 | super(BottleneckCSP, self).__init__() 56 | c_ = int(c2 * e) # hidden channels 57 | self.cv1 = Conv(c1, c_, 1, 1) 58 | self.cv2 = nn.Conv2d(c1, c_, 1, 1, bias=False) 59 | self.cv3 = nn.Conv2d(c_, c_, 1, 1, bias=False) 60 | self.cv4 = Conv(2 * c_, c2, 1, 1) 61 | self.bn = nn.BatchNorm2d(2 * c_) # applied to cat(cv2, cv3) 62 | self.act = nn.LeakyReLU(0.1, inplace=True) 63 | self.m = nn.Sequential(*[Bottleneck(c_, c_, shortcut, g, e=1.0) for _ in range(n)]) 64 | 65 | def forward(self, x): 66 | y1 = self.cv3(self.m(self.cv1(x))) 67 | y2 = self.cv2(x) 68 | return self.cv4(self.act(self.bn(torch.cat((y1, y2), dim=1)))) 69 | 70 | 71 | class SPP(nn.Module): 72 | # Spatial pyramid pooling layer used in YOLOv3-SPP 73 | def __init__(self, c1, c2, k=(5, 9, 13)): 74 | super(SPP, self).__init__() 75 | c_ = c1 // 2 # hidden channels 76 | self.cv1 = Conv(c1, c_, 1, 1) 77 | self.cv2 = Conv(c_ * (len(k) + 1), c2, 1, 1) 78 | self.m = nn.ModuleList([nn.MaxPool2d(kernel_size=x, stride=1, padding=x // 2) for x in k]) 79 | 80 | def forward(self, x): 81 | x = self.cv1(x) 82 | return self.cv2(torch.cat([x] + [m(x) for m in self.m], 1)) 83 | 84 | 85 | class Focus(nn.Module): 86 | # Focus wh information into c-space 87 | def __init__(self, c1, c2, k=1, s=1, p=None, g=1, act=True): # ch_in, ch_out, kernel, stride, padding, groups 88 | super(Focus, self).__init__() 89 | self.conv = Conv(c1 * 4, c2, k, s, p, g, act) 90 | 91 | def forward(self, x): # x(b,c,w,h) -> y(b,4c,w/2,h/2) 92 | return self.conv(torch.cat([x[..., ::2, ::2], x[..., 1::2, ::2], x[..., ::2, 1::2], x[..., 1::2, 1::2]], 1)) 93 | 94 | 95 | class Concat(nn.Module): 96 | # Concatenate a list of tensors along dimension 97 | def __init__(self, dimension=1): 98 | super(Concat, self).__init__() 99 | self.d = dimension 100 | 101 | def forward(self, x): 102 | return torch.cat(x, self.d) 103 | 104 | 105 | class NMS(nn.Module): 106 | # Non-Maximum Suppression (NMS) module 107 | conf = 0.25 # confidence threshold 108 | iou = 0.45 # IoU threshold 109 | classes = None # (optional list) filter by class 110 | 111 | def __init__(self): 112 | super(NMS, self).__init__() 113 | 114 | def forward(self, x): 115 | return non_max_suppression(x[0], conf_thres=self.conf, iou_thres=self.iou, classes=self.classes) 116 | 117 | 118 | class autoShape(nn.Module): 119 | # input-robust model wrapper for passing cv2/np/PIL/torch inputs. Includes preprocessing, inference and NMS 120 | img_size = 640 # inference size (pixels) 121 | conf = 0.25 # NMS confidence threshold 122 | iou = 0.45 # NMS IoU threshold 123 | classes = None # (optional list) filter by class 124 | 125 | def __init__(self, model): 126 | super(autoShape, self).__init__() 127 | self.model = model 128 | 129 | def forward(self, x, size=640, augment=False, profile=False): 130 | # supports inference from various sources. For height=720, width=1280, RGB images example inputs are: 131 | # opencv: x = cv2.imread('image.jpg')[:,:,::-1] # HWC BGR to RGB x(720,1280,3) 132 | # PIL: x = Image.open('image.jpg') # HWC x(720,1280,3) 133 | # numpy: x = np.zeros((720,1280,3)) # HWC 134 | # torch: x = torch.zeros(16,3,720,1280) # BCHW 135 | # multiple: x = [Image.open('image1.jpg'), Image.open('image2.jpg'), ...] # list of images 136 | 137 | p = next(self.model.parameters()) # for device and type 138 | if isinstance(x, torch.Tensor): # torch 139 | return self.model(x.to(p.device).type_as(p), augment, profile) # inference 140 | 141 | # Pre-process 142 | if not isinstance(x, list): 143 | x = [x] 144 | shape0, shape1 = [], [] # image and inference shapes 145 | batch = range(len(x)) # batch size 146 | for i in batch: 147 | x[i] = np.array(x[i])[:, :, :3] # up to 3 channels if png 148 | s = x[i].shape[:2] # HWC 149 | shape0.append(s) # image shape 150 | g = (size / max(s)) # gain 151 | shape1.append([y * g for y in s]) 152 | shape1 = [make_divisible(x, int(self.stride.max())) for x in np.stack(shape1, 0).max(0)] # inference shape 153 | x = [letterbox(x[i], new_shape=shape1, auto=False)[0] for i in batch] # pad 154 | x = np.stack(x, 0) if batch[-1] else x[0][None] # stack 155 | x = np.ascontiguousarray(x.transpose((0, 3, 1, 2))) # BHWC to BCHW 156 | x = torch.from_numpy(x).to(p.device).type_as(p) / 255. # uint8 to fp16/32 157 | 158 | # Inference 159 | x = self.model(x, augment, profile) # forward 160 | x = non_max_suppression(x[0], conf_thres=self.conf, iou_thres=self.iou, classes=self.classes) # NMS 161 | 162 | # Post-process 163 | for i in batch: 164 | if x[i] is not None: 165 | x[i][:, :4] = scale_coords(shape1, x[i][:, :4], shape0[i]) 166 | return x 167 | 168 | 169 | class Flatten(nn.Module): 170 | # Use after nn.AdaptiveAvgPool2d(1) to remove last 2 dimensions 171 | @staticmethod 172 | def forward(x): 173 | return x.view(x.size(0), -1) 174 | 175 | 176 | class Classify(nn.Module): 177 | # Classification head, i.e. x(b,c1,20,20) to x(b,c2) 178 | def __init__(self, c1, c2, k=1, s=1, p=None, g=1): # ch_in, ch_out, kernel, stride, padding, groups 179 | super(Classify, self).__init__() 180 | self.aap = nn.AdaptiveAvgPool2d(1) # to x(b,c1,1,1) 181 | self.conv = nn.Conv2d(c1, c2, k, s, autopad(k, p), groups=g, bias=False) # to x(b,c2,1,1) 182 | self.flat = Flatten() 183 | 184 | def forward(self, x): 185 | z = torch.cat([self.aap(y) for y in (x if isinstance(x, list) else [x])], 1) # cat if list 186 | return self.flat(self.conv(z)) # flatten to x(b,c2) 187 | -------------------------------------------------------------------------------- /yolo_ros/scripts/models/experimental.py: -------------------------------------------------------------------------------- 1 | # This file contains experimental modules 2 | 3 | import numpy as np 4 | import torch 5 | import torch.nn as nn 6 | 7 | from models.common import Conv, DWConv 8 | from utils.google_utils import attempt_download 9 | 10 | 11 | class CrossConv(nn.Module): 12 | # Cross Convolution Downsample 13 | def __init__(self, c1, c2, k=3, s=1, g=1, e=1.0, shortcut=False): 14 | # ch_in, ch_out, kernel, stride, groups, expansion, shortcut 15 | super(CrossConv, self).__init__() 16 | c_ = int(c2 * e) # hidden channels 17 | self.cv1 = Conv(c1, c_, (1, k), (1, s)) 18 | self.cv2 = Conv(c_, c2, (k, 1), (s, 1), g=g) 19 | self.add = shortcut and c1 == c2 20 | 21 | def forward(self, x): 22 | return x + self.cv2(self.cv1(x)) if self.add else self.cv2(self.cv1(x)) 23 | 24 | 25 | class C3(nn.Module): 26 | # Cross Convolution CSP 27 | def __init__(self, c1, c2, n=1, shortcut=True, g=1, e=0.5): # ch_in, ch_out, number, shortcut, groups, expansion 28 | super(C3, self).__init__() 29 | c_ = int(c2 * e) # hidden channels 30 | self.cv1 = Conv(c1, c_, 1, 1) 31 | self.cv2 = nn.Conv2d(c1, c_, 1, 1, bias=False) 32 | self.cv3 = nn.Conv2d(c_, c_, 1, 1, bias=False) 33 | self.cv4 = Conv(2 * c_, c2, 1, 1) 34 | self.bn = nn.BatchNorm2d(2 * c_) # applied to cat(cv2, cv3) 35 | self.act = nn.LeakyReLU(0.1, inplace=True) 36 | self.m = nn.Sequential(*[CrossConv(c_, c_, 3, 1, g, 1.0, shortcut) for _ in range(n)]) 37 | 38 | def forward(self, x): 39 | y1 = self.cv3(self.m(self.cv1(x))) 40 | y2 = self.cv2(x) 41 | return self.cv4(self.act(self.bn(torch.cat((y1, y2), dim=1)))) 42 | 43 | 44 | class Sum(nn.Module): 45 | # Weighted sum of 2 or more layers https://arxiv.org/abs/1911.09070 46 | def __init__(self, n, weight=False): # n: number of inputs 47 | super(Sum, self).__init__() 48 | self.weight = weight # apply weights boolean 49 | self.iter = range(n - 1) # iter object 50 | if weight: 51 | self.w = nn.Parameter(-torch.arange(1., n) / 2, requires_grad=True) # layer weights 52 | 53 | def forward(self, x): 54 | y = x[0] # no weight 55 | if self.weight: 56 | w = torch.sigmoid(self.w) * 2 57 | for i in self.iter: 58 | y = y + x[i + 1] * w[i] 59 | else: 60 | for i in self.iter: 61 | y = y + x[i + 1] 62 | return y 63 | 64 | 65 | class GhostConv(nn.Module): 66 | # Ghost Convolution https://github.com/huawei-noah/ghostnet 67 | def __init__(self, c1, c2, k=1, s=1, g=1, act=True): # ch_in, ch_out, kernel, stride, groups 68 | super(GhostConv, self).__init__() 69 | c_ = c2 // 2 # hidden channels 70 | self.cv1 = Conv(c1, c_, k, s, None, g, act) 71 | self.cv2 = Conv(c_, c_, 5, 1, None, c_, act) 72 | 73 | def forward(self, x): 74 | y = self.cv1(x) 75 | return torch.cat([y, self.cv2(y)], 1) 76 | 77 | 78 | class GhostBottleneck(nn.Module): 79 | # Ghost Bottleneck https://github.com/huawei-noah/ghostnet 80 | def __init__(self, c1, c2, k, s): 81 | super(GhostBottleneck, self).__init__() 82 | c_ = c2 // 2 83 | self.conv = nn.Sequential(GhostConv(c1, c_, 1, 1), # pw 84 | DWConv(c_, c_, k, s, act=False) if s == 2 else nn.Identity(), # dw 85 | GhostConv(c_, c2, 1, 1, act=False)) # pw-linear 86 | self.shortcut = nn.Sequential(DWConv(c1, c1, k, s, act=False), 87 | Conv(c1, c2, 1, 1, act=False)) if s == 2 else nn.Identity() 88 | 89 | def forward(self, x): 90 | return self.conv(x) + self.shortcut(x) 91 | 92 | 93 | class MixConv2d(nn.Module): 94 | # Mixed Depthwise Conv https://arxiv.org/abs/1907.09595 95 | def __init__(self, c1, c2, k=(1, 3), s=1, equal_ch=True): 96 | super(MixConv2d, self).__init__() 97 | groups = len(k) 98 | if equal_ch: # equal c_ per group 99 | i = torch.linspace(0, groups - 1E-6, c2).floor() # c2 indices 100 | c_ = [(i == g).sum() for g in range(groups)] # intermediate channels 101 | else: # equal weight.numel() per group 102 | b = [c2] + [0] * groups 103 | a = np.eye(groups + 1, groups, k=-1) 104 | a -= np.roll(a, 1, axis=1) 105 | a *= np.array(k) ** 2 106 | a[0] = 1 107 | c_ = np.linalg.lstsq(a, b, rcond=None)[0].round() # solve for equal weight indices, ax = b 108 | 109 | self.m = nn.ModuleList([nn.Conv2d(c1, int(c_[g]), k[g], s, k[g] // 2, bias=False) for g in range(groups)]) 110 | self.bn = nn.BatchNorm2d(c2) 111 | self.act = nn.LeakyReLU(0.1, inplace=True) 112 | 113 | def forward(self, x): 114 | return x + self.act(self.bn(torch.cat([m(x) for m in self.m], 1))) 115 | 116 | 117 | class Ensemble(nn.ModuleList): 118 | # Ensemble of models 119 | def __init__(self): 120 | super(Ensemble, self).__init__() 121 | 122 | def forward(self, x, augment=False): 123 | y = [] 124 | for module in self: 125 | y.append(module(x, augment)[0]) 126 | # y = torch.stack(y).max(0)[0] # max ensemble 127 | # y = torch.cat(y, 1) # nms ensemble 128 | y = torch.stack(y).mean(0) # mean ensemble 129 | return y, None # inference, train output 130 | 131 | 132 | def attempt_load(weights, map_location=None): 133 | # Loads an ensemble of models weights=[a,b,c] or a single model weights=[a] or weights=a 134 | model = Ensemble() 135 | for w in weights if isinstance(weights, list) else [weights]: 136 | attempt_download(w) 137 | model.append(torch.load(w, map_location=map_location)['model'].float().fuse().eval()) # load FP32 model 138 | 139 | # Compatibility updates 140 | for m in model.modules(): 141 | if type(m) in [nn.Hardswish, nn.LeakyReLU, nn.ReLU, nn.ReLU6]: 142 | m.inplace = True # pytorch 1.7.0 compatibility 143 | elif type(m) is Conv: 144 | m._non_persistent_buffers_set = set() # pytorch 1.6.0 compatibility 145 | 146 | if len(model) == 1: 147 | return model[-1] # return model 148 | else: 149 | print('Ensemble created with %s\n' % weights) 150 | for k in ['names', 'stride']: 151 | setattr(model, k, getattr(model[-1], k)) 152 | return model # return ensemble 153 | -------------------------------------------------------------------------------- /yolo_ros/scripts/models/export.py: -------------------------------------------------------------------------------- 1 | """Exports a YOLOv5 *.pt model to ONNX and TorchScript formats 2 | 3 | Usage: 4 | $ export PYTHONPATH="$PWD" && python models/export.py --weights ./weights/yolov5s.pt --img 640 --batch 1 5 | """ 6 | 7 | import argparse 8 | import sys 9 | import time 10 | 11 | sys.path.append('./') # to run '$ python *.py' files in subdirectories 12 | 13 | import torch 14 | import torch.nn as nn 15 | 16 | import models 17 | from models.experimental import attempt_load 18 | from utils.activations import Hardswish 19 | from utils.general import set_logging, check_img_size 20 | 21 | if __name__ == '__main__': 22 | parser = argparse.ArgumentParser() 23 | parser.add_argument('--weights', type=str, default='./yolov5s.pt', help='weights path') # from yolov5/models/ 24 | parser.add_argument('--img-size', nargs='+', type=int, default=[640, 640], help='image size') # height, width 25 | parser.add_argument('--batch-size', type=int, default=1, help='batch size') 26 | opt = parser.parse_args() 27 | opt.img_size *= 2 if len(opt.img_size) == 1 else 1 # expand 28 | print(opt) 29 | set_logging() 30 | t = time.time() 31 | 32 | # Load PyTorch model 33 | model = attempt_load(opt.weights, map_location=torch.device('cpu')) # load FP32 model 34 | labels = model.names 35 | 36 | # Checks 37 | gs = int(max(model.stride)) # grid size (max stride) 38 | opt.img_size = [check_img_size(x, gs) for x in opt.img_size] # verify img_size are gs-multiples 39 | 40 | # Input 41 | img = torch.zeros(opt.batch_size, 3, *opt.img_size) # image size(1,3,320,192) iDetection 42 | 43 | # Update model 44 | for k, m in model.named_modules(): 45 | m._non_persistent_buffers_set = set() # pytorch 1.6.0 compatibility 46 | if isinstance(m, models.common.Conv) and isinstance(m.act, nn.Hardswish): 47 | m.act = Hardswish() # assign activation 48 | # if isinstance(m, models.yolo.Detect): 49 | # m.forward = m.forward_export # assign forward (optional) 50 | model.model[-1].export = True # set Detect() layer export=True 51 | y = model(img) # dry run 52 | 53 | # TorchScript export 54 | try: 55 | print('\nStarting TorchScript export with torch %s...' % torch.__version__) 56 | f = opt.weights.replace('.pt', '.torchscript.pt') # filename 57 | ts = torch.jit.trace(model, img) 58 | ts.save(f) 59 | print('TorchScript export success, saved as %s' % f) 60 | except Exception as e: 61 | print('TorchScript export failure: %s' % e) 62 | 63 | # ONNX export 64 | try: 65 | import onnx 66 | 67 | print('\nStarting ONNX export with onnx %s...' % onnx.__version__) 68 | f = opt.weights.replace('.pt', '.onnx') # filename 69 | torch.onnx.export(model, img, f, verbose=False, opset_version=12, input_names=['images'], 70 | output_names=['classes', 'boxes'] if y is None else ['output']) 71 | 72 | # Checks 73 | onnx_model = onnx.load(f) # load onnx model 74 | onnx.checker.check_model(onnx_model) # check onnx model 75 | # print(onnx.helper.printable_graph(onnx_model.graph)) # print a human readable model 76 | print('ONNX export success, saved as %s' % f) 77 | except Exception as e: 78 | print('ONNX export failure: %s' % e) 79 | 80 | # CoreML export 81 | try: 82 | import coremltools as ct 83 | 84 | print('\nStarting CoreML export with coremltools %s...' % ct.__version__) 85 | # convert model from torchscript and apply pixel scaling as per detect.py 86 | model = ct.convert(ts, inputs=[ct.ImageType(name='image', shape=img.shape, scale=1 / 255.0, bias=[0, 0, 0])]) 87 | f = opt.weights.replace('.pt', '.mlmodel') # filename 88 | model.save(f) 89 | print('CoreML export success, saved as %s' % f) 90 | except Exception as e: 91 | print('CoreML export failure: %s' % e) 92 | 93 | # Finish 94 | print('\nExport complete (%.2fs). Visualize with https://github.com/lutzroeder/netron.' % (time.time() - t)) 95 | -------------------------------------------------------------------------------- /yolo_ros/scripts/models/hub/yolov3-spp.yaml: -------------------------------------------------------------------------------- 1 | # parameters 2 | nc: 80 # number of classes 3 | depth_multiple: 1.0 # model depth multiple 4 | width_multiple: 1.0 # layer channel multiple 5 | 6 | # anchors 7 | anchors: 8 | - [10,13, 16,30, 33,23] # P3/8 9 | - [30,61, 62,45, 59,119] # P4/16 10 | - [116,90, 156,198, 373,326] # P5/32 11 | 12 | # darknet53 backbone 13 | backbone: 14 | # [from, number, module, args] 15 | [[-1, 1, Conv, [32, 3, 1]], # 0 16 | [-1, 1, Conv, [64, 3, 2]], # 1-P1/2 17 | [-1, 1, Bottleneck, [64]], 18 | [-1, 1, Conv, [128, 3, 2]], # 3-P2/4 19 | [-1, 2, Bottleneck, [128]], 20 | [-1, 1, Conv, [256, 3, 2]], # 5-P3/8 21 | [-1, 8, Bottleneck, [256]], 22 | [-1, 1, Conv, [512, 3, 2]], # 7-P4/16 23 | [-1, 8, Bottleneck, [512]], 24 | [-1, 1, Conv, [1024, 3, 2]], # 9-P5/32 25 | [-1, 4, Bottleneck, [1024]], # 10 26 | ] 27 | 28 | # YOLOv3-SPP head 29 | head: 30 | [[-1, 1, Bottleneck, [1024, False]], 31 | [-1, 1, SPP, [512, [5, 9, 13]]], 32 | [-1, 1, Conv, [1024, 3, 1]], 33 | [-1, 1, Conv, [512, 1, 1]], 34 | [-1, 1, Conv, [1024, 3, 1]], # 15 (P5/32-large) 35 | 36 | [-2, 1, Conv, [256, 1, 1]], 37 | [-1, 1, nn.Upsample, [None, 2, 'nearest']], 38 | [[-1, 8], 1, Concat, [1]], # cat backbone P4 39 | [-1, 1, Bottleneck, [512, False]], 40 | [-1, 1, Bottleneck, [512, False]], 41 | [-1, 1, Conv, [256, 1, 1]], 42 | [-1, 1, Conv, [512, 3, 1]], # 22 (P4/16-medium) 43 | 44 | [-2, 1, Conv, [128, 1, 1]], 45 | [-1, 1, nn.Upsample, [None, 2, 'nearest']], 46 | [[-1, 6], 1, Concat, [1]], # cat backbone P3 47 | [-1, 1, Bottleneck, [256, False]], 48 | [-1, 2, Bottleneck, [256, False]], # 27 (P3/8-small) 49 | 50 | [[27, 22, 15], 1, Detect, [nc, anchors]], # Detect(P3, P4, P5) 51 | ] 52 | -------------------------------------------------------------------------------- /yolo_ros/scripts/models/hub/yolov5-fpn.yaml: -------------------------------------------------------------------------------- 1 | # parameters 2 | nc: 80 # number of classes 3 | depth_multiple: 1.0 # model depth multiple 4 | width_multiple: 1.0 # layer channel multiple 5 | 6 | # anchors 7 | anchors: 8 | - [10,13, 16,30, 33,23] # P3/8 9 | - [30,61, 62,45, 59,119] # P4/16 10 | - [116,90, 156,198, 373,326] # P5/32 11 | 12 | # YOLOv5 backbone 13 | backbone: 14 | # [from, number, module, args] 15 | [[-1, 1, Focus, [64, 3]], # 0-P1/2 16 | [-1, 1, Conv, [128, 3, 2]], # 1-P2/4 17 | [-1, 3, Bottleneck, [128]], 18 | [-1, 1, Conv, [256, 3, 2]], # 3-P3/8 19 | [-1, 9, BottleneckCSP, [256]], 20 | [-1, 1, Conv, [512, 3, 2]], # 5-P4/16 21 | [-1, 9, BottleneckCSP, [512]], 22 | [-1, 1, Conv, [1024, 3, 2]], # 7-P5/32 23 | [-1, 1, SPP, [1024, [5, 9, 13]]], 24 | [-1, 6, BottleneckCSP, [1024]], # 9 25 | ] 26 | 27 | # YOLOv5 FPN head 28 | head: 29 | [[-1, 3, BottleneckCSP, [1024, False]], # 10 (P5/32-large) 30 | 31 | [-1, 1, nn.Upsample, [None, 2, 'nearest']], 32 | [[-1, 6], 1, Concat, [1]], # cat backbone P4 33 | [-1, 1, Conv, [512, 1, 1]], 34 | [-1, 3, BottleneckCSP, [512, False]], # 14 (P4/16-medium) 35 | 36 | [-1, 1, nn.Upsample, [None, 2, 'nearest']], 37 | [[-1, 4], 1, Concat, [1]], # cat backbone P3 38 | [-1, 1, Conv, [256, 1, 1]], 39 | [-1, 3, BottleneckCSP, [256, False]], # 18 (P3/8-small) 40 | 41 | [[18, 14, 10], 1, Detect, [nc, anchors]], # Detect(P3, P4, P5) 42 | ] 43 | -------------------------------------------------------------------------------- /yolo_ros/scripts/models/hub/yolov5-panet.yaml: -------------------------------------------------------------------------------- 1 | # parameters 2 | nc: 80 # number of classes 3 | depth_multiple: 1.0 # model depth multiple 4 | width_multiple: 1.0 # layer channel multiple 5 | 6 | # anchors 7 | anchors: 8 | - [116,90, 156,198, 373,326] # P5/32 9 | - [30,61, 62,45, 59,119] # P4/16 10 | - [10,13, 16,30, 33,23] # P3/8 11 | 12 | # YOLOv5 backbone 13 | backbone: 14 | # [from, number, module, args] 15 | [[-1, 1, Focus, [64, 3]], # 0-P1/2 16 | [-1, 1, Conv, [128, 3, 2]], # 1-P2/4 17 | [-1, 3, BottleneckCSP, [128]], 18 | [-1, 1, Conv, [256, 3, 2]], # 3-P3/8 19 | [-1, 9, BottleneckCSP, [256]], 20 | [-1, 1, Conv, [512, 3, 2]], # 5-P4/16 21 | [-1, 9, BottleneckCSP, [512]], 22 | [-1, 1, Conv, [1024, 3, 2]], # 7-P5/32 23 | [-1, 1, SPP, [1024, [5, 9, 13]]], 24 | [-1, 3, BottleneckCSP, [1024, False]], # 9 25 | ] 26 | 27 | # YOLOv5 PANet head 28 | head: 29 | [[-1, 1, Conv, [512, 1, 1]], 30 | [-1, 1, nn.Upsample, [None, 2, 'nearest']], 31 | [[-1, 6], 1, Concat, [1]], # cat backbone P4 32 | [-1, 3, BottleneckCSP, [512, False]], # 13 33 | 34 | [-1, 1, Conv, [256, 1, 1]], 35 | [-1, 1, nn.Upsample, [None, 2, 'nearest']], 36 | [[-1, 4], 1, Concat, [1]], # cat backbone P3 37 | [-1, 3, BottleneckCSP, [256, False]], # 17 (P3/8-small) 38 | 39 | [-1, 1, Conv, [256, 3, 2]], 40 | [[-1, 14], 1, Concat, [1]], # cat head P4 41 | [-1, 3, BottleneckCSP, [512, False]], # 20 (P4/16-medium) 42 | 43 | [-1, 1, Conv, [512, 3, 2]], 44 | [[-1, 10], 1, Concat, [1]], # cat head P5 45 | [-1, 3, BottleneckCSP, [1024, False]], # 23 (P5/32-large) 46 | 47 | [[17, 20, 23], 1, Detect, [nc, anchors]], # Detect(P5, P4, P3) 48 | ] 49 | -------------------------------------------------------------------------------- /yolo_ros/scripts/models/yolov5l.yaml: -------------------------------------------------------------------------------- 1 | # parameters 2 | nc: 80 # number of classes 3 | depth_multiple: 1.0 # model depth multiple 4 | width_multiple: 1.0 # layer channel multiple 5 | 6 | # anchors 7 | anchors: 8 | - [10,13, 16,30, 33,23] # P3/8 9 | - [30,61, 62,45, 59,119] # P4/16 10 | - [116,90, 156,198, 373,326] # P5/32 11 | 12 | # YOLOv5 backbone 13 | backbone: 14 | # [from, number, module, args] 15 | [[-1, 1, Focus, [64, 3]], # 0-P1/2 16 | [-1, 1, Conv, [128, 3, 2]], # 1-P2/4 17 | [-1, 3, BottleneckCSP, [128]], 18 | [-1, 1, Conv, [256, 3, 2]], # 3-P3/8 19 | [-1, 9, BottleneckCSP, [256]], 20 | [-1, 1, Conv, [512, 3, 2]], # 5-P4/16 21 | [-1, 9, BottleneckCSP, [512]], 22 | [-1, 1, Conv, [1024, 3, 2]], # 7-P5/32 23 | [-1, 1, SPP, [1024, [5, 9, 13]]], 24 | [-1, 3, BottleneckCSP, [1024, False]], # 9 25 | ] 26 | 27 | # YOLOv5 head 28 | head: 29 | [[-1, 1, Conv, [512, 1, 1]], 30 | [-1, 1, nn.Upsample, [None, 2, 'nearest']], 31 | [[-1, 6], 1, Concat, [1]], # cat backbone P4 32 | [-1, 3, BottleneckCSP, [512, False]], # 13 33 | 34 | [-1, 1, Conv, [256, 1, 1]], 35 | [-1, 1, nn.Upsample, [None, 2, 'nearest']], 36 | [[-1, 4], 1, Concat, [1]], # cat backbone P3 37 | [-1, 3, BottleneckCSP, [256, False]], # 17 (P3/8-small) 38 | 39 | [-1, 1, Conv, [256, 3, 2]], 40 | [[-1, 14], 1, Concat, [1]], # cat head P4 41 | [-1, 3, BottleneckCSP, [512, False]], # 20 (P4/16-medium) 42 | 43 | [-1, 1, Conv, [512, 3, 2]], 44 | [[-1, 10], 1, Concat, [1]], # cat head P5 45 | [-1, 3, BottleneckCSP, [1024, False]], # 23 (P5/32-large) 46 | 47 | [[17, 20, 23], 1, Detect, [nc, anchors]], # Detect(P3, P4, P5) 48 | ] 49 | -------------------------------------------------------------------------------- /yolo_ros/scripts/models/yolov5m.yaml: -------------------------------------------------------------------------------- 1 | # parameters 2 | nc: 80 # number of classes 3 | depth_multiple: 0.67 # model depth multiple 4 | width_multiple: 0.75 # layer channel multiple 5 | 6 | # anchors 7 | anchors: 8 | - [10,13, 16,30, 33,23] # P3/8 9 | - [30,61, 62,45, 59,119] # P4/16 10 | - [116,90, 156,198, 373,326] # P5/32 11 | 12 | # YOLOv5 backbone 13 | backbone: 14 | # [from, number, module, args] 15 | [[-1, 1, Focus, [64, 3]], # 0-P1/2 16 | [-1, 1, Conv, [128, 3, 2]], # 1-P2/4 17 | [-1, 3, BottleneckCSP, [128]], 18 | [-1, 1, Conv, [256, 3, 2]], # 3-P3/8 19 | [-1, 9, BottleneckCSP, [256]], 20 | [-1, 1, Conv, [512, 3, 2]], # 5-P4/16 21 | [-1, 9, BottleneckCSP, [512]], 22 | [-1, 1, Conv, [1024, 3, 2]], # 7-P5/32 23 | [-1, 1, SPP, [1024, [5, 9, 13]]], 24 | [-1, 3, BottleneckCSP, [1024, False]], # 9 25 | ] 26 | 27 | # YOLOv5 head 28 | head: 29 | [[-1, 1, Conv, [512, 1, 1]], 30 | [-1, 1, nn.Upsample, [None, 2, 'nearest']], 31 | [[-1, 6], 1, Concat, [1]], # cat backbone P4 32 | [-1, 3, BottleneckCSP, [512, False]], # 13 33 | 34 | [-1, 1, Conv, [256, 1, 1]], 35 | [-1, 1, nn.Upsample, [None, 2, 'nearest']], 36 | [[-1, 4], 1, Concat, [1]], # cat backbone P3 37 | [-1, 3, BottleneckCSP, [256, False]], # 17 (P3/8-small) 38 | 39 | [-1, 1, Conv, [256, 3, 2]], 40 | [[-1, 14], 1, Concat, [1]], # cat head P4 41 | [-1, 3, BottleneckCSP, [512, False]], # 20 (P4/16-medium) 42 | 43 | [-1, 1, Conv, [512, 3, 2]], 44 | [[-1, 10], 1, Concat, [1]], # cat head P5 45 | [-1, 3, BottleneckCSP, [1024, False]], # 23 (P5/32-large) 46 | 47 | [[17, 20, 23], 1, Detect, [nc, anchors]], # Detect(P3, P4, P5) 48 | ] 49 | -------------------------------------------------------------------------------- /yolo_ros/scripts/models/yolov5s.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/yolo_ros/scripts/models/yolov5s.pt -------------------------------------------------------------------------------- /yolo_ros/scripts/models/yolov5s.yaml: -------------------------------------------------------------------------------- 1 | # parameters 2 | nc: 80 # number of classes 3 | depth_multiple: 0.33 # model depth multiple 4 | width_multiple: 0.50 # layer channel multiple 5 | 6 | # anchors 7 | anchors: 8 | - [10,13, 16,30, 33,23] # P3/8 9 | - [30,61, 62,45, 59,119] # P4/16 10 | - [116,90, 156,198, 373,326] # P5/32 11 | 12 | # YOLOv5 backbone 13 | backbone: 14 | # [from, number, module, args] 15 | [[-1, 1, Focus, [64, 3]], # 0-P1/2 16 | [-1, 1, Conv, [128, 3, 2]], # 1-P2/4 17 | [-1, 3, BottleneckCSP, [128]], 18 | [-1, 1, Conv, [256, 3, 2]], # 3-P3/8 19 | [-1, 9, BottleneckCSP, [256]], 20 | [-1, 1, Conv, [512, 3, 2]], # 5-P4/16 21 | [-1, 9, BottleneckCSP, [512]], 22 | [-1, 1, Conv, [1024, 3, 2]], # 7-P5/32 23 | [-1, 1, SPP, [1024, [5, 9, 13]]], 24 | [-1, 3, BottleneckCSP, [1024, False]], # 9 25 | ] 26 | 27 | # YOLOv5 head 28 | head: 29 | [[-1, 1, Conv, [512, 1, 1]], 30 | [-1, 1, nn.Upsample, [None, 2, 'nearest']], 31 | [[-1, 6], 1, Concat, [1]], # cat backbone P4 32 | [-1, 3, BottleneckCSP, [512, False]], # 13 33 | 34 | [-1, 1, Conv, [256, 1, 1]], 35 | [-1, 1, nn.Upsample, [None, 2, 'nearest']], 36 | [[-1, 4], 1, Concat, [1]], # cat backbone P3 37 | [-1, 3, BottleneckCSP, [256, False]], # 17 (P3/8-small) 38 | 39 | [-1, 1, Conv, [256, 3, 2]], 40 | [[-1, 14], 1, Concat, [1]], # cat head P4 41 | [-1, 3, BottleneckCSP, [512, False]], # 20 (P4/16-medium) 42 | 43 | [-1, 1, Conv, [512, 3, 2]], 44 | [[-1, 10], 1, Concat, [1]], # cat head P5 45 | [-1, 3, BottleneckCSP, [1024, False]], # 23 (P5/32-large) 46 | 47 | [[17, 20, 23], 1, Detect, [nc, anchors]], # Detect(P3, P4, P5) 48 | ] 49 | -------------------------------------------------------------------------------- /yolo_ros/scripts/models/yolov5x.yaml: -------------------------------------------------------------------------------- 1 | # parameters 2 | nc: 80 # number of classes 3 | depth_multiple: 1.33 # model depth multiple 4 | width_multiple: 1.25 # layer channel multiple 5 | 6 | # anchors 7 | anchors: 8 | - [10,13, 16,30, 33,23] # P3/8 9 | - [30,61, 62,45, 59,119] # P4/16 10 | - [116,90, 156,198, 373,326] # P5/32 11 | 12 | # YOLOv5 backbone 13 | backbone: 14 | # [from, number, module, args] 15 | [[-1, 1, Focus, [64, 3]], # 0-P1/2 16 | [-1, 1, Conv, [128, 3, 2]], # 1-P2/4 17 | [-1, 3, BottleneckCSP, [128]], 18 | [-1, 1, Conv, [256, 3, 2]], # 3-P3/8 19 | [-1, 9, BottleneckCSP, [256]], 20 | [-1, 1, Conv, [512, 3, 2]], # 5-P4/16 21 | [-1, 9, BottleneckCSP, [512]], 22 | [-1, 1, Conv, [1024, 3, 2]], # 7-P5/32 23 | [-1, 1, SPP, [1024, [5, 9, 13]]], 24 | [-1, 3, BottleneckCSP, [1024, False]], # 9 25 | ] 26 | 27 | # YOLOv5 head 28 | head: 29 | [[-1, 1, Conv, [512, 1, 1]], 30 | [-1, 1, nn.Upsample, [None, 2, 'nearest']], 31 | [[-1, 6], 1, Concat, [1]], # cat backbone P4 32 | [-1, 3, BottleneckCSP, [512, False]], # 13 33 | 34 | [-1, 1, Conv, [256, 1, 1]], 35 | [-1, 1, nn.Upsample, [None, 2, 'nearest']], 36 | [[-1, 4], 1, Concat, [1]], # cat backbone P3 37 | [-1, 3, BottleneckCSP, [256, False]], # 17 (P3/8-small) 38 | 39 | [-1, 1, Conv, [256, 3, 2]], 40 | [[-1, 14], 1, Concat, [1]], # cat head P4 41 | [-1, 3, BottleneckCSP, [512, False]], # 20 (P4/16-medium) 42 | 43 | [-1, 1, Conv, [512, 3, 2]], 44 | [[-1, 10], 1, Concat, [1]], # cat head P5 45 | [-1, 3, BottleneckCSP, [1024, False]], # 23 (P5/32-large) 46 | 47 | [[17, 20, 23], 1, Detect, [nc, anchors]], # Detect(P3, P4, P5) 48 | ] 49 | -------------------------------------------------------------------------------- /yolo_ros/scripts/utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/yolo_ros/scripts/utils/__init__.py -------------------------------------------------------------------------------- /yolo_ros/scripts/utils/activations.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | import torch.nn.functional as F 4 | 5 | 6 | # Swish https://arxiv.org/pdf/1905.02244.pdf --------------------------------------------------------------------------- 7 | class Swish(nn.Module): # 8 | @staticmethod 9 | def forward(x): 10 | return x * torch.sigmoid(x) 11 | 12 | 13 | class Hardswish(nn.Module): # export-friendly version of nn.Hardswish() 14 | @staticmethod 15 | def forward(x): 16 | # return x * F.hardsigmoid(x) # for torchscript and CoreML 17 | return x * F.hardtanh(x + 3, 0., 6.) / 6. # for torchscript, CoreML and ONNX 18 | 19 | 20 | class MemoryEfficientSwish(nn.Module): 21 | class F(torch.autograd.Function): 22 | @staticmethod 23 | def forward(ctx, x): 24 | ctx.save_for_backward(x) 25 | return x * torch.sigmoid(x) 26 | 27 | @staticmethod 28 | def backward(ctx, grad_output): 29 | x = ctx.saved_tensors[0] 30 | sx = torch.sigmoid(x) 31 | return grad_output * (sx * (1 + x * (1 - sx))) 32 | 33 | def forward(self, x): 34 | return self.F.apply(x) 35 | 36 | 37 | # Mish https://github.com/digantamisra98/Mish -------------------------------------------------------------------------- 38 | class Mish(nn.Module): 39 | @staticmethod 40 | def forward(x): 41 | return x * F.softplus(x).tanh() 42 | 43 | 44 | class MemoryEfficientMish(nn.Module): 45 | class F(torch.autograd.Function): 46 | @staticmethod 47 | def forward(ctx, x): 48 | ctx.save_for_backward(x) 49 | return x.mul(torch.tanh(F.softplus(x))) # x * tanh(ln(1 + exp(x))) 50 | 51 | @staticmethod 52 | def backward(ctx, grad_output): 53 | x = ctx.saved_tensors[0] 54 | sx = torch.sigmoid(x) 55 | fx = F.softplus(x).tanh() 56 | return grad_output * (fx + x * sx * (1 - fx * fx)) 57 | 58 | def forward(self, x): 59 | return self.F.apply(x) 60 | 61 | 62 | # FReLU https://arxiv.org/abs/2007.11824 ------------------------------------------------------------------------------- 63 | class FReLU(nn.Module): 64 | def __init__(self, c1, k=3): # ch_in, kernel 65 | super().__init__() 66 | self.conv = nn.Conv2d(c1, c1, k, 1, 1, groups=c1) 67 | self.bn = nn.BatchNorm2d(c1) 68 | 69 | def forward(self, x): 70 | return torch.max(x, self.bn(self.conv(x))) 71 | -------------------------------------------------------------------------------- /yolo_ros/scripts/utils/evolve.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Hyperparameter evolution commands (avoids CUDA memory leakage issues) 3 | # Replaces train.py python generations 'for' loop with a bash 'for' loop 4 | 5 | # Start on 4-GPU machine 6 | #for i in 0 1 2 3; do 7 | # t=ultralytics/yolov5:evolve && sudo docker pull $t && sudo docker run -d --ipc=host --gpus all -v "$(pwd)"/VOC:/usr/src/VOC $t bash utils/evolve.sh $i 8 | # sleep 60 # avoid simultaneous evolve.txt read/write 9 | #done 10 | 11 | # Hyperparameter evolution commands 12 | while true; do 13 | # python train.py --batch 64 --weights yolov5m.pt --data voc.yaml --img 512 --epochs 50 --evolve --bucket ult/evolve/voc --device $1 14 | python train.py --batch 40 --weights yolov5m.pt --data coco.yaml --img 640 --epochs 30 --evolve --bucket ult/evolve/coco --device $1 15 | done 16 | -------------------------------------------------------------------------------- /yolo_ros/scripts/utils/google_app_engine/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM gcr.io/google-appengine/python 2 | 3 | # Create a virtualenv for dependencies. This isolates these packages from 4 | # system-level packages. 5 | # Use -p python3 or -p python3.7 to select python version. Default is version 2. 6 | RUN virtualenv /env -p python3 7 | 8 | # Setting these environment variables are the same as running 9 | # source /env/bin/activate. 10 | ENV VIRTUAL_ENV /env 11 | ENV PATH /env/bin:$PATH 12 | 13 | RUN apt-get update && apt-get install -y python-opencv 14 | 15 | # Copy the application's requirements.txt and run pip to install all 16 | # dependencies into the virtualenv. 17 | ADD requirements.txt /app/requirements.txt 18 | RUN pip install -r /app/requirements.txt 19 | 20 | # Add the application source code. 21 | ADD . /app 22 | 23 | # Run a WSGI server to serve the application. gunicorn must be declared as 24 | # a dependency in requirements.txt. 25 | CMD gunicorn -b :$PORT main:app 26 | -------------------------------------------------------------------------------- /yolo_ros/scripts/utils/google_app_engine/additional_requirements.txt: -------------------------------------------------------------------------------- 1 | # add these requirements in your app on top of the existing ones 2 | pip==18.1 3 | Flask==1.0.2 4 | gunicorn==19.9.0 5 | -------------------------------------------------------------------------------- /yolo_ros/scripts/utils/google_app_engine/app.yaml: -------------------------------------------------------------------------------- 1 | runtime: custom 2 | env: flex 3 | 4 | service: yolov5app 5 | 6 | liveness_check: 7 | initial_delay_sec: 600 8 | 9 | manual_scaling: 10 | instances: 1 11 | resources: 12 | cpu: 1 13 | memory_gb: 4 14 | disk_size_gb: 20 -------------------------------------------------------------------------------- /yolo_ros/scripts/utils/google_utils.py: -------------------------------------------------------------------------------- 1 | # This file contains google utils: https://cloud.google.com/storage/docs/reference/libraries 2 | # pip install --upgrade google-cloud-storage 3 | # from google.cloud import storage 4 | 5 | import os 6 | import platform 7 | import subprocess 8 | import time 9 | from pathlib import Path 10 | 11 | import torch 12 | 13 | 14 | def gsutil_getsize(url=''): 15 | # gs://bucket/file size https://cloud.google.com/storage/docs/gsutil/commands/du 16 | s = subprocess.check_output('gsutil du %s' % url, shell=True).decode('utf-8') 17 | return eval(s.split(' ')[0]) if len(s) else 0 # bytes 18 | 19 | 20 | def attempt_download(weights): 21 | # Attempt to download pretrained weights if not found locally 22 | weights = weights.strip().replace("'", '') 23 | file = Path(weights).name 24 | 25 | msg = weights + ' missing, try downloading from https://github.com/ultralytics/yolov5/releases/' 26 | models = ['yolov5s.pt', 'yolov5m.pt', 'yolov5l.pt', 'yolov5x.pt'] # available models 27 | 28 | if file in models and not os.path.isfile(weights): 29 | # Google Drive 30 | # d = {'yolov5s.pt': '1R5T6rIyy3lLwgFXNms8whc-387H0tMQO', 31 | # 'yolov5m.pt': '1vobuEExpWQVpXExsJ2w-Mbf3HJjWkQJr', 32 | # 'yolov5l.pt': '1hrlqD1Wdei7UT4OgT785BEk1JwnSvNEV', 33 | # 'yolov5x.pt': '1mM8aZJlWTxOg7BZJvNUMrTnA2AbeCVzS'} 34 | # r = gdrive_download(id=d[file], name=weights) if file in d else 1 35 | # if r == 0 and os.path.exists(weights) and os.path.getsize(weights) > 1E6: # check 36 | # return 37 | 38 | try: # GitHub 39 | url = 'https://github.com/ultralytics/yolov5/releases/download/v3.0/' + file 40 | print('Downloading %s to %s...' % (url, weights)) 41 | torch.hub.download_url_to_file(url, weights) 42 | assert os.path.exists(weights) and os.path.getsize(weights) > 1E6 # check 43 | except Exception as e: # GCP 44 | print('Download error: %s' % e) 45 | url = 'https://storage.googleapis.com/ultralytics/yolov5/ckpt/' + file 46 | print('Downloading %s to %s...' % (url, weights)) 47 | r = os.system('curl -L %s -o %s' % (url, weights)) # torch.hub.download_url_to_file(url, weights) 48 | finally: 49 | if not (os.path.exists(weights) and os.path.getsize(weights) > 1E6): # check 50 | os.remove(weights) if os.path.exists(weights) else None # remove partial downloads 51 | print('ERROR: Download failure: %s' % msg) 52 | print('') 53 | return 54 | 55 | 56 | def gdrive_download(id='1n_oKgR81BJtqk75b00eAjdv03qVCQn2f', name='coco128.zip'): 57 | # Downloads a file from Google Drive. from utils.google_utils import *; gdrive_download() 58 | t = time.time() 59 | 60 | print('Downloading https://drive.google.com/uc?export=download&id=%s as %s... ' % (id, name), end='') 61 | os.remove(name) if os.path.exists(name) else None # remove existing 62 | os.remove('cookie') if os.path.exists('cookie') else None 63 | 64 | # Attempt file download 65 | out = "NUL" if platform.system() == "Windows" else "/dev/null" 66 | os.system('curl -c ./cookie -s -L "drive.google.com/uc?export=download&id=%s" > %s ' % (id, out)) 67 | if os.path.exists('cookie'): # large file 68 | s = 'curl -Lb ./cookie "drive.google.com/uc?export=download&confirm=%s&id=%s" -o %s' % (get_token(), id, name) 69 | else: # small file 70 | s = 'curl -s -L -o %s "drive.google.com/uc?export=download&id=%s"' % (name, id) 71 | r = os.system(s) # execute, capture return 72 | os.remove('cookie') if os.path.exists('cookie') else None 73 | 74 | # Error check 75 | if r != 0: 76 | os.remove(name) if os.path.exists(name) else None # remove partial 77 | print('Download error ') # raise Exception('Download error') 78 | return r 79 | 80 | # Unzip if archive 81 | if name.endswith('.zip'): 82 | print('unzipping... ', end='') 83 | os.system('unzip -q %s' % name) # unzip 84 | os.remove(name) # remove zip to free space 85 | 86 | print('Done (%.1fs)' % (time.time() - t)) 87 | return r 88 | 89 | 90 | def get_token(cookie="./cookie"): 91 | with open(cookie) as f: 92 | for line in f: 93 | if "download" in line: 94 | return line.split()[-1] 95 | return "" 96 | 97 | # def upload_blob(bucket_name, source_file_name, destination_blob_name): 98 | # # Uploads a file to a bucket 99 | # # https://cloud.google.com/storage/docs/uploading-objects#storage-upload-object-python 100 | # 101 | # storage_client = storage.Client() 102 | # bucket = storage_client.get_bucket(bucket_name) 103 | # blob = bucket.blob(destination_blob_name) 104 | # 105 | # blob.upload_from_filename(source_file_name) 106 | # 107 | # print('File {} uploaded to {}.'.format( 108 | # source_file_name, 109 | # destination_blob_name)) 110 | # 111 | # 112 | # def download_blob(bucket_name, source_blob_name, destination_file_name): 113 | # # Uploads a blob from a bucket 114 | # storage_client = storage.Client() 115 | # bucket = storage_client.get_bucket(bucket_name) 116 | # blob = bucket.blob(source_blob_name) 117 | # 118 | # blob.download_to_filename(destination_file_name) 119 | # 120 | # print('Blob {} downloaded to {}.'.format( 121 | # source_blob_name, 122 | # destination_file_name)) 123 | -------------------------------------------------------------------------------- /yolo_ros/scripts/utils/torch_utils.py: -------------------------------------------------------------------------------- 1 | import logging 2 | import os 3 | import time 4 | from copy import deepcopy 5 | 6 | import math 7 | import torch 8 | import torch.backends.cudnn as cudnn 9 | import torch.nn as nn 10 | import torch.nn.functional as F 11 | import torchvision 12 | 13 | logger = logging.getLogger(__name__) 14 | 15 | 16 | def init_torch_seeds(seed=0): 17 | torch.manual_seed(seed) 18 | 19 | # Speed-reproducibility tradeoff https://pytorch.org/docs/stable/notes/randomness.html 20 | if seed == 0: # slower, more reproducible 21 | cudnn.deterministic = True 22 | cudnn.benchmark = False 23 | else: # faster, less reproducible 24 | cudnn.deterministic = False 25 | cudnn.benchmark = True 26 | 27 | 28 | def select_device(device='', batch_size=None): 29 | # device = 'cpu' or '0' or '0,1,2,3' 30 | cpu_request = device.lower() == 'cpu' 31 | if device and not cpu_request: # if device requested other than 'cpu' 32 | os.environ['CUDA_VISIBLE_DEVICES'] = device # set environment variable 33 | assert torch.cuda.is_available(), 'CUDA unavailable, invalid device %s requested' % device # check availablity 34 | 35 | cuda = False if cpu_request else torch.cuda.is_available() 36 | if cuda: 37 | c = 1024 ** 2 # bytes to MB 38 | ng = torch.cuda.device_count() 39 | if ng > 1 and batch_size: # check that batch_size is compatible with device_count 40 | assert batch_size % ng == 0, 'batch-size %g not multiple of GPU count %g' % (batch_size, ng) 41 | x = [torch.cuda.get_device_properties(i) for i in range(ng)] 42 | s = 'Using CUDA ' 43 | for i in range(0, ng): 44 | if i == 1: 45 | s = ' ' * len(s) 46 | logger.info("%sdevice%g _CudaDeviceProperties(name='%s', total_memory=%dMB)" % 47 | (s, i, x[i].name, x[i].total_memory / c)) 48 | else: 49 | logger.info('Using CPU') 50 | 51 | logger.info('') # skip a line 52 | return torch.device('cuda:0' if cuda else 'cpu') 53 | 54 | 55 | def time_synchronized(): 56 | torch.cuda.synchronize() if torch.cuda.is_available() else None 57 | return time.time() 58 | 59 | 60 | def is_parallel(model): 61 | return type(model) in (nn.parallel.DataParallel, nn.parallel.DistributedDataParallel) 62 | 63 | 64 | def intersect_dicts(da, db, exclude=()): 65 | # Dictionary intersection of matching keys and shapes, omitting 'exclude' keys, using da values 66 | return {k: v for k, v in da.items() if k in db and not any(x in k for x in exclude) and v.shape == db[k].shape} 67 | 68 | 69 | def initialize_weights(model): 70 | for m in model.modules(): 71 | t = type(m) 72 | if t is nn.Conv2d: 73 | pass # nn.init.kaiming_normal_(m.weight, mode='fan_out', nonlinearity='relu') 74 | elif t is nn.BatchNorm2d: 75 | m.eps = 1e-3 76 | m.momentum = 0.03 77 | elif t in [nn.Hardswish, nn.LeakyReLU, nn.ReLU, nn.ReLU6]: 78 | m.inplace = True 79 | 80 | 81 | def find_modules(model, mclass=nn.Conv2d): 82 | # Finds layer indices matching module class 'mclass' 83 | return [i for i, m in enumerate(model.module_list) if isinstance(m, mclass)] 84 | 85 | 86 | def sparsity(model): 87 | # Return global model sparsity 88 | a, b = 0., 0. 89 | for p in model.parameters(): 90 | a += p.numel() 91 | b += (p == 0).sum() 92 | return b / a 93 | 94 | 95 | def prune(model, amount=0.3): 96 | # Prune model to requested global sparsity 97 | import torch.nn.utils.prune as prune 98 | print('Pruning model... ', end='') 99 | for name, m in model.named_modules(): 100 | if isinstance(m, nn.Conv2d): 101 | prune.l1_unstructured(m, name='weight', amount=amount) # prune 102 | prune.remove(m, 'weight') # make permanent 103 | print(' %.3g global sparsity' % sparsity(model)) 104 | 105 | 106 | def fuse_conv_and_bn(conv, bn): 107 | # Fuse convolution and batchnorm layers https://tehnokv.com/posts/fusing-batchnorm-and-conv/ 108 | 109 | # init 110 | fusedconv = nn.Conv2d(conv.in_channels, 111 | conv.out_channels, 112 | kernel_size=conv.kernel_size, 113 | stride=conv.stride, 114 | padding=conv.padding, 115 | groups=conv.groups, 116 | bias=True).requires_grad_(False).to(conv.weight.device) 117 | 118 | # prepare filters 119 | w_conv = conv.weight.clone().view(conv.out_channels, -1) 120 | w_bn = torch.diag(bn.weight.div(torch.sqrt(bn.eps + bn.running_var))) 121 | fusedconv.weight.copy_(torch.mm(w_bn, w_conv).view(fusedconv.weight.size())) 122 | 123 | # prepare spatial bias 124 | b_conv = torch.zeros(conv.weight.size(0), device=conv.weight.device) if conv.bias is None else conv.bias 125 | b_bn = bn.bias - bn.weight.mul(bn.running_mean).div(torch.sqrt(bn.running_var + bn.eps)) 126 | fusedconv.bias.copy_(torch.mm(w_bn, b_conv.reshape(-1, 1)).reshape(-1) + b_bn) 127 | 128 | return fusedconv 129 | 130 | 131 | def model_info(model, verbose=False): 132 | # Plots a line-by-line description of a PyTorch model 133 | n_p = sum(x.numel() for x in model.parameters()) # number parameters 134 | n_g = sum(x.numel() for x in model.parameters() if x.requires_grad) # number gradients 135 | if verbose: 136 | print('%5s %40s %9s %12s %20s %10s %10s' % ('layer', 'name', 'gradient', 'parameters', 'shape', 'mu', 'sigma')) 137 | for i, (name, p) in enumerate(model.named_parameters()): 138 | name = name.replace('module_list.', '') 139 | print('%5g %40s %9s %12g %20s %10.3g %10.3g' % 140 | (i, name, p.requires_grad, p.numel(), list(p.shape), p.mean(), p.std())) 141 | 142 | try: # FLOPS 143 | from thop import profile 144 | flops = profile(deepcopy(model), inputs=(torch.zeros(1, 3, 64, 64),), verbose=False)[0] / 1E9 * 2 145 | fs = ', %.1f GFLOPS' % (flops * 100) # 640x640 FLOPS 146 | except: 147 | fs = '' 148 | 149 | logger.info( 150 | 'Model Summary: %g layers, %g parameters, %g gradients%s' % (len(list(model.parameters())), n_p, n_g, fs)) 151 | 152 | 153 | def load_classifier(name='resnet101', n=2): 154 | # Loads a pretrained model reshaped to n-class output 155 | model = torchvision.models.__dict__[name](pretrained=True) 156 | 157 | # ResNet model properties 158 | # input_size = [3, 224, 224] 159 | # input_space = 'RGB' 160 | # input_range = [0, 1] 161 | # mean = [0.485, 0.456, 0.406] 162 | # std = [0.229, 0.224, 0.225] 163 | 164 | # Reshape output to n classes 165 | filters = model.fc.weight.shape[1] 166 | model.fc.bias = nn.Parameter(torch.zeros(n), requires_grad=True) 167 | model.fc.weight = nn.Parameter(torch.zeros(n, filters), requires_grad=True) 168 | model.fc.out_features = n 169 | return model 170 | 171 | 172 | def scale_img(img, ratio=1.0, same_shape=False): # img(16,3,256,416), r=ratio 173 | # scales img(bs,3,y,x) by ratio 174 | if ratio == 1.0: 175 | return img 176 | else: 177 | h, w = img.shape[2:] 178 | s = (int(h * ratio), int(w * ratio)) # new size 179 | img = F.interpolate(img, size=s, mode='bilinear', align_corners=False) # resize 180 | if not same_shape: # pad/crop img 181 | gs = 32 # (pixels) grid size 182 | h, w = [math.ceil(x * ratio / gs) * gs for x in (h, w)] 183 | return F.pad(img, [0, w - s[1], 0, h - s[0]], value=0.447) # value = imagenet mean 184 | 185 | 186 | def copy_attr(a, b, include=(), exclude=()): 187 | # Copy attributes from b to a, options to only include [...] and to exclude [...] 188 | for k, v in b.__dict__.items(): 189 | if (len(include) and k not in include) or k.startswith('_') or k in exclude: 190 | continue 191 | else: 192 | setattr(a, k, v) 193 | 194 | 195 | class ModelEMA: 196 | """ Model Exponential Moving Average from https://github.com/rwightman/pytorch-image-models 197 | Keep a moving average of everything in the model state_dict (parameters and buffers). 198 | This is intended to allow functionality like 199 | https://www.tensorflow.org/api_docs/python/tf/train/ExponentialMovingAverage 200 | A smoothed version of the weights is necessary for some training schemes to perform well. 201 | This class is sensitive where it is initialized in the sequence of model init, 202 | GPU assignment and distributed training wrappers. 203 | """ 204 | 205 | def __init__(self, model, decay=0.9999, updates=0): 206 | # Create EMA 207 | self.ema = deepcopy(model.module if is_parallel(model) else model).eval() # FP32 EMA 208 | # if next(model.parameters()).device.type != 'cpu': 209 | # self.ema.half() # FP16 EMA 210 | self.updates = updates # number of EMA updates 211 | self.decay = lambda x: decay * (1 - math.exp(-x / 2000)) # decay exponential ramp (to help early epochs) 212 | for p in self.ema.parameters(): 213 | p.requires_grad_(False) 214 | 215 | def update(self, model): 216 | # Update EMA parameters 217 | with torch.no_grad(): 218 | self.updates += 1 219 | d = self.decay(self.updates) 220 | 221 | msd = model.module.state_dict() if is_parallel(model) else model.state_dict() # model state_dict 222 | for k, v in self.ema.state_dict().items(): 223 | if v.dtype.is_floating_point: 224 | v *= d 225 | v += (1. - d) * msd[k].detach() 226 | 227 | def update_attr(self, model, include=(), exclude=('process_group', 'reducer')): 228 | # Update EMA attributes 229 | copy_attr(self.ema, model, include, exclude) 230 | -------------------------------------------------------------------------------- /yolo_ros/scripts/yolo_bridge.py: -------------------------------------------------------------------------------- 1 | #!/home/eric/miniconda3/envs/torch/bin/python 2 | from matplotlib import image 3 | from requests import head 4 | import rospy 5 | import torch 6 | import torch.backends.cudnn as cudnn 7 | import numpy as np 8 | import time 9 | import cv2 10 | import message_filters 11 | from models.experimental import attempt_load 12 | from utils.general import ( 13 | check_img_size, non_max_suppression, apply_classifier, scale_coords, 14 | xyxy2xywh, plot_one_box, strip_optimizer, set_logging) 15 | from utils.torch_utils import select_device, load_classifier, time_synchronized 16 | from sensor_msgs.msg import Image 17 | from sensor_msgs.msg import PointCloud2 18 | from std_msgs.msg import Header 19 | from yolo_ros.msg import BoundingBox 20 | from yolo_ros.msg import BoundingBoxes 21 | from yolo_ros.msg import ObjectCount 22 | from copy import deepcopy 23 | 24 | ros_image = 0 25 | header = Header() 26 | 27 | class ROS2YOLO: 28 | def __init__(self, node_name='yolo_detection', name='yolo'): 29 | self.yolo5_path = rospy.get_param('yolov5/path', None) 30 | self.model = rospy.get_param('yolov5/model', 'yolov5s') 31 | self.weight = rospy.get_param('yolov5/weight', None) 32 | self.device = rospy.get_param('yolov5/device', 'gpu') 33 | self.img_size = rospy.get_param('yolov5/img_size', 640) 34 | self.image_topic = rospy.get_param('yolov5/image_topic', None) 35 | self.lidar_topic = rospy.get_param('yolov5/lidar_topic','lidar_points') 36 | self.valid = False if (self.yolo5_path is None) and (self.weight is None) else True 37 | self.stride = 32 38 | self.half = True if self.device == 'gpu' else False 39 | #for ROS 40 | # self.sub = rospy.Subscriber(self.image_topic, Image, self.image_callback, queue_size=10) 41 | self.sub_img = message_filters.Subscriber(self.image_topic, Image) 42 | self.sub_lidar = message_filters.Subscriber(self.lidar_topic, PointCloud2) 43 | self.syn = message_filters.ApproximateTimeSynchronizer([self.sub_img, self.sub_lidar], 10, 3) 44 | self.pub = rospy.Publisher('yolo_result', Image, queue_size=1) 45 | self.box_pub = rospy.Publisher('bounding_boxes',BoundingBoxes, queue_size=10) 46 | self.num_pub = rospy.Publisher('object_num',ObjectCount, queue_size=10) 47 | 48 | self.frame_id = 'camera_link' 49 | self.image_height = 720 50 | self.image_width = 1280 51 | 52 | if not self.load_model(): 53 | print('error occurred while loading !') 54 | self.syn.registerCallback(self.image_callback) 55 | 56 | def load_model(self): 57 | if not self.valid: 58 | return False 59 | self.device = torch.device('cuda:0' if (self.device != 'cpu' and torch.cuda.is_available()) else 'cpu') 60 | if self.yolo5_path is None: 61 | self.model = torch.hub.load('ultralytics/yolov5', 'custom', path_or_model=self.weight) 62 | self.model = self.model.cuda() if self.device != 'cpu' else self.model 63 | else: 64 | print('debug', self.weight, self.device) 65 | self.model = attempt_load(self.weight, self.device) 66 | self.stride = int(self.model.stride.max()) 67 | self.img_size = check_img_size(self.img_size, s=self.stride) 68 | self.half = self.device.type != 'cpu' 69 | if self.half: 70 | self.model.half() 71 | self.names = self.model.module.names if hasattr(self.model, 'module') else self.model.names 72 | print('labels: '), self.print_list_multiline(self.names) 73 | return True 74 | 75 | @staticmethod 76 | def print_list_multiline(names, num_line=5): 77 | length = len(names) 78 | print('[', end="") 79 | for i in range(0, length): 80 | print(str(i) + ':' + '\'', end=''), print(names[i], end=''), print('\'', end='') 81 | if i == length - 1: 82 | print(']') 83 | elif i % num_line == num_line - 1: 84 | print(',\n ', end='') 85 | else: 86 | print(',', end=' ') 87 | 88 | def image_callback(self, image, lidar_points): 89 | global ros_image 90 | global header 91 | header.frame_id = image.header.frame_id 92 | header.stamp = image.header.stamp 93 | self.image_height = image.height 94 | self.image_width = image.width 95 | ros_image = np.frombuffer(image.data, dtype=np.uint8).reshape(image.height, image.width, -1) 96 | 97 | self.detect(ros_image) 98 | 99 | def detect(self, img): 100 | time1 = time.time() 101 | global ros_image 102 | cudnn.benchmark = True 103 | dataset = self.loadimg(img) 104 | conf_thres = 0.3 105 | iou_thres = 0.45 106 | classes = (0,1,2,3,5,7, 67) 107 | agnostic_nms = 'store_true' 108 | img = torch.zeros((1, 3, self.img_size, self.img_size), device=self.device) # init img 109 | path = dataset[0] 110 | img = dataset[1] 111 | im0s = dataset[2] 112 | img = torch.from_numpy(img).to(self.device) 113 | img = img.half() if self.half else img.float() # uint8 to fp16/32 114 | img /= 255.0 # 0 - 255 to 0.0 - 1.0 115 | 116 | time2 = time.time() 117 | if img.ndimension() == 3: 118 | img = img.unsqueeze(0) 119 | 120 | with torch.no_grad(): 121 | # Inference 122 | pred = self.model(img)[0] 123 | # Apply NMS 124 | pred = non_max_suppression(pred, conf_thres, iou_thres, classes=classes, agnostic=agnostic_nms) 125 | 126 | view_img = 1 127 | time3 = time.time() 128 | object_index = 0 129 | b_boxes = BoundingBoxes() 130 | b_boxes.header.frame_id = header.frame_id 131 | b_boxes.header.stamp = header.stamp 132 | for i, det in enumerate(pred): # detections per image 133 | p, s, im0 = path, '', im0s 134 | s += '%gx%g ' % img.shape[2:] # print string 135 | gn = torch.tensor(im0.shape)[[1, 0, 1, 0]] # normalization gain whwh 136 | if det is not None: 137 | #print(det) 138 | # Rescale boxes from img_size to im0 size 139 | det[:, :4] = scale_coords(img.shape[2:], det[:, :4], im0.shape).round() 140 | # Print results 141 | for c in det[:, -1].unique(): 142 | n = (det[:, -1] == c).sum() # detections per class 143 | s += '%g %ss, ' % (n, self.names[int(c)]) # add to string 144 | # Write results 145 | for *xyxy, conf, cls in reversed(det): 146 | if view_img: # Add bbox to image 147 | label = '%s %.2f' % (self.names[int(cls)], conf) 148 | plot_one_box(xyxy, im0, label=label, color=[0,255,0], line_thickness=1) 149 | b_box = BoundingBox() 150 | b_box.Class = self.names[int(cls)] 151 | b_box.id = object_index 152 | b_box.probability = float(conf) 153 | b_box.xmin = int(xyxy[0]) 154 | b_box.ymin = int(xyxy[1]) 155 | b_box.xmax = int(xyxy[2]) 156 | b_box.ymax = int(xyxy[3]) 157 | b_boxes.bounding_boxes.append(b_box) 158 | object_index += 1 159 | self.box_pub.publish(b_boxes) 160 | time4 = time.time() 161 | # print('detect time %.2f'%((time3-time1) * 1000), 'ms') 162 | out_img = im0[:, :, [2, 1, 0]] 163 | ros_image=out_img 164 | # cv2.imshow('YOLOV5', out_img) 165 | # a = cv2.waitKey(1) 166 | #### Create CompressedIamge #### 167 | count_num = ObjectCount() 168 | if(pred[0]==None): 169 | count_num.count = 0 170 | else: 171 | count_num.count = pred[0].size()[0] 172 | count_num.header.stamp = rospy.Time.now() 173 | self.publish_image(im0) 174 | self.num_pub.publish(count_num) 175 | 176 | 177 | 178 | 179 | 180 | def letterbox(self, img, new_shape=(640, 640), color=(114, 114, 114), auto=True, scaleFill=False, scaleup=True): 181 | # Resize image to a 32-pixel-multiple rectangle https://github.com/ultralytics/yolov3/issues/232 182 | shape = img.shape[:2] # current shape [height, width] 183 | if isinstance(new_shape, int): 184 | new_shape = (new_shape, new_shape) 185 | 186 | # Scale ratio (new / old) 187 | r = min(new_shape[0] / shape[0], new_shape[1] / shape[1]) 188 | if not scaleup: # only scale down, do not scale up (for better test mAP) 189 | r = min(r, 1.0) 190 | 191 | # Compute padding 192 | ratio = r, r # width, height ratios 193 | new_unpad = int(round(shape[1] * r)), int(round(shape[0] * r)) 194 | dw, dh = new_shape[1] - new_unpad[0], new_shape[0] - new_unpad[1] # wh padding 195 | if auto: # minimum rectangle 196 | dw, dh = np.mod(dw, 32), np.mod(dh, 32) # wh padding 197 | elif scaleFill: # stretch 198 | dw, dh = 0.0, 0.0 199 | new_unpad = (new_shape[1], new_shape[0]) 200 | ratio = new_shape[1] / shape[1], new_shape[0] / shape[0] # width, height ratios 201 | 202 | dw /= 2 # divide padding into 2 sides 203 | dh /= 2 204 | 205 | if shape[::-1] != new_unpad: # resize 206 | img = cv2.resize(img, new_unpad, interpolation=cv2.INTER_LINEAR) 207 | top, bottom = int(round(dh - 0.1)), int(round(dh + 0.1)) 208 | left, right = int(round(dw - 0.1)), int(round(dw + 0.1)) 209 | img = cv2.copyMakeBorder(img, top, bottom, left, right, cv2.BORDER_CONSTANT, value=color) # add border 210 | return img, ratio, (dw, dh) 211 | 212 | def loadimg(self, img): # 接受opencv图片 213 | cap=None 214 | path=None 215 | img0 = img 216 | img = self.letterbox(img0, new_shape=self.img_size)[0] 217 | img = img[:, :, ::-1].transpose(2, 0, 1) # BGR to RGB, to 3x416x416 218 | img = np.ascontiguousarray(img) 219 | return path, img, img0, cap 220 | def publish_image(self, img): 221 | image_temp = Image() 222 | global header 223 | header.frame_id = self.frame_id 224 | image_temp.height = self.image_height 225 | image_temp.width = self.image_width 226 | image_temp.encoding = 'rgb8' 227 | image_temp.data = np.array(img).tostring() 228 | image_temp.header.stamp = header.stamp 229 | image_temp.header.frame_id = header.frame_id 230 | self.pub.publish(image_temp) 231 | 232 | -------------------------------------------------------------------------------- /yolo_ros/scripts/yolo_bridge_srv.py: -------------------------------------------------------------------------------- 1 | #!/home/eric/miniconda3/envs/torch/bin/python 2 | from ast import Return 3 | from matplotlib import image 4 | from requests import head, request 5 | import rospy 6 | import torch 7 | import torch.backends.cudnn as cudnn 8 | import numpy as np 9 | import time 10 | import cv2 11 | from models.experimental import attempt_load 12 | from utils.general import ( 13 | check_img_size, non_max_suppression, apply_classifier, scale_coords, 14 | xyxy2xywh, plot_one_box, strip_optimizer, set_logging) 15 | from utils.torch_utils import select_device, load_classifier, time_synchronized 16 | from sensor_msgs.msg import Image 17 | from std_msgs.msg import Header 18 | from yolo_ros.msg import BoundingBox 19 | from yolo_ros.msg import BoundingBoxes 20 | from yolo_ros.msg import ObjectCount 21 | from copy import deepcopy 22 | from yolo_ros.srv import yolo_srv, yolo_srvRequest, yolo_srvResponse 23 | 24 | ros_image = 0 25 | header = Header() 26 | 27 | class ROS2YOLO: 28 | def __init__(self, node_name='yolo_detection', name='yolo'): 29 | self.yolo5_path = rospy.get_param('yolov5/path', None) 30 | self.model = rospy.get_param('yolov5/model', 'yolov5s') 31 | self.weight = rospy.get_param('yolov5/weight', None) 32 | self.device = rospy.get_param('yolov5/device', 'gpu') 33 | self.img_size = rospy.get_param('yolov5/img_size', 640) 34 | self.image_topic = rospy.get_param('yolov5/image_topic', None) 35 | self.valid = False if (self.yolo5_path is None) and (self.weight is None) else True 36 | self.stride = 32 37 | self.half = True if self.device == 'gpu' else False 38 | #for ROS 39 | self.sub = rospy.Subscriber(self.image_topic, Image, self.image_callback, queue_size=10) 40 | self.pub = rospy.Publisher('yolo_result', Image, queue_size=1) 41 | self.box_pub = rospy.Publisher('bounding_boxes',BoundingBoxes, queue_size=10) 42 | self.num_pub = rospy.Publisher('object_num',ObjectCount, queue_size=10) 43 | self.yolo_srv = rospy.Service("yolo_server",yolo_srv, self.doReq) 44 | 45 | self.frame_id = 'camera_link' 46 | self.image_height = 480 47 | self.image_width = 640 48 | self.sub_flag = False 49 | self.deal_falg = False 50 | 51 | if not self.load_model(): 52 | print('error occurred while loading !') 53 | 54 | def load_model(self): 55 | if not self.valid: 56 | return False 57 | self.device = torch.device('cuda:0' if (self.device != 'cpu' and torch.cuda.is_available()) else 'cpu') 58 | if self.yolo5_path is None: 59 | self.model = torch.hub.load('ultralytics/yolov5', 'custom', path_or_model=self.weight) 60 | self.model = self.model.cuda() if self.device != 'cpu' else self.model 61 | else: 62 | print('debug', self.weight, self.device) 63 | self.model = attempt_load(self.weight, self.device) 64 | self.stride = int(self.model.stride.max()) 65 | self.img_size = check_img_size(self.img_size, s=self.stride) 66 | self.half = self.device.type != 'cpu' 67 | if self.half: 68 | self.model.half() 69 | self.names = self.model.module.names if hasattr(self.model, 'module') else self.model.names 70 | print('labels: '), self.print_list_multiline(self.names) 71 | return True 72 | 73 | @staticmethod 74 | def print_list_multiline(names, num_line=5): 75 | length = len(names) 76 | print('[', end="") 77 | for i in range(0, length): 78 | print(str(i) + ':' + '\'', end=''), print(names[i], end=''), print('\'', end='') 79 | if i == length - 1: 80 | print(']') 81 | elif i % num_line == num_line - 1: 82 | print(',\n ', end='') 83 | else: 84 | print(',', end=' ') 85 | 86 | def image_callback(self, image): 87 | if(self.sub_flag == False): 88 | return 89 | global ros_image 90 | global header 91 | header.frame_id = image.header.frame_id 92 | header.stamp = image.header.stamp 93 | self.image_height = image.height 94 | self.image_width = image.width 95 | ros_image = np.frombuffer(image.data, dtype=np.uint8).reshape(image.height, image.width, -1) 96 | 97 | self.detect(ros_image) 98 | self.deal_falg = True 99 | 100 | def detect(self, img): 101 | time1 = time.time() 102 | global ros_image 103 | cudnn.benchmark = True 104 | dataset = self.loadimg(img) 105 | conf_thres = 0.3 106 | iou_thres = 0.45 107 | classes = (0,1,2,3,5,7, 67) 108 | agnostic_nms = 'store_true' 109 | img = torch.zeros((1, 3, self.img_size, self.img_size), device=self.device) # init img 110 | path = dataset[0] 111 | img = dataset[1] 112 | im0s = dataset[2] 113 | img = torch.from_numpy(img).to(self.device) 114 | img = img.half() if self.half else img.float() # uint8 to fp16/32 115 | img /= 255.0 # 0 - 255 to 0.0 - 1.0 116 | 117 | time2 = time.time() 118 | if img.ndimension() == 3: 119 | img = img.unsqueeze(0) 120 | 121 | with torch.no_grad(): 122 | # Inference 123 | pred = self.model(img)[0] 124 | # Apply NMS 125 | pred = non_max_suppression(pred, conf_thres, iou_thres, classes=classes, agnostic=agnostic_nms) 126 | 127 | view_img = 1 128 | time3 = time.time() 129 | object_index = 0 130 | b_boxes = BoundingBoxes() 131 | b_boxes.header.frame_id = header.frame_id 132 | b_boxes.header.stamp = header.stamp 133 | for i, det in enumerate(pred): # detections per image 134 | p, s, im0 = path, '', im0s 135 | s += '%gx%g ' % img.shape[2:] # print string 136 | gn = torch.tensor(im0.shape)[[1, 0, 1, 0]] # normalization gain whwh 137 | if det is not None: 138 | #print(det) 139 | # Rescale boxes from img_size to im0 size 140 | det[:, :4] = scale_coords(img.shape[2:], det[:, :4], im0.shape).round() 141 | # Print results 142 | for c in det[:, -1].unique(): 143 | n = (det[:, -1] == c).sum() # detections per class 144 | s += '%g %ss, ' % (n, self.names[int(c)]) # add to string 145 | # Write results 146 | for *xyxy, conf, cls in reversed(det): 147 | if view_img: # Add bbox to image 148 | label = '%s %.2f' % (self.names[int(cls)], conf) 149 | plot_one_box(xyxy, im0, label=label, color=[0,255,0], line_thickness=1) 150 | b_box = BoundingBox() 151 | b_box.Class = self.names[int(cls)] 152 | b_box.id = object_index 153 | b_box.probability = float(conf) 154 | b_box.xmin = int(xyxy[0]) 155 | b_box.ymin = int(xyxy[1]) 156 | b_box.xmax = int(xyxy[2]) 157 | b_box.ymax = int(xyxy[3]) 158 | b_boxes.bounding_boxes.append(b_box) 159 | object_index += 1 160 | self.box_pub.publish(b_boxes) 161 | time4 = time.time() 162 | # print('detect time %.2f'%((time3-time1) * 1000), 'ms') 163 | out_img = im0[:, :, [2, 1, 0]] 164 | ros_image=out_img 165 | # cv2.imshow('YOLOV5', out_img) 166 | # a = cv2.waitKey(10) 167 | #### Create CompressedIamge #### 168 | count_num = ObjectCount() 169 | if(pred[0]==None): 170 | count_num.count = 0 171 | else: 172 | count_num.count = pred[0].size()[0] 173 | count_num.header.stamp = rospy.Time.now() 174 | self.publish_image(im0) 175 | self.num_pub.publish(count_num) 176 | 177 | 178 | def letterbox(self, img, new_shape=(640, 640), color=(114, 114, 114), auto=True, scaleFill=False, scaleup=True): 179 | # Resize image to a 32-pixel-multiple rectangle https://github.com/ultralytics/yolov3/issues/232 180 | shape = img.shape[:2] # current shape [height, width] 181 | if isinstance(new_shape, int): 182 | new_shape = (new_shape, new_shape) 183 | 184 | # Scale ratio (new / old) 185 | r = min(new_shape[0] / shape[0], new_shape[1] / shape[1]) 186 | if not scaleup: # only scale down, do not scale up (for better test mAP) 187 | r = min(r, 1.0) 188 | 189 | # Compute padding 190 | ratio = r, r # width, height ratios 191 | new_unpad = int(round(shape[1] * r)), int(round(shape[0] * r)) 192 | dw, dh = new_shape[1] - new_unpad[0], new_shape[0] - new_unpad[1] # wh padding 193 | if auto: # minimum rectangle 194 | dw, dh = np.mod(dw, 32), np.mod(dh, 32) # wh padding 195 | elif scaleFill: # stretch 196 | dw, dh = 0.0, 0.0 197 | new_unpad = (new_shape[1], new_shape[0]) 198 | ratio = new_shape[1] / shape[1], new_shape[0] / shape[0] # width, height ratios 199 | 200 | dw /= 2 # divide padding into 2 sides 201 | dh /= 2 202 | 203 | if shape[::-1] != new_unpad: # resize 204 | img = cv2.resize(img, new_unpad, interpolation=cv2.INTER_LINEAR) 205 | top, bottom = int(round(dh - 0.1)), int(round(dh + 0.1)) 206 | left, right = int(round(dw - 0.1)), int(round(dw + 0.1)) 207 | img = cv2.copyMakeBorder(img, top, bottom, left, right, cv2.BORDER_CONSTANT, value=color) # add border 208 | return img, ratio, (dw, dh) 209 | 210 | def loadimg(self, img): # 接受opencv图片 211 | cap=None 212 | path=None 213 | img0 = img 214 | img = self.letterbox(img0, new_shape=self.img_size)[0] 215 | img = img[:, :, ::-1].transpose(2, 0, 1) # BGR to RGB, to 3x416x416 216 | img = np.ascontiguousarray(img) 217 | return path, img, img0, cap 218 | def publish_image(self, img): 219 | image_temp = Image() 220 | global header 221 | header.frame_id = self.frame_id 222 | image_temp.height = self.image_height 223 | image_temp.width = self.image_width 224 | image_temp.encoding = 'rgb8' 225 | image_temp.data = np.array(img).tostring() 226 | image_temp.header.stamp = header.stamp 227 | image_temp.header.frame_id = header.frame_id 228 | self.pub.publish(image_temp) 229 | def doReq(self, req): 230 | resp = yolo_srvResponse() 231 | if(req.yolo_msg != "yolo_detect"): 232 | return resp 233 | self.sub_flag = True 234 | self.deal_falg = False 235 | while(self.deal_falg == False): 236 | pass 237 | self.sub_flag = False 238 | return resp 239 | 240 | -------------------------------------------------------------------------------- /yolo_ros/scripts/yolo_node.py: -------------------------------------------------------------------------------- 1 | #!/home/eric/miniconda3/envs/torch/bin/python 2 | import rospy 3 | from yolo_bridge import ROS2YOLO 4 | 5 | if __name__ == "__main__": 6 | rospy.init_node('ros_yolo') 7 | yoloBridge = ROS2YOLO() 8 | rospy.spin() -------------------------------------------------------------------------------- /yolo_ros/scripts/yolo_node_srv.py: -------------------------------------------------------------------------------- 1 | #!/home/eric/miniconda3/envs/torch/bin/python 2 | import rospy 3 | from yolo_bridge_srv import ROS2YOLO 4 | 5 | if __name__ == "__main__": 6 | rospy.init_node('ros_yolo') 7 | yoloBridge = ROS2YOLO() 8 | rospy.spin() -------------------------------------------------------------------------------- /yolo_ros/scripts/yolov5s.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/lvruichen/3d-object-detection/36d765d9e1a690b88804f15f78b998ab86d146fa/yolo_ros/scripts/yolov5s.pt -------------------------------------------------------------------------------- /yolo_ros/srv/yolo_srv.srv: -------------------------------------------------------------------------------- 1 | string yolo_msg 2 | --- 3 | --------------------------------------------------------------------------------