From 08effd1d6fd885695234540dbbd3548ffd17b0be Mon Sep 17 00:00:00 2001 From: "shengjian.chen" Date: Tue, 13 Oct 2020 00:02:26 -0700 Subject: [PATCH 01/13] add detector --- detectron2_detector/config/detectron2.yaml | 9 + .../detectron2_detector/__init__.py | 0 .../detectron2_detector/detectron2_node.py | 182 ++++++++++++++++++ .../detectron2_detector/utils.py | 39 ++++ .../launch/detectron.launch.py | 21 ++ detectron2_detector/package.xml | 22 +++ .../resource/detectron2_detector | 0 detectron2_detector/setup.cfg | 4 + detectron2_detector/setup.py | 29 +++ detectron2_detector/test/test_copyright.py | 23 +++ detectron2_detector/test/test_flake8.py | 25 +++ detectron2_detector/test/test_pep257.py | 23 +++ 12 files changed, 377 insertions(+) create mode 100644 detectron2_detector/config/detectron2.yaml create mode 100644 detectron2_detector/detectron2_detector/__init__.py create mode 100644 detectron2_detector/detectron2_detector/detectron2_node.py create mode 100644 detectron2_detector/detectron2_detector/utils.py create mode 100644 detectron2_detector/launch/detectron.launch.py create mode 100644 detectron2_detector/package.xml create mode 100644 detectron2_detector/resource/detectron2_detector create mode 100644 detectron2_detector/setup.cfg create mode 100644 detectron2_detector/setup.py create mode 100644 detectron2_detector/test/test_copyright.py create mode 100644 detectron2_detector/test/test_flake8.py create mode 100644 detectron2_detector/test/test_pep257.py diff --git a/detectron2_detector/config/detectron2.yaml b/detectron2_detector/config/detectron2.yaml new file mode 100644 index 0000000..0f43668 --- /dev/null +++ b/detectron2_detector/config/detectron2.yaml @@ -0,0 +1,9 @@ +detectron2_node: + ros__parameters: + detectron_config_file: "COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml" + detectron_score_thresh: 0.8 + pointcloud2_topic: "/realsense/camera/pointcloud" + pc_downsample_factor: 16 + min_mask: 20 # minimum mask to be considered as an obstacle candidate + categories: [0] # please check out COCO dataset category_id list if you want to config this; if you want to track all, leave it empty + nms_filter: 0.3 # 3D non-max suppression threshold, [0, 1] \ No newline at end of file diff --git a/detectron2_detector/detectron2_detector/__init__.py b/detectron2_detector/detectron2_detector/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/detectron2_detector/detectron2_detector/detectron2_node.py b/detectron2_detector/detectron2_detector/detectron2_node.py new file mode 100644 index 0000000..2efcf9f --- /dev/null +++ b/detectron2_detector/detectron2_detector/detectron2_node.py @@ -0,0 +1,182 @@ +import detectron2 +from detectron2.utils.logger import setup_logger +setup_logger() + +# import from common libraries +import numpy as np +import os, json, cv2, random + +# import some common detectron2 utilities +from detectron2 import model_zoo +from detectron2.engine import DefaultPredictor +from detectron2.config import get_cfg +from detectron2.utils.visualizer import Visualizer +from detectron2.data import MetadataCatalog, DatasetCatalog + +import rclpy +from rclpy.node import Node + +from sensor_msgs.msg import Image, PointCloud2 +from nav2_dynamic_msgs.msg import Obstacle, ObstacleArray +from geometry_msgs.msg import Pose, Point +from detectron2_detector.utils import NMS_3D + +class Detectron2Detector(Node): + '''use Detectron2 to detect object masks from 2D image and estimate 3D position with Pointcloud2 data + ''' + def __init__(self): + super().__init__('detectron_node') + self.declare_parameters( + namespace='', + parameters=[ + ('global_frame', "camera_link"), + ('detectron_config_file', "COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml"), + ('detectron_score_thresh', 0.8), + ('pointcloud2_topic', "/realsense/camera/pointcloud"), + ('pc_downsample_factor', 16), + ('min_mask', 20), + ('categories', [0]), + ('z_filter', [-2., 2.]), + ('nms_filter', 0.3) + ]) + self.global_frame = self.get_parameter("global_frame")._value + self.pc_downsample_factor = int(self.get_parameter("pc_downsample_factor")._value) + self.min_mask = self.get_parameter("min_mask")._value + self.categories = self.get_parameter("categories")._value + self.z_filter = self.get_parameter("z_filter")._value + self.nms_filter = self.get_parameter("nms_filter")._value + + # setup detectron model + self.cfg = get_cfg() + config_file = self.get_parameter("detectron_config_file")._value + self.cfg.merge_from_file(model_zoo.get_config_file(config_file)) + self.cfg.MODEL.ROI_HEADS.SCORE_THRESH_TEST = self.get_parameter("detectron_score_thresh")._value + self.cfg.MODEL.WEIGHTS = model_zoo.get_checkpoint_url(config_file) + self.predictor = DefaultPredictor(self.cfg) + + # subscribe to sensor + self.subscription = self.create_subscription( + PointCloud2, + self.get_parameter("pointcloud2_topic")._value, + self.callback, + 1) + + # setup publisher + self.detect_obj_pub = self.create_publisher(ObstacleArray, 'detection', 2) + self.detect_img_pub = self.create_publisher(Image, 'image', 2) + + self.count = -1 + + def outlier_filter(self, x, z, idx): + '''simple outlier filter, assume Gaussian distribution and drop points with low probability (too far away from center)''' + x_mean = np.mean(x) + x_var = np.var(x) + z_mean = np.mean(z) + z_var = np.var(z) + # probability under Gaussian distribution + gaussian_kernel = np.exp(-0.5 * (np.power(x-x_mean, 2) / x_var + np.power(z-z_mean, 2) / z_var)) / (2 * np.pi * np.sqrt(x_var * z_var)) + return idx[gaussian_kernel > 0.5] + + def callback(self, msg): + # check if there is subscirbers + if self.detect_obj_pub.get_subscription_count() == 0 and self.detect_img_pub.get_subscription_count() == 0: + return + + # extract data from msg + height = msg.height + width = msg.width + points = np.array(msg.data, dtype = 'uint8') + + # decode rgb image + rgb_offset = msg.fields[3].offset + point_step = msg.point_step + r = points[rgb_offset::point_step] + g = points[(rgb_offset+1)::point_step] + b = points[(rgb_offset+2)::point_step] + img = np.concatenate([r[:, None], g[:, None], b[:, None]], axis = -1) + img = img.reshape((height, width, 3)) + + # decode point cloud data + if msg.fields[0].datatype < 3: + byte = 1 + elif msg.fields[0].datatype < 5: + byte = 2 + elif msg.fields[0].datatype < 8: + byte = 4 + else: + byte = 8 + points = points.view(' threshold: + flag = False + break + if flag: + bboxes.append(bbox) + return bboxes \ No newline at end of file diff --git a/detectron2_detector/launch/detectron.launch.py b/detectron2_detector/launch/detectron.launch.py new file mode 100644 index 0000000..faed0d0 --- /dev/null +++ b/detectron2_detector/launch/detectron.launch.py @@ -0,0 +1,21 @@ +import os +import launch +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('detectron2_detector'), + 'config', + 'detectron2.yaml' + ) + + detectron_node = Node( + package = 'detectron2_detector', + name = 'detectron2_node', + executable = 'detectron2_node', + parameters = [config] + ) + + return launch.LaunchDescription([detectron_node]) \ No newline at end of file diff --git a/detectron2_detector/package.xml b/detectron2_detector/package.xml new file mode 100644 index 0000000..b0c494a --- /dev/null +++ b/detectron2_detector/package.xml @@ -0,0 +1,22 @@ + + + + detectron2_detector + 0.0.0 + This detector uses detectron2 to get object mask and then uses pointcloud2 data to estimate 3D position. + Steven Macenski + Shengjian Chen + Apache-2.0 + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + launch_ros + detectron2 + + + ament_python + + diff --git a/detectron2_detector/resource/detectron2_detector b/detectron2_detector/resource/detectron2_detector new file mode 100644 index 0000000..e69de29 diff --git a/detectron2_detector/setup.cfg b/detectron2_detector/setup.cfg new file mode 100644 index 0000000..ae88a03 --- /dev/null +++ b/detectron2_detector/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/detectron2_detector +[install] +install-scripts=$base/lib/detectron2_detector diff --git a/detectron2_detector/setup.py b/detectron2_detector/setup.py new file mode 100644 index 0000000..2f14564 --- /dev/null +++ b/detectron2_detector/setup.py @@ -0,0 +1,29 @@ +from setuptools import setup +import os, glob + +package_name = 'detectron2_detector' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/config', glob.glob('config/*.yaml')), + ('share/' + package_name + '/launch', glob.glob('launch/*.launch.py')) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='tony', + maintainer_email='csj15thu@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'detectron2_node = detectron2_detector.detectron2_node:main' + ], + }, +) diff --git a/detectron2_detector/test/test_copyright.py b/detectron2_detector/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/detectron2_detector/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/detectron2_detector/test/test_flake8.py b/detectron2_detector/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/detectron2_detector/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/detectron2_detector/test/test_pep257.py b/detectron2_detector/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/detectron2_detector/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From 1a4349ba7e72cb0bf3aa972f19e7769891e78a96 Mon Sep 17 00:00:00 2001 From: "shengjian.chen" Date: Tue, 13 Oct 2020 00:04:09 -0700 Subject: [PATCH 02/13] update gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index db60fde..4d1490a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ *~ *.pyc \#* +__pycache__/ From 6acfa79026c8346f4382e3fbf7012d07da084d47 Mon Sep 17 00:00:00 2001 From: "shengjian.chen" Date: Tue, 13 Oct 2020 00:07:04 -0700 Subject: [PATCH 03/13] small changes --- detectron2_detector/config/detectron2.yaml | 3 ++- .../detectron2_detector/detectron2_node.py | 2 +- detectron2_detector/detectron2_detector/utils.py | 2 +- detectron2_detector/package.xml | 2 +- detectron2_detector/setup.py | 10 +++++----- 5 files changed, 10 insertions(+), 9 deletions(-) diff --git a/detectron2_detector/config/detectron2.yaml b/detectron2_detector/config/detectron2.yaml index 0f43668..86ee856 100644 --- a/detectron2_detector/config/detectron2.yaml +++ b/detectron2_detector/config/detectron2.yaml @@ -6,4 +6,5 @@ detectron2_node: pc_downsample_factor: 16 min_mask: 20 # minimum mask to be considered as an obstacle candidate categories: [0] # please check out COCO dataset category_id list if you want to config this; if you want to track all, leave it empty - nms_filter: 0.3 # 3D non-max suppression threshold, [0, 1] \ No newline at end of file + nms_filter: 0.3 # 3D non-max suppression threshold, [0, 1] + \ No newline at end of file diff --git a/detectron2_detector/detectron2_detector/detectron2_node.py b/detectron2_detector/detectron2_detector/detectron2_node.py index 2efcf9f..527a6ea 100644 --- a/detectron2_detector/detectron2_detector/detectron2_node.py +++ b/detectron2_detector/detectron2_detector/detectron2_node.py @@ -179,4 +179,4 @@ def main(): rclpy.shutdown() if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/detectron2_detector/detectron2_detector/utils.py b/detectron2_detector/detectron2_detector/utils.py index c090a44..e52a922 100644 --- a/detectron2_detector/detectron2_detector/utils.py +++ b/detectron2_detector/detectron2_detector/utils.py @@ -36,4 +36,4 @@ def NMS_3D(obstacles, threshold): break if flag: bboxes.append(bbox) - return bboxes \ No newline at end of file + return bboxes diff --git a/detectron2_detector/package.xml b/detectron2_detector/package.xml index b0c494a..af0d7a7 100644 --- a/detectron2_detector/package.xml +++ b/detectron2_detector/package.xml @@ -2,7 +2,7 @@ detectron2_detector - 0.0.0 + 0.0.1 This detector uses detectron2 to get object mask and then uses pointcloud2 data to estimate 3D position. Steven Macenski Shengjian Chen diff --git a/detectron2_detector/setup.py b/detectron2_detector/setup.py index 2f14564..4d2377f 100644 --- a/detectron2_detector/setup.py +++ b/detectron2_detector/setup.py @@ -5,7 +5,7 @@ setup( name=package_name, - version='0.0.0', + version='0.0.1', packages=[package_name], data_files=[ ('share/ament_index/resource_index/packages', @@ -16,10 +16,10 @@ ], install_requires=['setuptools'], zip_safe=True, - maintainer='tony', - maintainer_email='csj15thu@gmail.com', - description='TODO: Package description', - license='TODO: License declaration', + maintainer='Shengjian Chen, Steven Macenski', + maintainer_email='csj15thu@gmail.com, stevenmacenski@gmail.com', + description='This detector uses detectron2 to get object mask and then uses pointcloud2 data to estimate 3D position.', + license='Apache-2.0', tests_require=['pytest'], entry_points={ 'console_scripts': [ From 891ec21be35315db3b1fcd34cc1bcfbd254ee9f0 Mon Sep 17 00:00:00 2001 From: "shengjian.chen" Date: Tue, 13 Oct 2020 10:37:11 -0700 Subject: [PATCH 04/13] tracker to snake_case, fix small bugs --- .../detectron2_detector/detectron2_node.py | 4 --- kf_hungarian_tracker/config/kf_hungarian.yaml | 6 ++-- .../kf_hungarian_tracker/kf_hungarian_node.py | 15 +++++----- .../kf_hungarian_tracker/obstacle_class.py | 28 +++++++++---------- 4 files changed, 25 insertions(+), 28 deletions(-) diff --git a/detectron2_detector/detectron2_detector/detectron2_node.py b/detectron2_detector/detectron2_detector/detectron2_node.py index 527a6ea..d9ffc1e 100644 --- a/detectron2_detector/detectron2_detector/detectron2_node.py +++ b/detectron2_detector/detectron2_detector/detectron2_node.py @@ -29,21 +29,17 @@ def __init__(self): self.declare_parameters( namespace='', parameters=[ - ('global_frame', "camera_link"), ('detectron_config_file', "COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml"), ('detectron_score_thresh', 0.8), ('pointcloud2_topic', "/realsense/camera/pointcloud"), ('pc_downsample_factor', 16), ('min_mask', 20), ('categories', [0]), - ('z_filter', [-2., 2.]), ('nms_filter', 0.3) ]) - self.global_frame = self.get_parameter("global_frame")._value self.pc_downsample_factor = int(self.get_parameter("pc_downsample_factor")._value) self.min_mask = self.get_parameter("min_mask")._value self.categories = self.get_parameter("categories")._value - self.z_filter = self.get_parameter("z_filter")._value self.nms_filter = self.get_parameter("nms_filter")._value # setup detectron model diff --git a/kf_hungarian_tracker/config/kf_hungarian.yaml b/kf_hungarian_tracker/config/kf_hungarian.yaml index 28bc2c3..8ddc6f8 100644 --- a/kf_hungarian_tracker/config/kf_hungarian.yaml +++ b/kf_hungarian_tracker/config/kf_hungarian.yaml @@ -4,9 +4,9 @@ kf_hungarian_node: global_frame: "camera_link" # none means camera frame is the global frame # Kamlan filter related top_down: False # whether use top-down (x, y) view or 3D (x, y, z) - measurementNoiseCov: [1., 1., 1.] # if it's use top-down view, set the 3rd number to 0 - errorCovPost: [1., 1., 1., 10., 10., 10.] # if use top-down view, set the 3rd and 6th to 0 - a_noise: [2., 2., 0.5] # acceration noise, if use top-down view, the 3rd number should be 0 + measurement_noise_cov: [1., 1., 1.] # if it's use top-down view, set the 3rd number to 0 + error_cov_post: [1., 1., 1., 10., 10., 10.] # if use top-down view, set the 3rd and 6th to 0 + process_noise_cov: [2., 2., 0.5] # processNoiseCov, model as acceration noise, if use top-down view, the 3rd number should be 0 # obstacle filter vel_filter: [0.1, 2.0] # minimum and maximum velocity to filter obstacles height_filter: [-2.0, 2.0] # minimum and maximum height (z) to filter obstacles diff --git a/kf_hungarian_tracker/kf_hungarian_tracker/kf_hungarian_node.py b/kf_hungarian_tracker/kf_hungarian_tracker/kf_hungarian_node.py index 2f781e3..8575f6f 100644 --- a/kf_hungarian_tracker/kf_hungarian_tracker/kf_hungarian_node.py +++ b/kf_hungarian_tracker/kf_hungarian_tracker/kf_hungarian_node.py @@ -38,20 +38,20 @@ def __init__(self): namespace='', parameters=[ ('global_frame', "camera_link"), - ('a_noise', [2., 2., 0.5]), + ('process_noise_cov', [2., 2., 0.5]), ('top_down', False), ('death_threshold', 3), - ('measurementNoiseCov', [1., 1., 1.]), - ('errorCovPost', [1., 1., 1., 10., 10., 10.]), + ('measurement_noise_cov', [1., 1., 1.]), + ('error_cov_post', [1., 1., 1., 10., 10., 10.]), ('vel_filter', [0.1, 2.0]), ('height_filter', [-2.0, 2.0]), ('cost_filter', 1.0) ]) self.global_frame = self.get_parameter("global_frame")._value self.death_threshold = self.get_parameter("death_threshold")._value - self.measurementNoiseCov = self.get_parameter("measurementNoiseCov")._value - self.errorCovPost = self.get_parameter("errorCovPost")._value - self.a_noise = self.get_parameter("a_noise")._value + self.measurement_noise_cov = self.get_parameter("measurement_noise_cov")._value + self.error_cov_post = self.get_parameter("error_cov_post")._value + self.process_noise_cov = self.get_parameter("process_noise_cov")._value self.vel_filter = self.get_parameter("vel_filter")._value self.height_filter = self.get_parameter("height_filter")._value self.top_down = self.get_parameter("top_down")._value @@ -98,6 +98,7 @@ def callback(self, msg): if self.global_frame is not None: try: trans = self.tf_buffer.lookup_transform(self.global_frame, msg.header.frame_id, rclpy.time.Time()) + msg.header.frame_id = self.global_frame for i in range(len(detections)): # transform position (point) p = PointStamped() @@ -220,7 +221,7 @@ def birth(self, det_ind, num_of_detect, detections): '''generate new ObstacleClass for detections that do not match any in current obstacle list''' for det in range(num_of_detect): if det not in det_ind: - self.obstacle_list.append(ObstacleClass(detections[det], self.max_id, self.top_down, self.measurementNoiseCov, self.errorCovPost, self.a_noise)) + self.obstacle_list.append(ObstacleClass(detections[det], self.max_id, self.top_down, self.measurement_noise_cov, self.error_cov_post, self.process_noise_cov)) self.max_id = self.max_id + 1 def death(self, obj_ind, num_of_obstacle): diff --git a/kf_hungarian_tracker/kf_hungarian_tracker/obstacle_class.py b/kf_hungarian_tracker/kf_hungarian_tracker/obstacle_class.py index 55b0da1..0a5fd1b 100644 --- a/kf_hungarian_tracker/kf_hungarian_tracker/obstacle_class.py +++ b/kf_hungarian_tracker/kf_hungarian_tracker/obstacle_class.py @@ -13,7 +13,7 @@ class ObstacleClass: dying: count missing frames for this obstacle, if reach threshold, delete this obstacle """ - def __init__(self, obstacle_msg, idx, top_down, measurementNoiseCov, errorCovPost, a_noise): + def __init__(self, obstacle_msg, idx, top_down, measurement_noise_cov, error_cov_post, process_noise_cov): '''Initialize with an Obstacle msg and an assigned id''' self.msg = obstacle_msg self.msg.id = idx @@ -22,21 +22,21 @@ def __init__(self, obstacle_msg, idx, top_down, measurementNoiseCov, errorCovPos # check aganist state space dimension, top_down or not if top_down: - measurementNoiseCov[2] = 0. - errorCovPost[2] = 0. - errorCovPost[5] = 0. - a_noise[2] = 0. + measurement_noise_cov[2] = 0. + error_cov_post[2] = 0. + error_cov_post[5] = 0. + process_noise_cov[2] = 0. # setup kalman filter self.kalman = cv2.KalmanFilter(6,3) # 3d by default, 6d state space and 3d observation space self.kalman.measurementMatrix = np.array([[1,0,0,0,0,0], [0,1,0,0,0,0], [0,0,1,0,0,0]], np.float32) - self.kalman.measurementNoiseCov = np.diag(measurementNoiseCov).astype(np.float32) + self.kalman.measurementNoiseCov = np.diag(measurement_noise_cov).astype(np.float32) self.kalman.statePost = np.concatenate([position, velocity]).astype(np.float32) - self.kalman.errorCovPost = np.diag(errorCovPost).astype(np.float32) + self.kalman.errorCovPost = np.diag(error_cov_post).astype(np.float32) self.dying = 0 self.top_down = top_down - self.a_noise = a_noise + self.process_noise_cov = process_noise_cov def predict(self, dt): '''update F and Q matrices, call KalmanFilter.predict and store position and velocity''' @@ -67,12 +67,12 @@ def predict(self, dt): dt2 = dt**2 dt3 = dt*dt2 dt4 = dt2**2 - Q = np.array([[dt4*self.a_noise[0]/4, 0, 0, dt3*self.a_noise[0]/2, 0, 0], - [0, dt4*self.a_noise[1]/4, 0, 0, dt3*self.a_noise[1]/2, 0], - [0, 0, dt4*self.a_noise[2]/4, 0, 0, dt3*self.a_noise[2]/2], - [dt3*self.a_noise[0]/2, 0, 0, dt2*self.a_noise[0], 0, 0], - [0, dt3*self.a_noise[1]/2, 0, 0, dt2*self.a_noise[1], 0], - [0, 0, dt3*self.a_noise[2]/2, 0, 0, dt2*self.a_noise[2]]]).astype(np.float32) + Q = np.array([[dt4*self.process_noise_cov[0]/4, 0, 0, dt3*self.process_noise_cov[0]/2, 0, 0], + [0, dt4*self.process_noise_cov[1]/4, 0, 0, dt3*self.process_noise_cov[1]/2, 0], + [0, 0, dt4*self.process_noise_cov[2]/4, 0, 0, dt3*self.process_noise_cov[2]/2], + [dt3*self.process_noise_cov[0]/2, 0, 0, dt2*self.process_noise_cov[0], 0, 0], + [0, dt3*self.process_noise_cov[1]/2, 0, 0, dt2*self.process_noise_cov[1], 0], + [0, 0, dt3*self.process_noise_cov[2]/2, 0, 0, dt2*self.process_noise_cov[2]]]).astype(np.float32) self.kalman.transitionMatrix = F self.kalman.processNoiseCov = Q From 3a4050596356eb599f3e52c64f6da009c5602a53 Mon Sep 17 00:00:00 2001 From: "shengjian.chen" Date: Wed, 14 Oct 2020 16:00:55 -0700 Subject: [PATCH 05/13] update outlier filter --- detectron2_detector/config/detectron2.yaml | 6 ++- .../detectron2_detector/detectron2_node.py | 50 ++++++++++--------- 2 files changed, 31 insertions(+), 25 deletions(-) diff --git a/detectron2_detector/config/detectron2.yaml b/detectron2_detector/config/detectron2.yaml index 86ee856..3dc1441 100644 --- a/detectron2_detector/config/detectron2.yaml +++ b/detectron2_detector/config/detectron2.yaml @@ -1,10 +1,12 @@ detectron2_node: ros__parameters: + # detectron parameters detectron_config_file: "COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml" detectron_score_thresh: 0.8 + # processing parameters pointcloud2_topic: "/realsense/camera/pointcloud" + categories: [0] # please check out COCO dataset category_id list if you want to config this; if you want to track all, leave it empty pc_downsample_factor: 16 min_mask: 20 # minimum mask to be considered as an obstacle candidate - categories: [0] # please check out COCO dataset category_id list if you want to config this; if you want to track all, leave it empty nms_filter: 0.3 # 3D non-max suppression threshold, [0, 1] - \ No newline at end of file + outlier_filter: 0.5 \ No newline at end of file diff --git a/detectron2_detector/detectron2_detector/detectron2_node.py b/detectron2_detector/detectron2_detector/detectron2_node.py index d9ffc1e..39d2871 100644 --- a/detectron2_detector/detectron2_detector/detectron2_node.py +++ b/detectron2_detector/detectron2_detector/detectron2_node.py @@ -4,6 +4,7 @@ # import from common libraries import numpy as np +from scipy.stats import multivariate_normal import os, json, cv2, random # import some common detectron2 utilities @@ -35,12 +36,14 @@ def __init__(self): ('pc_downsample_factor', 16), ('min_mask', 20), ('categories', [0]), - ('nms_filter', 0.3) + ('nms_filter', 0.3), + ('outlier_filter', 0.5) ]) self.pc_downsample_factor = int(self.get_parameter("pc_downsample_factor")._value) self.min_mask = self.get_parameter("min_mask")._value self.categories = self.get_parameter("categories")._value self.nms_filter = self.get_parameter("nms_filter")._value + self.outlier_filter = self.get_parameter("outlier_filter")._value # setup detectron model self.cfg = get_cfg() @@ -63,15 +66,14 @@ def __init__(self): self.count = -1 - def outlier_filter(self, x, z, idx): + def outlier_filter(self, x, y, z, idx): '''simple outlier filter, assume Gaussian distribution and drop points with low probability (too far away from center)''' - x_mean = np.mean(x) - x_var = np.var(x) - z_mean = np.mean(z) - z_var = np.var(z) - # probability under Gaussian distribution - gaussian_kernel = np.exp(-0.5 * (np.power(x-x_mean, 2) / x_var + np.power(z-z_mean, 2) / z_var)) / (2 * np.pi * np.sqrt(x_var * z_var)) - return idx[gaussian_kernel > 0.5] + mean = [np.mean(x), np.mean(y), np.mean(z)] + cov = np.diag([np.var(x), np.var(y), np.var(z)]) + rv = multivariate_normal(mean, cov) + points = np.dstack((x, y, z)) + p = rv.pdf(points) + return idx[p > self.outlier_filter] def callback(self, msg): # check if there is subscirbers @@ -127,7 +129,7 @@ def callback(self, msg): # if user does not specify any interested category, keep all; else select those interested objects if (len(self.categories) == 0) or (outputs["instances"].pred_classes[i] in self.categories): idx = np.where(masks[i])[0] - idx = self.outlier_filter(x[idx], z[idx], idx) + idx = self.outlier_filter(x[idx], y[idx], z[idx], idx) if idx.shape[0] < self.min_mask: continue obstacle_msg = Obstacle() @@ -149,21 +151,23 @@ def callback(self, msg): detections.append(obstacle_msg) # publish detection result - obstacle_array.obstacles = detections - self.detect_obj_pub.publish(obstacle_array) + if self.detect_obj_pub.get_subscription_count() > 0: + obstacle_array.obstacles = detections + self.detect_obj_pub.publish(obstacle_array) # visualize detection with detectron API - v = Visualizer(img[:, :, ::-1], MetadataCatalog.get(self.cfg.DATASETS.TRAIN[0]), scale=1) - out = v.draw_instance_predictions(outputs["instances"].to("cpu")) - out_img = out.get_image()[:, :, ::-1] - out_img_msg = Image() - out_img_msg.header = msg.header - out_img_msg.height = height - out_img_msg.width = width - out_img_msg.encoding = 'rgb8' - out_img_msg.step = 3 * width - out_img_msg.data = out_img.flatten().tolist() - self.detect_img_pub.publish(out_img_msg) + if self.detect_img_pub.get_subscription_count() > 0: + v = Visualizer(img[:, :, ::-1], MetadataCatalog.get(self.cfg.DATASETS.TRAIN[0]), scale=1) + out = v.draw_instance_predictions(outputs["instances"].to("cpu")) + out_img = out.get_image()[:, :, ::-1] + out_img_msg = Image() + out_img_msg.header = msg.header + out_img_msg.height = height + out_img_msg.width = width + out_img_msg.encoding = 'rgb8' + out_img_msg.step = 3 * width + out_img_msg.data = out_img.flatten().tolist() + self.detect_img_pub.publish(out_img_msg) def main(): rclpy.init(args = None) From 954b06c2c504a5c3d8685440dd9e0509b391bb8e Mon Sep 17 00:00:00 2001 From: "shengjian.chen" Date: Wed, 14 Oct 2020 16:28:03 -0700 Subject: [PATCH 06/13] add line --- detectron2_detector/config/detectron2.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/detectron2_detector/config/detectron2.yaml b/detectron2_detector/config/detectron2.yaml index 3dc1441..5a628bc 100644 --- a/detectron2_detector/config/detectron2.yaml +++ b/detectron2_detector/config/detectron2.yaml @@ -9,4 +9,5 @@ detectron2_node: pc_downsample_factor: 16 min_mask: 20 # minimum mask to be considered as an obstacle candidate nms_filter: 0.3 # 3D non-max suppression threshold, [0, 1] - outlier_filter: 0.5 \ No newline at end of file + outlier_filter: 0.5 + \ No newline at end of file From 6c61a934134cc32512fd0c1bfc07c12b97f01672 Mon Sep 17 00:00:00 2001 From: Shengjian Chen Date: Wed, 14 Oct 2020 17:00:24 -0700 Subject: [PATCH 07/13] Create README.md --- detectron2_detector/README.md | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 detectron2_detector/README.md diff --git a/detectron2_detector/README.md b/detectron2_detector/README.md new file mode 100644 index 0000000..5060b65 --- /dev/null +++ b/detectron2_detector/README.md @@ -0,0 +1,16 @@ +# detectron2_detector + +This package implements a 3D object detector with [Mask RCNN](https://arxiv.org/pdf/1703.06870.pdf) ([detectron2](https://github.com/facebookresearch/detectron2) implementation) and basic pointcloud processing. +We assume ordered pointcloud aligned with pixels is available. We use Mask RCNN to produce masks for objects, then use these masks to extract points belong to the objects and estimate position and size. + +### Parameters +| parameters | Meaning | Default | +| ---------------- | ------------- | ------- | +| detectron_config_file | config file to load detectron model,
check out [model zoo](https://github.com/facebookresearch/detectron2/blob/master/MODEL_ZOO.md) for more information | COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml | +| detectron_score_thresh | min score to be considered as an object | 0.8 | +| pointcloud2_topic | ros topic for pointcloud data | /realsense/camera/pointcloud | +| categories | list of interested object categories,
left empty `[]` to detect all categories | [0] (person) | +| pc_downsample_factor | factor to downsample the aligned pointcloud | 16 | +| min_mask | min number of pixels in a mask | 20 | +| nms_filter | IoU threshold for non-max suppression | 0.3 | +| outlier_filter | threshold to filter outlier points,
model as multivariate normal distribution | 0.5 | From b8d3460322d14ace91301c6c8d2f8ee24bfbec74 Mon Sep 17 00:00:00 2001 From: "shengjian.chen" Date: Wed, 14 Oct 2020 17:06:04 -0700 Subject: [PATCH 08/13] update depend --- detectron2_detector/detectron2_detector/detectron2_node.py | 1 - detectron2_detector/package.xml | 6 ++++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/detectron2_detector/detectron2_detector/detectron2_node.py b/detectron2_detector/detectron2_detector/detectron2_node.py index 39d2871..2a0ee87 100644 --- a/detectron2_detector/detectron2_detector/detectron2_node.py +++ b/detectron2_detector/detectron2_detector/detectron2_node.py @@ -5,7 +5,6 @@ # import from common libraries import numpy as np from scipy.stats import multivariate_normal -import os, json, cv2, random # import some common detectron2 utilities from detectron2 import model_zoo diff --git a/detectron2_detector/package.xml b/detectron2_detector/package.xml index af0d7a7..0287a20 100644 --- a/detectron2_detector/package.xml +++ b/detectron2_detector/package.xml @@ -13,8 +13,14 @@ ament_pep257 python3-pytest + rclpy launch_ros + numpy + scipy detectron2 + sensor_msgs + nav2_dynamic_msgs + geometry_msgs ament_python From d27ad33eb50c142b0e4e94b7d953ce8503633418 Mon Sep 17 00:00:00 2001 From: Shengjian Chen Date: Wed, 14 Oct 2020 17:10:36 -0700 Subject: [PATCH 09/13] Update README.md --- detectron2_detector/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/detectron2_detector/README.md b/detectron2_detector/README.md index 5060b65..68b5135 100644 --- a/detectron2_detector/README.md +++ b/detectron2_detector/README.md @@ -9,7 +9,7 @@ We assume ordered pointcloud aligned with pixels is available. We use Mask RCNN | detectron_config_file | config file to load detectron model,
check out [model zoo](https://github.com/facebookresearch/detectron2/blob/master/MODEL_ZOO.md) for more information | COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml | | detectron_score_thresh | min score to be considered as an object | 0.8 | | pointcloud2_topic | ros topic for pointcloud data | /realsense/camera/pointcloud | -| categories | list of interested object categories,
left empty `[]` to detect all categories | [0] (person) | +| categories | list of interested categories in COCO dataset,
left empty `[]` to detect all categories | [0] (person) | | pc_downsample_factor | factor to downsample the aligned pointcloud | 16 | | min_mask | min number of pixels in a mask | 20 | | nms_filter | IoU threshold for non-max suppression | 0.3 | From a4df8cf112fd813b2f52dc4138bb2eaa41886bc1 Mon Sep 17 00:00:00 2001 From: Shengjian Chen Date: Wed, 14 Oct 2020 17:13:13 -0700 Subject: [PATCH 10/13] Update README.md --- detectron2_detector/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/detectron2_detector/README.md b/detectron2_detector/README.md index 68b5135..00296ab 100644 --- a/detectron2_detector/README.md +++ b/detectron2_detector/README.md @@ -6,7 +6,7 @@ We assume ordered pointcloud aligned with pixels is available. We use Mask RCNN ### Parameters | parameters | Meaning | Default | | ---------------- | ------------- | ------- | -| detectron_config_file | config file to load detectron model,
check out [model zoo](https://github.com/facebookresearch/detectron2/blob/master/MODEL_ZOO.md) for more information | COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml | +| detectron_config_file | config file to load detectron model,
check out [model zoo](https://github.com/facebookresearch/detectron2/blob/master/MODEL_ZOO.md) | COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml | | detectron_score_thresh | min score to be considered as an object | 0.8 | | pointcloud2_topic | ros topic for pointcloud data | /realsense/camera/pointcloud | | categories | list of interested categories in COCO dataset,
left empty `[]` to detect all categories | [0] (person) | From 3b0d4c6099b22f46c5e5852332064bd5983bb211 Mon Sep 17 00:00:00 2001 From: "shengjian.chen" Date: Thu, 15 Oct 2020 11:40:24 -0700 Subject: [PATCH 11/13] update point topic --- detectron2_detector/README.md | 2 +- detectron2_detector/config/detectron2.yaml | 2 +- detectron2_detector/detectron2_detector/detectron2_node.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/detectron2_detector/README.md b/detectron2_detector/README.md index 5060b65..2cec363 100644 --- a/detectron2_detector/README.md +++ b/detectron2_detector/README.md @@ -8,7 +8,7 @@ We assume ordered pointcloud aligned with pixels is available. We use Mask RCNN | ---------------- | ------------- | ------- | | detectron_config_file | config file to load detectron model,
check out [model zoo](https://github.com/facebookresearch/detectron2/blob/master/MODEL_ZOO.md) for more information | COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml | | detectron_score_thresh | min score to be considered as an object | 0.8 | -| pointcloud2_topic | ros topic for pointcloud data | /realsense/camera/pointcloud | +| pointcloud2_topic | ros topic for pointcloud data | /camera/depth/points | | categories | list of interested object categories,
left empty `[]` to detect all categories | [0] (person) | | pc_downsample_factor | factor to downsample the aligned pointcloud | 16 | | min_mask | min number of pixels in a mask | 20 | diff --git a/detectron2_detector/config/detectron2.yaml b/detectron2_detector/config/detectron2.yaml index 5a628bc..d1533ae 100644 --- a/detectron2_detector/config/detectron2.yaml +++ b/detectron2_detector/config/detectron2.yaml @@ -4,7 +4,7 @@ detectron2_node: detectron_config_file: "COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml" detectron_score_thresh: 0.8 # processing parameters - pointcloud2_topic: "/realsense/camera/pointcloud" + pointcloud2_topic: "/camera/depth/points" categories: [0] # please check out COCO dataset category_id list if you want to config this; if you want to track all, leave it empty pc_downsample_factor: 16 min_mask: 20 # minimum mask to be considered as an obstacle candidate diff --git a/detectron2_detector/detectron2_detector/detectron2_node.py b/detectron2_detector/detectron2_detector/detectron2_node.py index 2a0ee87..f2bdc51 100644 --- a/detectron2_detector/detectron2_detector/detectron2_node.py +++ b/detectron2_detector/detectron2_detector/detectron2_node.py @@ -31,7 +31,7 @@ def __init__(self): parameters=[ ('detectron_config_file', "COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml"), ('detectron_score_thresh', 0.8), - ('pointcloud2_topic', "/realsense/camera/pointcloud"), + ('pointcloud2_topic', "/camera/depth/points"), ('pc_downsample_factor', 16), ('min_mask', 20), ('categories', [0]), From 4d666228d1a113517bec5af173e0702f49fa5ce0 Mon Sep 17 00:00:00 2001 From: "shengjian.chen" Date: Thu, 15 Oct 2020 12:28:47 -0700 Subject: [PATCH 12/13] split detector callback --- detectron2_detector/config/detectron2.yaml | 2 +- .../detectron2_detector/detectron2_node.py | 43 +++++++++++++------ 2 files changed, 30 insertions(+), 15 deletions(-) diff --git a/detectron2_detector/config/detectron2.yaml b/detectron2_detector/config/detectron2.yaml index d1533ae..59c8efc 100644 --- a/detectron2_detector/config/detectron2.yaml +++ b/detectron2_detector/config/detectron2.yaml @@ -9,5 +9,5 @@ detectron2_node: pc_downsample_factor: 16 min_mask: 20 # minimum mask to be considered as an obstacle candidate nms_filter: 0.3 # 3D non-max suppression threshold, [0, 1] - outlier_filter: 0.5 + outlier_thresh: 0.5 \ No newline at end of file diff --git a/detectron2_detector/detectron2_detector/detectron2_node.py b/detectron2_detector/detectron2_detector/detectron2_node.py index f2bdc51..8b43804 100644 --- a/detectron2_detector/detectron2_detector/detectron2_node.py +++ b/detectron2_detector/detectron2_detector/detectron2_node.py @@ -36,13 +36,13 @@ def __init__(self): ('min_mask', 20), ('categories', [0]), ('nms_filter', 0.3), - ('outlier_filter', 0.5) + ('outlier_thresh', 0.5) ]) self.pc_downsample_factor = int(self.get_parameter("pc_downsample_factor")._value) self.min_mask = self.get_parameter("min_mask")._value self.categories = self.get_parameter("categories")._value self.nms_filter = self.get_parameter("nms_filter")._value - self.outlier_filter = self.get_parameter("outlier_filter")._value + self.outlier_thresh = self.get_parameter("outlier_thresh")._value # setup detectron model self.cfg = get_cfg() @@ -72,7 +72,7 @@ def outlier_filter(self, x, y, z, idx): rv = multivariate_normal(mean, cov) points = np.dstack((x, y, z)) p = rv.pdf(points) - return idx[p > self.outlier_filter] + return idx[p > self.outlier_thresh] def callback(self, msg): # check if there is subscirbers @@ -91,7 +91,7 @@ def callback(self, msg): g = points[(rgb_offset+1)::point_step] b = points[(rgb_offset+2)::point_step] img = np.concatenate([r[:, None], g[:, None], b[:, None]], axis = -1) - img = img.reshape((height, width, 3)) + self.img = img.reshape((height, width, 3)) # decode point cloud data if msg.fields[0].datatype < 3: @@ -107,11 +107,17 @@ def callback(self, msg): y = points[1::int(self.pc_downsample_factor * point_step / byte)] z = points[2::int(self.pc_downsample_factor * point_step / byte)] - # call detectron2 model - outputs = self.predictor(img) + self.points = [x, y, z] + self.header = msg.header + + # call detect function + self.detect() + + def process_points(self, outputs): + '''estimate 3D position and size with detectron output and pointcloud data''' + x, y, z = self.points # map mask to point cloud data - color = np.zeros_like(x, dtype = 'uint8') num_classes = outputs['instances'].pred_classes.shape[0] if num_classes == 0: self.detect_obj_pub.publish(ObstacleArray()) @@ -121,8 +127,6 @@ def callback(self, msg): scores = outputs["instances"].scores.cpu().numpy().astype(np.float) # estimate 3D position with simple averaging of obstacle's points - obstacle_array = ObstacleArray() - obstacle_array.header = msg.header detections = [] for i in range(num_classes): # if user does not specify any interested category, keep all; else select those interested objects @@ -149,22 +153,33 @@ def callback(self, msg): obstacle_msg.size.z = np.float(z_max - z_min) detections.append(obstacle_msg) + return detections + + def detect(self): + # call detectron2 model + outputs = self.predictor(self.img) + + # process pointcloud to get 3D position and size + detections = self.process_points(outputs) + # publish detection result + obstacle_array = ObstacleArray() + obstacle_array.header = self.header if self.detect_obj_pub.get_subscription_count() > 0: obstacle_array.obstacles = detections self.detect_obj_pub.publish(obstacle_array) # visualize detection with detectron API if self.detect_img_pub.get_subscription_count() > 0: - v = Visualizer(img[:, :, ::-1], MetadataCatalog.get(self.cfg.DATASETS.TRAIN[0]), scale=1) + v = Visualizer(self.img[:, :, ::-1], MetadataCatalog.get(self.cfg.DATASETS.TRAIN[0]), scale=1) out = v.draw_instance_predictions(outputs["instances"].to("cpu")) out_img = out.get_image()[:, :, ::-1] out_img_msg = Image() - out_img_msg.header = msg.header - out_img_msg.height = height - out_img_msg.width = width + out_img_msg.header = self.header + out_img_msg.height = out_img.shape[0] + out_img_msg.width = out_img.shape[1] out_img_msg.encoding = 'rgb8' - out_img_msg.step = 3 * width + out_img_msg.step = 3 * out_img.shape[1] out_img_msg.data = out_img.flatten().tolist() self.detect_img_pub.publish(out_img_msg) From 8cde0f8dc477c6d4912fd1f7c788f378c3018989 Mon Sep 17 00:00:00 2001 From: Shengjian Chen Date: Thu, 15 Oct 2020 12:37:21 -0700 Subject: [PATCH 13/13] Update README.md --- detectron2_detector/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/detectron2_detector/README.md b/detectron2_detector/README.md index 0418520..e1bfe4f 100644 --- a/detectron2_detector/README.md +++ b/detectron2_detector/README.md @@ -1,7 +1,7 @@ # detectron2_detector This package implements a 3D object detector with [Mask RCNN](https://arxiv.org/pdf/1703.06870.pdf) ([detectron2](https://github.com/facebookresearch/detectron2) implementation) and basic pointcloud processing. -We assume ordered pointcloud aligned with pixels is available. We use Mask RCNN to produce masks for objects, then use these masks to extract points belong to the objects and estimate position and size. +We assume colored and ordered pointcloud aligned with pixels is available. We use Mask RCNN to produce masks for objects, then use these masks to extract points belong to the objects and estimate position and size. ### Parameters | parameters | Meaning | Default | @@ -9,7 +9,7 @@ We assume ordered pointcloud aligned with pixels is available. We use Mask RCNN | detectron_config_file | config file to load detectron model,
check out [model zoo](https://github.com/facebookresearch/detectron2/blob/master/MODEL_ZOO.md) | COCO-InstanceSegmentation/mask_rcnn_R_50_FPN_3x.yaml | | detectron_score_thresh | min score to be considered as an object | 0.8 | | pointcloud2_topic | ros topic for pointcloud data | /camera/depth/points | -| categories | list of interested categories in COCO dataset,
left empty `[]` to detect all categories | [0] (person) | +| categories | list of interested [categories](https://github.com/amikelive/coco-labels/blob/master/coco-labels-2014_2017.txt) in COCO dataset, left empty `[]` to detect all | [0] (person) | | pc_downsample_factor | factor to downsample the aligned pointcloud | 16 | | min_mask | min number of pixels in a mask | 20 | | nms_filter | IoU threshold for non-max suppression | 0.3 |