def inference_time():
seq_name = './data/DROWv2-data/test/run_t_2015-11-26-11-55-45.bag.csv'
scans = np.genfromtxt(seq_name, delimiter=',')[:, 2:]
# inference time
use_gpu = True
model_names = ("DR-SPAAM", "DROW", "DROW-T5")
ckpts = ("./ckpts/dr_spaam_e40.pth", "./ckpts/drow_e40.pth",
"./ckpts/drow5_e40.pth")
for model_name, ckpt in zip(model_names, ckpts):
detector = Detector(model_name=model_name,
ckpt_file=ckpt,
gpu=use_gpu,
stride=1)
detector.set_laser_spec(angle_inc=np.radians(0.5), num_pts=450)
t_list = []
for i in range(60):
s = scans[i:i + 5] if model_name == "DROW-T5" else scans[i]
t0 = time.time()
dets_xy, dets_cls, instance_mask = detector(s)
t_list.append(1e3 * (time.time() - t0))
t = np.array(t_list[10:]).mean()
print("inference time (model: %s, gpu: %s): %f ms (%.1f FPS)" %
(model_name, use_gpu, t, 1e3 / t))
def __init__(self):
self._read_params()
self._detector = Detector(model_name="DR-SPAAM",
ckpt_file=self.weight_file,
gpu=True,
stride=self.stride)
self._init()
def __init__(self):
self._read_params()
self._detector = Detector(
self.weight_file,
model=self.detector_model,
gpu=True,
stride=self.stride,
panoramic_scan=self.panoramic_scan,
)
self._init()
def __init__(self):
self._read_params()
self._detector = Detector(
self.weight_file,
model=self.detector_model,
gpu=torch.cuda.is_available(),
stride=self.stride,
panoramic_scan=self.panoramic_scan,
)
self._init()
class DrSpaamDetector:
def __init__(self,
num_pts,
ang_inc_degree=0.5,
tracking=False,
gpu=True,
ckpt=default_ckpts):
# Detector class wraps up preprocessing, inference, and postprocessing for DR-SPAAM.
# Checkout the comment in the code for meanings of the parameters.
if not os.path.exists(ckpt):
raise ValueError("the checkpoint file for DR-SPAAM is not valid")
self.detector = Detector(model_name="DR-SPAAM",
ckpt_file=ckpt,
gpu=gpu,
stride=1,
tracking=tracking)
# set angular grid (this is only required once)
ang_inc = np.radians(
ang_inc_degree) # angular increment of the scanner
# num_pts = 450 # number of points in a scan
self.detector.set_laser_spec(ang_inc, num_pts)
def detect(self, scan):
"""
:param scan: 1D numpy array with positive values
:return:
"""
dets_xy, dets_cls, instance_mask = self.detector(scan) # get detection
if len(dets_xy):
dets_xy = np.stack([dets_xy[:, 1], dets_xy[:, 0]]).T
# confidence threshold
cls_thresh = 0.2
cls_mask = dets_cls.squeeze() > cls_thresh
dets_xy = dets_xy[cls_mask]
dets_cls = dets_cls[cls_mask]
return dets_xy, dets_cls
def get_tracklets(self):
tracklets = self.detector.get_tracklets()
return [[np.stack(tr)[:, ::-1] for tr in tracklets[0]], tracklets[1]]
def __init__(self,
num_pts,
ang_inc_degree=0.5,
tracking=False,
gpu=True,
ckpt=default_ckpts):
# Detector class wraps up preprocessing, inference, and postprocessing for DR-SPAAM.
# Checkout the comment in the code for meanings of the parameters.
if not os.path.exists(ckpt):
raise ValueError("the checkpoint file for DR-SPAAM is not valid")
self.detector = Detector(model_name="DR-SPAAM",
ckpt_file=ckpt,
gpu=gpu,
stride=1,
tracking=tracking)
# set angular grid (this is only required once)
ang_inc = np.radians(
ang_inc_degree) # angular increment of the scanner
# num_pts = 450 # number of points in a scan
self.detector.set_laser_spec(ang_inc, num_pts)
def test_inference_speed_on_jrdb():
data_handle = JRDBHandle(
split="train",
cfg={
"data_dir": "./data/JRDB",
"num_scans": 1,
"scan_stride": 1
},
)
ckpt_file = "./ckpts/ckpt_jrdb_ann_drow3_e40.pth"
detector_drow3 = Detector(ckpt_file,
model="DROW3",
gpu=True,
stride=_STRIDE,
panoramic_scan=True)
detector_drow3.set_laser_fov(360)
ckpt_file = "./ckpts/ckpt_jrdb_ann_dr_spaam_e20.pth"
detector_dr_spaam = Detector(ckpt_file,
model="DR-SPAAM",
gpu=True,
stride=_STRIDE,
panoramic_scan=True)
detector_dr_spaam.set_laser_fov(360)
frame_inds = np.random.randint(0, len(data_handle), size=(_FRAME_NUM, ))
# sample random frames, discard beginning frames, where PyTorch is searching
# for optimal algorithm
frame_inds = np.random.randint(0,
len(data_handle),
size=(_FRAME_NUM + 20, ))
for n, detector in zip(["DROW3", "DR-SPAAM"],
[detector_drow3, detector_dr_spaam]):
t_list = []
for frame_idx in frame_inds:
data_dict = data_handle[frame_idx]
scan_r = data_dict["laser_data"][-1, ::-1] # to DROW frame
t0 = time.time()
dets_xy, dets_cls, _ = detector(scan_r)
t_list.append(time.time() - t0)
t_ave = np.array(t_list[20:]).mean()
print(f"{n} on JRDB: {1.0 / t_ave:.1f} FPS "
f"({t_ave:.6f} seconds per frame)")
def play_sequence():
# scans
seq_name = './data/DROWv2-data/test/run_t_2015-11-26-11-22-03.bag.csv'
# seq_name = './data/DROWv2-data/val/run_2015-11-26-15-52-55-k.bag.csv'
scans_data = np.genfromtxt(seq_name, delimiter=',')
scans_t = scans_data[:, 1]
scans = scans_data[:, 2:]
scan_phi = u.get_laser_phi()
# odometry, used only for plotting
odo_name = seq_name[:-3] + 'odom2'
odos = np.genfromtxt(odo_name, delimiter=',')
odos_t = odos[:, 1]
odos_phi = odos[:, 4]
# detector
ckpt = './ckpts/dr_spaam_e40.pth'
detector = Detector(model_name="DR-SPAAM",
ckpt_file=ckpt,
gpu=True,
stride=1)
detector.set_laser_spec(angle_inc=np.radians(0.5), num_pts=450)
# scanner location
rad_tmp = 0.5 * np.ones(len(scan_phi), dtype=np.float)
xy_scanner = u.rphi_to_xy(rad_tmp, scan_phi)
xy_scanner = np.stack(xy_scanner, axis=1)
# plot
fig = plt.figure(figsize=(10, 10))
ax = fig.add_subplot(111)
_break = False
def p(event):
nonlocal _break
_break = True
fig.canvas.mpl_connect('key_press_event', p)
# video sequence
odo_idx = 0
for i in range(len(scans)):
# for i in range(0, len(scans), 20):
plt.cla()
ax.set_aspect('equal')
ax.set_xlim(-15, 15)
ax.set_ylim(-15, 15)
# ax.set_title('Frame: %s' % i)
ax.set_title('Press any key to exit.')
ax.axis("off")
# find matching odometry
while odo_idx < len(odos_t) - 1 and odos_t[odo_idx] < scans_t[i]:
odo_idx += 1
odo_phi = odos_phi[odo_idx]
odo_rot = np.array(
[[np.cos(odo_phi), np.sin(odo_phi)],
[-np.sin(odo_phi), np.cos(odo_phi)]],
dtype=np.float32)
# plot scanner location
xy_scanner_rot = np.matmul(xy_scanner, odo_rot.T)
ax.plot(xy_scanner_rot[:, 0], xy_scanner_rot[:, 1], c='black')
ax.plot((0, xy_scanner_rot[0, 0] * 1.0),
(0, xy_scanner_rot[0, 1] * 1.0),
c='black')
ax.plot((0, xy_scanner_rot[-1, 0] * 1.0),
(0, xy_scanner_rot[-1, 1] * 1.0),
c='black')
# plot points
scan = scans[i]
scan_x, scan_y = u.rphi_to_xy(scan, scan_phi + odo_phi)
ax.scatter(scan_x, scan_y, s=1, c='blue')
# inference
dets_xy, dets_cls, instance_mask = detector(scan)
# plot detection
dets_xy_rot = np.matmul(dets_xy, odo_rot.T)
cls_thresh = 0.5
for j in range(len(dets_xy)):
if dets_cls[j] < cls_thresh:
continue
# c = plt.Circle(dets_xy_rot[j], radius=0.5, color='r', fill=False)
c = plt.Circle(dets_xy_rot[j],
radius=0.5,
color='r',
fill=False,
linewidth=2)
ax.add_artist(c)
# plt.savefig('/home/dan/tmp/det_img/frame_%04d.png' % i)
plt.pause(0.001)
if _break:
break
def play_sequence_with_tracking():
# scans
seq_name = './data/DROWv2-data/train/lunch_2015-11-26-12-04-23.bag.csv'
seq0, seq1 = 109170, 109360
scans, scans_t = [], []
with open(seq_name) as f:
for line in f:
scan_seq, scan_t, scan = line.split(",", 2)
scan_seq = int(scan_seq)
if scan_seq < seq0:
continue
scans.append(np.fromstring(scan, sep=','))
scans_t.append(float(scan_t))
if scan_seq > seq1:
break
scans = np.stack(scans, axis=0)
scans_t = np.array(scans_t)
scan_phi = u.get_laser_phi()
# odometry, used only for plotting
odo_name = seq_name[:-3] + 'odom2'
odos = np.genfromtxt(odo_name, delimiter=',')
odos_t = odos[:, 1]
odos_phi = odos[:, 4]
# detector
ckpt = './ckpts/dr_spaam_e40.pth'
detector = Detector(model_name="DR-SPAAM",
ckpt_file=ckpt,
gpu=True,
stride=1,
tracking=True)
detector.set_laser_spec(angle_inc=np.radians(0.5), num_pts=450)
# scanner location
rad_tmp = 0.5 * np.ones(len(scan_phi), dtype=np.float)
xy_scanner = u.rphi_to_xy(rad_tmp, scan_phi)
xy_scanner = np.stack(xy_scanner, axis=1)
# plot
fig = plt.figure(figsize=(6, 8))
ax = fig.add_subplot(111)
_break = False
def p(event):
nonlocal _break
_break = True
fig.canvas.mpl_connect('key_press_event', p)
# video sequence
odo_idx = 0
for i in range(len(scans)):
plt.cla()
ax.set_aspect('equal')
ax.set_xlim(-10, 5)
ax.set_ylim(-5, 15)
# ax.set_title('Frame: %s' % i)
ax.set_title('Press any key to exit.')
ax.axis("off")
# find matching odometry
while odo_idx < len(odos_t) - 1 and odos_t[odo_idx] < scans_t[i]:
odo_idx += 1
odo_phi = odos_phi[odo_idx]
odo_rot = np.array(
[[np.cos(odo_phi), np.sin(odo_phi)],
[-np.sin(odo_phi), np.cos(odo_phi)]],
dtype=np.float32)
# plot scanner location
xy_scanner_rot = np.matmul(xy_scanner, odo_rot.T)
ax.plot(xy_scanner_rot[:, 0], xy_scanner_rot[:, 1], c='black')
ax.plot((0, xy_scanner_rot[0, 0] * 1.0),
(0, xy_scanner_rot[0, 1] * 1.0),
c='black')
ax.plot((0, xy_scanner_rot[-1, 0] * 1.0),
(0, xy_scanner_rot[-1, 1] * 1.0),
c='black')
# plot points
scan = scans[i]
scan_x, scan_y = u.rphi_to_xy(scan, scan_phi + odo_phi)
ax.scatter(scan_x, scan_y, s=1, c='blue')
# inference
dets_xy, dets_cls, instance_mask = detector(scan)
# plot detection
dets_xy_rot = np.matmul(dets_xy, odo_rot.T)
cls_thresh = 0.3
for j in range(len(dets_xy)):
if dets_cls[j] < cls_thresh:
continue
c = plt.Circle(dets_xy_rot[j],
radius=0.5,
color='r',
fill=False,
linewidth=2)
ax.add_artist(c)
# plot track
cls_thresh = 0.2
tracks, tracks_cls = detector.get_tracklets()
for t, tc in zip(tracks, tracks_cls):
if tc >= cls_thresh and len(t) > 1:
t_rot = np.matmul(t, odo_rot.T)
ax.plot(t_rot[:, 0], t_rot[:, 1], color='g', linewidth=2)
# plt.savefig('/home/dan/tmp/track3_img/frame_%04d.png' % i)
plt.pause(0.001)
if _break:
break
class DrSpaamROS:
"""ROS node to detect pedestrian using DROW3 or DR-SPAAM."""
def __init__(self):
self._read_params()
self._detector = Detector(
self.weight_file,
model=self.detector_model,
gpu=True,
stride=self.stride,
panoramic_scan=self.panoramic_scan,
)
self._init()
def _read_params(self):
"""
@brief Reads parameters from ROS server.
"""
self.weight_file = rospy.get_param("~weight_file")
self.conf_thresh = rospy.get_param("~conf_thresh")
self.stride = rospy.get_param("~stride")
self.detector_model = rospy.get_param("~detector_model")
self.panoramic_scan = rospy.get_param("~panoramic_scan")
def _init(self):
"""
@brief Initialize ROS connection.
"""
# Publisher
topic, queue_size, latch = read_publisher_param("detections")
self._dets_pub = rospy.Publisher(topic,
PoseArray,
queue_size=queue_size,
latch=latch)
topic, queue_size, latch = read_publisher_param("rviz")
self._rviz_pub = rospy.Publisher(topic,
Marker,
queue_size=queue_size,
latch=latch)
# Subscriber
topic, queue_size = read_subscriber_param("scan")
self._scan_sub = rospy.Subscriber(topic,
LaserScan,
self._scan_callback,
queue_size=queue_size)
def _scan_callback(self, msg):
if (self._dets_pub.get_num_connections() == 0
and self._rviz_pub.get_num_connections() == 0):
return
# TODO check the computation here
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
# t = time.time()
dets_xy, dets_cls, _ = self._detector(scan)
# print("[DrSpaamROS] End-to-end inference time: %f" % (t - time.time()))
# confidence threshold
conf_mask = (dets_cls >= self.conf_thresh).reshape(-1)
dets_xy = dets_xy[conf_mask]
dets_cls = dets_cls[conf_mask]
# convert to ros msg and publish
dets_msg = detections_to_pose_array(dets_xy, dets_cls)
dets_msg.header = msg.header
self._dets_pub.publish(dets_msg)
rviz_msg = detections_to_rviz_marker(dets_xy, dets_cls)
rviz_msg.header = msg.header
self._rviz_pub.publish(rviz_msg)
class DrSpaamROS():
"""ROS node to detect pedestrian using DR-SPAAM."""
def __init__(self):
self._read_params()
self._detector = Detector(model_name="DR-SPAAM",
ckpt_file=self.weight_file,
gpu=True,
stride=self.stride)
self._init()
def _read_params(self):
"""
@brief Reads parameters from ROS server.
"""
self.weight_file = rospy.get_param("~weight_file")
self.conf_thresh = rospy.get_param("~conf_thresh")
self.stride = rospy.get_param("~stride")
def _init(self):
"""
@brief Initialize ROS connection.
"""
# Publisher
topic, queue_size, latch = read_publisher_param("detections")
self._dets_pub = rospy.Publisher(topic,
PoseArray,
queue_size=queue_size,
latch=latch)
topic, queue_size, latch = read_publisher_param("rviz")
self._rviz_pub = rospy.Publisher(topic,
Marker,
queue_size=queue_size,
latch=latch)
# Subscriber
topic, queue_size = read_subscriber_param("scan")
self._scan_sub = rospy.Subscriber(topic,
LaserScan,
self._scan_callback,
queue_size=queue_size)
def _scan_callback(self, msg):
if self._dets_pub.get_num_connections() == 0:
return
if not self._detector.laser_spec_set():
self._detector.set_laser_spec(angle_inc=msg.angle_increment,
num_pts=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
# t = time.time()
dets_xy, dets_cls, _ = self._detector(scan)
# print("[DrSpaamROS] End-to-end inference time: %f" % (t - time.time()))
# confidence threshold
conf_mask = (dets_cls >= self.conf_thresh).reshape(-1)
# if not np.sum(conf_mask) > 0:
# return
dets_xy = dets_xy[conf_mask]
dets_cls = dets_cls[conf_mask]
# convert and publish ros msg
dets_msg = detections_to_pose_array(dets_xy, dets_cls)
dets_msg.header = msg.header
self._dets_pub.publish(dets_msg)
rviz_msg = detections_to_rviz_marker(dets_xy, dets_cls)
rviz_msg.header = msg.header
self._rviz_pub.publish(rviz_msg)
def test_detector():
data_handle = JRDBHandle(
split="train",
cfg={
"data_dir": "./data/JRDB",
"num_scans": 10,
"scan_stride": 1
},
)
# ckpt_file = "/home/jia/ckpts/ckpt_jrdb_ann_drow3_e40.pth"
# d = Detector(
# ckpt_file, model="DROW3", gpu=True, stride=1, panoramic_scan=True
# )
ckpt_file = "/home/jia/ckpts/ckpt_jrdb_ann_dr_spaam_e20.pth"
d = Detector(ckpt_file,
model="DR-SPAAM",
gpu=True,
stride=1,
panoramic_scan=True)
d.set_laser_fov(360)
fig = plt.figure(figsize=(10, 10))
ax = fig.add_subplot(111)
_break = False
if _INTERACTIVE:
def p(event):
nonlocal _break
_break = True
fig.canvas.mpl_connect("key_press_event", p)
else:
if os.path.exists(_SAVE_DIR):
shutil.rmtree(_SAVE_DIR)
os.makedirs(_SAVE_DIR)
for i, data_dict in enumerate(data_handle):
if _break:
break
# plot scans
scan_r = data_dict["laser_data"][-1, ::-1] # to DROW frame
scan_x, scan_y = u.rphi_to_xy(scan_r, data_dict["laser_grid"])
plt.cla()
ax.set_aspect("equal")
ax.set_xlim(_X_LIM[0], _X_LIM[1])
ax.set_ylim(_Y_LIM[0], _Y_LIM[1])
ax.set_xlabel("x [m]")
ax.set_ylabel("y [m]")
ax.set_title(f"Frame {data_dict['idx']}. Press any key to exit.")
# ax.axis("off")
ax.scatter(scan_x, scan_y, s=1, c="black")
# plot annotation
ann_xyz = [(ann["box"]["cx"], ann["box"]["cy"], ann["box"]["cz"])
for ann in data_dict["pc_anns"]]
if len(ann_xyz) > 0:
ann_xyz = np.array(ann_xyz, dtype=np.float32).T
ann_xyz = jt.transform_pts_base_to_laser(ann_xyz)
ann_xyz[1] = -ann_xyz[1] # to DROW frame
for xyz in ann_xyz.T:
c = plt.Circle(
(xyz[0], xyz[1]),
radius=0.4,
color="red",
fill=False,
linestyle="--",
)
ax.add_artist(c)
# plot detection
dets_xy, dets_cls, _ = d(scan_r)
dets_cls_norm = np.clip(dets_cls, 0, 0.3) / 0.3
for xy, cls_norm in zip(dets_xy, dets_cls_norm):
color = (1.0 - cls_norm, 1.0, 1.0 - cls_norm)
c = plt.Circle((xy[0], xy[1]),
radius=0.4,
color=color,
fill=False,
linestyle="-")
ax.add_artist(c)
if _INTERACTIVE:
plt.pause(0.1)
else:
plt.savefig(
os.path.join(_SAVE_DIR, f"frame_{data_dict['idx']:04}.png"))
if _INTERACTIVE:
plt.show()