def test_construct_with_matrix(): so3 = SO3.from_roll_pitch_yaw(np.pi / 10, np.pi / 4, -np.pi / 2) t = np.array([[1, 2, 3]]).T T = np.block([[so3.matrix, t], [0, 0, 0, 1]]) se3 = SE3.from_matrix(T) np.testing.assert_almost_equal(se3.rotation.matrix, so3.matrix, 14) np.testing.assert_equal(se3.translation, t)
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()