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 configure(self): assert (self.nssm_params.cov_samples == 0 or self.nssm_params.cov_samples < self.nssm_params.initialization_params[0] * self.nssm_params.initialization_params[1]) assert (self.ssm_params.cov_samples == 0 or self.ssm_params.cov_samples < self.ssm_params.initialization_params[0] * self.ssm_params.initialization_params[1]) assert self.nssm_params.source_frames < self.nssm_params.min_st_sep self.prior_model = self.create_noise_model(self.prior_sigmas) self.odom_model = self.create_noise_model(self.odom_sigmas) self.icp_odom_model = self.create_noise_model(self.icp_odom_sigmas) self.isam = gtsam.ISAM2(self.isam_params)
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 __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 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 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 mhjcbb(sim, num_tracks=10, prob=0.95, posterior_pose_md_threshold=1.5, prune2_skip=10, max_observed_diff=3): slams = [[gtsam.ISAM2(), set()]] prune2_count = 1 observed = set() for x, (odom, obs) in enumerate(sim.step()): for isam2, observed in slams: graph = gtsam.NonlinearFactorGraph() values = gtsam.Values() if x == 0: prior_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta])) prior_factor = gtsam.PriorFactorPose2(X(0), odom, prior_model) graph.add(prior_factor) values.insert(X(0), odom) else: odom_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta])) odom_factor = gtsam.BetweenFactorPose2(X(x - 1), X(x), odom, odom_model) graph.add(odom_factor) pose0 = isam2.calculateEstimatePose2(X(x - 1)) values.insert(X(x), pose0.compose(odom)) isam2.update(graph, values) ################################################################################ mhjcbb = gtsam.da_MHJCBB2(num_tracks, prob, posterior_pose_md_threshold) for isam2, observed, in slams: mhjcbb.initialize(isam2) for l, br in obs.items(): br_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_bearing, sim.sigma_range])) mhjcbb.add(gtsam.Rot2(br[0]), br[1], br_model) mhjcbb.match() ################################################################################ new_slams = [] for i in range(mhjcbb.size()): track, keys = mhjcbb.get(i) keys = [gtsam.symbolIndex(keys.at(i)) for i in range(keys.size())] isam2 = gtsam.ISAM2() isam2.update(slams[track][0].getFactorsUnsafe(), slams[track][0].calculateEstimate()) graph = gtsam.NonlinearFactorGraph() values = gtsam.Values() observed = set(slams[track][1]) for (l_true, br), l in zip(obs.items(), keys): br_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_bearing, sim.sigma_range])) br_factor = gtsam.BearingRangeFactor2D(X(x), L(l), gtsam.Rot2(br[0]), br[1], br_model) graph.add(br_factor) if l not in observed: pose1 = isam2.calculateEstimatePose2(X(x)) point = gtsam.Point2(br[1] * np.cos(br[0]), br[1] * np.sin(br[0])) values.insert(L(l), pose1.transform_from(point)) observed.add(l) isam2.update(graph, values) new_slams.append([isam2, observed]) slams = new_slams slams = prune1(slams, x, posterior_pose_md_threshold) if len(slams[0][1]) > prune2_count * prune2_skip: slams = prune2(slams, max_observed_diff) prune2_count += 1 result = [] for isam2, observed in slams: traj_est = [ isam2.calculateEstimatePose2(X(x)) for x in range(len(sim.traj)) ] traj_est = np.array([(p.x(), p.y(), p.theta()) for p in traj_est]) landmark_est = [isam2.calculateEstimatePoint2(L(l)) for l in observed] landmark_est = np.array([(p.x(), p.y()) for p in landmark_est]) result.append((traj_est, landmark_est)) return result
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)) isam.update(graph,initialEstimate) initialEstimate.clear()
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 slam(sim, da='jcbb', prob=0.95): isam2 = gtsam.ISAM2() graph = gtsam.NonlinearFactorGraph() values = gtsam.Values() observed = set() for x, (odom, obs) in enumerate(sim.step()): if x == 0: prior_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta])) prior_factor = gtsam.PriorFactorPose2(X(0), odom, prior_model) graph.add(prior_factor) values.insert(X(0), odom) else: odom_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta])) odom_factor = gtsam.BetweenFactorPose2(X(x - 1), X(x), odom, odom_model) graph.add(odom_factor) pose0 = isam2.calculateEstimatePose2(X(x - 1)) values.insert(X(x), pose0.compose(odom)) isam2.update(graph, values) graph.resize(0) values.clear() estimate = isam2.calculateEstimate() if da == 'dr': for l_true, br in obs.items(): l = len(observed) br_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_bearing, sim.sigma_range])) br_factor = gtsam.BearingRangeFactor2D(X(x), L(l), gtsam.Rot2(br[0]), br[1], br_model) graph.add(br_factor) if l not in observed: pose1 = isam2.calculateEstimatePose2(X(x)) point = gtsam.Point2(br[1] * np.cos(br[0]), br[1] * np.sin(br[0])) values.insert(L(l), pose1.transform_from(point)) observed.add(l) elif da == 'perfect': for l_true, br in obs.items(): br_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_bearing, sim.sigma_range])) br_factor = gtsam.BearingRangeFactor2D(X(x), L(l_true), gtsam.Rot2(br[0]), br[1], br_model) graph.add(br_factor) if l_true not in observed: pose1 = isam2.calculateEstimatePose2(X(x)) point = gtsam.Point2(br[1] * np.cos(br[0]), br[1] * np.sin(br[0])) values.insert(L(l_true), pose1.transform_from(point)) observed.add(l_true) elif da == 'jcbb': ################################################################################ jcbb = gtsam.da_JCBB2(isam2, prob) for l, br in obs.items(): br_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_bearing, sim.sigma_range])) jcbb.add(gtsam.Rot2(br[0]), br[1], br_model) keys = jcbb.match() ################################################################################ keys = [gtsam.symbolIndex(keys.at(i)) for i in range(keys.size())] for (l_true, br), l in zip(obs.items(), keys): br_model = gtsam.noiseModel_Diagonal.Sigmas( np.array([sim.sigma_bearing, sim.sigma_range])) br_factor = gtsam.BearingRangeFactor2D(X(x), L(l), gtsam.Rot2(br[0]), br[1], br_model) graph.add(br_factor) if l not in observed: pose1 = isam2.calculateEstimatePose2(X(x)) point = gtsam.Point2(br[1] * np.cos(br[0]), br[1] * np.sin(br[0])) values.insert(L(l), pose1.transform_from(point)) observed.add(l) isam2.update(graph, values) graph.resize(0) values.clear() traj_est = [ isam2.calculateEstimatePose2(X(x)) for x in range(len(sim.traj)) ] traj_est = np.array([(p.x(), p.y(), p.theta()) for p in traj_est]) landmark_est = [isam2.calculateEstimatePoint2(L(l)) for l in observed] landmark_est = np.array([(p.x(), p.y()) for p in landmark_est]) return [[traj_est, landmark_est]]
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 IMU_example(): """Run iSAM 2 example with IMU factor.""" # Start with a camera on x-axis looking at origin radius = 30 up = gtsam.Point3(0, 0, 1) target = gtsam.Point3(0, 0, 0) position = gtsam.Point3(radius, 0, 0) camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2()) pose_0 = camera.pose() # Create the set of ground-truth landmarks and poses angular_velocity = math.radians(180) # rad/sec delta_t = 1.0/18 # makes for 10 degrees per step angular_velocity_vector = vector3(0, -angular_velocity, 0) linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) scenario = gtsam.ConstantTwistScenario( angular_velocity_vector, linear_velocity_vector, pose_0) # Create a factor graph newgraph = gtsam.NonlinearFactorGraph() # Create (incremental) ISAM2 solver isam = gtsam.ISAM2() # Create the initial estimate to the solution # Intentionally initialize the variables off from the ground truth initialEstimate = gtsam.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.3, 0.3, 0.3, 0.1, 0.1, 0.1])) newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise)) # Add imu priors biasKey = gtsam.symbol(ord('b'), 0) biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), biasnoise) newgraph.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 = gtsam.PriorFactorVector(V(0), n_velocity, velnoise) newgraph.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 = gtsam.BetweenFactorConstantBias( biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) newgraph.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 = gtsam.ImuFactor( X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) newgraph.add(imufac) # insert new velocity, which is wrong initialEstimate.insert(V(i), n_velocity) accum.resetIntegration() # Incremental solution isam.update(newgraph, initialEstimate) result = isam.calculateEstimate() ISAM2_plot(result) # reset newgraph = gtsam.NonlinearFactorGraph() initialEstimate.clear()
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)
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