def project(poses, ecef_pos): img_positions = np.zeros((len(poses), 2)) for i, p in enumerate(poses): cam_frame = orient.rotations_from_quats(p[3:]).T.dot(ecef_pos - p[:3]) img_positions[i] = np.array( [cam_frame[1] / cam_frame[0], cam_frame[2] / cam_frame[0]]) return img_positions
def device_from_ecef(pos_ecef, orientation_ecef, pt_ecef): # device from ecef frame # device frame is x -> forward, y-> right, z -> down # accepts single pt or array of pts input_shape = pt_ecef.shape pt_ecef = np.atleast_2d(pt_ecef) ecef_from_device_rot = orient.rotations_from_quats(orientation_ecef) device_from_ecef_rot = ecef_from_device_rot.T pt_ecef_rel = pt_ecef - pos_ecef pt_device = np.einsum('jk,ik->ij', device_from_ecef_rot, pt_ecef_rel) return pt_device.reshape(input_shape)
def compute_pos_python(self, poses, img_positions, check_quality=False): # This procedure is also described # in the MSCKF paper (Mourikis et al. 2007) x = np.array([img_positions[-1][0], img_positions[-1][1], 0.1]) res = opt.leastsq(self.residual, x, Dfun=self.residual_jac, args=(poses, img_positions)) # scipy opt #res = self.gauss_newton(self.residual, self.residual_jac, x, (poses, img_positions)) # diy gauss_newton alpha, beta, rho = res[0] rot_0_to_g = (orient.rotations_from_quats(poses[-1, 3:])).dot(self.to_c.T) return (rot_0_to_g.dot(np.array([alpha, beta, 1 ]))) / rho + poses[-1, :3]
def handle_gps(self, log, current_time): self.converter = coord.LocalCoord.from_geodetic([ log.gpsLocationExternal.latitude, log.gpsLocationExternal.longitude, log.gpsLocationExternal.altitude ]) fix_ecef = self.converter.ned2ecef([0, 0, 0]) # initing with bad bearing allowed, maybe bad? if not self.filter_ready and len(list(self.dog.orbits.keys( ))) > 6: # and log.gpsLocationExternal.speed > 5: self.filter_ready = True initial_ecef = fix_ecef initial_state = np.zeros(29) gps_bearing = log.gpsLocationExternal.bearing * (np.pi / 180) initial_pose_ecef = ecef_euler_from_ned(initial_ecef, [0, 0, gps_bearing]) initial_pose_ecef_quat = euler2quat(initial_pose_ecef) gps_speed = log.gpsLocationExternal.speed quat_uncertainty = 0.2**2 initial_pose_ecef_quat = euler2quat(initial_pose_ecef) initial_state[:3] = initial_ecef initial_state[3:7] = initial_pose_ecef_quat initial_state[7:10] = rotations_from_quats( initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0])) initial_state[18] = 1 initial_state[22] = 1 covs_diag = np.array([ 10**2, 10**2, 10**2, quat_uncertainty, quat_uncertainty, quat_uncertainty, 2**2, 2**2, 2**2, 1, 1, 1, 20000000**2, 100**2, 0.01**2, 0.01**2, 0.01**2, 0.02**2, 2**2, 2**2, 2**2, .01**2, 0.01**2, 0.01**2, 0.01**2, 10**2, 1**2, 0.2**2 ]) self.kf.init_state(initial_state, covs=np.diag(covs_diag), filter_time=current_time) print("Filter initialized") elif self.filter_ready: #self.update_kalman(current_time, ObservationKind.ECEF_POS, fix_ecef) gps_est_error = np.sqrt((self.kf.x[0] - fix_ecef[0])**2 + (self.kf.x[1] - fix_ecef[1])**2 + (self.kf.x[2] - fix_ecef[2])**2) if gps_est_error > 50: cloudlog.info( "Locationd vs ubloxLocation difference too large, kalman reset" ) self.reset_kalman()
def handle_gps(self, current_time, log): converter = coord.LocalCoord.from_geodetic( [log.latitude, log.longitude, log.altitude]) fix_ecef = converter.ned2ecef([0, 0, 0]) # TODO initing with bad bearing not allowed, maybe not bad? if not self.filter_ready and log.speed > 5: self.filter_ready = True initial_ecef = fix_ecef gps_bearing = math.radians(log.bearing) initial_pose_ecef = ecef_euler_from_ned(initial_ecef, [0, 0, gps_bearing]) initial_pose_ecef_quat = euler2quat(initial_pose_ecef) gps_speed = log.speed quat_uncertainty = 0.2**2 initial_pose_ecef_quat = euler2quat(initial_pose_ecef) initial_state = initial_x initial_covs_diag = initial_P_diag initial_state[States.ECEF_POS] = initial_ecef initial_state[States.ECEF_ORIENTATION] = initial_pose_ecef_quat initial_state[States.ECEF_VELOCITY] = rotations_from_quats( initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0])) initial_covs_diag[States.ECEF_POS_ERR] = 10**2 initial_covs_diag[States.ECEF_ORIENTATION_ERR] = quat_uncertainty initial_covs_diag[States.ECEF_VELOCITY_ERR] = 1**2 self.kf.init_state(initial_state, covs=np.diag(initial_covs_diag), filter_time=current_time) cloudlog.info("Filter initialized") elif self.filter_ready: self.update_kalman(current_time, ObservationKind.ECEF_POS, fix_ecef) gps_est_error = np.sqrt((self.kf.x[0] - fix_ecef[0])**2 + (self.kf.x[1] - fix_ecef[1])**2 + (self.kf.x[2] - fix_ecef[2])**2) if gps_est_error > 50: cloudlog.error( "Locationd vs ubloxLocation difference too large, kalman reset" ) self.reset_kalman()
def liveLocationMsg(self, time): fix = messaging.log.LiveLocationData.new_message() predicted_state = self.kf.x fix_ecef = predicted_state[States.ECEF_POS] fix_pos_geo = coord.ecef2geodetic(fix_ecef) fix.lat = float(fix_pos_geo[0]) fix.lon = float(fix_pos_geo[1]) fix.alt = float(fix_pos_geo[2]) fix.speed = float(np.linalg.norm( predicted_state[States.ECEF_VELOCITY])) orientation_ned_euler = ned_euler_from_ecef( fix_ecef, quat2euler(predicted_state[States.ECEF_ORIENTATION])) fix.roll = math.degrees(orientation_ned_euler[0]) fix.pitch = math.degrees(orientation_ned_euler[1]) fix.heading = math.degrees(orientation_ned_euler[2]) fix.gyro = [ float(predicted_state[10]), float(predicted_state[11]), float(predicted_state[12]) ] fix.accel = [ float(predicted_state[19]), float(predicted_state[20]), float(predicted_state[21]) ] local_vel = rotations_from_quats( predicted_state[States.ECEF_ORIENTATION]).T.dot( predicted_state[States.ECEF_VELOCITY]) fix.pitchCalibration = math.degrees( math.atan2(local_vel[2], local_vel[0])) fix.yawCalibration = math.degrees( math.atan2(local_vel[1], local_vel[0])) #fix.imuFrame = [(180/np.pi)*float(predicted_state[23]), (180/np.pi)*float(predicted_state[24]), (180/np.pi)*float(predicted_state[25])] return fix
def liveLocationMsg(self, time): fix = messaging.log.LiveLocationData.new_message() predicted_state = self.kf.x fix_ecef = predicted_state[0:3] fix_pos_geo = coord.ecef2geodetic(fix_ecef) fix.lat = float(fix_pos_geo[0]) fix.lon = float(fix_pos_geo[1]) fix.alt = float(fix_pos_geo[2]) fix.speed = float(np.linalg.norm(predicted_state[7:10])) orientation_ned_euler = ned_euler_from_ecef( fix_ecef, quat2euler(predicted_state[3:7])) fix.roll = float(orientation_ned_euler[0] * 180 / np.pi) fix.pitch = float(orientation_ned_euler[1] * 180 / np.pi) fix.heading = float(orientation_ned_euler[2] * 180 / np.pi) fix.gyro = [ float(predicted_state[10]), float(predicted_state[11]), float(predicted_state[12]) ] fix.accel = [ float(predicted_state[19]), float(predicted_state[20]), float(predicted_state[21]) ] local_vel = rotations_from_quats(predicted_state[3:7]).T.dot( predicted_state[7:10]) fix.pitchCalibration = float( (np.arctan2(local_vel[2], local_vel[0])) * 180 / np.pi) fix.yawCalibration = float( (np.arctan2(local_vel[1], local_vel[0])) * 180 / np.pi) fix.imuFrame = [(180 / np.pi) * float(predicted_state[23]), (180 / np.pi) * float(predicted_state[24]), (180 / np.pi) * float(predicted_state[25])] return fix