def test_TriangulationExample(self): """ Tests triangulation with shared Cal3_S2 calibration""" # Some common constants sharedCal = (1500, 1200, 0, 640, 480) measurements, _ = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, (sharedCal, sharedCal)) triangulated_landmark = gtsam.triangulatePoint3(self.poses, Cal3_S2(sharedCal), measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) measurements_noisy = Point2Vector() measurements_noisy.append(measurements[0] - np.array([0.1, 0.5])) measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3])) triangulated_landmark = gtsam.triangulatePoint3(self.poses, Cal3_S2(sharedCal), measurements_noisy, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-2)
def test_values(self): values = Values() E = EssentialMatrix(Rot3(), Unit3()) tol = 1e-9 values.insert(0, Point2(0, 0)) values.insert(1, Point3(0, 0, 0)) values.insert(2, Rot2()) values.insert(3, Pose2()) values.insert(4, Rot3()) values.insert(5, Pose3()) values.insert(6, Cal3_S2()) values.insert(7, Cal3DS2()) values.insert(8, Cal3Bundler()) values.insert(9, E) values.insert(10, imuBias_ConstantBias()) # Special cases for Vectors and Matrices # Note that gtsam's Eigen Vectors and Matrices requires double-precision # floating point numbers in column-major (Fortran style) storage order, # whereas by default, numpy.array is in row-major order and the type is # in whatever the number input type is, e.g. np.array([1,2,3]) # will have 'int' type. # # The wrapper will automatically fix the type and storage order for you, # but for performance reasons, it's recommended to specify the correct # type and storage order. # for vectors, the order is not important, but dtype still is vec = np.array([1., 2., 3.]) values.insert(11, vec) mat = np.array([[1., 2.], [3., 4.]], order='F') values.insert(12, mat) # Test with dtype int and the default order='C' # This still works as the wrapper converts to the correct type and order for you # but is nornally not recommended! mat2 = np.array([[1, 2, ], [3, 5]]) values.insert(13, mat2) self.assertTrue(values.atPoint2(0).equals(Point2(0,0), tol)) self.assertTrue(values.atPoint3(1).equals(Point3(0,0,0), tol)) self.assertTrue(values.atRot2(2).equals(Rot2(), tol)) self.assertTrue(values.atPose2(3).equals(Pose2(), tol)) self.assertTrue(values.atRot3(4).equals(Rot3(), tol)) self.assertTrue(values.atPose3(5).equals(Pose3(), tol)) self.assertTrue(values.atCal3_S2(6).equals(Cal3_S2(), tol)) self.assertTrue(values.atCal3DS2(7).equals(Cal3DS2(), tol)) self.assertTrue(values.atCal3Bundler(8).equals(Cal3Bundler(), tol)) self.assertTrue(values.atEssentialMatrix(9).equals(E, tol)) self.assertTrue(values.atimuBias_ConstantBias( 10).equals(imuBias_ConstantBias(), tol)) # special cases for Vector and Matrix: actualVector = values.atVector(11) self.assertTrue(np.allclose(vec, actualVector, tol)) actualMatrix = values.atMatrix(12) self.assertTrue(np.allclose(mat, actualMatrix, tol)) actualMatrix2 = values.atMatrix(13) self.assertTrue(np.allclose(mat2, actualMatrix2, tol))
def run(): """Execution.""" # Input images(undistorted) calibration fov, w, h = 60, 1280, 720 calibration = Cal3_S2(fov, w, h) # Camera to world rotation wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name # Create pose estimates pose_estimates = [Pose3(wRc, Point3(0, i, 2)) for i in range(3)] # Create measurement noise for bundle adjustment sigma = 1.0 measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, sigma) # Create 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 ]) pose_prior_noise = gtsam.noiseModel_Diagonal.Sigmas(pose_noise_sigmas) # Create MappingBackEnd instance data_directory = 'mapping/datasets/sim_match_data/' num_images = 3 min_landmark_seen = 3 back_end = MappingBackEnd(data_directory, num_images, calibration, pose_estimates, measurement_noise, pose_prior_noise, min_landmark_seen) # Bundle Adjustment tic_ba = time.time() sfm_result, poses, points = back_end.bundle_adjustment() toc_ba = time.time() print('BA spents ', toc_ba - tic_ba, 's') # Plot Result plot_sfm_result(sfm_result, poses, points)
def setUp(self): """Create mapping Back-end and read csv file.""" # Input images(undistorted) calibration fov, w, h = 60, 1280, 720 calibration = Cal3_S2(fov, w, h) # Create pose priors wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # camera to world rotation pose_estimates = [Pose3(wRc, Point3(0, i, 2)) for i in range(3)] # Create measurement noise for bundle adjustment sigma = 1.0 measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, sigma) # Create 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 ]) pose_prior_noise = gtsam.noiseModel_Diagonal.Sigmas(pose_noise_sigmas) # Create MappingBackEnd instance data_directory = 'mapping/sim_match_data/' min_landmark_seen = 3 self.num_images = 3 self.back_end = MappingBackEnd( data_directory, self.num_images, calibration, pose_estimates, measurement_noise, pose_prior_noise, min_landmark_seen) # pylint: disable=line-too-long
def test_back_projection(self): """Test back projection""" fov, width, height = 60, 1280, 720 calibration = Cal3_S2(fov, width, height) actual = back_projection( calibration, Point2(640, 360), Pose3(Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0), Point3()), 20) expected = Point3(0, 20, 0) self.gtsamAssertEquals(actual, expected) fov, width, height = 60, 640, 480 calibration = Cal3_S2(fov, width, height) actual = back_projection( calibration, Point2(320, 240), Pose3(Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0), Point3()), 20) expected = Point3(0, 20, 0) self.gtsamAssertEquals(actual, expected)
def __init__(self, triangle: bool = False, nrCameras: int = 3, K=Cal3_S2()) -> None: """ Options to generate test scenario @param triangle: generate a triangle scene with 3 points if True, otherwise a cube with 8 points @param nrCameras: number of cameras to generate @param K: camera calibration object """ self.triangle = triangle self.nrCameras = nrCameras
def run(): """Execution""" basedir = 'calibration/undistort_images/distort_images/' img_extension = '.jpg' output_dir = 'calibration/undistort_images/undistort_images/' output_prefix = 'undist_image' calibration = Cal3_S2(fx=347.820593, fy=329.096945, s=0, u0=295.717950, v0=222.964889).matrix() distortion = np.array([-0.284322, 0.055723, 0.006772, 0.005264, 0.000000]) undist_calibration = Cal3_S2(fx=232.0542, fy=252.8620, s=0, u0=325.3452, v0=240.2912).matrix() undistort(basedir, img_extension, output_dir, output_prefix, calibration, distortion, undist_calibration)
def run(): """Execution.""" # Input images(undistorted) calibration calibration = Cal3_S2( fx=232.0542, fy=252.8620, s=0, u0=325.3452, v0=240.2912) # Camera to world rotation wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name # Create pose estimates pose_estimates = [Pose3(wRc, Point3(1.58*i, 0, 1.2)) for i in range(6)] # Create measurement noise for bundle adjustment sigma = 1.0 measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, sigma) # Create 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]) pose_prior_noise = gtsam.noiseModel_Diagonal.Sigmas(pose_noise_sigmas) #"""Use 4D agri matching""" data_directory = 'mapping/datasets/Klaus_4d_agri_match_data/' num_images = 6 min_obersvation_number = 6 filter_bad_landmarks_enable = True prob = 0.9 threshold = 3 backprojection_depth = 15 back_end = MappingBackEnd(data_directory, num_images, calibration, pose_estimates, measurement_noise, pose_prior_noise, filter_bad_landmarks_enable, min_obersvation_number, prob, threshold, backprojection_depth) # Bundle Adjustment tic_ba = time.time() sfm_result1 = back_end.bundle_adjustment() toc_ba = time.time() print('BA spents ', toc_ba-tic_ba, 's') # Save map data back_end.save_map_to_file(sfm_result1) back_end.save_poses_to_file(sfm_result1) #"""Use Superpoint matching""" # Create MappingBackEnd instance data_directory = 'mapping/datasets/Klaus_Superpoint_match_data/' # data_directory = 'feature_matcher/Klaus_filter_match_data/' num_images = 6 min_obersvation_number = 3 back_end = MappingBackEnd(data_directory, num_images, calibration, pose_estimates, measurement_noise, pose_prior_noise, min_obersvation_number) # Bundle Adjustment tic_ba = time.time() sfm_result2 = back_end.bundle_adjustment() toc_ba = time.time() print('BA spents ', toc_ba-tic_ba, 's') # # Plot Result plot_with_results(sfm_result1, sfm_result2, 30, 30, 30)
def run(): """Execution.""" # Input images(undistorted) calibration calibration = Cal3_S2(fx=232.0542, fy=252.8620, s=0, u0=325.3452, v0=240.2912) # Create pose estimates theta = 45 delta_x = 1 delta_y = -0.5 delta_z = 1.2 rows = 2 cols = 2 angles = 8 prior1_delta = [0, -1, 1.2, 0] prior2_delta = [1, -1, 1.2, 0] pose_estimates = pose_estimate_generator(theta, delta_x, delta_y, delta_z, prior1_delta, prior2_delta, rows, cols, angles) # Create measurement noise for bundle adjustment sigma = 1.0 measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, sigma) # Create 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 ]) pose_prior_noise = gtsam.noiseModel_Diagonal.Sigmas(pose_noise_sigmas) # Create MappingBackEnd instance data_directory = 'mapping/datasets/library_data/library_4X8/undistort_images/features/' num_images = 34 filter_bad_landmarks_enable = True min_obersvation_number = 3 prob = 0.9 threshold = 3 backprojection_depth = 2 back_end = MappingBackEnd(data_directory, num_images, calibration, pose_estimates, measurement_noise, pose_prior_noise, filter_bad_landmarks_enable, min_obersvation_number, prob, threshold, backprojection_depth) # Bundle Adjustment tic_ba = time.time() sfm_result = back_end.bundle_adjustment() toc_ba = time.time() print('BA spents ', toc_ba - tic_ba, 's') # Plot Result plot_with_result(sfm_result, 5, 5, 5, 0.5)
def setUp(self): # Camera to world rotation wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name initial_pose = Pose3(wRc, Point3(0, 0, 1.2)) file_name = "" self.calibration = Cal3_S2(fx=1, fy=1, s=0, u0=320, v0=240) self.image_size = (640, 480) l2_thresh = 0.7 distance_thresh = [5, 5] self.trajectory_estimator = TrajectoryEstimator( initial_pose, file_name, self.calibration, self.image_size, l2_thresh, distance_thresh)
def setUp(self): image_directory_path = 'superpoint_descriptor/undistort_images/' image_extension = '*.jpg' image_size = (640, 480) nn_thresh = 0.7 # Input images(undistorted) calibration self.calibration = Cal3_S2(fx=232.0542, fy=252.8620, s=0, u0=325.3452, v0=240.2912).matrix() self.front_end = SuperpointWrapper(image_directory_path, image_extension, image_size, nn_thresh)
def run(): """Execution.""" # Camera to world rotation wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name initial_pose = Pose3(wRc, Point3(1.58, 0, 1.2)) directory_name = "localization/data_sets/Klaus_1X6_Localization/" calibration = Cal3_S2(fx=232.0542, fy=252.8620, s=0, u0=325.3452, v0=240.2912) image_size = (640, 480) l2_thresh = 0.7 distance_thresh = [5, 5] trajectory_estimator = TrajectoryEstimator(initial_pose, directory_name, calibration, image_size, l2_thresh, distance_thresh) camid = 1 skip = 1 start_index = 0 img_glob = "*.jpg" distort_calibration = Cal3_S2(fx=347.820593, fy=329.096945, s=0, u0=295.717950, v0=222.964889).matrix() distortion = np.array([-0.284322, 0.055723, 0.006772, 0.005264, 0.000000]) trajectory = trajectory_estimator.trajectory_generator( distort_calibration, distortion, directory_name, camid, skip, img_glob, start_index) actual_poses = load_poses_from_file(directory_name + "poses.dat") plot_trajectory_verification(trajectory_estimator.map.landmarks, actual_poses, trajectory)
def test_computation_graph(self): """Test the dask computation graph execution using a valid collection of relative unit-translations.""" """Test a simple case with 8 camera poses. The camera poses are arranged on the circle and point towards the center of the circle. The poses of 8 cameras are obtained from SFMdata and the unit translations directions between some camera pairs are computed from their global translations. This test is copied from GTSAM's TranslationAveragingExample. """ fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0 expected_wTi_list = SFMdata.createPoses(Cal3_S2(fx, fy, s, u0, v0)) wRi_list = [x.rotation() for x in expected_wTi_list] # create relative translation directions between a pose index and the # next two poses i2Ui1_dict = {} for i1 in range(len(expected_wTi_list) - 1): for i2 in range(i1 + 1, min(len(expected_wTi_list), i1 + 3)): # create relative translations using global R and T. i2Ui1_dict[(i1, i2)] = Unit3(expected_wTi_list[i2].between( expected_wTi_list[i1]).translation()) # use the `run` API to get expected results expected_wti_list = self.obj.run(len(wRi_list), i2Ui1_dict, wRi_list) expected_wTi_list = [ Pose3(wRi, wti) if wti is not None else None for (wRi, wti) in zip(wRi_list, expected_wti_list) ] # form computation graph and execute i2Ui1_graph = dask.delayed(i2Ui1_dict) wRi_graph = dask.delayed(wRi_list) computation_graph = self.obj.create_computation_graph( len(wRi_list), i2Ui1_graph, wRi_graph) with dask.config.set(scheduler="single-threaded"): wti_list = dask.compute(computation_graph)[0] wTi_list = [ Pose3(wRi, wti) if wti is not None else None for (wRi, wti) in zip(wRi_list, wti_list) ] # compare the entries self.assertTrue( geometry_comparisons.compare_global_poses(wTi_list, expected_wTi_list))
def run(): """Execution.""" basedir = 'superpoint_descriptor/' image_extension = '*.jpg' image_size = (640, 480) number_images = 6 # Input images(undistorted) calibration calibration = Cal3_S2(fx=232.0542, fy=252.8620, s=0, u0=325.3452, v0=240.2912).matrix() feature_matcher = FeatureMatcher(basedir, image_extension, image_size, number_images) feature_matcher.feature_matching(image_size, 'Superpoint', 'FLANN', calibration)
def __init__(self, K=Cal3_S2(), nrCameras: int = 3, nrPoints: int = 4) -> None: self.K = K self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras] self.J = [x[:] for x in [[0] * nrPoints] * nrCameras] self.odometry = [Pose3()] * nrCameras # Set Noise parameters self.noiseModels = Data.NoiseModels() self.noiseModels.posePrior = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])) # noiseModels.odometry = gtsam.noiseModel.Diagonal.Sigmas( # np.array([0.001,0.001,0.001,0.1,0.1,0.1])) self.noiseModels.odometry = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.05, 0.05, 0.05, 0.2, 0.2, 0.2])) self.noiseModels.pointPrior = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) self.noiseModels.measurement = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)
def generate_data(options) -> Tuple[Data, GroundTruth]: """ Generate ground-truth and measurement data. """ K = Cal3_S2(500, 500, 0, 640. / 2., 480. / 2.) nrPoints = 3 if options.triangle else 8 truth = GroundTruth(K=K, nrCameras=options.nrCameras, nrPoints=nrPoints) data = Data(K, nrCameras=options.nrCameras, nrPoints=nrPoints) # Generate simulated data if options.triangle: # Create a triangle target, just 3 points on a plane r = 10 for j in range(len(truth.points)): theta = j * 2 * pi / nrPoints truth.points[j] = Point3( r * math.cos(theta), r * math.sin(theta), 0) else: # 3D landmarks as vertices of a cube truth.points = [ Point3(10, 10, 10), Point3(-10, 10, 10), Point3(-10, -10, 10), Point3(10, -10, 10), Point3(10, 10, -10), Point3(-10, 10, -10), Point3(-10, -10, -10), Point3(10, -10, -10) ] # Create camera cameras on a circle around the triangle height = 10 r = 40 for i in range(options.nrCameras): theta = i * 2 * pi / options.nrCameras t = Point3(r * math.cos(theta), r * math.sin(theta), height) truth.cameras[i] = PinholeCameraCal3_S2.Lookat(t, Point3(0, 0, 0), Point3(0, 0, 1), truth.K) # Create measurements for j in range(nrPoints): # All landmarks seen in every frame data.Z[i][j] = truth.cameras[i].project(truth.points[j]) data.J[i][j] = j # Calculate odometry between cameras for i in range(1, options.nrCameras): data.odometry[i] = truth.cameras[i - 1].pose().between( truth.cameras[i].pose()) return data, truth
def setUp(self): # Camera to world rotation wRc = Rot3(1, 0, 0, 0, 0, 1, 0, -1, 0) # pylint: disable=invalid-name self.initial_pose = Pose3(wRc, Point3(0, 0, 1.2)) self.directory_name = "localization/data_sets/Klaus_1/" calibration = Cal3_S2(fx=232.0542, fy=252.8620, s=0, u0=325.3452, v0=240.2912) image_size = (640, 480) l2_thresh = 0.7 distance_thresh = [5, 5] self.trajectory_estimator = TrajectoryEstimator( self.initial_pose, self.directory_name, calibration, image_size, l2_thresh, distance_thresh)
def test_trajectory_generator(self): """Test initial pose recover""" camid = 1 skip = 1 start_index = 0 img_glob = "*.jpg" distort_calibration = Cal3_S2(fx=347.820593, fy=329.096945, s=0, u0=295.717950, v0=222.964889).matrix() distortion = np.array( [-0.284322, 0.055723, 0.006772, 0.005264, 0.000000]) trajectory = self.trajectory_estimator.trajectory_generator( distort_calibration, distortion, self.directory_name, camid, skip, img_glob, start_index) for pose in trajectory: print(pose) actual_poses = load_poses_from_file(self.directory_name + "poses.dat") plot_trajectory_verification(self.trajectory_estimator.map.landmarks, actual_poses, trajectory) self.gtsamAssertEquals(trajectory[-1], self.initial_pose, tol=0.1)
def __init__(self, K=Cal3_S2(), nrCameras: int = 3, nrPoints: int = 4) -> None: self.K = K self.cameras = [Pose3()] * nrCameras self.points = [Point3(0, 0, 0)] * nrPoints
"""Unit tests for comparison functions for geometry types. Authors: Ayush Baid """ import unittest from typing import List from unittest.mock import patch import numpy as np from gtsam import Cal3_S2, Point3, Pose3, Rot3, Similarity3, Unit3 from gtsam.examples import SFMdata import gtsfm.utils.geometry_comparisons as geometry_comparisons import tests.data.sample_poses as sample_poses POSE_LIST = SFMdata.createPoses(Cal3_S2()) ROT3_EULER_ANGLE_ERROR_THRESHOLD = 1e-2 POINT3_RELATIVE_ERROR_THRESH = 1e-1 POINT3_ABS_ERROR_THRESH = 1e-2 def rot3_compare(R: Rot3, R_: Rot3, msg=None) -> bool: return np.allclose(R.xyz(), R_.xyz(), atol=1e-2) def point3_compare(t: Point3, t_: Point3, msg=None) -> bool: return np.allclose(t, t_, rtol=POINT3_RELATIVE_ERROR_THRESH, atol=POINT3_ABS_ERROR_THRESH)
from gtsam import Cal3_S2, PinholeCameraCal3_S2 from gtsam.examples import SFMdata import gtsfm.utils.overlap_frustums as overlap_frustums_utils CUBE_SIZE = 4 CUBE_RESOLUTION = 128 # set the dummy image size as 400x300 IMAGE_W = 400 IMAGE_H = 300 # set dummy camera intrinsics CAMERA_INTRINSICS = Cal3_S2( fx=100.0, fy=100.0, s=0.0, u0=IMAGE_W // 2, v0=IMAGE_H // 2, ) # set dummy camera poses as described in GTSAM example CAMERA_POSES = SFMdata.createPoses(CAMERA_INTRINSICS) # set dummy camera instances CAMERAS = [ PinholeCameraCal3_S2(CAMERA_POSES[i], CAMERA_INTRINSICS) for i in range(len(CAMERA_POSES)) ] # set dummy sphere grid center SPHERE_CENTER = np.array([-1.5, -1.5, -1.5]) # set dummy sphere grid radius SPHERE_RADIUS = 0.2
LineSegment3D(points[8], points[9]) ] semantics = [0.8, 0.8, 0.8, 0.8, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] lines_array = [] for i in range(step): l = np.append(points[i*2], points[i*2 + 1]) lines_array.append(l) return [points, lines, semantics] K = Cal3_S2(320., 320., 0.0, 320., 240.) pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.3, 0.3, 0.3])) # RPY - XYZ measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v pl = createPointsLines() poses = createPoses(K) points = pl[0] sems = pl[2] # Create a factor graph graph = NonlinearFactorGraph() #Add Pose Prior pose_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) factor = PriorFactorPose3(X(0), poses[0], pose_noise) graph.push_back(factor)
def main(): """ Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). Each variable in the system (poses and landmarks) must be identified with a unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). Here we will use Symbols In GTSAM, measurement functions are represented as 'factors'. Several common factors have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. Here we will use Projection factors to model the camera's landmark observations. Also, we will initialize the robot at some location using a Prior factor. When the factors are created, we will add them to a Factor Graph. As the factors we are using are nonlinear factors, we will need a Nonlinear Factor Graph. Finally, once all of the factors have been added to our factor graph, we will want to solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. GTSAM includes several nonlinear optimizers to perform this step. Here we will use a trust-region method known as Powell's Degleg The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the nonlinear functions around an initial linearization point, then solve the linear system to update the linearization point. This happens repeatedly until the solver converges to a consistent set of variable values. This requires us to specify an initial guess for each variable, held in a Values container. """ # Define the camera calibration parameters K = 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 a factor graph graph = NonlinearFactorGraph() # Add a prior on pose x1. 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])) factor = PriorFactorPose3(X(0), poses[0], pose_noise) graph.push_back(factor) # Simulated measurements from each camera pose, adding them to the factor graph for i, pose in enumerate(poses): camera = PinholeCameraCal3_S2(pose, K) for j, point in enumerate(points): measurement = camera.project(point) factor = GenericProjectionFactorCal3_S2(measurement, measurement_noise, X(i), L(j), K) graph.push_back(factor) # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained # Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance # between the first camera and the first landmark. All other landmark positions are interpreted using this scale. point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) factor = PriorFactorPoint3(L(0), points[0], point_noise) graph.push_back(factor) graph.print_('Factor Graph:\n') # Create the data structure to hold the initial estimate to the solution # Intentionally initialize the variables off from the ground truth initial_estimate = Values() for i, pose in enumerate(poses): transformed_pose = pose.retract(0.1 * np.random.randn(6, 1)) initial_estimate.insert(X(i), transformed_pose) for j, point in enumerate(points): transformed_point = point + 0.1 * np.random.randn(3) initial_estimate.insert(L(j), transformed_point) initial_estimate.print_('Initial Estimates:\n') # Optimize the graph and print results params = gtsam.DoglegParams() params.setVerbosity('TERMINATION') optimizer = DoglegOptimizer(graph, initial_estimate, params) print('Optimizing:') result = optimizer.optimize() result.print_('Final results:\n') print('initial error = {}'.format(graph.error(initial_estimate))) print('final error = {}'.format(graph.error(result))) marginals = Marginals(graph, result) plot.plot_3d_points(1, result, marginals=marginals) plot.plot_trajectory(1, result, marginals=marginals, scale=8) plot.set_axes_equal(1) plt.show()
def run_matching(superpoint_wrapper): """Create Feature Matching after Feature Extraction.""" # Create matches and save both the information and the images(with matches displayed on the origin images) calibration = Cal3_S2(fx=232.0542, fy=252.8620, s=0, u0=325.3452, v0=240.2912).matrix() superpoint_wrapper.get_all_feature_matches(calibration, 1)
# path for data used in this test DATA_ROOT_PATH = Path(__file__).resolve().parent.parent / "data" DOOR_TRACKS_PATH = DATA_ROOT_PATH / "tracks2d_door.pickle" DOOR_DATASET_PATH = DATA_ROOT_PATH / "set1_lund_door" # focal length set to 50 px, with `px`, `py` set to zero CALIBRATION = Cal3Bundler(50, 0, 0, 0, 0) # Generate 8 camera poses arranged in a circle of radius 40 m CAMERAS = { i: PinholeCameraCal3Bundler(pose, CALIBRATION) for i, pose in enumerate( SFMdata.createPoses( Cal3_S2( CALIBRATION.fx(), CALIBRATION.fx(), 0, CALIBRATION.px(), CALIBRATION.py(), ))) } LANDMARK_POINT = Point3(0.0, 0.0, 0.0) MEASUREMENTS = [ SfmMeasurement(i, cam.project(LANDMARK_POINT)) for i, cam in CAMERAS.items() ] def get_track_with_one_outlier() -> List[SfmMeasurement]: """Generates a track with outlier measurement.""" # perturb one measurement idx_to_perturb = 5
wTi_list: global poses. pair_indices: pairs (i1, i2) to construct relative poses for. Returns: Dictionary (i1, i2) -> i2Ti1 for all requested pairs. """ return {(i1, i2): wTi_list[i2].between(wTi_list[i1]) for i1, i2 in pair_indices} """4 poses in the circle of radius 5m, all looking at the center of the circle. For relative poses, each pose has just 2 edges, connecting to the immediate neighbors. """ CIRCLE_TWO_EDGES_GLOBAL_POSES = SFMdata.createPoses( Cal3_S2(fx=1, fy=1, s=0, u0=0, v0=0))[::2] CIRCLE_TWO_EDGES_RELATIVE_POSES = generate_relative_from_global( CIRCLE_TWO_EDGES_GLOBAL_POSES, [(1, 0), (2, 1), (3, 2), (0, 3)]) """4 poses in the circle of radius 5m, all looking at the center of the circle. For relative poses, each pose is connected to every other (3) pose. """ CIRCLE_ALL_EDGES_GLOBAL_POSES = copy.copy(CIRCLE_TWO_EDGES_GLOBAL_POSES) CIRCLE_ALL_EDGES_RELATIVE_POSES = generate_relative_from_global( CIRCLE_TWO_EDGES_GLOBAL_POSES, [(0, 1), (0, 2), (0, 3), (1, 2), (1, 3), (2, 3)]) """3 poses on a line, simulating forward motion, with large translations and no relative rotation. For relative poses, we have a fully connected graph.
def main(): """ A structure-from-motion example with landmarks - The landmarks form a 10 meter cube - The robot rotates around the landmarks, always facing towards the cube """ # Define the camera calibration parameters K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0) # Define the camera observation noise model camera_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 a NonlinearISAM object which will relinearize and reorder the variables # every "reorderInterval" updates isam = NonlinearISAM(reorderInterval=3) # Create a Factor Graph and Values to hold the new data graph = NonlinearFactorGraph() initial_estimate = Values() # Loop over the different poses, adding the observations to iSAM incrementally for i, pose in enumerate(poses): camera = PinholeCameraCal3_S2(pose, K) # Add factors for each landmark observation for j, point in enumerate(points): measurement = camera.project(point) factor = GenericProjectionFactorCal3_S2(measurement, camera_noise, X(i), L(j), K) graph.push_back(factor) # Intentionally initialize the variables off from the ground truth noise = Pose3(r=Rot3.Rodrigues(-0.1, 0.2, 0.25), t=Point3(0.05, -0.10, 0.20)) initial_xi = pose.compose(noise) # Add an initial guess for the current pose initial_estimate.insert(X(i), initial_xi) # 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, with 0.3 rad std on roll,pitch,yaw and 0.1m x,y,z pose_noise = gtsam.noiseModel.Diagonal.Sigmas( np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) factor = PriorFactorPose3(X(0), poses[0], pose_noise) graph.push_back(factor) # Add a prior on landmark l0 point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) factor = PriorFactorPoint3(L(0), points[0], point_noise) graph.push_back(factor) # Add initial guesses to all observed landmarks noise = np.array([-0.25, 0.20, 0.15]) for j, point in enumerate(points): # Intentionally initialize the variables off from the ground truth initial_lj = points[j] + noise initial_estimate.insert(L(j), initial_lj) else: # Update iSAM with the new factors isam.update(graph, initial_estimate) current_estimate = isam.estimate() print('*' * 50) print('Frame {}:'.format(i)) current_estimate.print_('Current estimate: ') # Clear the factor graph and values for the next iteration graph.resize(0) initial_estimate.clear()
from gtsfm.densify.patchmatchnet_data import PatchmatchNetData # set the default image size as 800x600, with 3 channels DEFAULT_IMAGE_W = 800 DEFAULT_IMAGE_H = 600 DEFAULT_IMAGE_C = 3 # set default track points, the coordinates are in the world frame DEFAULT_NUM_TRACKS = 100 DEFAULT_TRACK_POINTS = [Point3(5, 5, float(i)) for i in range(DEFAULT_NUM_TRACKS)] # set default camera intrinsics DEFAULT_CAMERA_INTRINSICS = Cal3_S2( fx=100.0, fy=100.0, s=1.0, u0=DEFAULT_IMAGE_W // 2, v0=DEFAULT_IMAGE_H // 2, ) # set default camera poses as described in GTSAM example DEFAULT_CAMERA_POSES = SFMdata.createPoses(DEFAULT_CAMERA_INTRINSICS) # set default camera instances DEFAULT_CAMERAS = [ PinholeCameraCal3_S2(DEFAULT_CAMERA_POSES[i], DEFAULT_CAMERA_INTRINSICS) for i in range(len(DEFAULT_CAMERA_POSES)) ] DEFAULT_NUM_CAMERAS = len(DEFAULT_CAMERAS) # the number of valid images should be equal to the number of cameras (with estimated pose) DEFAULT_NUM_IMAGES = DEFAULT_NUM_CAMERAS # build dummy image dictionary with default image shape DEFAULT_DUMMY_IMAGE_DICT = {
run_feature_extraction = False run_feature_matching = False run_bundle_adjustment = True save_result = True basedir = "mapping/datasets/flann_klaus_1x1x8/" image_extension = ".jpg" source_image_size = (640, 480) # Undistortion # resize_output = True, output will be resized to the same shape as the input image # resize_output = False, output image will shrink due to rectification of radian distort resize_output = False distort_calibration_matrix = Cal3_S2(fx=347.820593, fy=329.096945, s=0, u0=295.717950, v0=222.964889).matrix() distortion_coefficients = np.array( [-0.284322, 0.055723, 0.006772, 0.005264, 0.000000]) # calibration_matrix = Cal3_S2(fx=232.0542, fy=252.8620, s=0, # u0=325.3452, v0=240.2912).matrix() # Resize calibration_matrix = Cal3_S2(fx=211.8927, fy=197.7030, s=0, u0=281.1168, v0=179.2954) undistort_img_size = (583, 377) number_images = 10
def point_on_line(a, b, p): ap = p - a ab = b - a result = a + np.dot(ap, ab) / np.dot(ab, ab) * ab return result # p = Point2(1,1) # # a = Point2(1,1) # b = Point2(-1,-1) # dis = point_on_line(a,b, p) K = Cal3_S2(320, 320, 0.0, 320, 240) target = gtsam.Point3(0, 0, 0) position = gtsam.Point3(20, 0, 0) up = gtsam.Point3(0, 0, 1) # NED -> NORTH_EAST_DOWN ??? cam = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K) pw = Point3(10.0, 3.0, 0.0) # pi_groundtruth = cam.project(pw) # pi,Dpose,Dpoint = cal_PointProject_Jacobians(cam,pw) q = cam.pose().transformTo(pw) pn = cam.Project(q) pi = cam.calibration().uncalibrate(pn) d = 1 / q[2] #