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)