def horiz_errorFunc(): # compute error function using global data structures horiz_ned = [0, 0, 0] # any value works here (as long as it's consistent result = [] for r in data: camera.update_PROJ(horiz_ned, r[3], r[4], r[5]) # aircraft body attit #print("video:", r[1]*r2d, r[2]*r2d) roll, pitch = camera.find_horizon() #result.append( r[1] - (r[5] + xk[2]) ) #result.append( r[2] - (r[4] + xk[1]) ) if not roll is None: result.append( r[1] - roll ) result.append( r[2] - pitch ) return np.array(result)
flight_mode = 'auto' if 'mission' in data: excite_mode = mission['excite'] test_index = mission['test_index'] record = iter.next() hud1.update_task(record) while 'imu' in record and record['imu']['time'] < time: record = iter.next() hud1.update_task(record) ned = navpy.lla2ned( lat_deg, lon_deg, filt_alt, ref[0], ref[1], ref[2] ) #print 'ned:', ned camera.update_PROJ(ned, yaw_rad, pitch_rad, roll_rad) method = cv2.INTER_AREA #method = cv2.INTER_LANCZOS4 frame_scale = cv2.resize(frame, (0,0), fx=args.scale, fy=args.scale, interpolation=method) frame_undist = cv2.undistort(frame_scale, K, np.array(dist)) # Create hud draw space if not experimental_overlay: hud1_frame = frame_undist.copy() else: hud1_frame = np.zeros((frame_undist.shape), np.uint8) hud1.update_time(time, gps['unix_sec']) if 'event' in data: