def estimate_trajectory_householder(spline_template, observed_accel_timestamps, observed_accel_orientations, observed_accel_readings, observed_frame_timestamps, observed_frame_orientations, observed_features, imu_to_camera=np.eye(3), camera_matrix=np.eye(3), accel_weight=1.): assert isinstance(spline_template, spline.SplineTemplate) num_tracks = max(f.track_id for f in observed_features) + 1 accel_bias_offset = spline_template.control_size gravity_offset = spline_template.control_size + 3 structure_offset = spline_template.control_size + 6 num_vars = structure_offset + num_tracks * 3 j_blocks = [] r_blocks = [] # Add terms for accel residuals print 'Constructing linear systems for %d accel readings...' % len(observed_accel_readings) accel_coefficients = spline_template.coefficients_d2(observed_accel_timestamps) for r, a, c in zip(observed_accel_orientations, observed_accel_readings, accel_coefficients): amat = spline.diagify(c, 3) j = np.zeros((3, num_vars)) j[:, :spline_template.control_size] = np.dot(r, amat) j[:, gravity_offset:gravity_offset+3] = r j[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3) j_blocks.append(j * accel_weight) r_blocks.append(a * accel_weight) # Add terms for features print 'Constructing linear systems for %d features...' % len(observed_features) pos_coefficients = spline_template.coefficients(observed_frame_timestamps) pos_multidim_coefs = [spline.diagify(x, 3) for x in pos_coefficients] for feature in observed_features: z = calibrated(feature.position, camera_matrix) h = householder(z) r = observed_frame_orientations[feature.frame_id] pmat = pos_multidim_coefs[feature.frame_id] point_offset = structure_offset + feature.track_id*3 j = np.zeros((2, num_vars)) j[:, :spline_template.control_size] = -np.dot(h, np.dot(imu_to_camera, np.dot(r, pmat))) j[:, point_offset:point_offset+3] = np.dot(h, np.dot(imu_to_camera, r)) j_blocks.append(j) r_blocks.append(np.zeros(2)) # Assemble full linear system j = np.vstack(j_blocks) r = np.hstack(r_blocks) # Eliminate global position j = j[:, 3:] # Solve print 'Solving linear system of size %d x %d' % j.shape solution, _, _, _ = np.linalg.lstsq(j, r) # Replace global position solution = np.hstack((np.zeros(3), solution)) # Extract individual variables from solution position_controls = solution[:spline_template.control_size].reshape((-1, 3)) position_curve = spline.Spline(spline_template, position_controls) gravity = solution[gravity_offset:gravity_offset+3] accel_bias = solution[accel_bias_offset:accel_bias_offset+3] landmarks = solution[structure_offset:].reshape((-1, 3)) return structures.PositionEstimate(position_curve, gravity, accel_bias, landmarks)
def construct_problem_inf(spline_template, observed_accel_timestamps, observed_accel_orientations, observed_accel_readings, observed_frame_timestamps, observed_frame_orientations, observed_features, imu_to_camera=np.eye(3), camera_matrix=np.eye(3), feature_tolerance=1e-2, accel_tolerance=1e-3, gravity_magnitude=9.8, max_bias_magnitude=.1): # Sanity checks assert isinstance(spline_template, spline.SplineTemplate) assert len(observed_accel_orientations) == len(observed_accel_readings) assert len(observed_accel_timestamps) == len(observed_accel_readings) assert len(observed_frame_timestamps) == len(observed_frame_orientations) assert all(0 <= f.frame_id < len(observed_frame_timestamps) for f in observed_features) assert np.ndim(observed_accel_timestamps) == 1 assert np.ndim(observed_frame_timestamps) == 1 # Compute offsets position_offset = 0 position_len = spline_template.control_size gravity_offset = position_offset + position_len accel_bias_offset = gravity_offset + 3 structure_offset = accel_bias_offset + 3 track_ids = set(f.track_id for f in observed_features) num_frames = len(observed_frame_timestamps) num_tracks = max(track_ids) + 1 num_vars = structure_offset + num_tracks * 3 # Make sure each track has at least one observation counts_by_frame = np.zeros(num_frames, int) counts_by_track = np.zeros(num_tracks, int) for f in observed_features: counts_by_frame[f.frame_id] += 1 counts_by_track[f.track_id] += 1 assert np.all(counts_by_frame > 0),\ 'These frames had zero features: ' + ','.join(map(str, np.flatnonzero(counts_by_frame == 0))) assert np.all(counts_by_track > 0),\ 'These tracks had zero features: ' + ','.join(map(str, np.flatnonzero(counts_by_track == 0))) # Track IDs should be exactly 0..n-1 assert all(track_id < num_tracks for track_id in track_ids) # Initialize the problem objective = np.zeros(num_vars) problem = socp.SocpProblem(objective, []) # Construct gravity constraints a_gravity = np.zeros((3, num_vars)) a_gravity[:, gravity_offset:gravity_offset+3] = np.eye(3) d_gravity = gravity_magnitude problem.add_constraint(a=a_gravity, d=d_gravity) # Construct accel bias constraints a_bias = np.zeros((3, num_vars)) a_bias[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3) d_bias = max_bias_magnitude problem.add_constraint(a=a_bias, d=d_bias) # Construct accel constraints print 'Constructing constraints for %d accel readings...' % len(observed_accel_readings) accel_coefficients = spline_template.coefficients_d2(observed_accel_timestamps) for r, a, c in zip(observed_accel_orientations, observed_accel_readings, accel_coefficients): amat = spline.diagify(c, 3) j = np.zeros((3, num_vars)) j[:, :position_len] = np.dot(r, amat) j[:, gravity_offset:gravity_offset+3] = r j[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3) r = -a problem.add_constraint(a=j, b=r, d=accel_tolerance) # Construct vision constraints print 'Constructing constraints for %d features...' % len(observed_features) pos_coefficients = spline_template.coefficients(observed_frame_timestamps) pos_multidim_coefs = [spline.diagify(x, 3) for x in pos_coefficients] for feature in observed_features: r = observed_frame_orientations[feature.frame_id] pmat = pos_multidim_coefs[feature.frame_id] point_offset = structure_offset + feature.track_id*3 assert point_offset + 3 <= num_vars, 'track id was %d, num vars was %d' % (feature.track_id, num_vars) k_rc_r = np.dot(camera_matrix, np.dot(imu_to_camera, r)) ymat = np.zeros((3, num_vars)) ymat[:, :position_len] = -np.dot(k_rc_r, pmat) ymat[:, point_offset:point_offset+3] = k_rc_r a_feature = ymat[:2] - np.outer(feature.position, ymat[2]) c_feature = ymat[2] * feature_tolerance problem.add_constraint(a=a_feature, c=c_feature) return problem
def construct_problem_mixed(spline_template, observed_accel_timestamps, observed_accel_orientations, observed_accel_readings, observed_frame_timestamps, observed_frame_orientations, observed_features, imu_to_camera=np.eye(3), camera_matrix=np.eye(3), feature_tolerance=1e-2, gravity_magnitude=9.8, max_bias_magnitude=.1): if len(observed_features) < 5: raise InsufficientObservationsError() # Sanity checks assert isinstance(spline_template, spline.SplineTemplate) assert len(observed_accel_orientations) == len(observed_accel_readings) assert len(observed_accel_timestamps) == len(observed_accel_readings) assert len(observed_frame_timestamps) == len(observed_frame_orientations) assert all(0 <= f.frame_id < len(observed_frame_timestamps) for f in observed_features) assert np.ndim(observed_accel_timestamps) == 1 assert np.ndim(observed_frame_timestamps) == 1 # Compute offsets position_offset = 0 position_len = spline_template.control_size gravity_offset = position_offset + position_len accel_bias_offset = gravity_offset + 3 structure_offset = accel_bias_offset + 3 track_ids = set(f.track_id for f in observed_features) num_aux_vars = 1 # one extra variable representing the objective num_frames = len(observed_frame_timestamps) num_tracks = max(track_ids) + 1 num_vars = structure_offset + num_tracks * 3 + num_aux_vars # Make sure each track has at least one observation counts_by_frame = np.zeros(num_frames, int) counts_by_track = np.zeros(num_tracks, int) for f in observed_features: counts_by_frame[f.frame_id] += 1 counts_by_track[f.track_id] += 1 if not np.all(counts_by_frame > 0): raise InsufficientObservationsError( 'These frames had zero features: ' + ','.join(map(str, np.flatnonzero(counts_by_frame == 0)))) if not np.all(counts_by_track > 0): raise InsufficientObservationsError( 'These tracks had zero features: ' + ','.join(map(str, np.flatnonzero(counts_by_track == 0)))) # Track IDs should be exactly 0..n-1 assert all(track_id < num_tracks for track_id in track_ids) # Initialize the problem objective = utils.unit(num_vars-1, num_vars) # the last variable is the objective we minimize problem = socp.SocpProblem(objective) # Construct accel constraints print 'Constructing constraints for %d accel readings...' % len(observed_accel_readings) accel_coefficients = spline_template.coefficients_d2(observed_accel_timestamps) accel_j_blocks = [] accel_r_blocks = [] for r, a, c in zip(observed_accel_orientations, observed_accel_readings, accel_coefficients): amat = spline.diagify(c, 3) j = np.zeros((3, num_vars)) j[:, :position_len] = np.dot(r, amat) j[:, gravity_offset:gravity_offset+3] = r j[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3) accel_j_blocks.append(j) accel_r_blocks.append(a) # Form the least squares objective || J*x + r ||^2 accel_j = np.vstack(accel_j_blocks) accel_r = np.hstack(accel_r_blocks) # Form the quadratic objective: x' J' J x + b' x + c <= objective ("objective" is the variable we minimize) accel_c = np.dot(accel_r, accel_r) accel_b = -2. * np.dot(accel_j.T, accel_r) accel_b[-1] = -1. # Convert to an SOCP objective problem.add_constraint(*soc_constraint_from_quadratic_constraint(accel_j, accel_b, accel_c)) # Construct gravity constraints a_gravity = np.zeros((3, num_vars)) a_gravity[:, gravity_offset:gravity_offset+3] = np.eye(3) d_gravity = gravity_magnitude problem.add_constraint(a=a_gravity, d=d_gravity) # Construct accel bias constraints a_bias = np.zeros((3, num_vars)) a_bias[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3) d_bias = max_bias_magnitude problem.add_constraint(a=a_bias, d=d_bias) # Construct vision constraints print 'Constructing constraints for %d features...' % len(observed_features) pos_coefficients = spline_template.coefficients(observed_frame_timestamps) pos_multidim_coefs = [spline.diagify(x, 3) for x in pos_coefficients] for feature in observed_features: r = observed_frame_orientations[feature.frame_id] pmat = pos_multidim_coefs[feature.frame_id] point_offset = structure_offset + feature.track_id*3 assert point_offset + 3 <= num_vars, 'track id was %d, num vars was %d' % (feature.track_id, num_vars) k_rc_r = np.dot(camera_matrix, np.dot(imu_to_camera, r)) ymat = np.zeros((3, num_vars)) ymat[:, :position_len] = -np.dot(k_rc_r, pmat) ymat[:, point_offset:point_offset+3] = k_rc_r a_feature = ymat[:2] - np.outer(feature.position, ymat[2]) c_feature = ymat[2] * feature_tolerance problem.add_constraint(a=a_feature, c=c_feature) return problem