[jsk_perception] Add Lidar person detection Node

.gitmodules

@@ -8,3 +8,6 @@
 [submodule "jsk_perception/node_scripts/deep_sort/deep_sort"]
 	path = jsk_perception/node_scripts/deep_sort/deep_sort
 	url = https://github.com/nwojke/deep_sort.git
+[submodule "jsk_perception/node_scripts/dr_spaam_libs/dr_spaam"]
+	path = jsk_perception/node_scripts/dr_spaam_libs/dr_spaam
+	url = https://github.com/VisualComputingInstitute/2D_lidar_person_detection

doc/jsk_perception/nodes/lidar_person_detection_node.md

@@ -0,0 +1,101 @@
+# lidar_person_detection_node.py
+
+![](images/lidar_person_detection_node.gif)
+
+Detect person from lidar sensor.
+
+For more detail, please see the thesis `DR-SPAAM: A Spatial-Attention and Auto-regressive Model for Person Detection in 2D Range Data`.
+
+In order to use this feature, you need to install `pytorch <https://pytorch.org/get-started/locally/>`_ (pytorch >= 1.4.0 is recommended).
+
+
+## Subscribing Topic
+
+* `~input` (`sensor_msgs/LaserScan`)
+
+  Input laser scan.
+
+
+## Publishing Topic
+
+* `~output` (`geometry_msgs/PoseArray`)
+
+  Position of detected people.
+
+  Based on the result of tracking, the direction of movement is the x direction.
+
+* `~output/markers` (`visualization_msgs/MarkerArray`)
+
+  MakerArray of detected people.
+
+  The color of the marker is determined based on the tracking result.
+
+## Parameters
+
+* `~queue_size` (`Int`, default: `1`)
+
+  Input queue size.
+
+* `~conf_thresh` (`Double`, default: `0.8`)
+
+  Threshold for confidence.
+
+* `~weight_file` (`String`, required)
+
+  Trained model's weight file path.
+
+* `~detector_model` (`String`, default: `DR-SPAAM`)
+
+  Detector model. Current only `DR-SPAAM` is supported.
+
+* `~stride` (`Int`, default: `1`)
+
+  Use this to skip laser points.
+
+* `~panoramic_scan` (`Bool`, default: `false`)
+
+  Set to true if the scan covers 360 degree.
+
+* `~gpu` (`Int`, default: `-1`)
+
+  Index of gpu used for prediction. Set `-1` for using CPU.
+
+* `~max_distance` (`Double`, default: `0.5`)
+
+  Threshold for tracking max distance.
+
+  If the position in the previous frame is farther than this distance, it will be excluded from the tracking candidates.
+
+* `~n_previous` (`Int`, default: `10`)
+
+  Determine the moving direction from the previous position and the current position.
+
+* `~map_link` (`String`, default: `None` optional)
+
+  If this value is specified, markers are published in `~map_link` frame.
+
+* `~duration_timeout` (`Double`, default: `0.05`)
+
+  Duration of timeout for lookup transform in case of specifying `~map_link`.
+
+* `~color_alpha` (`Double`, default: `0.8`)
+
+  Alpha value of visualization marker.
+
+* `~people_height` (`Double`, default: `1.6`)
+
+  Height of visualization marker.
+
+* `~people_head_radius` (`Double`, default: `0.3`)
+
+  Head radius of visualization marker.
+
+* `~people_body_radius` (`Double`, default: `0.3`)
+
+  Body radius of visualization marker.
+
+## Sample
+
+```bash
+roslaunch jsk_perception sample_lidar_person_detection.launch
+```

jsk_perception/CMakeLists.txt

@@ -140,6 +140,7 @@ generate_dynamic_reconfigure_options(
   cfg/OverlayImageColorOnMono.cfg
   cfg/SelectiveSearch.cfg
   cfg/LabelToMaskImage.cfg
+  cfg/LidarPersonDetection.cfg
   cfg/FlowVelocityThresholding.cfg
   cfg/FilterMaskImageWithSize.cfg
   cfg/FastRCNN.cfg
@@ -201,6 +202,12 @@ if(NOT EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/node_scripts/deep_sort/deep_sort/READM
   execute_process(COMMAND git submodule update deep_sort WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/node_scripts/deep_sort)
 endif()
 
+if(NOT EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/node_scripts/dr_spaam_libs/dr_spaam/README.md)
+  message(WARNING "node_scripts/dr_spaam_libs/dr_spaam is not exists, download this")
+  execute_process(COMMAND git submodule init dr_spaam WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/node_scripts/dr_spaam_libs)
+  execute_process(COMMAND git submodule update dr_spaam WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/node_scripts/dr_spaam_libs)
+endif()
+
 # Build nodelet executables
 include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} slic ${libcmt_INCLUDE_DIRS})
 if(robot_self_filter_FOUND)
@@ -510,6 +517,7 @@ if(CATKIN_ENABLE_TESTING)
     if("$ENV{ROS_DISTRO}" STRGREATER "indigo")
       jsk_add_rostest(test/craft_node.test)
       jsk_add_rostest(test/deep_sort_tracker.test)
+      jsk_add_rostest(test/lidar_person_detection.test)
       jsk_add_rostest(test/ocr_node.test)
     endif()
     jsk_add_rostest(test/draw_rects.test)

jsk_perception/cfg/LidarPersonDetection.cfg

@@ -0,0 +1,19 @@
+#!/usr/bin/env python
+
+PACKAGE = 'jsk_perception'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("conf_thresh", double_t, 0, "Threshold for confidence", 0.8, 0.0, 1.0)
+gen.add("max_distance", double_t, 0, "Threshold for tracking max distance", 0.5, 0.0, 100.0)
+gen.add("n_previous", int_t, 0, "Determine the moving direction from the previous position and the current position", 10, 1, 1000)
+
+viz_group = gen.add_group("VisualizationMarker")
+viz_group.add("color_alpha", double_t, 0, "Alpha value of visualization marker.", 0.8, 0.0, 1.0)
+viz_group.add("people_height", double_t, 0, "Height of visualization marker.", 1.6, 0.0, 10.0)
+viz_group.add("people_head_radius", double_t, 0, "Head radius of visualization marker.", 0.3, 0.0, 10.0)
+viz_group.add("people_body_radius", double_t, 0, "Body radius of visualization marker.", 0.3, 0.0, 10.0)
+
+exit(gen.generate(PACKAGE, PACKAGE, "LidarPersonDetection"))

jsk_perception/node_scripts/dr_spaam_libs/detector.py

@@ -0,0 +1,118 @@
+from __future__ import absolute_import
+
+import torch
+import numpy as np
+
+from .import_dr_spaam import dr_spaam
+from dr_spaam.model.drow_net import DrowNet
+from dr_spaam.model.dr_spaam import DrSpaam
+from dr_spaam.utils import utils as u
+
+
+class DRSpaamDetector(object):
+
+    def __init__(
+        self, ckpt_file, model="DROW3", gpu=-1, stride=1, panoramic_scan=False
+    ):
+        """A warpper class around DROW3 or DR-SPAAM network for end-to-end inference.
+
+        Args:
+            ckpt_file (str): Path to checkpoint
+            model (str): Model name, "DROW3" or "DR-SPAAM".
+            gpu (Int): If greater equal than 0, use gpu.
+            stride (int): Downsample scans for faster inference.
+            panoramic_scan (bool): True if the scan covers 360 degree.
+        """
+        if gpu >= 0:
+            torch.backends.cudnn.benchmark = True
+            self.device = torch.device('cuda:{}'.format(gpu))
+        else:
+            self.device = torch.device('cpu')
+        self._stride = stride
+        self._use_dr_spaam = model == "DR-SPAAM"
+
+        self._scan_phi = None
+        self._laser_fov_deg = None
+
+        if model == "DROW3":
+            self._model = DrowNet(
+                dropout=0.5, cls_loss=None, mixup_alpha=0.0, mixup_w=0.0
+            )
+        elif model == "DR-SPAAM":
+            self._model = DrSpaam(
+                dropout=0.5,
+                num_pts=56,
+                embedding_length=128,
+                alpha=0.5,
+                window_size=17,
+                panoramic_scan=panoramic_scan,
+                cls_loss=None,
+                mixup_alpha=0.0,
+                mixup_w=0.0,
+            )
+        else:
+            raise NotImplementedError(
+                "model should be 'DROW3' or 'DR-SPAAM', received {} instead.".format(
+                    model
+                )
+            )
+
+        ckpt = torch.load(ckpt_file, map_location=torch.device('cpu'))
+        self._model.load_state_dict(ckpt["model_state"])
+
+        self._model.eval()
+        self._model = self._model.to(self.device)
+
+    def __call__(self, scan):
+        if self._scan_phi is None:
+            assert self.is_ready(), "Call set_laser_fov() first."
+            half_fov_rad = 0.5 * np.deg2rad(self._laser_fov_deg)
+            self._scan_phi = np.linspace(
+                -half_fov_rad, half_fov_rad, len(scan), dtype=np.float32
+            )
+
+        # preprocess
+        ct = u.scans_to_cutout(
+            scan[None, ...],
+            self._scan_phi,
+            stride=self._stride,
+            centered=True,
+            fixed=True,
+            window_width=1.0,
+            window_depth=0.5,
+            num_cutout_pts=56,
+            padding_val=29.99,
+            area_mode=True,
+        )
+        ct = torch.from_numpy(ct).float()
+        ct = ct.to(self.device)
+
+        # inference
+        sim = None
+        with torch.no_grad():
+            # one extra dimension for batch
+            if self._use_dr_spaam:
+                pred_cls, pred_reg, sim = self._model(ct.unsqueeze(dim=0), inference=True)
+            else:
+                pred_cls, pred_reg = self._model(ct.unsqueeze(dim=0))
+        if sim is not None:
+            sim = sim.data.cpu().numpy()
+
+        pred_cls = torch.sigmoid(pred_cls[0]).data.cpu().numpy()
+        pred_reg = pred_reg[0].data.cpu().numpy()
+
+        # postprocess
+        dets_xy, dets_cls, instance_mask = u.nms_predicted_center(
+            scan[:: self._stride],
+            self._scan_phi[:: self._stride],
+            pred_cls[:, 0],
+            pred_reg,
+        )
+
+        return dets_xy, dets_cls, instance_mask, sim
+
+    def set_laser_fov(self, fov_deg):
+        self._laser_fov_deg = fov_deg
+
+    def is_ready(self):
+        return self._laser_fov_deg is not None

jsk_perception/node_scripts/dr_spaam_libs/import_dr_spaam.py

@@ -0,0 +1,9 @@
+import imp
+import os.path as osp
+
+
+abs_path = osp.dirname(osp.abspath(__file__))
+
+
+dr_spaam = imp.load_package(
+    'dr_spaam', osp.join(abs_path, 'dr_spaam/dr_spaam/dr_spaam'))