def addPrior(self, i: int, graph: gtsam.NonlinearFactorGraph): """Add a prior on the navigation state at time `i`.""" state = self.scenario.navState(i) graph.push_back( gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise)) graph.push_back( gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise))
def save_results(isam: gtsam.ISAM2, output_filename: str, first_gps_pose: int, gps_measurements: List[GpsMeasurement]): """Write the results from `isam` to `output_filename`.""" # Save results to file print("Writing results to file...") with open(output_filename, 'w', encoding='UTF-8') as fp_out: fp_out.write( "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n") result = isam.calculateEstimate() for i in range(first_gps_pose, len(gps_measurements)): pose_key = X(i) vel_key = V(i) bias_key = B(i) pose = result.atPose3(pose_key) velocity = result.atVector(vel_key) bias = result.atConstantBias(bias_key) pose_quat = pose.rotation().toQuaternion() gps = gps_measurements[i].position print(f"State at #{i}") print(f"Pose:\n{pose}") print(f"Velocity:\n{velocity}") print(f"Bias:\n{bias}") fp_out.write("{},{},{},{},{},{},{},{},{},{},{}\n".format( gps_measurements[i].time, pose.x(), pose.y(), pose.z(), pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), gps[0], gps[1], gps[2]))
def optimize(gps_measurements: List[GpsMeasurement], imu_measurements: List[ImuMeasurement], sigma_init_x: gtsam.noiseModel.Diagonal, sigma_init_v: gtsam.noiseModel.Diagonal, sigma_init_b: gtsam.noiseModel.Diagonal, noise_model_gps: gtsam.noiseModel.Diagonal, kitti_calibration: KittiCalibration, first_gps_pose: int, gps_skip: int) -> gtsam.ISAM2: """Run ISAM2 optimization on the measurements.""" # Set initial conditions for the estimated trajectory # initial pose is the reference frame (navigation frame) current_pose_global = Pose3(gtsam.Rot3(), gps_measurements[first_gps_pose].position) # the vehicle is stationary at the beginning at position 0,0,0 current_velocity_global = np.zeros(3) current_bias = gtsam.imuBias.ConstantBias() # init with zero bias imu_params = getImuParams(kitti_calibration) # Set ISAM2 parameters and create ISAM2 solver object isam_params = gtsam.ISAM2Params() isam_params.setFactorization("CHOLESKY") isam_params.relinearizeSkip = 10 isam = gtsam.ISAM2(isam_params) # Create the factor graph and values object that will store new factors and # values to add to the incremental graph new_factors = gtsam.NonlinearFactorGraph() # values storing the initial estimates of new nodes in the factor graph new_values = gtsam.Values() # Main loop: # (1) we read the measurements # (2) we create the corresponding factors in the graph # (3) we solve the graph to obtain and optimal estimate of robot trajectory print("-- Starting main loop: inference is performed at each time step, " "but we plot trajectory every 10 steps") j = 0 included_imu_measurement_count = 0 for i in range(first_gps_pose, len(gps_measurements)): # At each non=IMU measurement we initialize a new node in the graph current_pose_key = X(i) current_vel_key = V(i) current_bias_key = B(i) t = gps_measurements[i].time if i == first_gps_pose: # Create initial estimate and prior on initial pose, velocity, and biases new_values.insert(current_pose_key, current_pose_global) new_values.insert(current_vel_key, current_velocity_global) new_values.insert(current_bias_key, current_bias) new_factors.addPriorPose3(current_pose_key, current_pose_global, sigma_init_x) new_factors.addPriorVector(current_vel_key, current_velocity_global, sigma_init_v) new_factors.addPriorConstantBias(current_bias_key, current_bias, sigma_init_b) else: t_previous = gps_measurements[i - 1].time # Summarize IMU data between the previous GPS measurement and now current_summarized_measurement = gtsam.PreintegratedImuMeasurements( imu_params, current_bias) while (j < len(imu_measurements) and imu_measurements[j].time <= t): if imu_measurements[j].time >= t_previous: current_summarized_measurement.integrateMeasurement( imu_measurements[j].accelerometer, imu_measurements[j].gyroscope, imu_measurements[j].dt) included_imu_measurement_count += 1 j += 1 # Create IMU factor previous_pose_key = X(i - 1) previous_vel_key = V(i - 1) previous_bias_key = B(i - 1) new_factors.push_back( gtsam.ImuFactor(previous_pose_key, previous_vel_key, current_pose_key, current_vel_key, previous_bias_key, current_summarized_measurement)) # Bias evolution as given in the IMU metadata sigma_between_b = gtsam.noiseModel.Diagonal.Sigmas( np.asarray([ np.sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma ] * 3 + [ np.sqrt(included_imu_measurement_count) * kitti_calibration.gyroscope_bias_sigma ] * 3)) new_factors.push_back( gtsam.BetweenFactorConstantBias(previous_bias_key, current_bias_key, gtsam.imuBias.ConstantBias(), sigma_between_b)) # Create GPS factor gps_pose = Pose3(current_pose_global.rotation(), gps_measurements[i].position) if (i % gps_skip) == 0: new_factors.addPriorPose3(current_pose_key, gps_pose, noise_model_gps) new_values.insert(current_pose_key, gps_pose) print(f"############ POSE INCLUDED AT TIME {t} ############") print(gps_pose.translation(), "\n") else: new_values.insert(current_pose_key, current_pose_global) # Add initial values for velocity and bias based on the previous # estimates new_values.insert(current_vel_key, current_velocity_global) new_values.insert(current_bias_key, current_bias) # Update solver # ======================================================================= # We accumulate 2*GPSskip GPS measurements before updating the solver at # first so that the heading becomes observable. if i > (first_gps_pose + 2 * gps_skip): print(f"############ NEW FACTORS AT TIME {t:.6f} ############") new_factors.print() isam.update(new_factors, new_values) # Reset the newFactors and newValues list new_factors.resize(0) new_values.clear() # Extract the result/current estimates result = isam.calculateEstimate() current_pose_global = result.atPose3(current_pose_key) current_velocity_global = result.atVector(current_vel_key) current_bias = result.atConstantBias(current_bias_key) print(f"############ POSE AT TIME {t} ############") current_pose_global.print() print("\n") return isam
def run(self, T: int = 12, compute_covariances: bool = False, verbose: bool = True): """ Main runner. Args: T: Total trajectory time. compute_covariances: Flag indicating whether to compute marginal covariances. verbose: Flag indicating if printing should be verbose. """ graph = gtsam.NonlinearFactorGraph() # initialize data structure for pre-integrated IMU measurements pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) num_poses = T # assumes 1 factor per second initial = gtsam.Values() initial.insert(BIAS_KEY, self.actualBias) # simulate the loop i = 0 # state index initial_state_i = self.scenario.navState(0) initial.insert(X(i), initial_state_i.pose()) initial.insert(V(i), initial_state_i.velocity()) # add prior on beginning self.addPrior(0, graph) for k, t in enumerate(np.arange(0, T, self.dt)): # get measurements and add them to PIM measuredOmega = self.runner.measuredAngularVelocity(t) measuredAcc = self.runner.measuredSpecificForce(t) pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) # Plot IMU many times if k % 10 == 0: self.plotImu(t, measuredOmega, measuredAcc) if (k + 1) % int(1 / self.dt) == 0: # Plot every second self.plotGroundTruthPose(t, scale=1) plt.title("Ground Truth Trajectory") # create IMU factor every second factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim) graph.push_back(factor) if verbose: print(factor) print(pim.predict(initial_state_i, self.actualBias)) pim.resetIntegration() rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1) translationNoise = gtsam.Point3(*np.random.randn(3) * 1) poseNoise = gtsam.Pose3(rotationNoise, translationNoise) actual_state_i = self.scenario.navState(t + self.dt) print("Actual state at {0}:\n{1}".format( t + self.dt, actual_state_i)) noisy_state_i = gtsam.NavState( actual_state_i.pose().compose(poseNoise), actual_state_i.velocity() + np.random.randn(3) * 0.1) initial.insert(X(i + 1), noisy_state_i.pose()) initial.insert(V(i + 1), noisy_state_i.velocity()) i += 1 # add priors on end self.addPrior(num_poses - 1, graph) initial.print("Initial values:") result = self.optimize(graph, initial) result.print("Optimized values:") print("------------------") print(graph.error(initial)) print(graph.error(result)) print("------------------") if compute_covariances: # Calculate and print marginal covariances marginals = gtsam.Marginals(graph, result) print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY)) for i in range(num_poses): print("Covariance on pose {}:\n{}\n".format( i, marginals.marginalCovariance(X(i)))) print("Covariance on vel {}:\n{}\n".format( i, marginals.marginalCovariance(V(i)))) self.plot(result, show=True)
def add_imu_measurements(self, measured_poses, measured_acc, measured_omega, measured_vel, delta_t, n_skip, initial_poses=None): n_frames = measured_poses.shape[0] # Check if sizes are correct assert measured_poses.shape[0] == n_frames assert measured_acc.shape[0] == n_frames assert measured_vel.shape[0] == n_frames # Pose prior pose_key = X(0) pose_noise = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.2, 0.2, 0.2, 0.2, 0.2, 0.2])) pose_0 = gtsam.Pose3(measured_poses[0]) self.graph.push_back( gtsam.PriorFactorPose3(pose_key, pose_0, pose_noise)) self.initial_estimate.insert(pose_key, gtsam.Pose3(measured_poses[0])) # IMU prior bias_key = B(0) bias_noise = gtsam.noiseModel.Isotropic.Sigma(6, 0.5) self.graph.push_back( gtsam.PriorFactorConstantBias(bias_key, gtsam.imuBias.ConstantBias(), bias_noise)) self.initial_estimate.insert(bias_key, gtsam.imuBias.ConstantBias()) # Velocity prior velocity_key = V(0) velocity_noise = gtsam.noiseModel.Isotropic.Sigma(3, .5) velocity_0 = measured_vel[0] self.graph.push_back( gtsam.PriorFactorVector(velocity_key, velocity_0, velocity_noise)) self.initial_estimate.insert(velocity_key, velocity_0) # Preintegrator accum = gtsam.PreintegratedImuMeasurements(self.IMU_PARAMS) # Add measurements to factor graph for i in range(1, n_frames): accum.integrateMeasurement(measured_acc[i], measured_omega[i], delta_t[i - 1]) if i % n_skip == 0: pose_key += 1 DELTA = gtsam.Pose3( gtsam.Rot3.Rodrigues(0, 0, 0.1 * np.random.randn()), gtsam.Point3(4 * np.random.randn(), 4 * np.random.randn(), 4 * np.random.randn())) self.initial_estimate.insert( pose_key, gtsam.Pose3(measured_poses[i]).compose(DELTA)) velocity_key += 1 self.initial_estimate.insert(velocity_key, measured_vel[i]) bias_key += 1 self.graph.add( gtsam.BetweenFactorConstantBias( bias_key - 1, bias_key, gtsam.imuBias.ConstantBias(), self.BIAS_COVARIANCE)) self.initial_estimate.insert(bias_key, gtsam.imuBias.ConstantBias()) # Add IMU Factor self.graph.add( gtsam.ImuFactor(pose_key - 1, velocity_key - 1, pose_key, velocity_key, bias_key, accum)) # Reset preintegration accum.resetIntegration()
def run(self, T=12, compute_covariances=False, verbose=True): graph = gtsam.NonlinearFactorGraph() # initialize data structure for pre-integrated IMU measurements pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias) # T = 12 num_poses = T # assumes 1 factor per second initial = gtsam.Values() initial.insert(BIAS_KEY, self.actualBias) # simulate the loop i = 0 # state index initial_state_i = self.scenario.navState(0) initial.insert(X(i), initial_state_i.pose()) initial.insert(V(i), initial_state_i.velocity()) # add prior on beginning self.addPrior(0, graph) gtNavStates = [] predictedNavStates = [] integrationTime = [] for k, t in enumerate(np.arange(0, T, self.dt)): # get measurements and add them to PIM measuredOmega = self.runner.measuredAngularVelocity(t) measuredAcc = self.runner.measuredSpecificForce(t) start = time() pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt) integrationTime.append(time() - start) # Plot IMU many times if k % 10 == 0 and not DISABLE_VISUAL: self.plotImu(t, measuredOmega, measuredAcc) if (k + 1) % int(1 / self.dt) == 0: # Plot every second gtNavState = self.plotGroundTruthPose(t, scale=0.3) gtNavStates.append(gtNavState) plt.title("Ground Truth + Estimated Trajectory") # create IMU factor every second factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1), BIAS_KEY, pim) graph.push_back(factor) if verbose: print(factor) print(pim.predict(initial_state_i, self.actualBias)) pim.resetIntegration() rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1 * 1) translationNoise = gtsam.Point3(*np.random.randn(3) * 1 * 1) poseNoise = gtsam.Pose3(rotationNoise, translationNoise) actual_state_i = self.scenario.navState(t + self.dt) if not DISABLE_VISUAL: print("Actual state at {0}:\n{1}".format( t + self.dt, actual_state_i)) noisy_state_i = gtsam.NavState( actual_state_i.pose().compose(poseNoise), actual_state_i.velocity() + np.random.randn(3) * 0.1) initial.insert(X(i + 1), noisy_state_i.pose()) initial.insert(V(i + 1), noisy_state_i.velocity()) i += 1 # add priors on end self.addPrior(num_poses - 1, graph) initial.print_("Initial values:") # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("SUMMARY") optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() result.print_("Optimized values:") if compute_covariances: # Calculate and print marginal covariances marginals = gtsam.Marginals(graph, result) print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY)) for i in range(num_poses): print("Covariance on pose {}:\n{}\n".format( i, marginals.marginalCovariance(X(i)))) print("Covariance on vel {}:\n{}\n".format( i, marginals.marginalCovariance(V(i)))) # Plot resulting poses i = 0 while result.exists(X(i)): pose_i = result.atPose3(X(i)) predictedNavStates.append(pose_i) if not DISABLE_VISUAL: plot_pose3(POSES_FIG, pose_i, 1) i += 1 # plt.title("Estimated Trajectory") if not DISABLE_VISUAL: gtsam.utils.plot.set_axes_equal(POSES_FIG) print("Bias Values", result.atConstantBias(BIAS_KEY)) ATE = [] # import ipdb; ipdb.set_trace() for gt, pred in zip(gtNavStates, predictedNavStates[1:]): delta = gt.inverse().compose(pred) ATE.append(np.linalg.norm(delta.Logmap(delta))**2) print("ATE={}".format(np.sqrt(np.mean(ATE)))) print("Run time={}".format(np.median(integrationTime))) plt.ioff() plt.show()
def addPrior(self, i, graph): state = self.scenario.navState(i) graph.push_back( gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise)) graph.push_back( gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise))
def IMU_example(): """Run iSAM 2 example with IMU factor.""" # Start with a camera on x-axis looking at origin radius = 30 camera = get_camera(radius) pose_0 = camera.pose() delta_t = 1.0 / 18 # makes for 10 degrees per step angular_velocity = math.radians(180) # rad/sec scenario = get_scenario(radius, pose_0, angular_velocity, delta_t) PARAMS, BIAS_COVARIANCE, DELTA = preintegration_parameters() # Create a factor graph graph = NonlinearFactorGraph() # Create (incremental) ISAM2 solver isam = ISAM2() # Create the initial estimate to the solution # Intentionally initialize the variables off from the ground truth initialEstimate = Values() # Add a prior on pose x0. This indirectly specifies where the origin is. # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noise = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3])) graph.push_back(PriorFactorPose3(X(0), pose_0, noise)) # Add imu priors biasKey = B(0) biasnoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias.ConstantBias(), biasnoise) graph.push_back(biasprior) initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias()) velnoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) # Calculate with correct initial velocity n_velocity = vector3(0, angular_velocity * radius, 0) velprior = PriorFactorVector(V(0), n_velocity, velnoise) graph.push_back(velprior) initialEstimate.insert(V(0), n_velocity) accum = gtsam.PreintegratedImuMeasurements(PARAMS) # Simulate poses and imu measurements, adding them to the factor graph for i in range(80): t = i * delta_t # simulation time if i == 0: # First time add two poses pose_1 = scenario.pose(delta_t) initialEstimate.insert(X(0), pose_0.compose(DELTA)) initialEstimate.insert(X(1), pose_1.compose(DELTA)) elif i >= 2: # Add more poses as necessary pose_i = scenario.pose(t) initialEstimate.insert(X(i), pose_i.compose(DELTA)) if i > 0: # Add Bias variables periodically if i % 5 == 0: biasKey += 1 factor = BetweenFactorConstantBias( biasKey - 1, biasKey, gtsam.imuBias.ConstantBias(), BIAS_COVARIANCE) graph.add(factor) initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias()) # Predict acceleration and gyro measurements in (actual) body frame nRb = scenario.rotation(t).matrix() bRn = np.transpose(nRb) measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity) measuredOmega = scenario.omega_b(t) accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) # Add Imu Factor imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) graph.add(imufac) # insert new velocity, which is wrong initialEstimate.insert(V(i), n_velocity) accum.resetIntegration() # Incremental solution isam.update(graph, initialEstimate) result = isam.calculateEstimate() plot.plot_incremental_trajectory(0, result, start=i, scale=3, time_interval=0.01) # reset graph = NonlinearFactorGraph() initialEstimate.clear() plt.show()