def __init__(self, LASER_PARAMS=None, CAMERA_PARAMS=None):
# These are the defaults.
# The extrinsics are set such that
# - Velo's coordinate is treated as world coordinate.
# - The camera has the same coordinate frame as the lidar, including
# same origin, only the axes have been permuted.
# - camera's X lies along velo's -Y
# - camera's Y lies along velo's -X
# - camera's Z lies along velo's +X.
# - This is given by roll, pitch, yaw of -90, 0, -90.
# - laser lies 20cm to the right of camera (0.2 along velo's -Y)
self.LASER_PARAMS = dict(
x=0.0,
y=-0.2,
z=0.0, # 20cm to right of camera
roll=-90.0,
pitch=0.0,
yaw=-90.0,
fov=80,
galvo_m=-2.2450289e+01,
galvo_b=-6.8641598e-01,
maxADC=15000,
thickness=0.00055,
divergence=0.11 / 2.,
laser_limit=14000,
laser_timestep=1.5e-5)
# Merge with defaults.
if LASER_PARAMS is not None:
for key in self.LASER_PARAMS:
if key in LASER_PARAMS:
self.LASER_PARAMS[key] = LASER_PARAMS[key]
self.CAMERA_PARAMS = dict(
x=0.0,
y=0.0,
z=0.0,
roll=-90.0,
pitch=0.0,
yaw=-90.0,
width=512,
height=512,
limit=1.0,
distortion=[-0.033918, 0.027494, -0.001691, -0.001078, 0.000000],
fov=80,
matrix=None)
# Merge with defaults.
if CAMERA_PARAMS is not None:
for key in self.CAMERA_PARAMS:
if key in CAMERA_PARAMS:
self.CAMERA_PARAMS[key] = CAMERA_PARAMS[key]
# Compute camera matrix if not already specified.
if self.CAMERA_PARAMS['matrix'] is None:
self.CAMERA_PARAMS['matrix'] = compute_camera_instrics_matrix(
self.CAMERA_PARAMS['fov'], self.CAMERA_PARAMS['width'],
self.CAMERA_PARAMS['height'])
# Compute transforms.
self.TRANSFORMS = self._compute_transforms()
# Creating laser datum.
l_datum = pylc_lib.Datum()
l_datum.type = 'laser'
l_datum.laser_name = u'laser01'
l_datum.fov = self.LASER_PARAMS['fov']
l_datum.galvo_m = self.LASER_PARAMS['galvo_m']
l_datum.galvo_b = self.LASER_PARAMS['galvo_b']
l_datum.maxADC = self.LASER_PARAMS['maxADC']
l_datum.thickness = self.LASER_PARAMS['thickness']
l_datum.divergence = self.LASER_PARAMS['divergence']
l_datum.laser_limit = self.LASER_PARAMS['laser_limit']
l_datum.laser_timestep = self.LASER_PARAMS['laser_timestep']
# Creating camera datum.
c_datum = pylc_lib.Datum()
c_datum.type = 'camera'
c_datum.camera_name = u'camera01'
c_datum.rgb_matrix = self.CAMERA_PARAMS['matrix']
c_datum.limit = self.CAMERA_PARAMS['limit']
c_datum.depth_matrix = self.CAMERA_PARAMS['matrix']
c_datum.cam_to_world = self.TRANSFORMS['cTw']
c_datum.cam_to_laser = {
u'laser01': self.TRANSFORMS['lTc']
} # TODO: figure out why cam_to_laser is assigned lTc
c_datum.fov = self.CAMERA_PARAMS['fov']
c_datum.distortion = np.array(self.CAMERA_PARAMS['distortion'],
dtype=np.float32).reshape(1, 5)
c_datum.imgh = self.CAMERA_PARAMS['height']
c_datum.imgw = self.CAMERA_PARAMS['width']
self.datum_processor = pylc_lib.DatumProcessor()
self.datum_processor.setSensors([c_datum], [l_datum])
sys.path.append(fpath + "../build")
import pylc_lib as pylc
import pickle
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.lines as mlines
import matplotlib.patches as patches
from helper import *
with open('sensor_real.json') as f:
data = json.load(f)
laser_obj = data[0]["laser01"]
l_datums = []
ldatum = pylc.Datum()
ldatum.type = "laser"
ldatum.laser_name = laser_obj["name"]
ldatum.fov = 200
l_datums.append(ldatum)
camera_obj = data[1]["camera01"]
c_datums = []
cdatum = pylc.Datum()
cdatum.type = "camera"
cdatum.camera_name = camera_obj["name"]
cdatum.rgb_matrix = np.asarray(camera_obj["matrix"]).astype(np.float32)
cdatum.limit = camera_obj["limit"]
cdatum.depth_matrix = np.asarray(camera_obj["matrix"]).astype(np.float32)
cdatum.cam_to_world = np.asarray(camera_obj["cam_to_world"]).astype(np.float32)
cam_to_laser = np.asarray(camera_obj["cam_to_laser"]["laser01"]).astype(
def setup_lc(self, sensors):
self.camera_data = dict()
self.camera_names = []
self.laser_data = dict()
self.tfarray = []
for sensor in sensors:
if sensor["type"] == "camera":
# Check if valid
if sensor["id"] not in self.c_names: continue
self.camera_names.append(sensor["id"])
# Attachment?
attachment = sensor["attach"]
if len(attachment):
attachment = "/" + attachment
# Matrix
if "intrinsics" in sensor.keys():
matrix = np.array(sensor["intrinsics"]).reshape(
(3, 3)).astype(np.float32)
else:
cx = sensor["width"] / 2.0
cy = sensor["height"] / 2.0
fx = sensor["width"] / (
2.0 * math.tan(float(sensor["fov"]) * math.pi / 360.0))
fy = fx
matrix = np.array([fx, 0, cx, 0, fy, cy, 0, 0,
1.]).reshape((3, 3))
# Distortion
distortion = np.array(sensor["distortion"]).reshape(
1, 5).astype(np.float32)
# Transform
transform = np.eye(4)
transform = tf.transformations.euler_matrix(
sensor["yaw"] * np.pi / 180.,
sensor["pitch"] * np.pi / 180.,
sensor["roll"] * np.pi / 180.)
transform[0, 3] = sensor["x"]
transform[1, 3] = sensor["y"]
transform[2, 3] = sensor["z"]
# W Transform
wTc = np.eye(4)
wTc = self.compute_transform(
sensor["z"], -sensor["x"],
-sensor["y"], 0, 0, 0) * self.compute_transform(
0, 0, 0, -90 + sensor["yaw"], sensor["roll"],
-90 + sensor["pitch"])
if self.oflipped:
wTc = self.compute_transform(
sensor["x"], sensor["y"], sensor["z"],
0, 0, 0) * self.compute_transform(
0, 0, 0, sensor["roll"], sensor["pitch"],
sensor["yaw"])
# Set
self.camera_data[sensor["id"]] = {
"attach": sensor["attach"],
"matrix": matrix,
"transform": transform,
"wTc": wTc,
"fov": sensor["fov"],
"distortion": distortion,
"limit": sensor["limit"],
"width": sensor["width"],
"height": sensor["height"],
"name": sensor["id"]
}
# Publish TF?
if self.rosmode:
self.tfarray.append(
self.lc_spec_to_tf(
sensor, sensor["attach"], sensor["attach"] +
"/camera/depth/" + sensor["id"]))
elif sensor["type"] == "laser":
# Check if valid
if sensor["id"] not in self.l_names: continue
# W Transform
wTl = np.eye(4)
wTl = self.compute_transform(
sensor["z"], -sensor["x"],
-sensor["y"], 0, 0, 0) * self.compute_transform(
0, 0, 0, -90 + sensor["yaw"], sensor["roll"],
-90 + sensor["pitch"])
if self.oflipped:
wTl = self.compute_transform(
sensor["x"], sensor["y"], sensor["z"],
0, 0, 0) * self.compute_transform(
0, 0, 0, sensor["roll"], sensor["pitch"],
sensor["yaw"])
self.laser_data[sensor["id"]] = {
"fov": sensor["fov"],
"name": sensor["id"],
"wTl": wTl
}
# Publish TF?
if self.rosmode:
self.tfarray.append(
self.lc_spec_to_tf(
sensor, sensor["attach"],
sensor["attach"] + "/laser/" + sensor["id"]))
# Iterate each camera to compute laser transform
for camera_name, data in self.camera_data.iteritems():
wTc = data["wTc"]
cTw = inv(wTc)
# Cam to World
data["cam_to_world"] = wTc.astype(np.float32)
# Iterate each laser
data["cam_to_laser"] = dict()
for sensor in sensors:
if sensor["type"] == "laser":
laser_name = sensor["id"]
wTl = self.laser_data[laser_name]["wTl"]
lTw = inv(wTl)
# Get Transform
lTc = inv(np.dot(lTw, wTc))
data["cam_to_laser"][laser_name] = lTc.astype(np.float32)
# print(camera_name)
# print(laser_name)
# print(lTc)
# L Datums
self.l_datums = []
for laser_name, data in self.laser_data.iteritems():
datum = pylc.Datum()
datum.type = "laser"
datum.laser_name = laser_name
datum.fov = data["fov"]
self.l_datums.append(datum)
# C Datum
self.c_datums = []
for i in range(0, len(self.camera_names)):
datum = pylc.Datum()
datum.type = "camera"
datum.camera_name = self.camera_names[i]
datum.rgb_matrix = self.camera_data[
datum.camera_name]["matrix"].astype(np.float32)
datum.limit = self.camera_data[datum.camera_name]["limit"]
datum.depth_matrix = self.camera_data[
datum.camera_name]["matrix"].astype(np.float32)
datum.cam_to_world = self.camera_data[
datum.camera_name]["cam_to_world"]
datum.cam_to_laser = self.camera_data[
datum.camera_name]["cam_to_laser"]
datum.fov = self.camera_data[datum.camera_name]["fov"]
datum.distortion = self.camera_data[
datum.camera_name]["distortion"]
datum.imgh = self.camera_data[datum.camera_name]["height"]
datum.imgw = self.camera_data[datum.camera_name]["width"]
datum.hit_mode = 1
datum.hit_noise = 0.01
self.c_datums.append(datum)
# Set
self.datum_processor.setSensors(self.c_datums, self.l_datums)
# Save
# with open('laser_data.pickle', 'wb') as handle:
# pickle.dump(self.laser_data, handle, protocol=pickle.HIGHEST_PROTOCOL)
# with open('camera_data.pickle', 'wb') as handle:
# pickle.dump(self.camera_data, handle, protocol=pickle.HIGHEST_PROTOCOL)
all_data = [self.laser_data, self.camera_data]
with open('sensor_real.json', 'w') as f:
json.dump(all_data, f, cls=NumpyEncoder)