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],
