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