def handle_cam_odom(self, trans, rot, trans_std, rot_std):
if ((trans[0] > MIN_SPEED_FILTER) and (trans_std[0] < MAX_SPEED_STD)
and (abs(rot[2]) < MAX_YAW_RATE_FILTER)):
# intrinsics are not eon intrinsics, since this is calibrated frame
intrinsics = intrinsics_from_vp(self.vp)
new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans))
new_vp = new_vp[:2] / new_vp[2]
new_vp = sanity_clip(new_vp)
self.vps[self.block_idx] = (
self.idx * self.vps[self.block_idx] +
(BLOCK_SIZE - self.idx) * new_vp) / 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.vp = np.mean(self.vps[:self.valid_blocks], axis=0)
self.update_status()
if self.param_put and ((self.idx == 0 and self.block_idx == 0)
or self.just_calibrated):
cal_params = {
"vanishing_point": list(self.vp),
"valid_blocks": self.valid_blocks
}
put_nonblocking("CalibrationParams",
json.dumps(cal_params).encode('utf8'))
return new_vp
else:
return None
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:
# intrinsics are not eon intrinsics, since this is calibrated frame
intrinsics = intrinsics_from_vp(self.vp)
new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans))
new_vp = new_vp[:2]/new_vp[2]
new_vp = sanity_clip(new_vp)
self.vps[self.block_idx] = (self.idx*self.vps[self.block_idx] + (BLOCK_SIZE - self.idx) * new_vp) / 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.vp = np.mean(self.vps[:self.valid_blocks], axis=0)
self.update_status()
if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated):
calib = get_calib_from_vp(self.vp)
cal_params = {"calib_radians": list(calib),
"valid_blocks": self.valid_blocks}
put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
return new_vp
else:
return None
def correct_pts(pts, rot_speeds, dt): pts = np.hstack((pts, np.ones((pts.shape[0],1)))) cam_rot = dt * view_frame_from_device_frame.dot(rot_speeds) rot = orient.rot_matrix(*cam_rot.T).T pts_corrected = rot.dot(pts.T).T pts_corrected[:,0] /= pts_corrected[:,2] pts_corrected[:,1] /= pts_corrected[:,2] return pts_corrected[:,:2]
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 get_warp_matrix(rpy_calib, wide_cam=False, big_model=False, tici=True):
from common.transformations.orientation import rot_from_euler
from common.transformations.camera import view_frame_from_device_frame, eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics
if tici and wide_cam:
intrinsics = tici_ecam_intrinsics
elif tici:
intrinsics = tici_fcam_intrinsics
else:
intrinsics = eon_fcam_intrinsics
if big_model:
sbigmodel_from_calib = sbigmodel_frame_from_calib_frame[:, (0, 1, 2)]
calib_from_model = np.linalg.inv(sbigmodel_from_calib)
else:
medmodel_from_calib = medmodel_frame_from_calib_frame[:, (0, 1, 2)]
calib_from_model = np.linalg.inv(medmodel_from_calib)
device_from_calib = rot_from_euler(rpy_calib)
camera_from_calib = intrinsics.dot(
view_frame_from_device_frame.dot(device_from_calib))
warp_matrix = camera_from_calib.dot(calib_from_model)
return warp_matrix
def get_view_frame_from_road_frame(roll, pitch, yaw, height):
device_from_road = rot_from_euler([roll, pitch,
yaw]).dot(np.diag([1, -1, -1]))
view_from_road = view_frame_from_device_frame.dot(device_from_road)
return np.hstack((view_from_road, [[0], [height], [0]]))
