def __init__(self, initial_state, covariance, alphas, beta): self._initial_state = gtsam.Pose2(initial_state[0], initial_state[1], initial_state[2]) self._prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([covariance, covariance, covariance])) # self.observation_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([beta[0] ** 2, np.deg2rad(beta[1]) ** 2])) #self.observation_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([beta[0] ** 2, np.deg2rad(beta[1]) ** 2])) self.observation_noise = gtsam.noiseModel_Diagonal.Sigmas( np.array(([np.deg2rad(beta[1]) ** 2, (beta[0] / 100) ** 2]))) self.beta = beta self.alphas = alphas ** 2 self.pose_num = 0 self.observation_num = 1000 self.landmark_indexes = list() self.states_new = np.array([[]]) self.observation_new = np.array([[]]) self.graph = gtsam.NonlinearFactorGraph() self.estimations = gtsam.Values() self.result = gtsam.Values() self.parameters = gtsam.ISAM2Params() self.parameters.setRelinearizeThreshold(1e-4) self.parameters.setRelinearizeSkip(1) self.slam = gtsam.ISAM2(self.parameters) self.graph.add(gtsam.PriorFactorPose2(self.pose_num, self._initial_state, self._prior_noise)) self.estimations.insert(self.pose_num, self._initial_state)
def __init__(self, relinearizeThreshold=0.01, relinearizeSkip=1): """ priorMean and priorCov should be in 1 dimensional array """ # init the graph self.graph = gtsam.NonlinearFactorGraph() # init the iSam2 solver parameters = gtsam.ISAM2Params() parameters.setRelinearizeThreshold(relinearizeThreshold) parameters.setRelinearizeSkip(relinearizeSkip) self.isam = gtsam.ISAM2(parameters) # init container for initial values self.initialValues = gtsam.Values() # setting the current position self.currentKey = 1 # current estimate self.currentEst = False self.currentPose = [0, 0, 0] return
def __init__(self, geo_model, da_model, lambda_model, prior_mean, prior_noise, lambda_prior_mean=(0., 0.), lambda_prior_noise=((0.5, 0.), (0., 0.5)), cls_enable=True): # Symbol initialization self.X = lambda i: X(i) self.XO = lambda j: O(j) self.Lam = lambda k: L(k) # Enable Lambda inference self.cls_enable = cls_enable # Camera pose prior self.graph = gtsam.NonlinearFactorGraph() self.graph.add( gtsam.PriorFactorPose3(self.X(0), prior_mean, prior_noise)) # Setting initial values self.initial = gtsam.Values() self.initial.insert(self.X(0), prior_mean) self.prev_step_camera_pose = prior_mean self.daRealization = list() # Setting up models self.geoModel = geo_model self.daModel = da_model self.lambdaModel = lambda_model # Setting up ISAM2 params2 = gtsam.ISAM2Params() #params2.relinearize_threshold = 0.01 #params2.relinearize_skip = 1 self.isam = gtsam.ISAM2(params2) self.isam.update(self.graph, self.initial) # Set custom lambda if type(lambda_prior_mean) is np.ndarray: self.lambda_prior_mean = lambda_prior_mean else: self.lambda_prior_mean = np.array(lambda_prior_mean) self.lambda_prior_cov = gtsam.noiseModel.Gaussian.Covariance( np.matrix(lambda_prior_noise)) self.num_cls = len(self.lambda_prior_mean) + 1 # Initialize object last lambda database self.object_lambda_dict = dict()
def __init__(self, class_probability_prior, geo_model, da_model, cls_model, prior_mean, prior_noise, cls_enable=True): # Symbol initialization self.X = lambda i: X(i) self.XO = lambda j: O(j) # Camera pose prior self.graph = gtsam.NonlinearFactorGraph() self.graph.add( gtsam.PriorFactorPose3(self.X(0), prior_mean, prior_noise)) # Class realization and prior probabilities. if one of the class probabilities is 0 it will # set the logWeight to -inf self.cls_enable = cls_enable # Setting initial values self.initial = gtsam.Values() self.initial.insert(self.X(0), prior_mean) self.prev_step_camera_pose = prior_mean self.daRealization = list() # self.initial.print() # Setting up models self.geoModel = geo_model self.daModel = da_model self.clsModel = cls_model # self.classifierModel = classifierModel # Setting up ISAM2 TODO: make ISAM2 work. As of now, it doesn't take the initial values at optimize_isam2 method params2 = gtsam.ISAM2Params() # params2.relinearize_threshold = 0.01 # params2.relinearize_skip = 1 self.isam = gtsam.ISAM2(params2) self.isam.update(self.graph, self.initial) # Setting up weight memory self.weight_memory = list() self.normalized_weight = 0 # Setting up class realization self.classRealization = dict() self.classProbabilityPrior = class_probability_prior
def __init__(self, pose0=np.eye(4), pose0_to_pose1_range=1.0, K=np.eye(3), relinearizeThreshold=0.1, relinearizeSkip=10, proj_noise_val=1.0): self.graph = NonlinearFactorGraph() # Add a prior on pose x0. This indirectly specifies where the origin is. # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z pose_noise = gtsam.noiseModel_Diagonal.Sigmas( np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) x0factor = PriorFactorPose3(iSAM2Wrapper.get_key('x', 0), gtsam.gtsam.Pose3(pose0), pose_noise) self.graph.push_back(x0factor) # Set scale between pose 0 and pose 1 to Unity x0x1_noise = gtsam.noiseModel_Isotropic.Sigma(1, 0.1) x1factor = RangeFactorPose3(iSAM2Wrapper.get_key('x', 0), iSAM2Wrapper.get_key('x', 1), pose0_to_pose1_range, x0x1_noise) self.graph.push_back(x1factor) iS2params = gtsam.ISAM2Params() iS2params.setRelinearizeThreshold(relinearizeThreshold) iS2params.setRelinearizeSkip(relinearizeSkip) self.isam2 = gtsam.ISAM2(iS2params) self.projection_noise = gtsam.noiseModel_Isotropic.Sigma( 2, proj_noise_val) self.K = gtsam.Cal3_S2(iSAM2Wrapper.CameraMatrix_to_Cal3_S2(K)) #self.opt_params = gtsam.DoglegParams() #self.opt_params.setVerbosity('Error') #self.opt_params.setErrorTol(0.1) self.initial_estimate = gtsam.Values() self.initial_estimate.insert(iSAM2Wrapper.get_key('x', 0), gtsam.gtsam.Pose3(pose0)) self.lm_factor_ids = set()
def __init__(self, config, pose_3rd): quat = R_scipy.from_matrix(pose_3rd[:3, :3]).as_quat() quat = np.roll(quat, 1) t = pose_3rd[:3, -1] init_pose = np.array([*t, *quat]) self.result = None self.marginals = None self.node_idx = 0 self.odom_noise = gtsam.noiseModel.Diagonal.Sigmas( config["odom_noise"]) self.skip_noise = gtsam.noiseModel.Diagonal.Sigmas( config["skip_noise"]) self.prior_noise = gtsam.noiseModel.Diagonal.Sigmas( config["prior_noise"]) self.gps_pose_noise = gtsam.noiseModel.Diagonal.Sigmas( config["gps_pose_noise"]) self.gps_noise = gtsam.noiseModel.Isotropic.Sigmas(config["gps_noise"]) self.skip_num = config["skip_num"] # self.visualize=config["visualize"] self.graph = gtsam.NonlinearFactorGraph() self.initial_estimate = gtsam.Values() self.init_pose = gen_pose( init_pose ) #gtsam.Pose3(gtsam.Rot3.Rodrigues(*init_pose[3:]), gtsam.Point3(*init_pose[:3]))#gen_pose([0,0,0]) self.parameters = gtsam.ISAM2Params() self.parameters.setRelinearizeThreshold(0.01) self.parameters.setRelinearizeSkip(10) self.isam = gtsam.ISAM2(self.parameters) #! Add a prior on pose x0 self.graph.push_back( gtsam.PriorFactorPose3(X(self.node_idx), self.init_pose, self.prior_noise)) self.initial_estimate.insert(X(self.node_idx), self.init_pose) self.current_estimate = None self.last_transform = None
def clone(self): """ :type new_object: GaussianBelief """ new_object = copy.copy(self) new_object.graph = self.graph.clone() new_object.daRealization = self.daRealization.copy() new_object.classRealization = self.classRealization.copy() params3 = gtsam.ISAM2Params() #params3.relinearize_threshold = 0.01 #params3.relinearize_skip = 1 new_object.isam = gtsam.ISAM2(params3) new_object.isam.update(self.graph, self.initial) new_object.initial = gtsam.Values() for key in self.initial.keys(): try: new_object.initial.insert(key, self.initial.atPose3(key)) except: new_object.initial.insert(key, self.initial.atVector(key)) return new_object
def incremental_factorgraph_example(): # Create an ISAM2 object. # You can balance computation time vs accuracy by changing the parameters to ISAM2 in the ISAM2Params struct # (see https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/ISAM2Params.h). # The parameters are here set to default. isam_params = gtsam.ISAM2Params() isam = gtsam.ISAM2(isam_params) # We will keep the variables in lists for simplicity. pose_variables = [] landmark_variables = [] # Step 1: Initialize with a prior on the first pose X1. def step1_initialize(): # Create an empty nonlinear factor graph. # We will need to do this for every update. graph = gtsam.NonlinearFactorGraph() # Create a key for the first pose. X1 = X(1) # Update the list with the new pose variable key. pose_variables.append(X1) # Add a prior on pose X1 at the origin. prior_noise = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.1, 0.1, 0.1])) graph.add( gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), prior_noise)) # Set an initial estimate for the first pose. initial_estimate = gtsam.Values() initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15)) # Update ISAM2 with the initial factor graph. isam.update(graph, initial_estimate) step1_initialize() # Compute the current MAP estimate for all variables. result = isam.calculateEstimate() # Define a function that extracts the marginals (which we will reuse below). # Notice that we compute the marginal covariance directly from the isam object. def extract_marginals(): # Extract the marginal distributions for each variable X_marginals = [] for var in pose_variables: X_marginals.append( MultivariateNormalParameters(result.atPose2(var), isam.marginalCovariance(var))) L_marginals = [] for var in landmark_variables: L_marginals.append( MultivariateNormalParameters(result.atPoint2(var), isam.marginalCovariance(var))) return X_marginals, L_marginals pose_marginals, landmark_marginals = extract_marginals() # Show current results. plot_result(pose_marginals, landmark_marginals) # Step 2: Add a landmark observation from the pose X1 def step2_add_landmark_observation(): # Create an empty nonlinear factor graph. graph = gtsam.NonlinearFactorGraph() # Create keys for the involved variables. X1 = X(1) L1 = L(1) # Update the list with the new landmark variable key. landmark_variables.append(L1) # Add the landmark observation. measurement_noise = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.05, 0.1])) graph.add( gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0 + 4.0), measurement_noise)) # Set initial estimates only for the new variables. initial_estimate = gtsam.Values() initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10)) # Update ISAM2 with the new factor graph. isam.update(graph, initial_estimate) step2_add_landmark_observation() # ISAM2 performs only one iteration of the iterative nonlinear solver per update() call. # We can use "free time" until the next update from the sensors to perform additional iterations. for _ in range(5): isam.update() # Compute the updated MAP estimate. result = isam.calculateEstimate() pose_marginals, landmark_marginals = extract_marginals() # Show updated results. plot_result(pose_marginals, landmark_marginals) # Step 3: Add a new pose with a relative odometry constraint. def step3_add_pose_from_odometry(): # Create an empty nonlinear factor graph. graph = gtsam.NonlinearFactorGraph() # Create keys for the involved variables. X1 = X(1) X2 = X(2) # Update the list with the new pose variable key. pose_variables.append(X2) # Add an odometry measurement. odometry_noise = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.1, 0.1, 0.1])) graph.add( gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), odometry_noise)) # Set initial estimates only for the new variables. initial_estimate = gtsam.Values() initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20)) # Update ISAM2 with the new factor graph. isam.update(graph, initial_estimate) step3_add_pose_from_odometry() # Run a few additional update iterations. for _ in range(5): isam.update() # Compute the updated MAP estimate. result = isam.calculateEstimate() pose_marginals, landmark_marginals = extract_marginals() # Show updated results. plot_result(pose_marginals, landmark_marginals) # Step 4: Add new observation of the landmark L1 from X2. def step4_add_new_landmark_observation(): # Create an empty nonlinear factor graph. graph = gtsam.NonlinearFactorGraph() # Create keys for the involved variables. X2 = X(2) L1 = L(1) # Add the landmark observation. measurement_noise = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.05, 0.1])) graph.add( gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, measurement_noise)) # Update ISAM2 with the new factor graph. # There are no new variables this update, so no new initial values. isam.update(graph, gtsam.Values()) step4_add_new_landmark_observation() # Run a few additional update iterations. for _ in range(5): isam.update() # Compute the updated MAP estimate. result = isam.calculateEstimate() pose_marginals, landmark_marginals = extract_marginals() # Show updated results. plot_result(pose_marginals, landmark_marginals) # Step 5: Add both new odometry and landmark observations for X3. def step5_add_odometry_and_landmark_observations(): # Create an empty nonlinear factor graph. graph = gtsam.NonlinearFactorGraph() # Create keys for the involved variables. X2 = X(2) X3 = X(3) L2 = L(2) # Update the list with the new variable keys. pose_variables.append(X3) landmark_variables.append(L2) # Add the odometry measurement. odometry_noise = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.1, 0.1, 0.1])) graph.add( gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), odometry_noise)) # Add the landmark observation. measurement_noise = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.05, 0.1])) graph.add( gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, measurement_noise)) # Set initial estimates only for the new variables. initial_estimate = gtsam.Values() initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10)) initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80)) # Update ISAM2 with the new factor graph. isam.update(graph, initial_estimate) step5_add_odometry_and_landmark_observations() # Run a few additional update iterations. for _ in range(5): isam.update() # Compute the updated MAP estimate. result = isam.calculateEstimate() pose_marginals, landmark_marginals = extract_marginals() # Show updated results. plot_result(pose_marginals, landmark_marginals)
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 Pose2SLAM_ISAM2_example(): """Perform 2D SLAM given the ground truth changes in pose as well as simple loop closure detection.""" plt.ion() # Declare the 2D translational standard deviations of the prior factor's Gaussian model, in meters. prior_xy_sigma = 0.3 # Declare the 2D rotational standard deviation of the prior factor's Gaussian model, in degrees. prior_theta_sigma = 5 # Declare the 2D translational standard deviations of the odometry factor's Gaussian model, in meters. odometry_xy_sigma = 0.2 # Declare the 2D rotational standard deviation of the odometry factor's Gaussian model, in degrees. odometry_theta_sigma = 5 # Although this example only uses linear measurements and Gaussian noise models, it is important # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_xy_sigma, prior_xy_sigma, prior_theta_sigma*np.pi/180])) ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_xy_sigma, odometry_xy_sigma, odometry_theta_sigma*np.pi/180])) # Create a Nonlinear factor graph as well as the data structure to hold state estimates. graph = gtsam.NonlinearFactorGraph() initial_estimate = gtsam.Values() # Create iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many # update calls are required to perform the relinearization. parameters = gtsam.ISAM2Params() parameters.setRelinearizeThreshold(0.1) parameters.relinearizeSkip = 1 isam = gtsam.ISAM2(parameters) # Create the ground truth odometry measurements of the robot during the trajectory. true_odometry = [(2, 0, 0), (2, 0, math.pi/2), (2, 0, math.pi/2), (2, 0, math.pi/2), (2, 0, math.pi/2)] # Corrupt the odometry measurements with gaussian noise to create noisy odometry measurements. odometry_measurements = [np.random.multivariate_normal(true_odom, ODOMETRY_NOISE.covariance()) for true_odom in true_odometry] # Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate # iSAM2 incremental optimization. graph.push_back(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) # Initialize the current estimate which is used during the incremental inference loop. current_estimate = initial_estimate for i in range(len(true_odometry)): # Obtain the noisy odometry that is received by the robot and corrupted by gaussian noise. noisy_odom_x, noisy_odom_y, noisy_odom_theta = odometry_measurements[i] # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state. loop = determine_loop_closure(odometry_measurements[i], current_estimate, i, xy_tol=0.8, theta_tol=25) # Add a binary factor in between two existing states if loop closure is detected. # Otherwise, add a binary factor between a newly observed state and the previous state. if loop: graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop, gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE)) else: graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2, gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE)) # Compute and insert the initialization estimate for the current pose using the noisy odometry measurement. computed_estimate = current_estimate.atPose2(i + 1).compose(gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta)) initial_estimate.insert(i + 2, computed_estimate) # Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables. isam.update(graph, initial_estimate) current_estimate = isam.calculateEstimate() # Report all current state estimates from the iSAM2 optimzation. report_on_progress(graph, current_estimate, i) initial_estimate.clear() # Print the final covariance matrix for each pose after completing inference on the trajectory. marginals = gtsam.Marginals(graph, current_estimate) i = 1 for i in range(1, len(true_odometry)+1): print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") plt.ioff() plt.show()
def visual_ISAM2_example(): plt.ion() # Define the camera calibration parameters K = gtsam.Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) # Define the camera observation noise model measurement_noise = gtsam.noiseModel_Isotropic.Sigma( 2, 1.0) # one pixel in u and v # Create the set of ground-truth landmarks points = SFMdata.createPoints() # Create the set of ground-truth poses poses = SFMdata.createPoses(K) # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps # to maintain proper linearization and efficient variable ordering, iSAM2 # performs partial relinearization/reordering at each step. A parameter # structure is available that allows the user to set various properties, such # as the relinearization threshold and type of linear solver. For this # example, we we set the relinearization threshold small so the iSAM2 result # will approach the batch result. parameters = gtsam.ISAM2Params() parameters.setRelinearizeThreshold(0.01) parameters.setRelinearizeSkip(1) isam = gtsam.ISAM2(parameters) # Create a Factor Graph and Values to hold the new data graph = gtsam.NonlinearFactorGraph() initial_estimate = gtsam.Values() # Loop over the different poses, adding the observations to iSAM incrementally for i, pose in enumerate(poses): # Add factors for each landmark observation for j, point in enumerate(points): camera = gtsam.PinholeCameraCal3_S2(pose, K) measurement = camera.project(point) graph.push_back( gtsam.GenericProjectionFactorCal3_S2(measurement, measurement_noise, X(i), L(j), K)) # Add an initial guess for the current pose # Intentionally initialize the variables off from the ground truth initial_estimate.insert( X(i), pose.compose( gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) # If this is the first iteration, add a prior on the first pose to set the # coordinate frame and a prior on the first landmark to set the scale. # Also, as iSAM solves incrementally, we must wait until each is observed # at least twice before adding it to iSAM. if i == 0: # Add a prior on pose x0 pose_noise = gtsam.noiseModel_Diagonal.Sigmas( np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise)) # Add a prior on landmark l0 point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) graph.push_back( gtsam.PriorFactorPoint3(L(0), points[0], point_noise)) # add directly to graph # Add initial guesses to all observed landmarks # Intentionally initialize the variables off from the ground truth for j, point in enumerate(points): initial_estimate.insert( L(j), gtsam.Point3(point.x() - 0.25, point.y() + 0.20, point.z() + 0.15)) else: # Update iSAM with the new factors isam.update(graph, initial_estimate) # Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver. # If accuracy is desired at the expense of time, update(*) can be called additional # times to perform multiple optimizer iterations every step. isam.update() current_estimate = isam.calculateEstimate() print("****************************************************") print("Frame", i, ":") for j in range(i + 1): print(X(j), ":", current_estimate.atPose3(X(j))) for j in range(len(points)): print(L(j), ":", current_estimate.atPoint3(L(j))) visual_ISAM2_plot(current_estimate) # Clear the factor graph and values for the next iteration graph.resize(0) initial_estimate.clear() plt.ioff() plt.show()
def initialize(data, truth, options): # Initialize iSAM params = gtsam.ISAM2Params() if options.alwaysRelinearize: params.relinearizeSkip = 1 isam = gtsam.ISAM2(params=params) # Add constraints/priors # TODO: should not be from ground truth! newFactors = gtsam.NonlinearFactorGraph() initialEstimates = gtsam.Values() for i in range(2): ii = symbol('x', i) if i == 0: if options.hardConstraint: # add hard constraint newFactors.add( gtsam.NonlinearEqualityPose3(ii, truth.cameras[0].pose())) else: newFactors.add( gtsam.PriorFactorPose3(ii, truth.cameras[i].pose(), data.noiseModels.posePrior)) initialEstimates.insert(ii, truth.cameras[i].pose()) nextPoseIndex = 2 # Add visual measurement factors from two first poses and initialize # observed landmarks for i in range(2): ii = symbol('x', i) for k in range(len(data.Z[i])): j = data.J[i][k] jj = symbol('l', j) newFactors.add( gtsam.GenericProjectionFactorCal3_S2( data.Z[i][k], data.noiseModels.measurement, ii, jj, data.K)) # TODO: initial estimates should not be from ground truth! if not initialEstimates.exists(jj): initialEstimates.insert(jj, truth.points[j]) if options.pointPriors: # add point priors newFactors.add( gtsam.PriorFactorPoint3(jj, truth.points[j], data.noiseModels.pointPrior)) # Add odometry between frames 0 and 1 newFactors.add( gtsam.BetweenFactorPose3(symbol('x', 0), symbol('x', 1), data.odometry[1], data.noiseModels.odometry)) # Update ISAM if options.batchInitialization: # Do a full optimize for first two poses batchOptimizer = gtsam.LevenbergMarquardtOptimizer( newFactors, initialEstimates) fullyOptimized = batchOptimizer.optimize() isam.update(newFactors, fullyOptimized) else: isam.update(newFactors, initialEstimates) # figure(1)tic # t=toc plot(frame_i,t,'r.') tic result = isam.calculateEstimate() # t=toc plot(frame_i,t,'g.') return isam, result, nextPoseIndex
def __init__(self): self.oculus = OculusProperty() # Create a new factor when # - |ti - tj| > min_duration and # - |xi - xj| > max_translation or # - |ri - rj| > max_rotation self.keyframe_duration = None self.keyframe_translation = None self.keyframe_rotation = None # List of keyframes self.keyframes = [] # Current (non-key)frame with real-time pose update # FIXME propagate cov from previous keyframe self.current_frame = None self.isam_params = gtsam.ISAM2Params() self.graph = gtsam.NonlinearFactorGraph() self.values = gtsam.Values() # [x, y, theta] self.prior_sigmas = None # Noise model without ICP # [x, y, theta] self.odom_sigmas = None # Downsample point cloud for ICP and publishing self.point_resolution = 0.5 # Noise radius in overlap self.point_noise = 0.5 self.ssm_params = SMParams() self.ssm_params.initialization = True self.ssm_params.initialization_params = 50, 1, 0.01 self.ssm_params.min_st_sep = 1 self.ssm_params.min_points = 50 self.ssm_params.max_translation = 2.0 self.ssm_params.max_rotation = np.pi / 6 self.ssm_params.target_frames = 3 # Don't use ICP covariance self.ssm_params.cov_samples = 0 self.nssm_params = SMParams() self.nssm_params.initialization = True self.nssm_params.initialization_params = 100, 5, 0.01 self.nssm_params.min_st_sep = 10 self.nssm_params.min_points = 100 self.nssm_params.max_translation = 6.0 self.nssm_params.max_rotation = np.pi / 2 self.nssm_params.source_frames = 5 self.nssm_params.cov_samples = 30 self.icp = pcl.ICP() # Pairwise consistent measurement self.nssm_queue = [] self.pcm_queue_size = 5 self.min_pcm = 3 # Use fixed noise model in two cases # - Sequential scan matching # - ICP cov is too small in non-sequential scan matching # [x, y, theta] self.icp_odom_sigmas = None # FIXME Can't save fig in online mode self.save_fig = False self.save_data = False
for elem in xVals: val = elem.split(',') x[val[0]] = val[1] elif data[i][0:2] == 'l0': i+=1 lString = data[i].split('[')[1].split(']')[0] lVals = lString[1:-1].split('), (') for elem in lVals: val = elem.split(',') l[val[0]] = val[1] parameters = gtsam.ISAM2Params() parameters.relinearize_threshold = 0.01 parameters.relinearize_skip = 1 isam = gtsam.ISAM2(parameters) graph = gtsam.NonlinearFactorGraph() initialEstimate = gtsam.Values() priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.000, 0.0, 0.0])) graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0.000, 0.0, 0.0), priorNoise)) initialEstimate.insert(1, gtsam.Pose2(0.0, 0.0, 0.0)) priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.000, 0.0, 0.0])) graph.add(gtsam.PriorFactorPose2(5, gtsam.Pose2(2.034, 0.0, 0.0), priorNoise)) initialEstimate.insert(5, gtsam.Pose2(2.034, 0.0, 0.0))
def __init__(self, params): """ Initialize variables """ self.imu_meas_predict = [] # IMU measurements for pose prediction self.imu_opt_meas = [] # IMU measurements for pose prediction between measurement updates self.imu_samples = [] # imu samples self.opt_meas = [] # optimizes measurements self.__opt_meas_buffer_time = params['opt_meas_buffer_time'] """ IMU Preintegration """ # IMU preintegration parameters self.imu_preint_params = gtsam.PreintegrationParams(np.asarray(params['g'])) self.imu_preint_params.setAccelerometerCovariance(np.eye(3) * np.power(params['acc_nd_sigma'], 2)) self.imu_preint_params.setGyroscopeCovariance(np.eye(3) * np.power(params['acc_nd_sigma'], 2)) self.imu_preint_params.setIntegrationCovariance(np.eye(3) * params['int_cov_sigma']**2) self.imu_preint_params.setUse2ndOrderCoriolis(params['setUse2ndOrderCoriolis']) self.imu_preint_params.setOmegaCoriolis(np.array(params['omega_coriolis'])) """ Initialize Parameters """ # ISAM2 initialization isam2_params = gtsam.ISAM2Params() isam2_params.setRelinearizeThreshold(params['relinearize_th']) isam2_params.setRelinearizeSkip(params['relinearize_skip']) isam2_params.setFactorization(params['factorization']) # self.isam2 = gtsam.ISAM2(isam2_params) self.isam2 = gtsam.ISAM2() self.new_factors = gtsam.NonlinearFactorGraph() self.new_initial_ests = gtsam.Values() self.min_imu_sample = 2 # minimum imu sample count needed for integration # ISAM2 keys self.poseKey = gtsam.symbol(ord('x'), 0) self.velKey = gtsam.symbol(ord('v'), 0) self.biasKey = gtsam.symbol(ord('b'), 0) """ Set initial variables """ # Initial state self.last_opt_time = self.current_time = 0 self.current_global_pose = numpy_pose_to_gtsam( params['init_pos'], params['init_ori']) self.current_global_vel = np.asarray( params['init_vel']) self.current_bias = gtsam.imuBias_ConstantBias( np.asarray(params['init_acc_bias']), np.asarray(params['init_gyr_bias'])) self.imu_accum = gtsam.PreintegratedImuMeasurements(self.imu_preint_params) # uncertainty of the initial state self.init_pos_cov = gtsam.noiseModel_Isotropic.Sigmas(np.array([ params['init_ori_cov'], params['init_ori_cov'], params['init_ori_cov'], params['init_pos_cov'], params['init_pos_cov'], params['init_pos_cov']])) self.init_vel_cov = gtsam.noiseModel_Isotropic.Sigmas(np.array([ params['init_vel_cov'], params['init_vel_cov'], params['init_vel_cov']])) self.init_bias_cov = gtsam.noiseModel_Isotropic.Sigmas(np.array([ params['init_acc_bias_cov'], params['init_acc_bias_cov'], params['init_acc_bias_cov'], params['init_gyr_bias_cov'], params['init_gyr_bias_cov'], params['init_gyr_bias_cov']])) # measurement noise self.dvl_cov = gtsam.noiseModel_Isotropic.Sigmas(np.asarray( params['dvl_cov'])) self.bar_cov = gtsam.noiseModel_Isotropic.Sigmas(np.asarray( params['bar_cov'])) self.bias_cov = gtsam.noiseModel_Isotropic.Sigmas(np.concatenate(( params['sigma_acc_bias_evol'], params['sigma_gyr_bias_evol']))) """ Set initial state """ # Initial factors prior_pose_factor = gtsam.PriorFactorPose3( self.poseKey, self.current_global_pose, self.init_pos_cov) self.new_factors.add(prior_pose_factor) prior_vel_factor = gtsam.PriorFactorVector( self.velKey, self.current_global_vel, self.init_vel_cov) self.new_factors.add(prior_vel_factor) prior_bias_factor = gtsam.PriorFactorConstantBias( self.biasKey, self.current_bias, self.init_bias_cov) self.new_factors.add(prior_bias_factor) # Initial estimates self.new_initial_ests.insert(self.poseKey, self.current_global_pose) self.new_initial_ests.insert(self.velKey, self.current_global_vel) self.new_initial_ests.insert(self.biasKey, self.current_bias)