def add_mr_object_measurement(self, stack, class_realization, new_input_object_prior=None, new_input_object_covariance=None): graph_add = gtsam.NonlinearFactorGraph() initial_add = gtsam.Values() idx = 0 for obj in stack: cov_noise_model = stack[obj]['covariance'] exp_gtsam = gtsam.Pose3( gtsam.Rot3.Ypr(stack[obj]['expectation'][2], stack[obj]['expectation'][1], stack[obj]['expectation'][0]), gtsam.Point3(stack[obj]['expectation'][3], stack[obj]['expectation'][4], stack[obj]['expectation'][5])) pose_rotation_matrix = exp_gtsam.rotation().matrix() cov_noise_model_rotated = self.rotate_cov_6x6( cov_noise_model, np.transpose(pose_rotation_matrix)) cov_noise_model_rotated = gtsam.noiseModel.Gaussian.Covariance( cov_noise_model_rotated) #updating the graph self.graph.add( gtsam.PriorFactorPose3(self.XO(obj), exp_gtsam, cov_noise_model_rotated)) graph_add.add( gtsam.PriorFactorPose3(self.XO(obj), exp_gtsam, cov_noise_model_rotated)) self.prop_belief.add( gtsam.PriorFactorPose3(self.XO(obj), exp_gtsam, cov_noise_model_rotated)) if self.prop_initial.exists(self.XO(obj)) is False: self.prop_initial.insert( self.XO(obj), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0))) # if obj in new_input_object_prior and obj not in self.classRealization: # self.graph.add(gtsam.PriorFactorPose3(self.XO(obj), new_input_object_prior[obj], # new_input_object_covariance[obj])) # self.prop_belief.add(gtsam.PriorFactorPose3(self.XO(obj), new_input_object_prior[obj], # new_input_object_covariance[obj])) self.classRealization[obj] = class_realization[obj] if self.isam.value_exists(self.XO(obj)) is False: initial_add.insert(self.XO(obj), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0))) idx += 1 if stack[obj]['weight'] > -322: self.logWeight += stack[obj]['weight'] # + 150 else: self.logWeight = -math.inf
def gtsamOpt2PoseStampedarray(inputfname, pose_array): pointID = 0 graph, initial = gtsam.readG2o(inputfname, True) # Add Prior on the first key priorModel = gtsam.noiseModel_Diagonal.Variances( vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)) print("Adding prior to g2o file ") graphWithPrior = graph firstKey = initial.keys().at(0) graphWithPrior.add( gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) params = gtsam.GaussNewtonParams() params.setVerbosity( "Termination") # this will show info about stopping conds optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params) result = optimizer.optimize() print("Optimization complete") print("initial error = ", graphWithPrior.error(initial)) print("final error = ", graphWithPrior.error(result)) resultPoses = gtsam.allPose3s(result) keys = resultPoses.keys() for i in range(keys.size()): posetmp = gstamPose2Transform(resultPoses.atPose3(keys.at(i))) posestampedtmp = PoseStamped() posestampedtmp.header.frame_id = str(keys.at(i)) posestampedtmp.pose = posetmp pose_array.poseArray.append(posestampedtmp)
def main(): """Main runner.""" # Create an empty nonlinear factor graph graph = gtsam.NonlinearFactorGraph() # Add a prior on the first point, setting it to the origin # A prior factor consists of a mean and a noise model (covariance matrix) priorMean = gtsam.Pose3() # prior at origin graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE)) # Add GPS factors gps = gtsam.Point3(lat0, lon0, h0) graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE)) print("\nFactor Graph:\n{}".format(graph)) # Create the data structure to hold the initialEstimate estimate to the solution # For illustrative purposes, these have been deliberately set to incorrect values initial = gtsam.Values() initial.insert(1, gtsam.Pose3()) print("\nInitial Estimate:\n{}".format(initial)) # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() print("\nFinal Result:\n{}".format(result))
def gtsamOpt(inputfname, outputfname): graph, initial = gtsam.readG2o(inputfname, True) # Add Prior on the first key priorModel = gtsam.noiseModel_Diagonal.Variances( vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)) print("Adding prior to g2o file ") graphWithPrior = graph firstKey = initial.keys().at(0) graphWithPrior.add( gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) params = gtsam.GaussNewtonParams() params.setVerbosity( "Termination") # this will show info about stopping conds optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params) result = optimizer.optimize() print("Optimization complete") print("initial error = ", graphWithPrior.error(initial)) print("final error = ", graphWithPrior.error(result)) print("Writing results to file: ", outputfname) graphNoKernel, _ = gtsam.readG2o(inputfname, True) gtsam.writeG2o(graphNoKernel, result, outputfname) print("Done!")
def add_gps(self, cur_pose): print("Adding GPS factor at", self.node_idx + 1) # self.graph.add(gtsam.GPSFactor(X(self.node_idx+1), gps_pose, self.gps_noise)) gps_pose = gen_pose(cur_pose) self.graph.add( gtsam.PriorFactorPose3(X(self.node_idx + 1), gps_pose, self.gps_pose_noise))
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 test_SFMExample(self): options = generator.Options() options.triangle = False options.nrCameras = 10 [data, truth] = generator.generate_data(options) measurementNoiseSigma = 1.0 pointNoiseSigma = 0.1 poseNoiseSigmas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]) graph = gtsam.NonlinearFactorGraph() # Add factors for all measurements measurementNoise = Isotropic.Sigma(2, measurementNoiseSigma) for i in range(len(data.Z)): for k in range(len(data.Z[i])): j = data.J[i][k] graph.add(gtsam.GenericProjectionFactorCal3_S2( data.Z[i][k], measurementNoise, X(i), P(j), data.K)) posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas) graph.add(gtsam.PriorFactorPose3(X(0), truth.cameras[0].pose(), posePriorNoise)) pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma) graph.add(gtsam.PriorFactorPoint3(P(0), truth.points[0], pointPriorNoise)) # Initial estimate initialEstimate = gtsam.Values() for i in range(len(truth.cameras)): pose_i = truth.cameras[i].pose() initialEstimate.insert(X(i), pose_i) for j in range(len(truth.points)): point_j = truth.points[j] initialEstimate.insert(P(j), point_j) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) for i in range(5): optimizer.iterate() result = optimizer.values() # Marginalization marginals = gtsam.Marginals(graph, result) marginals.marginalCovariance(P(0)) marginals.marginalCovariance(X(0)) # Check optimized results, should be equal to ground truth for i in range(len(truth.cameras)): pose_i = result.atPose3(X(i)) self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) for j in range(len(truth.points)): point_j = result.atPoint3(P(j)) self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
def bundle_adjustment(self): """ Parameters: calibration - gtsam.Cal3_S2, camera calibration landmark_map - list, A map of landmarks and their correspondence """ # Initialize factor Graph graph = gtsam.NonlinearFactorGraph() initial_estimate = self.create_initial_estimate() # Create Projection Factors # """ # Measurement noise for bundle adjustment: # sigma = 1.0 # measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, sigma) # """ for landmark_idx, observation_list in enumerate(self._landmark_map): for obersvation in observation_list: pose_idx = obersvation[0] key_point = obersvation[1] # To test indeterminate system # if(landmark_idx == 2 or landmark_idx == 480): # continue # if(pose_idx == 6): # print("yes2") # continue graph.add( gtsam.GenericProjectionFactorCal3_S2( key_point, self._measurement_noise, X(pose_idx), P(landmark_idx), self._calibration)) # Create priors for first two poses # """ # Pose prior noise: # rotation_sigma = np.radians(60) # translation_sigma = 1 # pose_noise_sigmas = np.array([rotation_sigma, rotation_sigma, rotation_sigma, # translation_sigma, translation_sigma, translation_sigma]) # """ for idx in (0, 1): pose_i = initial_estimate.atPose3(X(idx)) graph.add( gtsam.PriorFactorPose3(X(idx), pose_i, self._pose_prior_noise)) # Optimization # Using QR rather than Cholesky decomposition # params = gtsam.LevenbergMarquardtParams() # params.setLinearSolverType("MULTIFRONTAL_QR") # optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params) optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate) sfm_result = optimizer.optimize() # Check if factor covariances are under constrain marginals = gtsam.Marginals( # pylint: disable=unused-variable graph, sfm_result) return sfm_result
def add_prior(self, var_name, transform, noise=None): """ Prior factor """ noise_gt = self._process_noise(noise) transform_gt = sophus2gtsam(transform) var = self.vars[var_name] factor = gtsam.PriorFactorPose3(var, transform_gt, noise_gt) self.gtsam_graph.push_back(factor)
def test_initializePoses(self): g2oFile = gtsam.findExampleDataFile("pose3example-grid") is3D = True inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D) priorModel = gtsam.noiseModel.Unit.Create(6) inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel)) initial = gtsam.InitializePose3.initialize(inputGraph) # TODO(frank): very loose !! self.gtsamAssertEquals(initial, expectedValues, 0.1)
def main(): """Main runner.""" parser = argparse.ArgumentParser( description="A 3D Pose SLAM example that reads input from g2o, and " "initializes Pose3") parser.add_argument('-i', '--input', help='input file g2o format') parser.add_argument( '-o', '--output', help="the path to the output file with optimized graph") parser.add_argument("-p", "--plot", action="store_true", help="Flag to plot results") args = parser.parse_args() g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \ else args.input is3D = True graph, initial = gtsam.readG2o(g2oFile, is3D) # Add Prior on the first key priorModel = gtsam.noiseModel.Diagonal.Variances( vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)) print("Adding prior to g2o file ") firstKey = initial.keys()[0] graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) params = gtsam.GaussNewtonParams() params.setVerbosity( "Termination") # this will show info about stopping conds optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) result = optimizer.optimize() print("Optimization complete") print("initial error = ", graph.error(initial)) print("final error = ", graph.error(result)) if args.output is None: print("Final Result:\n{}".format(result)) else: outputFile = args.output print("Writing results to file: ", outputFile) graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D) gtsam.writeG2o(graphNoKernel, result, outputFile) print("Done!") if args.plot: resultPoses = gtsam.utilities.allPose3s(result) for i in range(resultPoses.size()): plot.plot_pose3(1, resultPoses.atPose3(i)) plt.show()
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 setUp(cls): test_clouds = read_ply(*scans_fnames)[:6] PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) ICP_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) test_graph = gtsam.NonlinearFactorGraph() test_initial_estimates = gtsam.Values() initial_pose = gtsam.Pose3(gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955, -0.0221840, -0.0116519, 0.0228910, 0.9996701), gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633)) test_graph.add(gtsam.PriorFactorPose3(0, initial_pose, PRIOR_NOISE)) test_initial_estimates.insert(0, initial_pose) test_graph, test_initial_estimates = populate_factor_graph(test_graph, test_initial_estimates, initial_pose, test_clouds) cls.graph = test_graph cls.initial_estimates = test_initial_estimates
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 test_PriorFactor(self): values = gtsam.Values() key = 5 priorPose3 = gtsam.Pose3() model = gtsam.noiseModel_Unit.Create(6) factor = gtsam.PriorFactorPose3(key, priorPose3, model) values.insert(key, priorPose3) self.assertEqual(factor.error(values), 0) key = 3 priorVector = np.array([0., 0., 0.]) model = gtsam.noiseModel_Unit.Create(3) factor = gtsam.PriorFactorVector(key, priorVector, model) values.insert(key, priorVector) self.assertEqual(factor.error(values), 0)
def main(): """Main runner.""" # Read graph from file g2oFile = gtsam.findExampleDataFile("pose3example.txt") is3D = True graph, initial = gtsam.readG2o(g2oFile, is3D) # Add prior on the first key. TODO: assumes first key ios z priorModel = gtsam.noiseModel.Diagonal.Variances( np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) firstKey = initial.keys()[0] graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) # Initializing Pose3 - chordal relaxation initialization = gtsam.InitializePose3.initialize(graph) print(initialization)
def bundle_adjustment(self): """ Bundle Adjustment. Input: self._image_features self._image_points Output: result - gtsam optimzation result """ # Initialize factor Graph graph = gtsam.NonlinearFactorGraph() pose_indices, point_indices = self.create_index_sets() initial_estimate = self.create_initial_estimate(pose_indices) # Create Projection Factor sigma = 1.0 measurementNoise = gtsam.noiseModel_Isotropic.Sigma(2, sigma) for img_idx, features in enumerate(self._image_features): for kp_idx, keypoint in enumerate(features.keypoints): if self.image_points[img_idx].kp_3d_exist(kp_idx): landmark_id = self.image_points[img_idx].kp_3d(kp_idx) if self._landmark_estimates[ landmark_id].seen >= self._min_landmark_seen: key_point = Point2(keypoint[0], keypoint[1]) graph.add( gtsam.GenericProjectionFactorCal3_S2( key_point, measurementNoise, X(img_idx), P(landmark_id), self._calibration)) # Create priors for first two poses s = np.radians(60) poseNoiseSigmas = np.array([s, s, s, 1, 1, 1]) posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas) for idx in (0, 1): pose_i = initial_estimate.atPose3(X(idx)) graph.add(gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise)) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate) sfm_result = optimizer.optimize() return sfm_result, pose_indices, point_indices
def add_sfm_factors(graph, init_est, dataset_path, scale): f = open(dataset_path + '/reconstruction.json', 'r') data = json.load(f) images = data[0]['shots'] for img in images.items(): #print('Adding sfm factor for ' + img[0]) img_num = int(img[0].split('.')[0]) pos = np.array(img[1]['translation']) / scale rot_aa = np.array(img[1]['rotation']) #axis-angle rot_gtsam = gtsam.Rot3((Rotation.from_rotvec(rot_aa)).as_dcm()) pos = (Rotation.from_rotvec(rot_aa)).inv().as_dcm() @ pos pos[2] = -pos[2] trans_gtsam = gtsam.Point3(pos) pose_gtsam = gtsam.Pose3(rot_gtsam, trans_gtsam) factor = gtsam.PriorFactorPose3(C(img_num), pose_gtsam, sfm_noise) graph.push_back(factor) init_est.insert(C(img_num), pose_gtsam)
def add_sfm_factors_gt(graph, init_est, dataset_path): f = open(dataset_path + '/ep_data.pkl', 'rb') data = pickle.load(f) for ind in range(len(data['cam_pose'])): pose_mat = np.eye(4) quat = data['cam_pose'][ind][3:] quat[0], quat[1], quat[2], quat[3] = quat[1], quat[2], quat[3], quat[0] pose_mat[:3, :3] = ( Rotation.from_quat(quat) * Rotation.from_euler('xyz', [0, np.pi, 0])).as_dcm() pose_mat[:3, 3] = data['cam_pose'][ind][:3] pos = pose_mat[:3, 3] rot_gtsam = gtsam.Rot3(pose_mat[:3, :3]) trans_gtsam = gtsam.Point3(pos) pose_gtsam = gtsam.Pose3(rot_gtsam, trans_gtsam) factor = gtsam.PriorFactorPose3(C(ind), pose_gtsam, sfm_noise) graph.push_back(factor) init_est.insert(C(ind), pose_gtsam)
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 setUp(self): model = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) # We consider a small graph: # symbolic FG # x2 0 1 # / | \ 1 2 # / | \ 2 3 # x3 | x1 2 0 # \ | / 0 3 # \ | / # x0 # p0 = Point3(0, 0, 0) self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0])) p1 = Point3(1, 2, 0) self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796])) p2 = Point3(0, 2, 0) self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593])) p3 = Point3(-1, 1, 0) self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389])) pose0 = Pose3(self.R0, p0) pose1 = Pose3(self.R1, p1) pose2 = Pose3(self.R2, p2) pose3 = Pose3(self.R3, p3) g = NonlinearFactorGraph() g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model)) g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model)) g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model)) g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model)) g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model)) g.add(gtsam.PriorFactorPose3(x0, pose0, model)) self.graph = g
help="Flag to plot results") args = parser.parse_args() g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \ else args.input is3D = True graph, initial = gtsam.readG2o(g2oFile, is3D) # Add Prior on the first key priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4)) print("Adding prior to g2o file ") firstKey = initial.keys()[0] graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel)) params = gtsam.GaussNewtonParams() params.setVerbosity("Termination") # this will show info about stopping conds optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params) result = optimizer.optimize() print("Optimization complete") print("initial error = ", graph.error(initial)) print("final error = ", graph.error(result)) if args.output is None: print("Final Result:\n{}".format(result)) else: outputFile = args.output print("Writing results to file: ", outputFile)
def add_measurements(self, geo_measurements, sem_measurements, da_current_step, class_realization_current_step, number_of_samples=10, weight_update_flag=True, new_input_object_prior=None, new_input_object_covariance=None, ML_update=False): graph_add = gtsam.NonlinearFactorGraph() initial_add = gtsam.Values() # Setting up complete class realization running_index = 0 for obj in da_current_step: if obj not in self.classRealization: obj = int(obj) # Create a class realization dict from the realization key for obj_index in class_realization_current_step: if obj == obj_index[0]: self.classRealization[obj] = obj_index[1] self.logWeight += np.log( self.classProbabilityPrior[self.classRealization[obj] - 1]) # System is ill-defined without a prior for objects, so we define a prior. # Default is very uninformative prior new_object_prior = gtsam.Pose3(gtsam.Rot3.Ypr(0, 0, 0), np.array([0, 0, 0])) if new_input_object_prior is None: #new_object_prior = gtsam.Pose3(gtsam.Rot3.Ypr(0, 0, 0), np.array(0, 0, 0)) pass else: new_object_prior = new_input_object_prior[obj] self.graph.add( gtsam.PriorFactorPose3(self.XO(obj), new_object_prior, new_object_covariance)) self.prop_belief.add( gtsam.PriorFactorPose3(self.XO(obj), new_object_prior, new_object_covariance)) graph_add.add( gtsam.PriorFactorPose3(self.XO(obj), new_object_prior, new_object_covariance)) if new_input_object_covariance is None: pass #new_object_covariance_diag = np.array([1000, 1000, 1000, 1000, 1000, 1000]) #new_object_covariance = gtsam.noiseModel.Diagonal.Variances(new_object_covariance_diag) else: new_object_covariance = new_input_object_covariance[obj] self.graph.add( gtsam.PriorFactorPose3(self.XO(obj), new_object_prior, new_object_covariance)) self.prop_belief.add( gtsam.PriorFactorPose3(self.XO(obj), new_object_prior, new_object_covariance)) graph_add.add( gtsam.PriorFactorPose3(self.XO(obj), new_object_prior, new_object_covariance)) # Add to total graphs if self.initial.exists(self.XO(obj)) is False: self.initial.insert(self.XO(obj), new_object_prior) if self.prop_initial.exists(self.XO(obj)) is False: self.prop_initial.insert(self.XO(obj), new_object_prior) if initial_add.exists(self.XO(obj)) is False: initial_add.insert(self.XO(obj), new_object_prior) running_index += 1 for obj in self.classRealization: if self.initial.exists(self.XO(obj)) is False: self.initial.insert(self.XO(obj), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0))) if initial_add.exists(self.XO(obj)) is False: initial_add.insert(self.XO(obj), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0))) # Geometric measurement model self.geoModel.add_measurement(geo_measurements, da_current_step, self.graph, self.initial, self.daRealization) self.geoModel.add_measurement(geo_measurements, da_current_step, self.prop_belief, self.initial, self.daRealization) self.geoModel.add_measurement(geo_measurements, da_current_step, graph_add, self.initial, self.daRealization) self.isam.update(graph_add, initial_add) #self.isam_copy.update(graph_add, initial_add) graph_add = gtsam.NonlinearFactorGraph() initial_add = gtsam.Values() # Semantic measurement model if self.cls_enable is True: self.clsModel.add_measurement(sem_measurements, da_current_step, self.graph, self.initial, self.daRealization, class_realization_current_step) #self.clsModel.add_measurement(sem_measurements, da_current_step, graph_add, # self.initial, self.daRealization, class_realization_current_step) # Updating weights if weight_update_flag is True: if ML_update is False: self.weight_update(da_current_step, geo_measurements, sem_measurements, number_of_samples) else: self.weight_update_ML(da_current_step, geo_measurements, sem_measurements) # Semantic measurement model isam2 # if self.cls_enable is True: # self.clsModel.add_measurement(sem_measurements, da_current_step, graph_add, # self.initial, self.daRealization, class_realization_current_step) self.isam.update(graph_add, initial_add) self.optimize() self.marginals_after = gtsam.Marginals(self.graph, self.result) # Setting resultsFlag to false to require optimization self.resultsFlag = False
params.initial_pose_covariance = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) params.initial_velocity_covariance = gtsam.noiseModel_Isotropic.Sigma( 3, 0.1) ############################### Build the factor graph #################################### factor_graph = gtsam.NonlinearFactorGraph() # A technical hack for defining variable names in GTSAM python bindings def symbol(letter, index): return int(gtsam.symbol(ord(letter), index)) # Add a prior factor on the initial position factor_graph.push_back( gtsam.PriorFactorPose3(symbol('x', 0), params.initial_pose, params.initial_pose_covariance)) factor_graph.push_back( gtsam.PriorFactorVector(symbol('v', 0), params.initial_velocity, params.initial_velocity_covariance)) # Add IMU factors (or "motion model"/"transition" factors). # Ideally, we would add factors between every pair (xᵢ₋₁, xᵢ). But, to save computations, # we will add factors between pairs (x₀, xₖ), (xₖ, x₂ₖ) etc., and as an IMU "measurement" # between e.g. x₀ and xₖ we will use combined (pre-integrated) measurements `0, 1, ..., k-1`. # Below, `k == PREINTEGRATE_EVERY_STEPS`. PREINTEGRATE_EVERY_STEPS = 25 # For code generalization, create pairs (0, k), (k, 2k), (2k, 3k), ..., (mk, N-1) preintegration_steps = list( range(0, len(imu_measurements), PREINTEGRATE_EVERY_STEPS)) if preintegration_steps[-1] != len(
def build_graph(sequence, config): """ Adds noise to sequence variables / measurements. Returns graph, initial_estimate """ # create empty graph / estimate graph = gtsam.NonlinearFactorGraph() initial_estimate = gtsam.Values() # declare noise models prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([float(config['base']['PRIOR_SIGMA'])]*6, dtype=np.float)) odometry_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([float(config['base']['ODOM_SIGMA'])]*6, dtype=np.float)) bbox_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([float(config['base']['BOX_SIGMA'])]*4, dtype=np.float)) # get noisy odometry / boxes true_odometry = sequence.true_trajectory.as_odometry() noisy_odometry = true_odometry.add_noise(mu=0.0, sd=float(config['base']['ODOM_NOISE'])) noisy_boxes = sequence.true_boxes.add_noise(mu=0.0, sd=float(config['base']['BOX_NOISE'])) # initialize trajectory # TODO: ensure aligned in same reference frame initial_trajectory = noisy_odometry.as_trajectory(sequence.true_trajectory.data()[0]) # initial_trajectory = noisy_odometry.as_trajectory() # initialize quadrics # NOTE: careful initializing with true quadrics and noise traj as it may not make sense if config['base']['Initialization'] == 'SVD': initial_quadrics = System.initialize_quadrics(initial_trajectory, noisy_boxes, sequence.calibration) elif config['base']['Initialization'] == 'Dataset': initial_quadrics = sequence.true_quadrics # add prior pose prior_factor = gtsam.PriorFactorPose3(System.X(0), initial_trajectory.at(0), prior_noise) graph.add(prior_factor) # add odometry measurements for (start_key, end_key), rpose in noisy_odometry.items(): odometry_factor = gtsam.BetweenFactorPose3(System.X(start_key), System.X(end_key), rpose, odometry_noise) graph.add(odometry_factor) # add initial pose estimates for pose_key, pose in initial_trajectory.items(): initial_estimate.insert(System.X(pose_key), pose) # add valid box measurements valid_objects = [] initialized_quadrics = initial_quadrics.keys() for object_key in np.unique(noisy_boxes.object_keys()): # add if quadric initialized if object_key in initialized_quadrics: # get all views of quadric object_boxes = noisy_boxes.at_object(object_key) # add if enough views if len(object_boxes) > 3: # add measurements valid_objects.append(object_key) for (pose_key, t), box in object_boxes.items(): bbf = quadricslam.BoundingBoxFactor(box, sequence.calibration, System.X(pose_key), System.Q(object_key), bbox_noise) bbf.addToGraph(graph) # add initial landmark estimates for object_key, quadric in initial_quadrics.items(): # add if seen > 3 times if (object_key in valid_objects): quadric.addToValues(initial_estimate, System.Q(object_key)) return graph, initial_estimate
def full_bundle_adjustment_update(self, sfm_map: SfmMap): # Variable symbols for camera poses. X = gtsam.symbol_shorthand.X # Variable symbols for observed 3D point "landmarks". L = gtsam.symbol_shorthand.L # Create a factor graph. graph = gtsam.NonlinearFactorGraph() # Extract the first two keyframes (which we will assume has ids 0 and 1). kf_0 = sfm_map.get_keyframe(0) kf_1 = sfm_map.get_keyframe(1) # We will in this function assume that all keyframes have a common, given (constant) camera model. common_model = kf_0.camera_model() calibration = gtsam.Cal3_S2(common_model.fx(), common_model.fy(), 0, common_model.cx(), common_model.cy()) # Unfortunately, the dataset does not contain any information on uncertainty in the observations, # so we will assume a common observation uncertainty of 3 pixels in u and v directions. obs_uncertainty = gtsam.noiseModel.Isotropic.Sigma(2, 3.0) # Add measurements. for keyframe in sfm_map.get_keyframes(): for keypoint_id, map_point in keyframe.get_observations().items(): obs_point = keyframe.get_keypoint(keypoint_id).point() factor = gtsam.GenericProjectionFactorCal3_S2( obs_point, obs_uncertainty, X(keyframe.id()), L(map_point.id()), calibration) graph.push_back(factor) # Set prior on the first camera (which we will assume defines the reference frame). no_uncertainty_in_pose = gtsam.noiseModel.Constrained.All(6) factor = gtsam.PriorFactorPose3(X(kf_0.id()), gtsam.Pose3(), no_uncertainty_in_pose) graph.push_back(factor) # Set prior on distance to next camera. no_uncertainty_in_distance = gtsam.noiseModel.Constrained.All(1) prior_distance = 1.0 factor = gtsam.RangeFactorPose3(X(kf_0.id()), X(kf_1.id()), prior_distance, no_uncertainty_in_distance) graph.push_back(factor) # Set initial estimates from map. initial_estimate = gtsam.Values() for keyframe in sfm_map.get_keyframes(): pose_w_c = gtsam.Pose3(keyframe.pose_w_c().to_matrix()) initial_estimate.insert(X(keyframe.id()), pose_w_c) for map_point in sfm_map.get_map_points(): point_w = gtsam.Point3(map_point.point_w().squeeze()) initial_estimate.insert(L(map_point.id()), point_w) # Optimize the graph. params = gtsam.GaussNewtonParams() params.setVerbosity('TERMINATION') optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, params) print('Optimizing:') result = optimizer.optimize() print('initial error = {}'.format(graph.error(initial_estimate))) print('final error = {}'.format(graph.error(result))) # Update map with results. for keyframe in sfm_map.get_keyframes(): updated_pose_w_c = result.atPose3(X(keyframe.id())) keyframe.update_pose_w_c(SE3.from_matrix( updated_pose_w_c.matrix())) for map_point in sfm_map.get_map_points(): updated_point_w = result.atPoint3(L(map_point.id())).reshape(3, 1) map_point.update_point_w(updated_point_w) sfm_map.has_been_updated()
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.PriorFactorVector3(V(i), state.velocity(), self.velNoise))
poses.append( gtsam.SimpleCamera.Lookat(gtsam.Point3(10, 0, 0), gtsam.Point3(), gtsam.Point3(0, 0, 1)).pose()) # define quadrics quadrics = [] quadrics.append( quadricslam.ConstrainedDualQuadric(gtsam.Pose3(), np.array([1., 2., 3.]))) quadrics.append( quadricslam.ConstrainedDualQuadric( gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1, 0.1, 0.1)), np.array([1., 2., 3.]))) # add prior on first pose prior_factor = gtsam.PriorFactorPose3(X(0), poses[0], prior_noise) graph.add(prior_factor) # add trajectory estimate for i, pose in enumerate(poses): # add a perturbation to initial pose estimates to simulate noise perturbed_pose = poses[i].compose( gtsam.Pose3(gtsam.Rot3.Rodrigues(0.1, 0.1, 0.1), gtsam.Point3(0.1, 0.2, 0.3))) initial_estimate.insert(X(i), perturbed_pose) # add quadric estimate for i, quadric in enumerate(quadrics): quadric.addToValues(initial_estimate, Q(i))
# load in all clouds in our dataset clouds = read_ply(*scans_fnames) # Setting up our factor graph graph = gtsam.NonlinearFactorGraph() initial_estimates = gtsam.Values() # We get the initial pose of the car from Argo AI's dataset, and we add it to the graph as such PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas( np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) initial_pose = gtsam.Pose3( gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955, -0.0221840, -0.0116519, 0.0228910, 0.9996701), gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633)) graph.add(gtsam.PriorFactorPose3(0, initial_pose, PRIOR_NOISE)) initial_estimates.insert(0, initial_pose) # We'll use your function to populate the factor graph graph, initial_estimates = populate_factor_graph(graph, initial_estimates, initial_pose, clouds) # Now optimize for the states parameters = gtsam.GaussNewtonParams() parameters.setRelativeErrorTol(1e-5) parameters.setMaxIterations(100) optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimates, parameters) result = optimizer.optimize() """Let's plot these poses to see how our vechicle moves. Screenshot this for your reflection.
# Graph initialization graph_1 = gtsam.NonlinearFactorGraph() # Add prior lambda prior_noise = gtsam.noiseModel.Diagonal.Covariance( np.array([[3.2, 0.0, 0.0, 0.0], [0.0, 3.2, 0.0, 0.0], [0.0, 0.0, 3.2, 0.0], [0.0, 0.0, 0.0, 3.2]])) prior_noise_pose = gtsam.noiseModel.Diagonal.Variances( np.array([0.003, 0.003, 0.001, 0.003, 0.003, 0.003])) geo_noise = gtsam.noiseModel.Diagonal.Variances( np.array([0.003, 0.003, 0.001, 0.002, 0.002, 0.003])) graph_1.add( lambda_prior_factor.LambdaPriorFactor(L(0), np.array([0., 0., 0., 0.]), prior_noise)) graph_1.add( gtsam.PriorFactorPose3(X(0), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0)), prior_noise_pose)) graph_1.add( gtsam.BetweenFactorPose3(X(0), XO(1), gtsam.Pose3(gtsam.Pose2(1.0, 0.0, -0 * 3.14 / 2)), geo_noise)) graph_1.add( joint_lambda_pose_factor_fake_2d.JLPFactor( X(0), XO(1), Lambda(0), Lambda(1), [1.0, 0.0, 0.0, 0.0], [1.1, 0.0, 0.0, 0.0, 1.1, 0.0, 0.0, 1.1, 0.0, 1.1])) print(graph_1) initial = gtsam.Values() initial.insert(Lambda(0), [0.0, 0.0, 0.0, 0.0]) initial.insert(Lambda(1), [0.0, 0.0, 0.0, 0.0]) initial.insert( X(0),