def handle_cam_odom(self, trans: List[float], rot: List[float], trans_std: List[float]) -> Optional[np.ndarray]:
self.old_rpy_weight = min(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
if self.wide_camera:
angle_std_threshold = 4*MAX_VEL_ANGLE_STD
else:
angle_std_threshold = MAX_VEL_ANGLE_STD
certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < angle_std_threshold) or
(self.valid_blocks < INPUTS_NEEDED))
if not (straight_and_fast and certain_if_calib):
return None
observed_rpy = np.array([0,
-np.arctan2(trans[2], trans[0]),
np.arctan2(trans[1], trans[0])])
new_rpy = euler_from_rot(rot_from_euler(self.get_smooth_rpy()).dot(rot_from_euler(observed_rpy)))
new_rpy = sanity_clip(new_rpy)
self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] + (BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE)
self.idx = (self.idx + 1) % BLOCK_SIZE
if self.idx == 0:
self.block_idx += 1
self.valid_blocks = max(self.block_idx, self.valid_blocks)
self.block_idx = self.block_idx % INPUTS_WANTED
self.update_status()
return new_rpy
def handle_cam_odom(self, trans, rot, trans_std, rot_std):
straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or
(self.valid_blocks < INPUTS_NEEDED))
if straight_and_fast and certain_if_calib:
observed_rpy = np.array([0,
-np.arctan2(trans[2], trans[0]),
np.arctan2(trans[1], trans[0])])
new_rpy = euler_from_rot(rot_from_euler(self.rpy).dot(rot_from_euler(observed_rpy)))
new_rpy = sanity_clip(new_rpy)
self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] + (BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE)
self.idx = (self.idx + 1) % BLOCK_SIZE
if self.idx == 0:
self.block_idx += 1
self.valid_blocks = max(self.block_idx, self.valid_blocks)
self.block_idx = self.block_idx % INPUTS_WANTED
if self.valid_blocks > 0:
self.rpy = np.mean(self.rpys[:self.valid_blocks], axis=0)
self.update_status()
# TODO: this should use the liveCalibration struct from cereal
if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated):
cal_params = {"calib_radians": list(self.rpy),
"valid_blocks": self.valid_blocks}
put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
return new_rpy
else:
return None
def handle_cam_odom(self, trans, rot, trans_std, rot_std):
self.old_rpy_weight = min(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or
(self.valid_blocks < INPUTS_NEEDED))
if straight_and_fast and certain_if_calib:
observed_rpy = np.array([0,
-np.arctan2(trans[2], trans[0]),
np.arctan2(trans[1], trans[0])])
new_rpy = euler_from_rot(rot_from_euler(self.get_smooth_rpy()).dot(rot_from_euler(observed_rpy)))
new_rpy = sanity_clip(new_rpy)
self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] + (BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE)
self.idx = (self.idx + 1) % BLOCK_SIZE
if self.idx == 0:
self.block_idx += 1
self.valid_blocks = max(self.block_idx, self.valid_blocks)
self.block_idx = self.block_idx % INPUTS_WANTED
if self.valid_blocks > 0:
self.rpy = np.mean(self.rpys[:self.valid_blocks], axis=0)
self.update_status()
return new_rpy
else:
return None
def msg_from_state(converter, calib_from_device, H, predicted_state,
predicted_cov):
predicted_std = np.sqrt(np.diagonal(predicted_cov))
fix_ecef = predicted_state[States.ECEF_POS]
fix_ecef_std = predicted_std[States.ECEF_POS_ERR]
vel_ecef = predicted_state[States.ECEF_VELOCITY]
vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR]
fix_pos_geo = coord.ecef2geodetic(fix_ecef)
#fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo)
orientation_ecef = euler_from_quat(
predicted_state[States.ECEF_ORIENTATION])
orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR]
device_from_ecef = rot_from_quat(
predicted_state[States.ECEF_ORIENTATION]).T
calibrated_orientation_ecef = euler_from_rot(
calib_from_device.dot(device_from_ecef))
acc_calib = calib_from_device.dot(predicted_state[States.ACCELERATION])
acc_calib_std = np.sqrt(
np.diagonal(
calib_from_device.dot(
predicted_cov[States.ACCELERATION_ERR,
States.ACCELERATION_ERR]).dot(
calib_from_device.T)))
ang_vel_calib = calib_from_device.dot(
predicted_state[States.ANGULAR_VELOCITY])
ang_vel_calib_std = np.sqrt(
np.diagonal(
calib_from_device.dot(
predicted_cov[States.ANGULAR_VELOCITY_ERR,
States.ANGULAR_VELOCITY_ERR]).dot(
calib_from_device.T)))
vel_device = device_from_ecef.dot(vel_ecef)
device_from_ecef_eul = euler_from_quat(
predicted_state[States.ECEF_ORIENTATION]).T
idxs = list(range(States.ECEF_ORIENTATION_ERR.start, States.ECEF_ORIENTATION_ERR.stop)) + \
list(range(States.ECEF_VELOCITY_ERR.start, States.ECEF_VELOCITY_ERR.stop))
condensed_cov = predicted_cov[idxs][:, idxs]
HH = H(*list(np.concatenate([device_from_ecef_eul, vel_ecef])))
vel_device_cov = HH.dot(condensed_cov).dot(HH.T)
vel_device_std = np.sqrt(np.diagonal(vel_device_cov))
vel_calib = calib_from_device.dot(vel_device)
vel_calib_std = np.sqrt(
np.diagonal(
calib_from_device.dot(vel_device_cov).dot(
calib_from_device.T)))
orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef)
#orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
ned_vel = converter.ecef2ned(fix_ecef +
vel_ecef) - converter.ecef2ned(fix_ecef)
#ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef)
fix = messaging.log.LiveLocationKalman.new_message()
# write measurements to msg
measurements = [
# measurement field, value, std, valid
(fix.positionGeodetic, fix_pos_geo, np.nan * np.zeros(3), True),
(fix.positionECEF, fix_ecef, fix_ecef_std, True),
(fix.velocityECEF, vel_ecef, vel_ecef_std, True),
(fix.velocityNED, ned_vel, np.nan * np.zeros(3), True),
(fix.velocityDevice, vel_device, vel_device_std, True),
(fix.accelerationDevice, predicted_state[States.ACCELERATION],
predicted_std[States.ACCELERATION_ERR], True),
(fix.orientationECEF, orientation_ecef, orientation_ecef_std,
True),
(fix.calibratedOrientationECEF, calibrated_orientation_ecef,
np.nan * np.zeros(3), True),
(fix.orientationNED, orientation_ned, np.nan * np.zeros(3), True),
(fix.angularVelocityDevice,
predicted_state[States.ANGULAR_VELOCITY],
predicted_std[States.ANGULAR_VELOCITY_ERR], True),
(fix.velocityCalibrated, vel_calib, vel_calib_std, True),
(fix.angularVelocityCalibrated, ang_vel_calib, ang_vel_calib_std,
True),
(fix.accelerationCalibrated, acc_calib, acc_calib_std, True),
]
for field, value, std, valid in measurements:
# TODO: can we write the lists faster?
field.value = to_float(value)
field.std = to_float(std)
field.valid = valid
return fix
def msg_from_state(converter, calib_from_device, H, predicted_state,
predicted_cov):
predicted_std = np.sqrt(np.diagonal(predicted_cov))
fix_ecef = predicted_state[States.ECEF_POS]
fix_ecef_std = predicted_std[States.ECEF_POS_ERR]
vel_ecef = predicted_state[States.ECEF_VELOCITY]
vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR]
fix_pos_geo = coord.ecef2geodetic(fix_ecef)
#fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo)
orientation_ecef = euler_from_quat(
predicted_state[States.ECEF_ORIENTATION])
orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR]
device_from_ecef = rot_from_quat(
predicted_state[States.ECEF_ORIENTATION]).T
calibrated_orientation_ecef = euler_from_rot(
calib_from_device.dot(device_from_ecef))
acc_calib = calib_from_device.dot(predicted_state[States.ACCELERATION])
acc_calib_std = np.sqrt(
np.diagonal(
calib_from_device.dot(
predicted_cov[States.ACCELERATION_ERR,
States.ACCELERATION_ERR]).dot(
calib_from_device.T)))
ang_vel_calib = calib_from_device.dot(
predicted_state[States.ANGULAR_VELOCITY])
ang_vel_calib_std = np.sqrt(
np.diagonal(
calib_from_device.dot(
predicted_cov[States.ANGULAR_VELOCITY_ERR,
States.ANGULAR_VELOCITY_ERR]).dot(
calib_from_device.T)))
vel_device = device_from_ecef.dot(vel_ecef)
device_from_ecef_eul = euler_from_quat(
predicted_state[States.ECEF_ORIENTATION]).T
idxs = list(range(States.ECEF_ORIENTATION_ERR.start, States.ECEF_ORIENTATION_ERR.stop)) + \
list(range(States.ECEF_VELOCITY_ERR.start, States.ECEF_VELOCITY_ERR.stop))
condensed_cov = predicted_cov[idxs][:, idxs]
HH = H(*list(np.concatenate([device_from_ecef_eul, vel_ecef])))
vel_device_cov = HH.dot(condensed_cov).dot(HH.T)
vel_device_std = np.sqrt(np.diagonal(vel_device_cov))
vel_calib = calib_from_device.dot(vel_device)
vel_calib_std = np.sqrt(
np.diagonal(
calib_from_device.dot(vel_device_cov).dot(
calib_from_device.T)))
orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef)
#orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
ned_vel = converter.ecef2ned(fix_ecef +
vel_ecef) - converter.ecef2ned(fix_ecef)
#ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef)
fix = messaging.log.LiveLocationKalman.new_message()
fix.positionGeodetic.value = to_float(fix_pos_geo)
#fix.positionGeodetic.std = to_float(fix_pos_geo_std)
#fix.positionGeodetic.valid = True
fix.positionECEF.value = to_float(fix_ecef)
fix.positionECEF.std = to_float(fix_ecef_std)
fix.positionECEF.valid = True
fix.velocityECEF.value = to_float(vel_ecef)
fix.velocityECEF.std = to_float(vel_ecef_std)
fix.velocityECEF.valid = True
fix.velocityNED.value = to_float(ned_vel)
#fix.velocityNED.std = to_float(ned_vel_std)
#fix.velocityNED.valid = True
fix.velocityDevice.value = to_float(vel_device)
fix.velocityDevice.std = to_float(vel_device_std)
fix.velocityDevice.valid = True
fix.accelerationDevice.value = to_float(
predicted_state[States.ACCELERATION])
fix.accelerationDevice.std = to_float(
predicted_std[States.ACCELERATION_ERR])
fix.accelerationDevice.valid = True
fix.orientationECEF.value = to_float(orientation_ecef)
fix.orientationECEF.std = to_float(orientation_ecef_std)
fix.orientationECEF.valid = True
fix.calibratedOrientationECEF.value = to_float(
calibrated_orientation_ecef)
#fix.calibratedOrientationECEF.std = to_float(calibrated_orientation_ecef_std)
#fix.calibratedOrientationECEF.valid = True
fix.orientationNED.value = to_float(orientation_ned)
#fix.orientationNED.std = to_float(orientation_ned_std)
#fix.orientationNED.valid = True
fix.angularVelocityDevice.value = to_float(
predicted_state[States.ANGULAR_VELOCITY])
fix.angularVelocityDevice.std = to_float(
predicted_std[States.ANGULAR_VELOCITY_ERR])
fix.angularVelocityDevice.valid = True
fix.velocityCalibrated.value = to_float(vel_calib)
fix.velocityCalibrated.std = to_float(vel_calib_std)
fix.velocityCalibrated.valid = True
fix.angularVelocityCalibrated.value = to_float(ang_vel_calib)
fix.angularVelocityCalibrated.std = to_float(ang_vel_calib_std)
fix.angularVelocityCalibrated.valid = True
fix.accelerationCalibrated.value = to_float(acc_calib)
fix.accelerationCalibrated.std = to_float(acc_calib_std)
fix.accelerationCalibrated.valid = True
return fix
