def calibration_data(self, idx):
# TODO: add motion compensation, ref: https://github.com/waymo-research/waymo-open-dataset/issues/146
# https://github.com/waymo-research/waymo-open-dataset/issues/79
if isinstance(idx, int):
seq_id, _ = self._locate_frame(idx)
else:
seq_id, _ = idx
assert not self._return_file_path, "The calibration data is not in a single file!"
calib_params = TransformSet("vehicle")
# load json files
fname_cams = "context/calib_cams.json"
fname_lidars = "context/calib_lidars.json"
if self.inzip:
with PatchedZipFile(self.base_path / (seq_id + ".zip"),
to_extract=[fname_cams, fname_lidars]) as ar:
calib_cams = json.loads(ar.read(fname_cams))
calib_lidars = json.loads(ar.read(fname_lidars))
else:
with (self.base_path / seq_id / fname_cams).open() as fin:
calib_cams = json.load(fin)
with (self.base_path / seq_id / fname_lidars).open() as fin:
calib_lidars = json.load(fin)
# parse camera calibration
for frame, calib in calib_cams.items():
frame = "camera_" + frame
(fu, fv, cu,
cv), distort = calib['intrinsic'][:4], calib['intrinsic'][4:]
transform = np.array(calib['extrinsic']).reshape(4, 4)
size = (calib['width'], calib['height'])
calib_params.set_intrinsic_pinhole(frame,
size,
cu,
cv,
fu,
fv,
distort_coeffs=distort)
calib_params.set_extrinsic(transform, frame_from=frame)
# parse lidar calibration
for frame, calib in calib_lidars.items():
frame = "lidar_" + frame
calib_params.set_intrinsic_lidar(frame)
transform = np.array(calib['extrinsic']).reshape(4, 4)
calib_params.set_extrinsic(transform, frame_from=frame)
return calib_params
def visualize_detections(builder: XVIZBuilder,
visualizer_frame: str,
targets: Target3DArray,
calib: TransformSet,
stream_prefix: str,
id_prefix="",
tags=None,
text_offset=None):
'''
Add detection results to xviz builder
'''
# change frame to the same
if targets.frame != visualizer_frame:
targets = calib.transform_objects(targets, frame_to=visualizer_frame)
stream_prefix = stream_prefix.rstrip("/")
for box in targets:
vertices = box.corners[[0, 1, 3, 2, 0]]
builder.primitive(stream_prefix + "/objects")\
.polygon(vertices.tolist())\
.id(box.tid64)\
.style({"height": box.dimension[2]})\
.classes([box.tag.mapping(t).name for t in box.tag.labels])
builder.primitive(stream_prefix + "/label")\
.text("#" + box.tid64)\
.position(box.position if text_offset is None else box.position + text_offset)
builder.primitive(stream_prefix + "/tracking_point")\
.circle(box.position, 0.2)\
.id(box.tid64)
def visualize_detections_bev(ax: axes.Axes,
visualizer_frame: str,
targets: Target3DArray,
calib: TransformSet,
box_color=(0, 1, 0),
thickness=2,
tags=None):
# change frame to the same
if targets.frame != visualizer_frame:
targets = calib.transform_objects(targets, frame_to=visualizer_frame)
for target in targets.filter_tag(tags):
# draw vehicle frames
points = target.corners
pairs = [(0, 1), (2, 3), (0, 2), (1, 3)]
for i, j in pairs:
ax.add_line(
lines.Line2D((points[i, 0], points[j, 0]),
(points[i, 1], points[j, 1]),
c=box_color,
lw=thickness))
# draw velocity
if isinstance(target, TrackingTarget3D):
pstart = target.position[:2]
pend = target.position[:2] + target.velocity[:2]
ax.add_line(
lines.Line2D((pstart[0], pend[0]), (pstart[1], pend[1]),
c=box_color,
lw=thickness))
def calibration_data(self, idx):
seq_id, _ = idx
assert not self._return_file_path, "The calibration is not in a single file!"
calib_params = TransformSet("ego")
calib_fname = "scene/calib.json"
if self.inzip:
with PatchedZipFile(self.base_path / (seq_id + ".zip"),
to_extract=calib_fname) as ar:
calib_data = json.loads(ar.read(calib_fname))
else:
with Path(self.base_path, seq_id, calib_fname).open() as fin:
calib_data = json.load(fin)
for frame, calib in calib_data.items():
# set intrinsics
if frame.startswith('cam'):
image_size = (1600, 900
) # currently all the images have the same size
projection = np.array(calib['camera_intrinsic'])
calib_params.set_intrinsic_camera(frame,
projection,
image_size,
rotate=False)
elif frame.startswith('lidar'):
calib_params.set_intrinsic_lidar(frame)
elif frame.startswith('radar'):
calib_params.set_intrinsic_radar(frame)
else:
raise ValueError("Unrecognized frame name.")
# set extrinsics
r = Rotation.from_quat(calib['rotation'][1:] +
[calib['rotation'][0]])
t = np.array(calib['translation'])
extri = np.eye(4)
extri[:3, :3] = r.as_matrix()
extri[:3, 3] = t
calib_params.set_extrinsic(extri, frame_from=frame)
return calib_params
def _load_calib(self, basepath, uidx, raw=False):
# load the calibration file
fname = Path(self.phase_path, 'calib', '%06d.txt' % uidx)
filedata = utils.load_calib_file(basepath, fname)
if raw:
return filedata
# load image size, which could take additional time
if uidx not in self._image_size_cache:
self.camera_data((uidx, )) # fill image size cache
image_size = self._image_size_cache[uidx]
# load matrics
data = TransformSet("velo")
rect = filedata['R0_rect'].reshape(3, 3)
velo_to_cam = filedata['Tr_velo_to_cam'].reshape(3, 4)
for i in range(4):
P = filedata['P%d' % i].reshape(3, 4)
intri, offset = P[:, :3], P[:, 3]
projection = intri.dot(rect)
offset_cartesian = np.linalg.inv(projection).dot(offset)
extri = np.vstack([velo_to_cam, np.array([0, 0, 0, 1])])
extri[:3, 3] += offset_cartesian
frame = "cam%d" % i
data.set_intrinsic_camera(frame,
projection,
image_size,
rotate=False)
data.set_extrinsic(extri, frame_to=frame)
data.set_intrinsic_general("imu")
data.set_extrinsic(filedata['Tr_imu_to_velo'].reshape(3, 4),
frame_from="imu")
return data
def visualize_detections(ax: axes.Axes,
image_frame: str,
targets: Target3DArray,
calib: TransformSet,
box_color=(0, 1, 0),
thickness=2,
tags=None):
'''
Draw detected object on matplotlib canvas
'''
for target in targets.filter_tag(tags):
# add points for direction indicator
points = target.corners
indicator = np.array(
[[0, 0, -target.dimension[2] / 2],
[target.dimension[0] / 2, 0,
-target.dimension[2] / 2]]).dot(target.orientation.as_matrix().T)
points = np.vstack([points, target.position + indicator])
# project points
uv, mask, dmask = calib.project_points_to_camera(
points,
frame_to=image_frame,
frame_from=targets.frame,
remove_outlier=False,
return_dmask=True)
if len(uv[mask]) < 1:
continue # ignore boxes that is outside the image
uv = uv.astype(int)
# draw box
pairs = [(0, 1), (2, 3), (4, 5), (6, 7), (0, 4), (1, 5), (2, 6),
(3, 7), (0, 2), (1, 3), (4, 6), (5, 7)]
inlier = [i in mask for i in range(len(uv))]
for i, j in pairs:
if not inlier[i] and not inlier[j]:
continue
if i not in dmask or j not in dmask:
continue # only calculate for points ahead
ax.add_line(
lines.Line2D((uv[i, 0], uv[j, 0]), (uv[i, 1], uv[j, 1]),
c=box_color,
lw=thickness))
# draw direction
ax.add_line(
lines.Line2D((uv[-2, 0], uv[-1, 0]), (uv[-2, 1], uv[-1, 1]),
c=box_color,
lw=thickness))
def test_point_cloud_temporal_fusion(self):
# TODO: this fail for nuscenes and waymo
idx = selection or random.randint(0, len(self.tloader))
lidar = self.tloader.VALID_LIDAR_NAMES[0]
# load data
clouds = self.tloader.lidar_data(idx, lidar)
poses = self.tloader.pose(idx)
targets = self.tloader.annotation_3dobject(idx)
calib = self.oloader.calibration_data(idx)
cloud1, cloud2 = clouds[0][:, :4], clouds[-1][:, :4]
pose1, pose2 = poses[0], poses[-1]
targets1, targets2 = targets[0], targets[-1]
print("START", pose1)
print("END", pose2)
# create transforms
tf = TransformSet("global")
fname1, fname2 = "pose1", "pose2"
tf.set_intrinsic_map_pin(fname1)
tf.set_intrinsic_map_pin(fname2)
tf.set_extrinsic(pose1.h**o(), fname1)
tf.set_extrinsic(pose2.h**o(), fname2)
# make coordinate unified in frame
targets1 = calib.transform_objects(targets1, lidar)
targets2 = calib.transform_objects(targets2, lidar)
targets1.frame = fname1
targets2.frame = fname2
print("HOMO1", pose1.h**o())
print("HOMO2", pose2.h**o())
print(tf.get_extrinsic(fname1, fname2))
# visualize both point cloud in frame2
visualizer = pcl.Visualizer()
visualizer.addPointCloud(pcl.create_xyzi(
tf.transform_points(cloud1, frame_from=fname1, frame_to=fname2)),
field="intensity",
id="cloud1")
visualizer.addPointCloud(pcl.create_xyzi(cloud2),
field="intensity",
id="cloud2")
pcl_vis(visualizer,
fname2,
targets1,
tf,
box_color=(1, 1, 0),
id_prefix="frame1")
pcl_vis(visualizer,
fname2,
targets2,
tf,
box_color=(0, 1, 1),
id_prefix="frame2")
visualizer.setRepresentationToWireframeForAllActors()
visualizer.addCoordinateSystem()
visualizer.setWindowName(
"Please check whether the gt boxes are aligned!")
# visualizer.spinOnce(time=5000)
# visualizer.close()
visualizer.spin()
def _preload_calib(self):
# load data
import yaml
if self.inzip:
source = ZipFile(self.base_path / "calibration.zip")
else:
source = self.base_path
cam2pose = load_calib_file(source, "calibration/calib_cam_to_pose.txt")
perspective = load_calib_file(source, "calibration/perspective.txt")
if self.inzip:
cam2velo = np.fromstring(
source.read("calibration/calib_cam_to_velo.txt"), sep=" ")
sick2velo = np.fromstring(
source.read("calibration/calib_sick_to_velo.txt"), sep=" ")
intri2 = yaml.safe_load(
source.read("calibration/image_02.yaml")[10:]) # skip header
intri3 = yaml.safe_load(
source.read("calibration/image_03.yaml")[10:])
else:
cam2velo = np.loadtxt(source / "calibration/calib_cam_to_velo.txt")
sick2velo = np.loadtxt(source /
"calibration/calib_sick_to_velo.txt")
intri2 = yaml.safe_load(
(source / "calibration/image_02.yaml").read_text()[10:])
intri3 = yaml.safe_load(
(source / "calibration/image_03.yaml").read_text()[10:])
if self.inzip:
source.close()
# parse calibration
calib = TransformSet("pose")
calib.set_intrinsic_lidar("velo")
calib.set_intrinsic_lidar("sick")
calib.set_intrinsic_camera("cam1",
perspective["P_rect_00"].reshape(3, 4),
perspective["S_rect_00"],
rotate=False)
calib.set_intrinsic_camera("cam2",
perspective["P_rect_01"].reshape(3, 4),
perspective["S_rect_01"],
rotate=False)
def parse_mei_camera(intri):
size = [intri['image_width'], intri['image_height']]
distorts = intri['distortion_parameters']
distorts = np.array([
distorts['k1'], distorts['k2'], distorts['p1'], distorts['p2']
])
projection = intri['projection_parameters']
pmatrix = np.diag([projection["gamma1"], projection["gamma2"], 1])
pmatrix[0, 2] = projection["u0"]
pmatrix[1, 2] = projection["v0"]
return size, pmatrix, distorts, intri['mirror_parameters']['xi']
S, P, D, xi = parse_mei_camera(intri2)
calib.set_intrinsic_camera("cam3",
P,
S,
distort_coeffs=D,
intri_matrix=P,
mirror_coeff=xi)
S, P, D, xi = parse_mei_camera(intri3)
calib.set_intrinsic_camera("cam4",
P,
S,
distort_coeffs=D,
intri_matrix=P,
mirror_coeff=xi)
calib.set_extrinsic(cam2pose["image_00"].reshape(3, 4),
frame_from="cam1")
calib.set_extrinsic(cam2pose["image_01"].reshape(3, 4),
frame_from="cam2")
calib.set_extrinsic(cam2pose["image_02"].reshape(3, 4),
frame_from="cam3")
calib.set_extrinsic(cam2pose["image_03"].reshape(3, 4),
frame_from="cam4")
calib.set_extrinsic(cam2velo.reshape(3, 4),
frame_from="cam1",
frame_to="velo")
calib.set_extrinsic(sick2velo.reshape(3, 4),
frame_from="sick",
frame_to="velo")
self._calibration = calib
def _load_calib(self, seq, raw=False):
# load the calibration file data
self._preload_calib(seq)
date = self._get_date(seq)
filedata = self._calib_cache[date]
if raw:
return filedata
# load matrics
data = TransformSet("velo")
velo_to_cam = np.empty((3, 4))
velo_to_cam[:3, :3] = filedata['velo_to_cam']['R'].reshape(3, 3)
velo_to_cam[:3, 3] = filedata['velo_to_cam']['T']
for i in range(4):
S = filedata['cam_to_cam']['S_rect_%02d' % i].tolist()
# TODO: here we have different R_rect's, what's the difference of them against the one used in object detection
R = filedata['cam_to_cam']['R_rect_%02d' % i].reshape(3, 3)
P = filedata['cam_to_cam']['P_rect_%02d' % i].reshape(3, 4)
intri, offset = P[:, :3], P[:, 3]
projection = intri.dot(R)
offset_cartesian = np.linalg.inv(projection).dot(offset)
extri = np.vstack([velo_to_cam, np.array([0, 0, 0, 1])])
extri[:3, 3] += offset_cartesian
frame = "cam%d" % i
data.set_intrinsic_camera(frame, projection, S, rotate=False)
data.set_extrinsic(extri, frame_to=frame)
imu_to_velo = np.empty((3, 4))
imu_to_velo[:3, :3] = filedata['imu_to_velo']['R'].reshape(3, 3)
imu_to_velo[:3, 3] = filedata['imu_to_velo']['T']
data.set_intrinsic_general("imu")
data.set_extrinsic(imu_to_velo, frame_from="imu")
# add position of vehicle bottom center and rear axis center
bc_rt = np.array([[1, 0, 0, -0.27], [0, 1, 0, 0], [0, 0, 1, 1.73]],
dtype='f4')
data.set_intrinsic_general("bottom_center")
data.set_extrinsic(bc_rt, frame_to="bottom_center")
rc_rt = np.array([[1, 0, 0, -0.805], [0, 1, 0, 0], [0, 0, 1, 0.30]])
data.set_intrinsic_general("rear_center")
data.set_extrinsic(rc_rt,
frame_from="bottom_center",
frame_to="rear_center")
return data
def visualize_detections(visualizer: Visualizer,
visualizer_frame: str,
targets: Target3DArray,
calib: TransformSet,
text_scale=0.8,
box_color=(1, 1, 1),
text_color=(1, 0.8, 1),
id_prefix="",
tags=None,
text_offset=None,
viewport=0):
'''
Visualize detection targets in PCL Visualizer.
:param visualizer: The pcl.Visualizer instance used for visualization
:param visualizer_frame: The frame that visualizer is in
:param targets: Object array to be visualized
:param calib: TransformSet object storing calibration information. This is mandatory if the
targets are in different frames
:param text_scale: The scale for text tags. Set to 0 or negative if you want to suppress text
visualization
:param box_color: Specifying the color of bounding boxes.
If it's a tuple, then it's assumed that it contains three RGB values in range 0-1.
If it's a str or matplotlib colormap object, then the color comes from applying colormap to the object id.
:param text_color: Specifying the color of text tags.
:param id_prefix: Prefix of actor ids in PCL Visualizer, essential when this function is called multiple times
:param text_offset: Relative position of text tags with regard to the box center
:param viewport: Viewport for objects to be added. This is a PCL related feature
'''
if not _pcl_available:
raise RuntimeError(
"pcl is not available, please check the installation of package pcl.py"
)
if id_prefix != "" and not id_prefix.endswith("/"):
id_prefix = id_prefix + "/"
# change frame to the same
if targets.frame != visualizer_frame:
targets = calib.transform_objects(targets, frame_to=visualizer_frame)
# convert colormaps
if isinstance(box_color, str):
box_color = cm.get_cmap(box_color)
if isinstance(text_color, str):
text_color = cm.get_cmap(text_color)
for i, target in enumerate(targets.filter_tag(tags)):
tid = target.tid or i
# convert coordinate
orientation = target.orientation.as_quat()
orientation = [orientation[3]
] + orientation[:3].tolist() # To PCL quaternion
lx, ly, lz = target.dimension
cube_id = (id_prefix + "target%d") % i
color = box_color(tid % 256) if isinstance(box_color,
Colormap) else box_color
alpha = color[3] if len(color) > 3 else 0.8
visualizer.addCube(target.position,
orientation,
lx,
ly,
lz,
id=cube_id,
viewport=viewport)
visualizer.setShapeRenderingProperties(pv.RenderingProperties.Opacity,
alpha,
id=cube_id)
visualizer.setShapeRenderingProperties(pv.RenderingProperties.Color,
color[:3],
id=cube_id)
# draw tag
if text_scale >= 0:
text_id = (id_prefix + "target%d/tag") % i
if target.tid:
disp_text = "%s: %s" % (target.tid64, target.tag_top.name)
else:
disp_text = "#%d: %s" % (i, target.tag_top.name)
aux_text = []
if target.tag_top_score < 1:
aux_text.append("%.2f" % target.tag_top_score)
position_var = np.power(np.linalg.det(target.position_var),
1 / 6) # display standard deviation
if position_var > 0:
aux_text.append("%.2f" % position_var)
dimension_var = np.power(np.linalg.det(target.dimension_var),
1 / 6)
if dimension_var > 0:
aux_text.append("%.2f" % dimension_var)
if target.orientation_var > 0:
aux_text.append("%.2f" % target.orientation_var)
if len(aux_text) > 0:
disp_text += " (" + ", ".join(aux_text) + ")"
disp_pos = np.copy(target.position)
disp_pos[2] += lz / 2 # lift the text out of box
if text_offset is not None: # apply offset
disp_pos += text_offset
color = text_color(tid % 256) if isinstance(
text_color, Colormap) else text_color
visualizer.addText3D(disp_text,
list(disp_pos),
text_scale=text_scale,
color=text_color[:3],
id=text_id,
viewport=viewport)
# draw orientation
arrow_id = (id_prefix + "target%d/direction") % i
dir_x, dir_y, dir_z = np.hsplit(target.orientation.as_matrix(), 3)
off_x, off_y, off_z = dir_x * lx / 2, dir_y * ly / 2, dir_z * lz / 2
off_x, off_y, off_z = off_x.flatten(), off_y.flatten(), off_z.flatten()
pos_bottom = target.position - off_z
visualizer.addLine(pos_bottom - off_y - off_x,
pos_bottom + off_x,
id=arrow_id + "_1",
viewport=viewport)
visualizer.addLine(pos_bottom + off_y - off_x,
pos_bottom + off_x,
id=arrow_id + "_2",
viewport=viewport)
# draw velocity
if isinstance(target, TrackingTarget3D):
arrow_id = (id_prefix + "target%d/velocity") % i
pstart = target.position
pend = target.position + target.velocity
visualizer.addLine(pstart,
pend,
color=(0.5, 0.5, 1),
id=arrow_id,
viewport=viewport)