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 rotate_img(img, eulers, crop=None, intrinsics=eon_intrinsics): size = img.shape[:2] rot = orient.rot_from_euler(eulers) quadrangle = np.array([[0, 0], [size[1] - 1, 0], [0, size[0] - 1], [size[1] - 1, size[0] - 1]], dtype=np.float32) quadrangle_norm = np.hstack( (normalize(quadrangle, intrinsics=intrinsics), np.ones((4, 1)))) warped_quadrangle_full = np.einsum('ij, kj->ki', intrinsics.dot(rot), quadrangle_norm) warped_quadrangle = np.column_stack( (warped_quadrangle_full[:, 0] / warped_quadrangle_full[:, 2], warped_quadrangle_full[:, 1] / warped_quadrangle_full[:, 2])).astype( np.float32) if crop: W_border = (size[1] - crop[0]) / 2 H_border = (size[0] - crop[1]) / 2 outside_crop = (((warped_quadrangle[:, 0] < W_border) | (warped_quadrangle[:, 0] >= size[1] - W_border)) & ((warped_quadrangle[:, 1] < H_border) | (warped_quadrangle[:, 1] >= size[0] - H_border))) if not outside_crop.all(): raise ValueError("warped image not contained inside crop") else: H_border, W_border = 0, 0 M = cv2.getPerspectiveTransform(quadrangle, warped_quadrangle) img_warped = cv2.warpPerspective(img, M, size[::-1]) return img_warped[H_border:size[0] - H_border, W_border:size[1] - W_border]
def handle_log(self, t, which, msg): if which == 'liveLocation': roll, pitch, yaw = math.radians(msg.roll), math.radians(msg.pitch), math.radians(msg.heading) v_device = orient.rot_from_euler([roll, pitch, yaw]).T.dot(msg.vNED) self.speed = v_device[0] self.update_active() if self.active: self.kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_YAW_RATE, [-msg.gyro[2]]) self.kf.predict_and_observe(t, ObservationKind.CAL_DEVICE_FRAME_XY_SPEED, [[v_device[0], -v_device[1]]]) # Clamp values x = self.kf.x if not (10 < x[States.STEER_RATIO] < 25): self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, [15.0]) if not (0.5 < x[States.STIFFNESS] < 3.0): self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, [1.0]) else: self.kf.filter.filter_time = t - 0.1 elif which == 'carState': self.carstate_counter += 1 if self.carstate_counter % CARSTATE_DECIMATION == 0: self.steering_angle = msg.steeringAngle self.steering_pressed = msg.steeringPressed self.update_active() if self.active: self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, [math.radians(msg.steeringAngle)]) self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, [0]) else: self.kf.filter.filter_time = t - 0.1
def get_view_frame_from_road_frame(roll, pitch, yaw, height): # TODO # calibration pitch is currently defined # opposite to pitch in device frame pitch = -pitch device_from_road = orient.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]]))
def get_M(h=1.22): quadrangle = np.array([[0, cy + 20], [size[1]-1, cy + 20], [0, size[0]-1], [size[1]-1, size[0]-1]], dtype=np.float32) quadrangle_norm = np.hstack((normalize(quadrangle, intrinsics=from_intr), np.ones((4,1)))) quadrangle_world = np.column_stack((h*quadrangle_norm[:,0]/quadrangle_norm[:,1], h*np.ones(4), h/quadrangle_norm[:,1])) rot = orient.rot_from_euler(augment_eulers) to_extrinsics = np.hstack((rot.T, -augment_trans[:,None])) to_KE = to_intr.dot(to_extrinsics) warped_quadrangle_full = np.einsum('jk,ik->ij', to_KE, np.hstack((quadrangle_world, np.ones((4,1))))) warped_quadrangle = np.column_stack((warped_quadrangle_full[:,0]/warped_quadrangle_full[:,2], warped_quadrangle_full[:,1]/warped_quadrangle_full[:,2])).astype(np.float32) M = cv2.getPerspectiveTransform(quadrangle, warped_quadrangle.astype(np.float32)) return M
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 transform_img(base_img, augment_trans=np.array([0, 0, 0]), augment_eulers=np.array([0, 0, 0]), from_intr=eon_intrinsics, to_intr=eon_intrinsics, calib_rot_view=None, output_size=None): cy = from_intr[1, 2] size = base_img.shape[:2] if not output_size: output_size = size[::-1] h = 1.22 quadrangle = np.array([[0, cy + 20], [size[1] - 1, cy + 20], [0, size[0] - 1], [size[1] - 1, size[0] - 1]], dtype=np.float32) quadrangle_norm = np.hstack( (normalize(quadrangle, intrinsics=from_intr), np.ones((4, 1)))) quadrangle_world = np.column_stack( (h * quadrangle_norm[:, 0] / quadrangle_norm[:, 1], h * np.ones(4), h / quadrangle_norm[:, 1])) rot = orient.rot_from_euler(augment_eulers) if calib_rot_view is not None: rot = calib_rot_view.dot(rot) to_extrinsics = np.hstack((rot.T, -augment_trans[:, None])) to_KE = to_intr.dot(to_extrinsics) warped_quadrangle_full = np.einsum( 'jk,ik->ij', to_KE, np.hstack((quadrangle_world, np.ones((4, 1))))) warped_quadrangle = np.column_stack( (warped_quadrangle_full[:, 0] / warped_quadrangle_full[:, 2], warped_quadrangle_full[:, 1] / warped_quadrangle_full[:, 2])).astype( np.float32) M = cv2.getPerspectiveTransform(quadrangle, warped_quadrangle.astype(np.float32)) augmented_rgb = cv2.warpPerspective(base_img, M, output_size, borderMode=cv2.BORDER_REPLICATE) return augmented_rgb
def get_view_frame_from_calib_frame(roll, pitch, yaw, height): device_from_calib = orient.rot_from_euler([roll, pitch, yaw]) view_from_calib = view_frame_from_device_frame.dot(device_from_calib) return np.hstack((view_from_calib, [[0], [height], [0]]))
def get_view_frame_from_road_frame(roll, pitch, yaw, height): device_from_road = orient.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]]))
def handle_live_calib(self, current_time, log): if len(log.rpyCalib): self.calib = log.rpyCalib self.device_from_calib = rot_from_euler(self.calib) self.calib_from_device = self.device_from_calib.T self.calibrated = log.calStatus == 1
FULL_FRAME_SIZE = (1164, 874) W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1] eon_focal_length = FOCAL = 910.0 intrinsic_matrix = np.array([[FOCAL, 0., W / 2.], [0., FOCAL, H / 2.], [0., 0., 1.]]) MODEL_PATH_MAX_VERTICES_CNT = 50 view_frame_from_device_frame = device_frame_from_view_frame.T roll = 0 pitch = 0 yaw = 0 height = 1.0 device_from_road = orient.rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1])) view_from_road = view_frame_from_device_frame.dot(device_from_road) extrinsic_matrix = np.hstack((view_from_road, [[0], [height], [0]])).flatten() extrinsic_matrix_eigen = np.zeros((3, 4)) i = 0 while i < 4 * 3: extrinsic_matrix_eigen[int(i / 4), int(i % 4)] = extrinsic_matrix[i] i += 1 # extrinsic_matrix_eigen = np.array([[0,-1,0,0], # [0,0,-1,1], # [1,0,0,0]]) camera_frame_from_road_frame = np.dot(eon_intrinsics, extrinsic_matrix_eigen)