def get_msg(self) -> capnp.lib.capnp._DynamicStructBuilder: smooth_rpy = self.get_smooth_rpy() extrinsic_matrix = get_view_frame_from_road_frame( 0, smooth_rpy[1], smooth_rpy[2], model_height) msg = messaging.new_message('liveCalibration') liveCalibration = msg.liveCalibration liveCalibration.validBlocks = self.valid_blocks liveCalibration.calStatus = self.cal_status liveCalibration.calPerc = min( 100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) liveCalibration.extrinsicMatrix = extrinsic_matrix.flatten().tolist() liveCalibration.rpyCalib = smooth_rpy.tolist() liveCalibration.rpyCalibSpread = self.calib_spread.tolist() if self.CP.notCar: extrinsic_matrix = get_view_frame_from_road_frame( 0, 0, 0, model_height) liveCalibration.validBlocks = INPUTS_NEEDED liveCalibration.calStatus = Calibration.CALIBRATED liveCalibration.calPerc = 100. liveCalibration.extrinsicMatrix = extrinsic_matrix.flatten( ).tolist() liveCalibration.rpyCalib = [0, 0, 0] liveCalibration.rpyCalibSpread = self.calib_spread.tolist() return msg
def send_data(self, pm): if self.valid_blocks > 0: max_rpy_calib = np.array( np.max(self.rpys[:self.valid_blocks], axis=0)) min_rpy_calib = np.array( np.min(self.rpys[:self.valid_blocks], axis=0)) calib_spread = np.abs(max_rpy_calib - min_rpy_calib) else: calib_spread = np.zeros(3) extrinsic_matrix = get_view_frame_from_road_frame( 0, self.rpy[1], self.rpy[2], model_height) cal_send = messaging.new_message('liveCalibration') cal_send.liveCalibration.validBlocks = self.valid_blocks cal_send.liveCalibration.calStatus = self.cal_status cal_send.liveCalibration.calPerc = min( 100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) cal_send.liveCalibration.extrinsicMatrix = [ float(x) for x in extrinsic_matrix.flatten() ] cal_send.liveCalibration.rpyCalib = [float(x) for x in self.rpy] cal_send.liveCalibration.rpyCalibSpread = [ float(x) for x in calib_spread ] pm.send('liveCalibration', cal_send)
def send_data(self, pm): calib = get_calib_from_vp(self.vp) extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height) cal_send = messaging.new_message('liveCalibration') cal_send.liveCalibration.calStatus = self.cal_status cal_send.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()] cal_send.liveCalibration.rpyCalib = [float(x) for x in calib] pm.send('liveCalibration', cal_send)
def send_data(self, pm): calib = get_calib_from_vp(self.vp) extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height) cal_send = messaging.new_message() cal_send.init('liveCalibration') cal_send.liveCalibration.calStatus = self.cal_status cal_send.liveCalibration.calPerc = min(len(self.vps) * 100 // INPUTS_NEEDED, 100) cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()] cal_send.liveCalibration.rpyCalib = [float(x) for x in calib] pm.send('liveCalibration', cal_send)
def get_msg(self): smooth_rpy = self.get_smooth_rpy() extrinsic_matrix = get_view_frame_from_road_frame(0, smooth_rpy[1], smooth_rpy[2], model_height) msg = messaging.new_message('liveCalibration') msg.liveCalibration.validBlocks = self.valid_blocks msg.liveCalibration.calStatus = self.cal_status msg.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) msg.liveCalibration.extrinsicMatrix = extrinsic_matrix.flatten().tolist() msg.liveCalibration.rpyCalib = smooth_rpy.tolist() msg.liveCalibration.rpyCalibSpread = self.calib_spread.tolist() return msg
def send_data(self, pm): smooth_rpy = self.get_smooth_rpy() extrinsic_matrix = get_view_frame_from_road_frame(0, smooth_rpy[1], smooth_rpy[2], model_height) cal_send = messaging.new_message('liveCalibration') cal_send.liveCalibration.validBlocks = self.valid_blocks cal_send.liveCalibration.calStatus = self.cal_status cal_send.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()] cal_send.liveCalibration.rpyCalib = [float(x) for x in smooth_rpy] cal_send.liveCalibration.rpyCalibSpread = [float(x) for x in self.calib_spread] pm.send('liveCalibration', cal_send)
def send_data(self, livecalibration): calib = get_calib_from_vp(self.vp) extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height) ke = eon_intrinsics.dot(extrinsic_matrix) warp_matrix = get_camera_frame_from_model_frame(ke, model_height) cal_send = messaging.new_message() cal_send.init('liveCalibration') cal_send.liveCalibration.calStatus = self.cal_status cal_send.liveCalibration.calPerc = min(self.frame_counter * 100 / CALIBRATION_CYCLES_NEEDED, 100) cal_send.liveCalibration.warpMatrix2 = map(float, warp_matrix.flatten()) cal_send.liveCalibration.extrinsicMatrix = map(float, extrinsic_matrix.flatten()) livecalibration.send(cal_send.to_bytes())
def send_data(self, livecalibration): calib = get_calib_from_vp(self.vp) extrinsic_matrix = get_view_frame_from_road_frame( 0, calib[1], calib[2], model_height) ke = eon_intrinsics.dot(extrinsic_matrix) warp_matrix = get_camera_frame_from_model_frame(ke) warp_matrix_big = get_camera_frame_from_bigmodel_frame(ke) cal_send = messaging.new_message() cal_send.init('liveCalibration') cal_send.liveCalibration.calStatus = self.cal_status cal_send.liveCalibration.calPerc = min( len(self.vps) * 100 // INPUTS_NEEDED, 100) cal_send.liveCalibration.warpMatrix2 = [ float(x) for x in warp_matrix.flatten() ] cal_send.liveCalibration.warpMatrixBig = [ float(x) for x in warp_matrix_big.flatten() ] cal_send.liveCalibration.extrinsicMatrix = [ float(x) for x in extrinsic_matrix.flatten() ] livecalibration.send(cal_send.to_bytes())
def replace_calib(msg, calib): msg = msg.as_builder() if calib is not None: msg.liveCalibration.extrinsicMatrix = get_view_frame_from_road_frame( *calib, 1.22).flatten().tolist() return msg
bigmodel_zoom = 1. bigmodel_intrinsics = np.array( [[eon_focal_length / bigmodel_zoom, 0., 0.5 * BIGMODEL_INPUT_SIZE[0]], [0., eon_focal_length / bigmodel_zoom, 0.2 * BIGMODEL_INPUT_SIZE[1]], [0., 0., 1.]]) bigmodel_border = np.array([ [0, 0, 1], [BIGMODEL_INPUT_SIZE[0], 0, 1], [BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1], 1], [0, BIGMODEL_INPUT_SIZE[1], 1], ]) model_frame_from_road_frame = np.dot( model_intrinsics, get_view_frame_from_road_frame(0, 0, 0, model_height)) bigmodel_frame_from_road_frame = np.dot( bigmodel_intrinsics, get_view_frame_from_road_frame(0, 0, 0, model_height)) model_frame_from_bigmodel_frame = np.dot(model_intrinsics, np.linalg.inv(bigmodel_intrinsics)) # 'camera from model camera' def get_model_height_transform(camera_frame_from_road_frame, height): camera_frame_from_road_ground = np.dot( camera_frame_from_road_frame, np.array([ [1, 0, 0], [0, 1, 0],
[ 0. , 0. , 1.]]) # BIG model BIGMODEL_INPUT_SIZE = (1024, 512) BIGMODEL_YUV_SIZE = (BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1] * 3 // 2) bigmodel_zoom = 1. bigmodel_intrinsics = np.array( [[ eon_focal_length / bigmodel_zoom, 0. , 0.5 * BIGMODEL_INPUT_SIZE[0]], [ 0. , eon_focal_length / bigmodel_zoom, 256+MEDMODEL_CY], [ 0. , 0. , 1.]]) model_frame_from_road_frame = np.dot(model_intrinsics, get_view_frame_from_road_frame(0, 0, 0, model_height)) bigmodel_frame_from_road_frame = np.dot(bigmodel_intrinsics, get_view_frame_from_road_frame(0, 0, 0, model_height)) medmodel_frame_from_road_frame = np.dot(medmodel_intrinsics, get_view_frame_from_road_frame(0, 0, 0, model_height)) model_frame_from_bigmodel_frame = np.dot(model_intrinsics, np.linalg.inv(bigmodel_intrinsics)) medmodel_frame_from_bigmodel_frame = np.dot(medmodel_intrinsics, np.linalg.inv(bigmodel_intrinsics)) # 'camera from model camera' def get_model_height_transform(camera_frame_from_road_frame, height): camera_frame_from_road_ground = np.dot(camera_frame_from_road_frame, np.array([ [1, 0, 0], [0, 1, 0],