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 handle_cam_odom(self, log):
trans, rot = log.trans, log.rot
if np.linalg.norm(trans) > MIN_SPEED_FILTER and abs(rot[2]) < MAX_YAW_RATE_FILTER:
new_vp = eon_intrinsics.dot(view_frame_from_device_frame.dot(trans))
new_vp = new_vp[:2]/new_vp[2]
self.vps.append(new_vp)
self.vps = self.vps[-INPUTS_WANTED:]
self.vp = np.mean(self.vps, axis=0)
self.update_status()
self.write_counter += 1
if self.param_put and (self.write_counter % WRITE_CYCLES == 0 or self.just_calibrated):
cal_params = {"vanishing_point": list(self.vp),
"valid_points": len(self.vps)}
self.params.put("CalibrationParams", json.dumps(cal_params))
return new_vp
else:
return None
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())