def add_point(self, pointsInitial, measurements, octave): if pointsInitial[-1] > self.depth_threshold: information = self.inv_lvl_sigma2[octave] * np.identity(2) stereo_model = gt.noiseModel_Diagonal.Information(information) huber = gt.noiseModel_mEstimator_Huber.Create(self.deltaMono) robust_model = gt.noiseModel_Robust(huber, stereo_model) factor = gt.GenericProjectionFactorCal3_S2( gt.Point2(measurements[0], measurements[2]), robust_model, X(1), L(self.counter), self.K_mono) self.is_stereo.append(False) else: information = self.inv_lvl_sigma2[octave] * np.identity(3) stereo_model = gt.noiseModel_Diagonal.Information(information) huber = gt.noiseModel_mEstimator_Huber.Create(self.deltaStereo) robust_model = gt.noiseModel_Robust(huber, stereo_model) factor = gt.GenericStereoFactor3D( gt.StereoPoint2(*tuple(measurements)), robust_model, X(1), L(self.counter), self.K_stereo) self.is_stereo.append(True) self.graph.add( gt.NonlinearEqualityPoint3(L(self.counter), gt.Point3(pointsInitial))) self.initialEstimate.insert(L(self.counter), gt.Point3(pointsInitial)) self.graph.add(factor) self.octave.append(octave) self.counter += 1
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_GenericProjectionFactorCal3_S2_factor(self, pt_uv, X_id, L_id): if pt_uv.ndim == 1: raise ValueError( "Supplied point is 1-dimensional, required Nx2 array") if pt_uv.shape[1] != 2: raise ValueError( "2nd dimension on supplied point is not 2, required Nx2 array") for pt, l in zip(pt_uv, L_id): fact = gtsam.GenericProjectionFactorCal3_S2( gtsam.gtsam.Point2(*pt), self.projection_noise, self.get_key('x', X_id), self.get_key('l', l), self.K) self.graph.push_back(fact) self.lm_factor_ids.update(L_id)
def trajectory_estimator(self, features, visible_map): """ Estimate current pose with matched features through GTSAM and update the trajectory Parameters: features: A Feature Object. Stores all matched Superpoint features. visible_map: A Map Object. Stores all matched Landmark points. pose: gtsam.pose3 Object. The pose at the last state from the atrium map trajectory. Returns: current_pose: gtsam.pose3 Object. The current estimate pose. Use input matched features as the projection factors of the graph. Use input visible_map (match landmark points of the map) as the landmark priors of the graph. Use the last time step pose from the trajectory as the initial estimate value of the current pose """ assert len(self.atrium_map.trajectory) != 0, "Trajectory is empty." pose = self.atrium_map.trajectory[len(self.atrium_map.trajectory)-1] # Initialize factor graph graph = gtsam.NonlinearFactorGraph() initialEstimate = gtsam.Values() # Add projection factors with matched feature points for all measurements measurementNoiseSigma = 1.0 measurementNoise = gtsam.noiseModel_Isotropic.Sigma( 2, measurementNoiseSigma) for i, feature in enumerate(features.key_point_list): graph.add(gtsam.GenericProjectionFactorCal3_S2( feature, measurementNoise, X(0), P(i), self.calibration)) # Create initial estimate for the pose # Because the robot moves slowly, we can use the previous pose as an initial estimation of the current pose. initialEstimate.insert(X(0), pose) # Create priors for visual # Because the map is known, we use the landmarks from the visible map with nearly zero error as priors. pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.01) for i, landmark in enumerate(visible_map.landmark_list): graph.add(gtsam.PriorFactorPoint3(P(i), landmark, pointPriorNoise)) initialEstimate.insert(P(i), landmark) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) result = optimizer.optimize() # Add current pose to the trajectory current_pose = result.atPose3(symbol(ord('x'), 0)) self.atrium_map.append_trajectory(current_pose) return current_pose
def step(data, isam, result, truth, currPoseIndex): ''' Do one step isam update @param[in] data: measurement data (odometry and visual measurements and their noiseModels) @param[in/out] isam: current isam object, will be updated @param[in/out] result: current result object, will be updated @param[in] truth: ground truth data, used to initialize new variables @param[currPoseIndex]: index of the current pose ''' # iSAM expects us to give it a new set of factors # along with initial estimates for any new variables introduced. newFactors = gtsam.NonlinearFactorGraph() initialEstimates = gtsam.Values() # Add odometry prevPoseIndex = currPoseIndex - 1 odometry = data.odometry[prevPoseIndex] newFactors.add( gtsam.BetweenFactorPose3(symbol('x', prevPoseIndex), symbol('x', currPoseIndex), odometry, data.noiseModels.odometry)) # Add visual measurement factors and initializations as necessary for k in range(len(data.Z[currPoseIndex])): zij = data.Z[currPoseIndex][k] j = data.J[currPoseIndex][k] jj = symbol('l', j) newFactors.add( gtsam.GenericProjectionFactorCal3_S2(zij, data.noiseModels.measurement, symbol('x', currPoseIndex), jj, data.K)) # TODO: initialize with something other than truth if not result.exists(jj) and not initialEstimates.exists(jj): lmInit = truth.points[j] initialEstimates.insert(jj, lmInit) # Initial estimates for the new pose. prevPose = result.atPose3(symbol('x', prevPoseIndex)) initialEstimates.insert(symbol('x', currPoseIndex), prevPose.compose(odometry)) # Update ISAM # figure(1)tic isam.update(newFactors, initialEstimates) # t=toc plot(frame_i,t,'r.') tic newResult = isam.calculateEstimate() # t=toc plot(frame_i,t,'g.') return isam, newResult
def pose_estimate(self, observations, estimated_pose): """ Estimate current pose with matched features through GTSAM and update the trajectory Parameters: observations - [(Point2(), Point3())] pose - gtsam.pose3 Object. The pose at the last state from the trajectory. Returns: current_pose - gtsam.pose3 Object. The current estimate pose. """ if len(observations) == 0: print("No observation") # Initialize factor graph graph = gtsam.NonlinearFactorGraph() initial_estimate = gtsam.Values() measurement_noise_sigma = 1.0 measurement_noise = gtsam.noiseModel_Isotropic.Sigma( 2, measurement_noise_sigma) # Because the map is known, we use the landmarks from the visible map with nearly zero error as priors. point_prior_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.01) for i, observation in enumerate(observations): print(i) # Add projection factors with matched feature points graph.add( gtsam.GenericProjectionFactorCal3_S2(observation[0], measurement_noise, X(0), P(i), self.calibration)) # Create priors for all observations graph.add( gtsam.PriorFactorPoint3(P(i), observation[1], point_prior_noise)) initial_estimate.insert(P(i), observation[1]) # Create initial estimate for the pose # Because the robot moves slowly, we can use the previous pose as an initial estimation of the current pose. initial_estimate.insert(X(0), estimated_pose) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate) result = optimizer.optimize() # Add current pose to the trajectory current_pose = result.atPose3(X(0)) print(current_pose) return current_pose
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 bundle_adjustment(self, data, y_distance, rotation_error, translation_error): """ Use GTSAM to solve Structure from Motion bundle adjustment. Parameters: data -- a Data Object, input feature point data from the sfm_data.py file y_distance -- distances between two continuous poses along the y axis rotation_error - pose prior rotation error translation_error - pose prior translation error Returns: result -- GTSAM optimization result """ # Initialize factor Graph graph = gtsam.NonlinearFactorGraph() initialEstimate = gtsam.Values() # Add factors for all measurements measurementNoiseSigma = 1.0 measurementNoise = gtsam.noiseModel_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][j], measurementNoise, X(i), P(j), self.calibration)) # Create priors and initial estimate s = np.radians(60) poseNoiseSigmas = np.array([s, s, s, 10, 10, 10]) posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas) for i, y in enumerate([0, y_distance, 2 * y_distance]): # Do not consider camera rotation when creating Rot3 wRc = gtsam.Rot3(np.array([[0, 1, 0], [0, 0, -1], [1, 0, 0]]).T) # The approximate height measurement is 1.5 wTi = gtsam.Pose3(wRc, gtsam.Point3(0, y, 1.5)) initialEstimate.insert(X(i), wTi) # Add priors for two poses # Add the first pose prior with error to test similarity transform. graph.add( gtsam.PriorFactorPose3( X(0), gtsam.Pose3( gtsam.Rot3( np.array([[ 0 + rotation_error, 1 + rotation_error, 0 + rotation_error ], [ 0 + rotation_error, 0 + rotation_error, -1 + rotation_error ], [ 1 + rotation_error, 0 + rotation_error, 0 + rotation_error ]]).T), gtsam.Point3(0 + translation_error, 0 + translation_error, 1.5 + translation_error)), posePriorNoise)) graph.add( gtsam.PriorFactorPose3( X(1), gtsam.Pose3(wRc, gtsam.Point3(0, y_distance, 1.5)), posePriorNoise)) # Add initial estimates for Points. for j in range(self.nrPoints): # Generate initial estimates by back projecting key points extracted at the initial pose. The scale is set based on experience. point_j = self.back_project(data.Z[0][j], self.calibration, 15) initialEstimate.insert(P(j), point_j) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) result = optimizer.optimize() # Marginalization marginals = gtsam.Marginals(graph, result) return result
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 bundle_adjustment(self): MIN_LANDMARK_SEEN = 3 depth = 15 # Initialize factor Graph graph = gtsam.NonlinearFactorGraph() initialEstimate = gtsam.Values() # Add factors for all measurements measurementNoiseSigma = 1.0 measurementNoise = gtsam.noiseModel_Isotropic.Sigma( 2, measurementNoiseSigma) landmark_id_list = {} img_pose_id_list = {} for i in range(self.row*self.col*self.angles): ir,i_mod = divmod(i,self.angles*self.col) ic, ia = divmod(i_mod, self.angles) img_pose = self.img_pose[ir][ic][ia] for k in range(len(img_pose.kp)): if img_pose.kp_3d_exist(k): landmark_id = img_pose.kp_3d(k) if(self.landmark[landmark_id].seen >= MIN_LANDMARK_SEEN): key_point = Point2(img_pose.kp[k][0],img_pose.kp[k][1]) if landmark_id_list.get(landmark_id) == None: pose = self.pose_initial[ir][ic][ia] landmark_point = self.back_projection(key_point,pose,depth) landmark_id_list[landmark_id] = landmark_point # print("careful",ir,ic,ia,pose,landmark_point) if img_pose_id_list.get(i) == None: img_pose_id_list[i] = True graph.add(gtsam.GenericProjectionFactorCal3_S2( key_point, measurementNoise, X(i), P(landmark_id), self.cal)) s = np.radians(60) poseNoiseSigmas = np.array([s, s, s, 10, 10, 10]) # poseNoiseSigmas = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas) print(img_pose_id_list) for i,idx in enumerate(img_pose_id_list): ir,i_mod = divmod(idx,self.angles*self.col) ic, ia = divmod(i_mod, self.angles) # Initialize estimate for poses pose_i = self.pose_initial[ir][ic][ia] initialEstimate.insert(X(idx), pose_i) # Create priors for poses # counter = 0 # if(counter == 0 and ia == 4): # temp_a = ia # temp_r = ir # temp_c = ic # graph.add(gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise)) # counter+=1 # if(counter == 1 and temp_a == ia and (temp_r-ir)**2 + (temp_c-ic)**2>=4): # graph.add(gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise)) # counter +=1 # if i <2: # graph.add(gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise)) graph.add(gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise)) # Initialize estimate for landmarks print(landmark_id_list) for idx in landmark_id_list: point_j = landmark_id_list.get(idx) initialEstimate.insert(P(idx), point_j) self.landmark_descriptor.append(self.landmark[idx].desc) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) sfm_result = optimizer.optimize() # Marginalization marginals = gtsam.Marginals(graph, sfm_result) for idx in img_pose_id_list: marginals.marginalCovariance(X(idx)) for idx in landmark_id_list: marginals.marginalCovariance(P(idx)) return sfm_result,img_pose_id_list,landmark_id_list
def bundle_adjustment(self): MIN_LANDMARK_SEEN = 3 # Initialize factor Graph graph = gtsam.NonlinearFactorGraph() initialEstimate = gtsam.Values() # Add factors for all measurements measurementNoiseSigma = 1.0 measurementNoise = gtsam.noiseModel_Isotropic.Sigma( 2, measurementNoiseSigma) landmark_id_list = {} img_pose_id_list = {} for i, img_pose in enumerate(self.img_pose): for k in range(len(img_pose.kp)): if img_pose.kp_3d_exist(k): landmark_id = img_pose.kp_3d(k) if (self.landmark[landmark_id].seen >= MIN_LANDMARK_SEEN): landmark_id_list[landmark_id] = True img_pose_id_list[i] = True graph.add( gtsam.GenericProjectionFactorCal3_S2( Point2(img_pose.kp[k][0], img_pose.kp[k][1]), measurementNoise, X(i), P(landmark_id), self.cal)) # Initialize estimate for poses # Create priors s = np.radians(60) poseNoiseSigmas = np.array([s, s, s, 10, 10, 10]) # poseNoiseSigmas = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas) # theta = np.radians(30) # wRc = gtsam.Rot3(np.array([[-math.sin(theta), 0 , math.cos(theta)], # [-math.cos(theta), 0 , -math.sin(theta)], # [0, 1 , 0]])) wRc = gtsam.Rot3(np.identity(3)) for i, idx in enumerate(img_pose_id_list): initialEstimate.insert(X(idx), self.img_pose[idx].T) if i < 2: # graph.add(gtsam.PriorFactorPose3(X(idx), gtsam.Pose3( # wRc, gtsam.Point3(-math.sin(theta)*1.58*idx, -math.cos(theta)*1.58*idx, 1.2)), posePriorNoise)) graph.add( gtsam.PriorFactorPose3( X(idx), gtsam.Pose3(wRc, gtsam.Point3(1.38 * idx, 0, -0.29 * idx)), posePriorNoise)) # i+=1 # Initialize estimate for landmarks for idx in landmark_id_list: p = self.landmark[idx].point initialEstimate.insert(P(idx), Point3(p[0], p[1], p[2])) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) # sfm_result = optimizer.optimize() for i in range(1000): optimizer.iterate() sfm_result = optimizer.values() # print(sfm_result) # Marginalization marginals = gtsam.Marginals(graph, sfm_result) for idx in img_pose_id_list: marginals.marginalCovariance(X(idx)) for idx in landmark_id_list: marginals.marginalCovariance(P(idx)) nrCamera = len(img_pose_id_list) nrPoint = len(landmark_id_list) return sfm_result, img_pose_id_list, landmark_id_list
def add_keypoints(self, vision_data, measured_poses, n_skip, depth): measured_poses = np.reshape(measured_poses, (-1, 4, 4)) # pose_key = X(0) # self.initial_estimate.insert(pose_key, gtsam.Pose3(measured_poses[0])) # R_rect = np.array([[9.999239e-01, 9.837760e-03, -7.445048e-03, 0.], # [ -9.869795e-03, 9.999421e-01, -4.278459e-03, 0.], # [ 7.402527e-03, 4.351614e-03, 9.999631e-01, 0.], # [ 0., 0., 0., 1.]]) # R_cam_velo = np.array([[7.533745e-03, -9.999714e-01, -6.166020e-04], # [ 1.480249e-02, 7.280733e-04, -9.998902e-01], # [ 9.998621e-01, 7.523790e-03, 1.480755e-02]]) # R_velo_imu = np.array([[9.999976e-01, 7.553071e-04, -2.035826e-03], # [-7.854027e-04, 9.998898e-01, -1.482298e-02], # [2.024406e-03, 1.482454e-02, 9.998881e-01]]) # t_cam_velo = np.array([-4.069766e-03, -7.631618e-02, -2.717806e-01]) # t_velo_imu = np.array([-8.086759e-01, 3.195559e-01, -7.997231e-01]) # T_velo_imu = np.zeros((4,4)) # T_cam_velo = np.zeros((4,4)) # T_velo_imu[3,3] = 1. # T_cam_velo[3,3] = 1. # T_velo_imu[:3,:3] = R_velo_imu # T_velo_imu[:3,3] = t_velo_imu # T_cam_velo[:3,:3] = R_cam_velo # T_cam_velo[:3,3] = t_cam_velo # cam_to_imu = R_rect @ T_cam_velo @ T_velo_imu # CAM_TO_IMU_POSE = gtsam.Pose3(cam_to_imu) # imu_to_cam = np.linalg.inv(cam_to_imu) # IMU_TO_CAM_POSE = gtsam.Pose3(imu_to_cam) # K_np = np.array([[9.895267e+02, 0.000000e+00, 7.020000e+02], # [0.000000e+00, 9.878386e+02, 2.455590e+02], # [0.000000e+00, 0.000000e+00, 1.000000e+00]]) K_np = self.K K = gtsam.Cal3_S2(K_np[0, 0], K_np[1, 1], 0., K_np[0, 2], K_np[1, 2]) valid_track = np.zeros(vision_data.shape[0], dtype=bool) N = vision_data.shape[1] for i in range(vision_data.shape[0]): track_length = N - np.sum(vision_data[i, :, 0] == -1) if track_length > 1 and track_length < 0.5 * N: valid_track[i] = True count = 0 measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 10.0) for i in range(0, vision_data.shape[0], 20): if not valid_track[i]: continue key_point_initialized = False for j in range(vision_data.shape[1] - 1): if vision_data[i, j, 0] >= 0: zp = float(depth[j * n_skip][vision_data[i, j, 1], vision_data[i, j, 0]]) if zp == 0: continue self.graph.push_back( gtsam.GenericProjectionFactorCal3_S2( vision_data[i, j, :], measurement_noise, X(j), L(i), K, gtsam.Pose3(np.eye(4)))) if not key_point_initialized: count += 1 # Initialize landmark 3D coordinates fx = K_np[0, 0] fy = K_np[1, 1] cx = K_np[0, 2] cy = K_np[1, 2] # Depth: zp = float(depth[j * n_skip][vision_data[i, j, 1], vision_data[i, j, 0]]) xp = float(vision_data[i, j, 0] - cx) / fx * zp yp = float(vision_data[i, j, 1] - cy) / fy * zp # Convert to global Xg = measured_poses[j * n_skip] @ np.array( [xp, yp, zp, 1]) self.initial_estimate.insert(L(i), Xg[:3]) key_point_initialized = True print('==> Using ', count, ' tracks')
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 optimize(self, flag_verbose=False): # optimize edge_outlier = np.full(self.counter - 1, False) error_th_stereo = [7.815, 7.815, 5, 5] error_th_mono = [5.991, 5.991, 3.5, 3.5] # error_th_stereo = [7.815, 7.815, 7.815, 7.815] # error_th_mono = [5.991, 5.991, 5.991, 5.991] for iteration in range(4): if flag_verbose: errors = [] optimizer = gt.LevenbergMarquardtOptimizer(self.graph, self.initialEstimate) result = optimizer.optimize() n_bad = 0 if flag_verbose: print( f"Number of Factors: {self.graph.nrFactors()-self.graph.size()//2, self.graph.size()//2}" ) error_s = error_th_stereo[iteration] error_m = error_th_mono[iteration] for idx in range(1, self.graph.size(), 2): try: if self.is_stereo[idx]: factor = gt.dynamic_cast_GenericStereoFactor3D_NonlinearFactor( self.graph.at(idx)) else: factor = gt.dynamic_cast_GenericProjectionFactorCal3_S2_NonlinearFactor( self.graph.at(idx)) except: if flag_verbose: errors.append(0) continue error = factor.error(result) # print(error) if flag_verbose: errors.append(error) # if error > 7.815: if (self.is_stereo[idx] and error > error_s) or (not self.is_stereo[idx] and error > error_m): edge_outlier[idx // 2] = True self.graph.remove(idx) n_bad += 1 else: edge_outlier[idx // 2] = False if iteration == 2: if self.is_stereo[idx]: information = self.inv_lvl_sigma2[self.octave[ idx // 2]] * np.identity(3) stereo_model = gt.noiseModel_Diagonal.Information( information) new_factor = gt.GenericStereoFactor3D( factor.measured(), stereo_model, X(1), L(idx // 2 + 1), self.K_stereo) else: information = self.inv_lvl_sigma2[self.octave[ idx // 2]] * np.identity(2) stereo_model = gt.noiseModel_Diagonal.Information( information) new_factor = gt.GenericProjectionFactorCal3_S2( factor.measured(), stereo_model, X(1), L(idx // 2 + 1), self.K_mono) self.graph.replace(idx, new_factor) if flag_verbose: fig, ax = plt.subplots() ax.bar(np.arange(0, len(errors)).tolist(), errors) plt.show() print("NUM BADS: ", n_bad) pose = result.atPose3(X(1)) # marginals = gt.Marginals(self.graph, result) # cov = marginals.marginalCovariance(gt.X(1)) return pose, edge_outlier # self.graph, result
def bundle_adjustment(self): MIN_LANDMARK_SEEN = 3 theta = np.radians(30) wRc = Rot3( np.array([[-math.sin(theta), 0, math.cos(theta)], [-math.cos(theta), 0, -math.sin(theta)], [0, -1, 0]])) depth = 15 # Initialize factor Graph graph = gtsam.NonlinearFactorGraph() initialEstimate = gtsam.Values() # Add factors for all measurements measurementNoiseSigma = 1.0 measurementNoise = gtsam.noiseModel_Isotropic.Sigma( 2, measurementNoiseSigma) landmark_id_list = {} img_pose_id_list = {} for i, img_pose in enumerate(self.img_pose): for k in range(len(img_pose.kp)): if img_pose.kp_3d_exist(k): landmark_id = img_pose.kp_3d(k) if (self.landmark[landmark_id].seen >= MIN_LANDMARK_SEEN): key_point = Point2(img_pose.kp[k][0], img_pose.kp[k][1]) if landmark_id_list.get(landmark_id) == None: pose = Pose3( wRc, Point3(-math.sin(theta) * 1.58 * i, -math.cos(theta) * 1.58 * i, 1.2)) landmark_point = self.back_projection( key_point, pose, depth) landmark_id_list[landmark_id] = landmark_point if img_pose_id_list.get(i) == None: img_pose_id_list[i] = True graph.add( gtsam.GenericProjectionFactorCal3_S2( key_point, measurementNoise, X(i), P(landmark_id), self.cal)) s = np.radians(60) poseNoiseSigmas = np.array([s, s, s, 10, 10, 10]) # poseNoiseSigmas = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas) for i, idx in enumerate(img_pose_id_list): # Initialize estimate for poses pose_i = Pose3( wRc, Point3(-math.sin(theta) * 1.58 * idx, -math.cos(theta) * 1.58 * idx, 1.2)) initialEstimate.insert(X(idx), pose_i) # Create priors for poses if (idx == 0 or idx == 1): graph.add( gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise)) # graph.add(gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise)) # Initialize estimate for landmarks for idx in landmark_id_list: point_j = landmark_id_list.get(idx) initialEstimate.insert(P(idx), point_j) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate) sfm_result = optimizer.optimize() # Marginalization marginals = gtsam.Marginals(graph, sfm_result) for idx in img_pose_id_list: marginals.marginalCovariance(X(idx)) for idx in landmark_id_list: marginals.marginalCovariance(P(idx)) return sfm_result, img_pose_id_list, landmark_id_list
def bundle_adjustment(cal, kp1, kp2, undist=0): """ Use GTSAM to solve Structure from Motion. Parameters: cal - camera calibration matrix,gtsam.Cal3_S2/ gtsam.Cal3DS2 kp1 - keypoints extracted at camera pose 1,gtsam.Point2 kp2 - keypoints extracted at camera pose 2,gtsam.Point2 Output: landmarks - a list includes landmark points, gtsam.Point3 poses - a list includes camera poses, gtsam.Pose3 """ # Initialize factor Graph graph = gtsam.NonlinearFactorGraph() initial_estimate = gtsam.Values() # Add factors for all measurements measurement_noise_sigma = 1.0 measurement_noise = gtsam.noiseModel_Isotropic.Sigma( 2, measurement_noise_sigma) for i in range(6): if undist == 0: graph.add(gtsam.GenericProjectionFactorCal3DS2( kp1[i], measurement_noise, X(0), P(i), cal)) graph.add(gtsam.GenericProjectionFactorCal3DS2( kp2[i], measurement_noise, X(1), P(i), cal)) if undist == 1: graph.add(gtsam.GenericProjectionFactorCal3_S2( kp1[i], measurement_noise, X(0), P(i), cal)) graph.add(gtsam.GenericProjectionFactorCal3_S2( kp2[i], measurement_noise, X(1), P(i), cal)) # Create priors and initial estimate s = np.radians(60) # pylint: disable=invalid-name pose_noise_sigmas = np.array([s, s, s, 1, 1, 1]) # pose_noise_sigmas = np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) pose_prior_noise = gtsam.noiseModel_Diagonal.Sigmas(pose_noise_sigmas) # Create Rot3 wRc = Rot3(np.array([[0, 1, 0], [0, 0, -1], [1, 0, 0]] ).T) # pylint: disable=invalid-name for i, y in enumerate([0, 5*1.58]): # The approximate height measurement is 1.2 initial_estimate.insert(X(i), Pose3(wRc, Point3(0, y, 1.2))) graph.add(gtsam.PriorFactorPose3(X(i), Pose3( wRc, Point3(0, y, 1.2)), pose_prior_noise)) # Add initial estimates for Points. for j in range(6): # Generate initial estimates by back projecting feature points collected at the initial pose point_j = back_projection(cal, kp1[j], Pose3( wRc, Point3(0, 0, 1.2)), 30, undist) initial_estimate.insert(P(j), point_j) # Optimization optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate) result = optimizer.optimize() # Why this is not working? # tol_error = gtsam.reprojectionErrors(graph, result) # print("tol_error: ", tol_error) error = graph.error(result) print("Graph error:", error) # print(result) plot_with_result(result) landmarks = [result.atPoint3(P(i))for i in range(6)] poses = [result.atPose3(X(0)), result.atPose3(X(1))] return landmarks, poses
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