diff --git a/ros/src/super_fast_object_detection/README.md b/ros/src/super_fast_object_detection/README.md index 98b8190..7967551 100755 --- a/ros/src/super_fast_object_detection/README.md +++ b/ros/src/super_fast_object_detection/README.md @@ -1 +1,35 @@ -# ROS Node for Super Fast Accurate 3D object detection +# ROS Package for Super Fast Accurate 3D object detection + + +[![Build Status](https://travis-ci.org/joemccann/dillinger.svg?branch=master)](https://travis-ci.org/joemccann/dillinger) +Ubuntu 18.04 & ROS Melodic + +## Installation + +Clone and setup the main python package +``` +git clone https://github.com/maudzung/Super-Fast-Accurate-3D-Object-Detection +cd Super-Fast-Accurate-3D-Object-Detection/ +pip install . +``` +Install dependancies for ROS packages: +``` +sudo apt install ros-melodic-autoware-msgs +``` +## Building Workspace +``` +cd Super-Fast-Accurate-3D-Object-Detection/ros/ +catkin_make +``` + +## Running the node +Run the node by simply after you build the workspace +``` +source devel/setup.bash +rosrun super_fast_object_detection rosInference.py +``` + +### Subscriber +Topic Name: ```points_raw```, Message Type: ```sensor_msgs/PointCloud2``` +### Publisher +Topic Name: ```detected_objects```, Message Type: ```autoware_msgs/DetectedObjectArray``` diff --git a/ros/src/super_fast_object_detection/src/data_process/demo_dataset.py b/ros/src/super_fast_object_detection/src/data_process/demo_dataset.py deleted file mode 100755 index fd98d7e..0000000 --- a/ros/src/super_fast_object_detection/src/data_process/demo_dataset.py +++ /dev/null @@ -1,99 +0,0 @@ -""" -# -*- coding: utf-8 -*- ------------------------------------------------------------------------------------ -# Author: Nguyen Mau Dung -# DoC: 2020.08.17 -# email: nguyenmaudung93.kstn@gmail.com ------------------------------------------------------------------------------------ -# Description: This script for the KITTI dataset -""" - -import sys -import os -from builtins import int -from glob import glob - -import numpy as np -from torch.utils.data import Dataset -import cv2 -import torch - -sys.path.append('../') - -from data_process.kitti_data_utils import get_filtered_lidar -from data_process.kitti_bev_utils import makeBEVMap -import config.kitti_config as cnf - - -class Demo_KittiDataset(Dataset): - def __init__(self, configs): - self.dataset_dir = os.path.join(configs.dataset_dir, configs.foldername, configs.foldername[:10], - configs.foldername) - self.input_size = configs.input_size - self.hm_size = configs.hm_size - - self.num_classes = configs.num_classes - self.max_objects = configs.max_objects - - self.image_dir = os.path.join(self.dataset_dir, "image_02", "data") - self.lidar_dir = os.path.join(self.dataset_dir, "velodyne_points", "data") - self.label_dir = os.path.join(self.dataset_dir, "label_2", "data") - self.sample_id_list = sorted(glob(os.path.join(self.lidar_dir, '*.bin'))) - self.sample_id_list = [float(os.path.basename(fn)[:-4]) for fn in self.sample_id_list] - self.num_samples = len(self.sample_id_list) - - def __len__(self): - return len(self.sample_id_list) - - def __getitem__(self, index): - pass - - def load_bevmap_front(self, index): - """Load only image for the testing phase""" - sample_id = int(self.sample_id_list[index]) - img_path, img_rgb = self.get_image(sample_id) - lidarData = self.get_lidar(sample_id) - print(lidarData.shape) - front_lidar = get_filtered_lidar(lidarData, cnf.boundary) - print(type(front_lidar[0][0])) - - front_bevmap = makeBEVMap(front_lidar, cnf.boundary) - print(front_bevmap.shape) - front_bevmap = torch.from_numpy(front_bevmap) - - metadatas = { - 'img_path': img_path, - } - - return metadatas, front_bevmap, img_rgb - - def load_bevmap_front_vs_back(self, index): - """Load only image for the testing phase""" - sample_id = int(self.sample_id_list[index]) - img_path, img_rgb = self.get_image(sample_id) - lidarData = self.get_lidar(sample_id) - - front_lidar = get_filtered_lidar(lidarData, cnf.boundary) - front_bevmap = makeBEVMap(front_lidar, cnf.boundary) - front_bevmap = torch.from_numpy(front_bevmap) - - back_lidar = get_filtered_lidar(lidarData, cnf.boundary_back) - back_bevmap = makeBEVMap(back_lidar, cnf.boundary_back) - back_bevmap = torch.from_numpy(back_bevmap) - - metadatas = { - 'img_path': img_path, - } - - return metadatas, front_bevmap, back_bevmap, img_rgb - - def get_image(self, idx): - img_path = os.path.join(self.image_dir, '{:010d}.png'.format(idx)) - img = cv2.cvtColor(cv2.imread(img_path), cv2.COLOR_BGR2RGB) - - return img_path, img - - def get_lidar(self, idx): - lidar_file = os.path.join(self.lidar_dir, '{:010d}.bin'.format(idx)) - # assert os.path.isfile(lidar_file) - return np.fromfile(lidar_file, dtype=np.float32).reshape(-1, 4) diff --git a/ros/src/super_fast_object_detection/src/data_process/kitti_bev_utils.py b/ros/src/super_fast_object_detection/src/data_process/kitti_bev_utils.py deleted file mode 100755 index 494cbff..0000000 --- a/ros/src/super_fast_object_detection/src/data_process/kitti_bev_utils.py +++ /dev/null @@ -1,88 +0,0 @@ -""" -# -*- coding: utf-8 -*- ------------------------------------------------------------------------------------ -""" - -import math -import sys - -import cv2 -import numpy as np - -sys.path.append('../') - -import config.kitti_config as cnf - - -def makeBEVMap(PointCloud_, boundary): - Height = cnf.BEV_HEIGHT + 1 - Width = cnf.BEV_WIDTH + 1 - - # Discretize Feature Map - PointCloud = np.copy(PointCloud_) - PointCloud[:, 0] = np.int_(np.floor(PointCloud[:, 0] / cnf.DISCRETIZATION)) - PointCloud[:, 1] = np.int_(np.floor(PointCloud[:, 1] / cnf.DISCRETIZATION) + Width / 2) - - # sort-3times - indices = np.lexsort((-PointCloud[:, 2], PointCloud[:, 1], PointCloud[:, 0])) - PointCloud = PointCloud[indices] - - # Height Map - heightMap = np.zeros((Height, Width)) - - _, indices = np.unique(PointCloud[:, 0:2], axis=0, return_index=True) - PointCloud_frac = PointCloud[indices] - # some important problem is image coordinate is (y,x), not (x,y) - max_height = float(np.abs(boundary['maxZ'] - boundary['minZ'])) - heightMap[np.int_(PointCloud_frac[:, 0]), np.int_(PointCloud_frac[:, 1])] = PointCloud_frac[:, 2] / max_height - - # Intensity Map & DensityMap - intensityMap = np.zeros((Height, Width)) - densityMap = np.zeros((Height, Width)) - - _, indices, counts = np.unique(PointCloud[:, 0:2], axis=0, return_index=True, return_counts=True) - PointCloud_top = PointCloud[indices] - - normalizedCounts = np.minimum(1.0, np.log(counts + 1) / np.log(64)) - - intensityMap[np.int_(PointCloud_top[:, 0]), np.int_(PointCloud_top[:, 1])] = PointCloud_top[:, 3] - densityMap[np.int_(PointCloud_top[:, 0]), np.int_(PointCloud_top[:, 1])] = normalizedCounts - - RGB_Map = np.zeros((3, Height - 1, Width - 1)) - RGB_Map[2, :, :] = densityMap[:cnf.BEV_HEIGHT, :cnf.BEV_WIDTH] # r_map - RGB_Map[1, :, :] = heightMap[:cnf.BEV_HEIGHT, :cnf.BEV_WIDTH] # g_map - RGB_Map[0, :, :] = intensityMap[:cnf.BEV_HEIGHT, :cnf.BEV_WIDTH] # b_map - - return RGB_Map - - -# bev image coordinates format -def get_corners(x, y, w, l, yaw): - bev_corners = np.zeros((4, 2), dtype=np.float32) - cos_yaw = np.cos(yaw) - sin_yaw = np.sin(yaw) - # front left - bev_corners[0, 0] = x - w / 2 * cos_yaw - l / 2 * sin_yaw - bev_corners[0, 1] = y - w / 2 * sin_yaw + l / 2 * cos_yaw - - # rear left - bev_corners[1, 0] = x - w / 2 * cos_yaw + l / 2 * sin_yaw - bev_corners[1, 1] = y - w / 2 * sin_yaw - l / 2 * cos_yaw - - # rear right - bev_corners[2, 0] = x + w / 2 * cos_yaw + l / 2 * sin_yaw - bev_corners[2, 1] = y + w / 2 * sin_yaw - l / 2 * cos_yaw - - # front right - bev_corners[3, 0] = x + w / 2 * cos_yaw - l / 2 * sin_yaw - bev_corners[3, 1] = y + w / 2 * sin_yaw + l / 2 * cos_yaw - - return bev_corners - - -def drawRotatedBox(img, x, y, w, l, yaw, color): - bev_corners = get_corners(x, y, w, l, yaw) - corners_int = bev_corners.reshape(-1, 1, 2).astype(int) - cv2.polylines(img, [corners_int], True, color, 2) - corners_int = bev_corners.reshape(-1, 2) - cv2.line(img, (corners_int[0, 0], corners_int[0, 1]), (corners_int[3, 0], corners_int[3, 1]), (255, 255, 0), 2) diff --git a/ros/src/super_fast_object_detection/src/data_process/kitti_data_utils.py b/ros/src/super_fast_object_detection/src/data_process/kitti_data_utils.py deleted file mode 100755 index 8c3b8f9..0000000 --- a/ros/src/super_fast_object_detection/src/data_process/kitti_data_utils.py +++ /dev/null @@ -1,319 +0,0 @@ -""" -# -*- coding: utf-8 -*- ------------------------------------------------------------------------------------ -# Author: Nguyen Mau Dung -# DoC: 2020.08.17 -# email: nguyenmaudung93.kstn@gmail.com ------------------------------------------------------------------------------------ -# Description: The utils of the kitti dataset -""" - -from __future__ import print_function -import sys - -import numpy as np -import cv2 - -sys.path.append('../') - -import config.kitti_config as cnf - - -class Object3d(object): - ''' 3d object label ''' - - def __init__(self, label_file_line): - data = label_file_line.split(' ') - data[1:] = [float(x) for x in data[1:]] - # extract label, truncation, occlusion - self.type = data[0] # 'Car', 'Pedestrian', ... - self.cls_id = self.cls_type_to_id(self.type) - self.truncation = data[1] # truncated pixel ratio [0..1] - self.occlusion = int(data[2]) # 0=visible, 1=partly occluded, 2=fully occluded, 3=unknown - self.alpha = data[3] # object observation angle [-pi..pi] - - # extract 2d bounding box in 0-based coordinates - self.xmin = data[4] # left - self.ymin = data[5] # top - self.xmax = data[6] # right - self.ymax = data[7] # bottom - self.box2d = np.array([self.xmin, self.ymin, self.xmax, self.ymax]) - - # extract 3d bounding box information - self.h = data[8] # box height - self.w = data[9] # box width - self.l = data[10] # box length (in meters) - self.t = (data[11], data[12], data[13]) # location (x,y,z) in camera coord. - self.dis_to_cam = np.linalg.norm(self.t) - self.ry = data[14] # yaw angle (around Y-axis in camera coordinates) [-pi..pi] - self.score = data[15] if data.__len__() == 16 else -1.0 - self.level_str = None - self.level = self.get_obj_level() - - def cls_type_to_id(self, cls_type): - if cls_type not in cnf.CLASS_NAME_TO_ID.keys(): - return -1 - - return cnf.CLASS_NAME_TO_ID[cls_type] - - def get_obj_level(self): - height = float(self.box2d[3]) - float(self.box2d[1]) + 1 - - if height >= 40 and self.truncation <= 0.15 and self.occlusion <= 0: - self.level_str = 'Easy' - return 1 # Easy - elif height >= 25 and self.truncation <= 0.3 and self.occlusion <= 1: - self.level_str = 'Moderate' - return 2 # Moderate - elif height >= 25 and self.truncation <= 0.5 and self.occlusion <= 2: - self.level_str = 'Hard' - return 3 # Hard - else: - self.level_str = 'UnKnown' - return 4 - - def print_object(self): - print('Type, truncation, occlusion, alpha: %s, %d, %d, %f' % \ - (self.type, self.truncation, self.occlusion, self.alpha)) - print('2d bbox (x0,y0,x1,y1): %f, %f, %f, %f' % \ - (self.xmin, self.ymin, self.xmax, self.ymax)) - print('3d bbox h,w,l: %f, %f, %f' % \ - (self.h, self.w, self.l)) - print('3d bbox location, ry: (%f, %f, %f), %f' % \ - (self.t[0], self.t[1], self.t[2], self.ry)) - - def to_kitti_format(self): - kitti_str = '%s %.2f %d %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f' \ - % (self.type, self.truncation, int(self.occlusion), self.alpha, self.box2d[0], self.box2d[1], - self.box2d[2], self.box2d[3], self.h, self.w, self.l, self.t[0], self.t[1], self.t[2], - self.ry, self.score) - return kitti_str - - -def read_label(label_filename): - lines = [line.rstrip() for line in open(label_filename)] - objects = [Object3d(line) for line in lines] - return objects - - -class Calibration(object): - ''' Calibration matrices and utils - 3d XYZ in