diff --git a/.gitmodules b/.gitmodules
index 703463ef5e..b369ac9082 100644
--- a/.gitmodules
+++ b/.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
diff --git a/doc/jsk_perception/nodes/images/lidar_person_detection_node.gif b/doc/jsk_perception/nodes/images/lidar_person_detection_node.gif
new file mode 100644
index 0000000000..f5f4e96945
Binary files /dev/null and b/doc/jsk_perception/nodes/images/lidar_person_detection_node.gif differ
diff --git a/doc/jsk_perception/nodes/lidar_person_detection_node.md b/doc/jsk_perception/nodes/lidar_person_detection_node.md
new file mode 100644
index 0000000000..6b10fe3e6c
--- /dev/null
+++ b/doc/jsk_perception/nodes/lidar_person_detection_node.md
@@ -0,0 +1,101 @@
+# lidar_person_detection_node.py
+
+
+
+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 `_ (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
+```
diff --git a/jsk_perception/CMakeLists.txt b/jsk_perception/CMakeLists.txt
index c79bcdee74..90144f7731 100644
--- a/jsk_perception/CMakeLists.txt
+++ b/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)
diff --git a/jsk_perception/cfg/LidarPersonDetection.cfg b/jsk_perception/cfg/LidarPersonDetection.cfg
new file mode 100755
index 0000000000..c71a4067f9
--- /dev/null
+++ b/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"))
diff --git a/jsk_perception/node_scripts/dr_spaam_libs/__init__.py b/jsk_perception/node_scripts/dr_spaam_libs/__init__.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/jsk_perception/node_scripts/dr_spaam_libs/detector.py b/jsk_perception/node_scripts/dr_spaam_libs/detector.py
new file mode 100644
index 0000000000..c484b17e71
--- /dev/null
+++ b/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
diff --git a/jsk_perception/node_scripts/dr_spaam_libs/dr_spaam b/jsk_perception/node_scripts/dr_spaam_libs/dr_spaam
new file mode 160000
index 0000000000..1a365cddaa
--- /dev/null
+++ b/jsk_perception/node_scripts/dr_spaam_libs/dr_spaam
@@ -0,0 +1 @@
+Subproject commit 1a365cddaa95338e4abf658ff224b1aa3b57abe2
diff --git a/jsk_perception/node_scripts/dr_spaam_libs/import_dr_spaam.py b/jsk_perception/node_scripts/dr_spaam_libs/import_dr_spaam.py
new file mode 100644
index 0000000000..e24b4dcb4a
--- /dev/null
+++ b/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'))
diff --git a/jsk_perception/node_scripts/lidar_person_detection_node.py b/jsk_perception/node_scripts/lidar_person_detection_node.py
new file mode 100755
index 0000000000..b2442ff091
--- /dev/null
+++ b/jsk_perception/node_scripts/lidar_person_detection_node.py
@@ -0,0 +1,228 @@
+#!/usr/bin/env python
+
+from dynamic_reconfigure.server import Server as ReconfigureServer
+from geometry_msgs.msg import Point
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import PoseArray
+from jsk_recognition_utils.color import labelcolormap
+from jsk_recognition_utils.visualization_marker import make_human_marker
+from jsk_recognition_utils.geometry import rotation_matrix_from_axis
+from jsk_topic_tools import ConnectionBasedTransport
+import numpy as np
+import PyKDL
+import rospy
+from sensor_msgs.msg import LaserScan
+from tf.transformations import quaternion_from_matrix as matrix2quaternion
+import tf2_geometry_msgs
+import tf2_ros
+from visualization_msgs.msg import Marker
+from visualization_msgs.msg import MarkerArray
+
+from jsk_perception.cfg import LidarPersonDetectionConfig as Config
+
+from dr_spaam_libs.detector import DRSpaamDetector
+from jsk_recognition_utils.trackers import Sort
+from jsk_recognition_utils.trackers.kalman_tracker import KalmanPositionTracker
+from jsk_recognition_utils.geometry import euclidean_distances
+
+N = 256
+colors = labelcolormap(N=N) / 255.0
+
+
+class LidarPersonDetectionNode(ConnectionBasedTransport):
+
+ def __init__(self):
+ super(LidarPersonDetectionNode, self).__init__()
+ self.weight_file = rospy.get_param("~weight_file")
+ self.stride = rospy.get_param("~stride", 1)
+ self.map_link = rospy.get_param("~map_link", None)
+ self.detector_model = rospy.get_param("~detector_model", 'DR-SPAAM')
+ self.panoramic_scan = rospy.get_param("~panoramic_scan", False)
+ gpu = rospy.get_param('~gpu', -1)
+
+ self._tf_buffer = tf2_ros.Buffer(cache_time=rospy.Duration(30.0))
+ self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
+ self._last_time = rospy.Time.now()
+ self._duration_timeout = rospy.get_param('~duration_timeout', 0.05)
+
+ self._tracker = Sort(
+ tracker_class=KalmanPositionTracker,
+ distance_metric=euclidean_distances,
+ max_distance=Config.defaults['max_distance'])
+ self._detector = DRSpaamDetector(
+ self.weight_file,
+ model=self.detector_model,
+ gpu=gpu,
+ stride=self.stride,
+ panoramic_scan=self.panoramic_scan,
+ )
+
+ self._srv = ReconfigureServer(Config, self.config_callback)
+
+ # Publisher
+ self._dets_pub = self.advertise(
+ '~output', PoseArray, queue_size=1)
+ self._rviz_pub = self.advertise(
+ '~output/markers', MarkerArray, queue_size=1)
+
+ def config_callback(self, config, level):
+ self.conf_thresh = config.conf_thresh
+ self.max_distance = config.max_distance
+ self._tracker.max_distance = config.max_distance
+ self.n_previous = config.n_previous
+ self.color_alpha = config.color_alpha
+ self.people_head_radius = config.people_head_radius
+ self.people_body_radius = config.people_body_radius
+ self.people_height = config.people_height
+ return config
+
+ def subscribe(self):
+ self._scan_sub = rospy.Subscriber(
+ '~input', LaserScan, self._scan_callback,
+ queue_size=rospy.get_param('~queue_size', 1))
+
+ def unsubscribe(self):
+ self._scan_sub.unregister()
+
+ def _lookup_transform(self, from_frame_id, to_frame_id, stamp):
+ transform = None
+ try:
+ transform = tf2_geometry_msgs.transform_to_kdl(
+ self._tf_buffer.lookup_transform(
+ from_frame_id.lstrip('/'),
+ to_frame_id.lstrip('/'),
+ stamp,
+ timeout=rospy.Duration(self._duration_timeout)))
+ except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
+ tf2_ros.ExtrapolationException) as e:
+ rospy.logwarn('{}'.format(e))
+ return transform
+
+ def _scan_callback(self, msg):
+ frame_id = self.map_link or msg.header.frame_id
+ frame_id = frame_id.lstrip('/')
+
+ now = rospy.Time.now()
+ if now < self._last_time:
+ rospy.logwarn(
+ "Detected jump back in time of {}s. Clearing TF buffer."
+ .format((self._last_time - now).to_sec()))
+ self._tf_buffer.clear()
+ if self._tracker:
+ max_distance = self._tracker.max_distance
+ del self._tracker
+ self._tracker = Sort(
+ tracker_class=KalmanPositionTracker,
+ distance_metric=euclidean_distances,
+ max_distance=max_distance)
+ self._last_time = now
+
+ if not self._detector.is_ready():
+ self._detector.set_laser_fov(
+ np.rad2deg(msg.angle_increment * len(msg.ranges)))
+
+ scan = np.array(msg.ranges)
+ scan[scan == 0.0] = 29.99
+ scan[np.isinf(scan)] = 29.99
+ scan[np.isnan(scan)] = 29.99
+
+ dets_xy, dets_cls, _, _ = self._detector(scan)
+
+ # confidence threshold
+ conf_mask = (dets_cls >= self.conf_thresh).reshape(-1)
+ dets_xy = dets_xy[conf_mask]
+ dets_cls = dets_cls[conf_mask]
+
+ if self.map_link is not None:
+ base_to_laser = self._lookup_transform(
+ self.map_link,
+ msg.header.frame_id,
+ msg.header.stamp)
+ if base_to_laser is None:
+ return
+ new_dets_xy = np.empty((len(dets_xy), 3), 'f')
+ for i, (x, y) in enumerate(dets_xy):
+ base_to_target = base_to_laser * PyKDL.Vector(x, y, 0.0)
+ new_dets_xy[i] = np.array(
+ [base_to_target.x(), base_to_target.y(),
+ base_to_target.z()])
+ dets_xy = new_dets_xy
+
+ if self._tracker:
+ self._tracker.update(
+ np.array([[dets_xy[i][0], dets_xy[i][1], 0.0]
+ for i in range(len(dets_xy))]))
+
+ tracks, track_ids = self._tracker.get_tracklets()
+
+ # convert to ros msg and publish
+ if self._dets_pub.get_num_connections() > 0:
+ dets_msg = detections_to_pose_array(
+ tracks, track_ids, self.n_previous)
+ dets_msg.header.stamp = msg.header.stamp
+ dets_msg.header.frame_id = frame_id
+ self._dets_pub.publish(dets_msg)
+
+ if self._rviz_pub.get_num_connections() > 0:
+ marker_array = MarkerArray()
+ marker_frame_id = self.map_link or msg.header.frame_id
+ tracks, track_ids = self._tracker.get_tracklets()
+ for track, track_id in zip(tracks, track_ids):
+ d_xy = track[-1]
+ color = colors[track_id % N]
+ color = list(color) + [self.color_alpha]
+ markers = make_human_marker(
+ pos=(d_xy[0], d_xy[1], 0.0),
+ head_radius=self.people_head_radius,
+ body_radius=self.people_body_radius,
+ height=self.people_height,
+ frame_id=marker_frame_id.lstrip('/'),
+ stamp=msg.header.stamp,
+ id=len(marker_array.markers),
+ color=color)
+ marker_array.markers.extend(markers)
+ self._rviz_pub.publish(marker_array)
+
+
+def detections_to_pose_array(tracks, track_ids, n_previous):
+ pose_array = PoseArray()
+ for track, track_id in zip(tracks, track_ids):
+ # Detector uses following frame convention:
+ # x forward, y rightward, z downward, phi is angle w.r.t. x-axis
+ if len(track) > 1:
+ p = Pose()
+ p.position.x = track[-1][0]
+ p.position.y = track[-1][1]
+ p.position.z = 0.0
+
+ tmp = np.diff(track[-n_previous:], axis=0)
+ tmp = np.mean(tmp, axis=0)
+ x_axis = np.array([tmp[0], tmp[1], 0.0], 'f')
+ z_axis = (0, 0, 1)
+ if np.linalg.norm(x_axis, ord=2) == 0:
+ # invalid x_axis. x_axis shoule not be 0 vector.
+ p.orientation.w = 1.0
+ else:
+ rotation = np.eye(4)
+ rotation[:3, :3] = rotation_matrix_from_axis(
+ x_axis, z_axis, axes='xz')
+ q_xyzw = matrix2quaternion(rotation)
+ p.orientation.x = q_xyzw[0]
+ p.orientation.y = q_xyzw[1]
+ p.orientation.z = q_xyzw[2]
+ p.orientation.w = q_xyzw[3]
+ else:
+ p = Pose()
+ p.position.x = track[0][0]
+ p.position.y = track[0][1]
+ p.position.z = 0.0
+ p.orientation.w = 1.0
+ pose_array.poses.append(p)
+
+ return pose_array
+
+
+if __name__ == '__main__':
+ rospy.init_node('lidar_person_detection_node')
+ node = LidarPersonDetectionNode() # NOQA
+ rospy.spin()
diff --git a/jsk_perception/node_scripts/paper_finder.py b/jsk_perception/node_scripts/paper_finder.py
index 421346b680..aa99d20290 100755
--- a/jsk_perception/node_scripts/paper_finder.py
+++ b/jsk_perception/node_scripts/paper_finder.py
@@ -7,6 +7,7 @@
from image_geometry.cameramodels import PinholeCameraModel
from jsk_recognition_msgs.msg import BoundingBox
from jsk_recognition_msgs.msg import BoundingBoxArray
+from jsk_recognition_utils.geometry import rotation_matrix_from_axis
from jsk_topic_tools import ConnectionBasedTransport
import message_filters
import numpy as np
@@ -14,36 +15,6 @@
import sensor_msgs.msg
import std_msgs.msg
from tf.transformations import quaternion_from_matrix
-from tf.transformations import unit_vector as normalize_vector
-
-
-def outer_product_matrix(v):
- return np.array([[0, -v[2], v[1]],
- [v[2], 0, -v[0]],
- [-v[1], v[0], 0]])
-
-
-def cross_product(a, b):
- return np.dot(outer_product_matrix(a), b)
-
-
-def rotation_matrix_from_axis(
- first_axis=(1, 0, 0), second_axis=(0, 1, 0), axes='xy'):
- if axes not in ['xy', 'yx', 'xz', 'zx', 'yz', 'zy']:
- raise ValueError("Valid axes are 'xy', 'yx', 'xz', 'zx', 'yz', 'zy'.")
- e1 = normalize_vector(first_axis)
- e2 = normalize_vector(second_axis - np.dot(second_axis, e1) * e1)
- if axes in ['xy', 'zx', 'yz']:
- third_axis = cross_product(e1, e2)
- else:
- third_axis = cross_product(e2, e1)
- e3 = normalize_vector(
- third_axis - np.dot(third_axis, e1) * e1 - np.dot(third_axis, e2) * e2)
- first_index = ord(axes[0]) - ord('x')
- second_index = ord(axes[1]) - ord('x')
- third_index = ((first_index + 1) ^ (second_index + 1)) - 1
- indices = [first_index, second_index, third_index]
- return np.vstack([e1, e2, e3])[np.argsort(indices)].T
def area(poly):
diff --git a/jsk_perception/sample/config/sample_lidar_person_detection.rviz b/jsk_perception/sample/config/sample_lidar_person_detection.rviz
new file mode 100644
index 0000000000..e6dea7c02d
--- /dev/null
+++ b/jsk_perception/sample/config/sample_lidar_person_detection.rviz
@@ -0,0 +1,386 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /Status1
+ - /TF1/Frames1
+ - /Insta1
+ Splitter Ratio: 0.5
+ Tree Height: 320
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: BaseScan
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/LaserScan
+ Color: 252; 233; 79
+ Color Transformer: FlatColor
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: BaseScan
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.05000000074505806
+ Style: Flat Squares
+ Topic: /base_scan
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: false
+ base_link:
+ Value: true
+ bellows_link:
+ Value: false
+ bellows_link2:
+ Value: false
+ camera_infra_frame:
+ Value: false
+ camera_infra_optical_frame:
+ Value: false
+ elbow_flex_link:
+ Value: false
+ eng2:
+ Value: false
+ eng2/1f:
+ Value: false
+ eng2/2f:
+ Value: false
+ eng2/3f:
+ Value: false
+ eng2/7f:
+ Value: false
+ eng2/7f/73B2:
+ Value: true
+ eng2/8f:
+ Value: false
+ estop_link:
+ Value: false
+ forearm_roll_link:
+ Value: false
+ gripper_link:
+ Value: false
+ head_camera_depth_frame:
+ Value: false
+ head_camera_depth_optical_frame:
+ Value: false
+ head_camera_link:
+ Value: false
+ head_camera_rgb_frame:
+ Value: false
+ head_camera_rgb_optical_frame:
+ Value: true
+ head_l515_mount_link:
+ Value: false
+ head_l515_virtual_mount_link:
+ Value: false
+ head_pan_link:
+ Value: false
+ head_tilt_link:
+ Value: false
+ insta360_link:
+ Value: false
+ l515_head_depth_frame:
+ Value: false
+ l515_head_depth_optical_frame:
+ Value: false
+ l515_head_link:
+ Value: false
+ l_gripper_finger_link:
+ Value: false
+ l_wheel_link:
+ Value: false
+ laser_link:
+ Value: true
+ map:
+ Value: false
+ odom:
+ Value: false
+ r_gripper_finger_link:
+ Value: false
+ r_wheel_link:
+ Value: false
+ respeaker_base:
+ Value: false
+ shoulder_lift_link:
+ Value: false
+ shoulder_pan_link:
+ Value: false
+ torso_fixed_link:
+ Value: false
+ torso_lift_link:
+ Value: false
+ upperarm_roll_link:
+ Value: false
+ world:
+ Value: false
+ wrist_flex_link:
+ Value: false
+ wrist_roll_link:
+ Value: false
+ Marker Alpha: 1
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ world:
+ eng2:
+ eng2/1f:
+ {}
+ eng2/2f:
+ {}
+ eng2/3f:
+ {}
+ eng2/7f:
+ eng2/7f/73B2:
+ {}
+ map:
+ odom:
+ base_link:
+ estop_link:
+ {}
+ l_wheel_link:
+ {}
+ laser_link:
+ {}
+ r_wheel_link:
+ {}
+ torso_fixed_link:
+ {}
+ torso_lift_link:
+ bellows_link:
+ {}
+ bellows_link2:
+ {}
+ head_pan_link:
+ head_tilt_link:
+ head_camera_link:
+ head_camera_depth_frame:
+ head_camera_depth_optical_frame:
+ {}
+ head_camera_rgb_frame:
+ head_camera_rgb_optical_frame:
+ {}
+ head_l515_virtual_mount_link:
+ head_l515_mount_link:
+ l515_head_link:
+ camera_infra_frame:
+ camera_infra_optical_frame:
+ {}
+ l515_head_depth_frame:
+ l515_head_depth_optical_frame:
+ {}
+ respeaker_base:
+ {}
+ insta360_link:
+ {}
+ shoulder_pan_link:
+ shoulder_lift_link:
+ upperarm_roll_link:
+ elbow_flex_link:
+ forearm_roll_link:
+ wrist_flex_link:
+ wrist_roll_link:
+ gripper_link:
+ l_gripper_finger_link:
+ {}
+ r_gripper_finger_link:
+ {}
+ eng2/8f:
+ {}
+ Update Interval: 0
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: RGB8
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: HeadCameraCloud
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Spheres
+ Topic: /head_camera/depth_registered/points
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Class: rviz/Image
+ Enabled: true
+ Image Topic: /insta360/dual_fisheye_to_panorama/output
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Insta
+ Normalize Range: true
+ Queue Size: 1
+ Transport Hint: raw
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Arrow Length: 0.30000001192092896
+ Axes Length: 0.30000001192092896
+ Axes Radius: 0.009999999776482582
+ Class: rviz/PoseArray
+ Color: 255; 25; 0
+ Enabled: true
+ Head Length: 0.07000000029802322
+ Head Radius: 0.029999999329447746
+ Name: PeoplePositions
+ Queue Size: 10
+ Shaft Length: 0.23000000417232513
+ Shaft Radius: 0.009999999776482582
+ Shape: Axes
+ Topic: /dr_spaam_lidar_person_detection/output
+ Unreliable: false
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /dr_spaam_lidar_person_detection/output/markers
+ Name: PeopleMarker
+ Namespaces:
+ "": true
+ Queue Size: 100
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: eng2/7f/73B2
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 11.670811653137207
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Field of View: 0.7853981852531433
+ Focal Point:
+ X: 0.5149004459381104
+ Y: -0.06546753644943237
+ Z: 0.6353773474693298
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.5153976678848267
+ Target Frame:
+ Yaw: 6.270411014556885
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1136
+ Hide Left Dock: false
+ Hide Right Dock: false
+ Insta:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd00000004000000000000027b000003d2fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001cb000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006e007300740061010000020e000002010000001600ffffff000000010000010f000003d2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003d2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004b7000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: false
+ Width: 1848
+ X: 72
+ Y: 27
diff --git a/jsk_perception/sample/include/play_rosbag_lidar_people.xml b/jsk_perception/sample/include/play_rosbag_lidar_people.xml
new file mode 100644
index 0000000000..d3e06c94dd
--- /dev/null
+++ b/jsk_perception/sample/include/play_rosbag_lidar_people.xml
@@ -0,0 +1,59 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/jsk_perception/sample/sample_lidar_person_detection.launch b/jsk_perception/sample/sample_lidar_person_detection.launch
new file mode 100644
index 0000000000..376ab93d81
--- /dev/null
+++ b/jsk_perception/sample/sample_lidar_person_detection.launch
@@ -0,0 +1,34 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ map_link: $(arg map_link)
+ weight_file: "$(find jsk_perception)/trained_data/lidar_person_detection/ckpt_jrdb_ann_dr_spaam_e20.pth"
+ detector_model: "DR-SPAAM"
+ conf_thresh: $(arg conf_thresh)
+ stride: $(arg stride)
+ panoramic_scan: $(arg panoramic_scan)
+ gpu: $(arg gpu)
+
+
+
+
+
+
+
+
+
diff --git a/jsk_perception/scripts/install_sample_data.py b/jsk_perception/scripts/install_sample_data.py
index e733d79e23..79f9886823 100755
--- a/jsk_perception/scripts/install_sample_data.py
+++ b/jsk_perception/scripts/install_sample_data.py
@@ -147,6 +147,14 @@ def main():
'sample/data/sample_mask_rcnn_73b2_kitchen_japanese_label.bag',
],
)
+ # lidar person detection
+ download_data(
+ pkg_name=PKG,
+ path='sample/data/2022-06-24-20-26-33-people_lidar_in_lab.bag',
+ url='https://drive.google.com/uc?id=16MRv5YqPm_pzlSMkn0C3nWQ9txaglBMw',
+ md5='36a1951e90c550cc537f4e07004d87e8',
+ extract=False,
+ )
download_data(
pkg_name=PKG,
diff --git a/jsk_perception/scripts/install_trained_data.py b/jsk_perception/scripts/install_trained_data.py
index eeb246102b..d3bef33293 100755
--- a/jsk_perception/scripts/install_trained_data.py
+++ b/jsk_perception/scripts/install_trained_data.py
@@ -316,7 +316,15 @@ def main():
path='trained_data/craft_refiner_CTW1500.pth',
url='https://drive.google.com/uc?id=1XSaFwBkOaFOdtk4Ane3DFyJGPRw6v5bO',
md5='3d48f83540567d2a43d2f6ca4b4d9f13',
- quiet=quiet,
+ quiet=quiet)
+
+ # lidar person detection
+ download_data(
+ pkg_name=PKG,
+ path='trained_data/lidar_person_detection/'
+ 'ckpt_jrdb_ann_dr_spaam_e20.pth',
+ url='https://drive.google.com/uc?id=1d1L2FlUY1kEhVJxobzkuIDRe4cxT9ijS',
+ md5='eed48a0f5ef8c215c49510a283fc1d0c',
)
diff --git a/jsk_perception/test/lidar_person_detection.test b/jsk_perception/test/lidar_person_detection.test
new file mode 100644
index 0000000000..d71705c38b
--- /dev/null
+++ b/jsk_perception/test/lidar_person_detection.test
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+ topic_0: /dr_spaam_lidar_person_detection/output
+ timeout_0: 60
+ topic_1: /dr_spaam_lidar_person_detection/output/markers
+ timeout_1: 60
+
+
+
+
diff --git a/jsk_recognition_utils/node_scripts/polygon_array_to_box_array.py b/jsk_recognition_utils/node_scripts/polygon_array_to_box_array.py
index 020529ad11..910bdc7d2a 100755
--- a/jsk_recognition_utils/node_scripts/polygon_array_to_box_array.py
+++ b/jsk_recognition_utils/node_scripts/polygon_array_to_box_array.py
@@ -9,21 +9,12 @@
import rospy
import shapely.geometry
from jsk_recognition_utils.cfg import PolygonArrayToBoxArrayConfig
+from jsk_recognition_utils.geometry import rotation_matrix_from_axis
from jsk_topic_tools import ConnectionBasedTransport
from tf.transformations import quaternion_from_matrix as matrix2quaternion
from tf.transformations import unit_vector as normalize_vector
-def outer_product_matrix(v):
- return np.array([[0, -v[2], v[1]],
- [v[2], 0, -v[0]],
- [-v[1], v[0], 0]])
-
-
-def cross_product(a, b):
- return np.dot(outer_product_matrix(a), b)
-
-
# minimum_rotated_rectangle is available since 1.6.0 (melodic)
# https://github.com/shapely/shapely/pull/361
from distutils.version import LooseVersion
@@ -66,24 +57,6 @@ def _transformed_rects():
return affine_transform(transf_rect, inv_matrix)
shapely.geometry.Polygon.minimum_rotated_rectangle = minimum_rotated_rectangle
-def rotation_matrix_from_axis(
- first_axis=(1, 0, 0), second_axis=(0, 1, 0), axes='xy'):
- if axes not in ['xy', 'yx', 'xz', 'zx', 'yz', 'zy']:
- raise ValueError("Valid axes are 'xy', 'yx', 'xz', 'zx', 'yz', 'zy'.")
- e1 = normalize_vector(first_axis)
- e2 = normalize_vector(second_axis - np.dot(second_axis, e1) * e1)
- if axes in ['xy', 'zx', 'yz']:
- third_axis = cross_product(e1, e2)
- else:
- third_axis = cross_product(e2, e1)
- e3 = normalize_vector(
- third_axis - np.dot(third_axis, e1) * e1 - np.dot(third_axis, e2) * e2)
- first_index = ord(axes[0]) - ord('x')
- second_index = ord(axes[1]) - ord('x')
- third_index = ((first_index + 1) ^ (second_index + 1)) - 1
- indices = [first_index, second_index, third_index]
- return np.vstack([e1, e2, e3])[np.argsort(indices)].T
-
def angle_between_vectors(v1, v2, normalize=True,
directed=True):
diff --git a/jsk_recognition_utils/package.xml b/jsk_recognition_utils/package.xml
index cf7d9e8ce7..72daf7919d 100644
--- a/jsk_recognition_utils/package.xml
+++ b/jsk_recognition_utils/package.xml
@@ -47,6 +47,7 @@
python-skimage
python3-skimage
python-fcn-pip
+ python-filterpy-pip
python-shapely
python3-shapely
sensor_msgs
diff --git a/jsk_recognition_utils/python/jsk_recognition_utils/geometry.py b/jsk_recognition_utils/python/jsk_recognition_utils/geometry.py
index cc4c142e62..50da2f7dcb 100644
--- a/jsk_recognition_utils/python/jsk_recognition_utils/geometry.py
+++ b/jsk_recognition_utils/python/jsk_recognition_utils/geometry.py
@@ -1,3 +1,45 @@
+import numpy as np
+from tf.transformations import unit_vector as normalize_vector
+
+
+def outer_product_matrix(v):
+ return np.array([[0, -v[2], v[1]],
+ [v[2], 0, -v[0]],
+ [-v[1], v[0], 0]])
+
+
+def cross_product(a, b):
+ return np.dot(outer_product_matrix(a), b)
+
+
+def rotation_matrix_from_axis(
+ first_axis=(1, 0, 0), second_axis=(0, 1, 0), axes='xy'):
+ if axes not in ['xy', 'yx', 'xz', 'zx', 'yz', 'zy']:
+ raise ValueError("Valid axes are 'xy', 'yx', 'xz', 'zx', 'yz', 'zy'.")
+ e1 = normalize_vector(first_axis)
+ if np.linalg.norm(e1, ord=2) == 0:
+ raise ValueError(
+ 'Invalid first_axis value. '
+ 'Norm of axis should be greater than 0.0. Get first_axis: ({})'.
+ format(first_axis))
+ e2 = normalize_vector(second_axis - np.dot(second_axis, e1) * e1)
+ if np.linalg.norm(e2, ord=2) == 0:
+ raise ValueError(
+ 'Invalid second_axis({}) with respect to first_axis({}).'
+ .format(first_axis, second_axis))
+ if axes in ['xy', 'zx', 'yz']:
+ third_axis = cross_product(e1, e2)
+ else:
+ third_axis = cross_product(e2, e1)
+ e3 = normalize_vector(
+ third_axis - np.dot(third_axis, e1) * e1 - np.dot(third_axis, e2) * e2)
+ first_index = ord(axes[0]) - ord('x')
+ second_index = ord(axes[1]) - ord('x')
+ third_index = ((first_index + 1) ^ (second_index + 1)) - 1
+ indices = [first_index, second_index, third_index]
+ return np.vstack([e1, e2, e3])[np.argsort(indices)].T
+
+
def get_overlap_of_aabb(box1, box2, return_volumes=False):
x11, y11, z11, x12, y12, z12 = box1
dim_x1 = x12 - x11
@@ -26,3 +68,36 @@ def get_overlap_of_aabb(box1, box2, return_volumes=False):
return iu, intersect, union
else:
return iu
+
+
+def pairwise_distances(a, b):
+ """Compute pair-wise distance between points in `a` and `b`.
+
+ Parameters
+ ----------
+ a : array_like
+ An NxM matrix of N samples of dimensionality M.
+ b : array_like
+ An LxM matrix of L samples of dimensionality M.
+
+ Returns
+ -------
+ ndarray
+ Returns a matrix of size len(a), len(b) such that eleement (i, j)
+ contains the distance between `a[i]` and `b[j]`.
+ """
+ a, b = np.asarray(a), np.asarray(b)
+ if len(a) == 0 or len(b) == 0:
+ return np.zeros((len(a), len(b)))
+ a2, b2 = np.square(a).sum(axis=1), np.square(b).sum(axis=1)
+ r2 = -2. * np.dot(a, b.T) + a2[:, None] + b2[None, :]
+ r2 = np.clip(r2, 0., float(np.inf))
+ return np.sqrt(r2)
+
+
+def euclidean_distances(a, b, a_indices=None, b_indices=None):
+ if a is None:
+ a_indices = np.arange(len(a))
+ if b is None:
+ b_indices = np.arange(len(b))
+ return pairwise_distances(a[a_indices], b[b_indices])
diff --git a/jsk_recognition_utils/python/jsk_recognition_utils/matching.py b/jsk_recognition_utils/python/jsk_recognition_utils/matching.py
new file mode 100644
index 0000000000..274efb94b6
--- /dev/null
+++ b/jsk_recognition_utils/python/jsk_recognition_utils/matching.py
@@ -0,0 +1,40 @@
+import numpy as np
+from scipy.optimize import linear_sum_assignment as linear_assignment
+
+
+def min_cost_matching(
+ distance_metric, max_distance, tracks, detections, track_indices=None,
+ detection_indices=None):
+ if track_indices is None:
+ track_indices = np.arange(len(tracks))
+ if detection_indices is None:
+ detection_indices = np.arange(len(detections))
+
+ if len(detection_indices) == 0 or len(track_indices) == 0:
+ return [], track_indices, detection_indices # Nothing to match.
+
+ cost_matrix = distance_metric(
+ tracks, detections, track_indices, detection_indices)
+ cost_matrix[cost_matrix > max_distance] = max_distance + 1e-5
+
+ matched_track_indices, matched_detection_indices = linear_assignment(
+ cost_matrix)
+ indices = np.array(list(zip(
+ matched_track_indices, matched_detection_indices)))
+
+ matches, unmatched_tracks, unmatched_detections = [], [], []
+ for col, detection_idx in enumerate(detection_indices):
+ if col not in matched_detection_indices:
+ unmatched_detections.append(detection_idx)
+ for row, track_idx in enumerate(track_indices):
+ if row not in matched_track_indices:
+ unmatched_tracks.append(track_idx)
+ for row, col in indices:
+ track_idx = track_indices[row]
+ detection_idx = detection_indices[col]
+ if cost_matrix[row, col] > max_distance:
+ unmatched_tracks.append(track_idx)
+ unmatched_detections.append(detection_idx)
+ else:
+ matches.append((detection_idx, track_idx))
+ return matches, unmatched_tracks, unmatched_detections
diff --git a/jsk_recognition_utils/python/jsk_recognition_utils/trackers/__init__.py b/jsk_recognition_utils/python/jsk_recognition_utils/trackers/__init__.py
new file mode 100644
index 0000000000..55ab69e1f7
--- /dev/null
+++ b/jsk_recognition_utils/python/jsk_recognition_utils/trackers/__init__.py
@@ -0,0 +1,3 @@
+# flake8: noqa
+
+from .sort_tracker import Sort
diff --git a/jsk_recognition_utils/python/jsk_recognition_utils/trackers/kalman_tracker.py b/jsk_recognition_utils/python/jsk_recognition_utils/trackers/kalman_tracker.py
new file mode 100644
index 0000000000..7e557cf6af
--- /dev/null
+++ b/jsk_recognition_utils/python/jsk_recognition_utils/trackers/kalman_tracker.py
@@ -0,0 +1,51 @@
+from filterpy.kalman import KalmanFilter
+import numpy as np
+
+
+class KalmanPositionTracker(object):
+
+ def __init__(self, xyz, track_id=0):
+ # define constant velocity model
+ # X,Y,Z, dX, dY, dZ
+ self.kf = KalmanFilter(dim_x=6, dim_z=3)
+ self.kf.F = np.array(
+ [[1, 0, 0, 1, 0, 0],
+ [0, 1, 0, 0, 1, 0],
+ [0, 0, 1, 0, 0, 1],
+ [0, 0, 0, 1, 0, 0],
+ [0, 0, 0, 0, 1, 0],
+ [0, 0, 0, 0, 0, 1]])
+ self.kf.H = np.array(
+ [[1, 0, 0, 0, 0, 0],
+ [0, 1, 0, 0, 0, 0],
+ [0, 0, 1, 0, 0, 0]])
+
+ self.kf.R[3:, 3:] *= 10.
+ # give high uncertainty to the unobservable initial velocities
+ self.kf.P[3:, 3:] *= 1000.0
+ self.kf.P *= 10.
+ self.kf.Q[3:, 3:] *= 0.01
+ self.kf.R *= 0.01
+
+ xyz = np.array(xyz, 'f')
+ self.kf.x[:3] = xyz.reshape(-1, 1)
+ self.time_since_update = 0
+ self.track_id = track_id
+ self.history = [xyz.reshape(-1, 1)]
+
+ def update(self, xyz):
+ xyz = np.array(xyz, 'f')
+ self.time_since_update = 0
+ self.history.append(xyz.reshape(-1, 1))
+ self.kf.update(xyz.squeeze())
+
+ def predict(self):
+ self.kf.predict()
+ self.time_since_update += 1
+ return self.kf.x[:3]
+
+ def get_state(self):
+ return self.kf.x
+
+ def get_history(self):
+ return self.history
diff --git a/jsk_recognition_utils/python/jsk_recognition_utils/trackers/sort_tracker.py b/jsk_recognition_utils/python/jsk_recognition_utils/trackers/sort_tracker.py
new file mode 100644
index 0000000000..a409e36fa9
--- /dev/null
+++ b/jsk_recognition_utils/python/jsk_recognition_utils/trackers/sort_tracker.py
@@ -0,0 +1,68 @@
+from __future__ import division
+from __future__ import print_function
+
+import numpy as np
+import rospy
+
+from jsk_recognition_utils.matching import min_cost_matching
+
+
+class Sort(object):
+
+ def __init__(self, tracker_class, distance_metric,
+ max_distance, max_age=1):
+ self.tracker_class = tracker_class
+ self.distance_metric = distance_metric
+ self.max_distance = max_distance
+ self.max_age = max_age
+ self.trackers = []
+ self.track_id = 0
+
+ def update(self, detections):
+ # get predicted locations from existing trackers.
+ tracks = []
+ to_del = []
+ for t, tracker in enumerate(self.trackers):
+ pos = tracker.predict()
+ tracks.append(np.array(pos).reshape(-1))
+ if np.any(np.isnan(pos)):
+ to_del.append(t)
+ if len(tracks) > 0:
+ tracks = np.ma.compress_rows(np.ma.masked_invalid(tracks))
+ for t in reversed(to_del):
+ self.trackers.pop(t)
+ matched, unmatched_tracks, unmatched_dets = min_cost_matching(
+ self.distance_metric, self.max_distance, tracks, detections)
+
+ rospy.logdebug("Sort's min_cost_matching results.")
+ rospy.logdebug('len(matched) = {}'.format(len(matched)))
+ rospy.logdebug('len(unmatched_dets) = {}'.format(len(unmatched_dets)))
+ rospy.logdebug('len(unmatched_tracks) = {}'.format(
+ len(unmatched_tracks)))
+
+ # update matched trackers with assigned detections
+ for matched_detection_id, matched_track_id in matched:
+ self.trackers[matched_track_id].update(
+ detections[matched_detection_id, :])
+
+ # create and initialise new trackers for unmatched detections
+ for i in unmatched_dets:
+ trk = self.tracker_class(detections[i, :], track_id=self.track_id)
+ self.track_id += 1
+ self.trackers.append(trk)
+ i = len(self.trackers)
+ for trk in reversed(self.trackers):
+ i -= 1
+ # remove dead tracklet
+ if trk.time_since_update > self.max_age:
+ self.trackers.pop(i)
+
+ def get_tracklets(self):
+ tracks, track_ids = [], []
+ for tracker in self.trackers:
+ if tracker.time_since_update > 1:
+ continue
+ tracks.append(
+ np.stack(tracker.history, axis=0))
+ track_ids.append(tracker.track_id)
+ return tracks, track_ids
diff --git a/jsk_recognition_utils/python/jsk_recognition_utils/visualization_marker/__init__.py b/jsk_recognition_utils/python/jsk_recognition_utils/visualization_marker/__init__.py
new file mode 100644
index 0000000000..dcc2dc18fa
--- /dev/null
+++ b/jsk_recognition_utils/python/jsk_recognition_utils/visualization_marker/__init__.py
@@ -0,0 +1,5 @@
+# flake8: noqa
+
+from .cylinder import make_cylinder
+from .sphere import make_sphere
+from .human_marker import make_human_marker
diff --git a/jsk_recognition_utils/python/jsk_recognition_utils/visualization_marker/cylinder.py b/jsk_recognition_utils/python/jsk_recognition_utils/visualization_marker/cylinder.py
new file mode 100644
index 0000000000..f39b2b8486
--- /dev/null
+++ b/jsk_recognition_utils/python/jsk_recognition_utils/visualization_marker/cylinder.py
@@ -0,0 +1,34 @@
+import rospy
+from std_msgs.msg import Header
+import visualization_msgs.msg
+from visualization_msgs.msg import Marker
+
+
+def make_cylinder(radius=0.1,
+ height=1.0,
+ pos=[0.0, 0.0, 0.0],
+ q_xyzw=[0.0, 0.0, 0.0, 1.0],
+ color=(0.0, 0.0, 0.0, 1.0),
+ lifetime=0.25,
+ id=0,
+ frame_id='',
+ stamp=None):
+ header = Header()
+ header.frame_id = frame_id
+ header.stamp = stamp
+
+ mesh_marker = Marker(type=Marker.CYLINDER, header=header, id=id)
+ mesh_marker.pose.position.x = pos[0]
+ mesh_marker.pose.position.y = pos[1]
+ mesh_marker.pose.position.z = pos[2]
+ mesh_marker.pose.orientation.x = q_xyzw[0]
+ mesh_marker.pose.orientation.y = q_xyzw[1]
+ mesh_marker.pose.orientation.z = q_xyzw[2]
+ mesh_marker.pose.orientation.w = q_xyzw[3]
+ mesh_marker.scale.x = radius
+ mesh_marker.scale.y = radius
+ mesh_marker.scale.z = height
+ mesh_marker.color.b, mesh_marker.color.g, mesh_marker.color.r, \
+ mesh_marker.color.a = color
+ mesh_marker.lifetime = rospy.Duration(lifetime)
+ return mesh_marker
diff --git a/jsk_recognition_utils/python/jsk_recognition_utils/visualization_marker/human_marker.py b/jsk_recognition_utils/python/jsk_recognition_utils/visualization_marker/human_marker.py
new file mode 100644
index 0000000000..def7275fce
--- /dev/null
+++ b/jsk_recognition_utils/python/jsk_recognition_utils/visualization_marker/human_marker.py
@@ -0,0 +1,50 @@
+import numpy as np
+from tf.transformations import quaternion_matrix as quaternion2matrix
+
+from .cylinder import make_cylinder
+from .sphere import make_sphere
+
+
+def make_human_marker(head_radius=0.1,
+ body_radius=0.07,
+ height=1.8,
+ pos=[0.0, 0.0, 0.0],
+ q_xyzw=[0.0, 0.0, 0.0, 1.0],
+ color=(0.0, 0.0, 0.0, 1.0),
+ lifetime=0.25,
+ id=0,
+ frame_id='',
+ stamp=None):
+ if height <= 0.0:
+ raise ValueError('height should be greater than 0.0.')
+ if head_radius <= 0.0:
+ raise ValueError('head_radius should be greater than 0.0.')
+ if height <= head_radius:
+ raise ValueError('head_radius ({}) should be greater than height ({})'.
+ format(head_radius, height))
+
+ body_height = height - head_radius * 2
+ pos = np.array(pos, 'f')
+ rotation = quaternion2matrix(q_xyzw)[:3, :3]
+ head_pos = np.dot(rotation, [0, 0, height - head_radius]) + pos
+ body_pos = np.dot(rotation, [0, 0, body_height / 2.0]) + pos
+
+ head_marker = make_sphere(radius=head_radius,
+ pos=head_pos,
+ q_xyzw=q_xyzw,
+ color=color,
+ lifetime=lifetime,
+ id=id,
+ frame_id=frame_id,
+ stamp=stamp)
+ body_marker = make_cylinder(
+ radius=body_radius,
+ height=body_height,
+ pos=body_pos,
+ q_xyzw=q_xyzw,
+ color=color,
+ lifetime=lifetime,
+ id=id + 1,
+ frame_id=frame_id,
+ stamp=stamp)
+ return [head_marker, body_marker]
diff --git a/jsk_recognition_utils/python/jsk_recognition_utils/visualization_marker/sphere.py b/jsk_recognition_utils/python/jsk_recognition_utils/visualization_marker/sphere.py
new file mode 100644
index 0000000000..01fe674fbe
--- /dev/null
+++ b/jsk_recognition_utils/python/jsk_recognition_utils/visualization_marker/sphere.py
@@ -0,0 +1,34 @@
+import rospy
+from std_msgs.msg import Header
+import visualization_msgs.msg
+from visualization_msgs.msg import Marker
+
+
+def make_sphere(radius=0.1,
+ pos=[0.0, 0.0, 0.0],
+ q_xyzw=[0.0, 0.0, 0.0, 1.0],
+ color=(0.0, 0.0, 0.0, 1.0),
+ lifetime=0.25,
+ id=0,
+ frame_id='',
+ stamp=None):
+ header = Header()
+ header.frame_id = frame_id
+ header.stamp = stamp
+
+ mesh_marker = Marker(type=Marker.SPHERE, header=header, id=id)
+ mesh_marker.pose.position.x = pos[0]
+ mesh_marker.pose.position.y = pos[1]
+ mesh_marker.pose.position.z = pos[2]
+ mesh_marker.pose.orientation.x = q_xyzw[0]
+ mesh_marker.pose.orientation.y = q_xyzw[1]
+ mesh_marker.pose.orientation.z = q_xyzw[2]
+ mesh_marker.pose.orientation.w = q_xyzw[3]
+ mesh_marker.scale.x = radius
+ mesh_marker.scale.y = radius
+ mesh_marker.scale.z = radius
+ mesh_marker.color.b, mesh_marker.color.g, mesh_marker.color.r, \
+ mesh_marker.color.a = color
+ mesh_marker.lifetime = lifetime
+ mesh_marker.lifetime = rospy.Duration(lifetime)
+ return mesh_marker