def __init__(self,image_directory_path = 'datasets/4.5/mapping/source_images/', image_extension = '*.jpg', image_size = (160,120), down_sample = 1, nn_thresh = 0.7): self.basedir = image_directory_path self.img_extension = image_extension self.nn_thresh = nn_thresh self.fe = SuperPointFrontend(weights_path="SuperPointPretrainedNetwork/superpoint_v1.pth", nms_dist=4, conf_thresh=0.015, nn_thresh=0.7, cuda=False) self.point_tracker = PointTracker(5, self.nn_thresh) self.img_size = image_size self.scale = down_sample # fov_in_degrees = 128, image_width = 640, image_height = 480) # fx = w/2*math.tan(fov*3.14/360) # How resizing an image affect the intrinsic camera matrix:https://dsp.stackexchange.com/questions/6055/how-does-resizing-an-image-affect-the-intrinsic-camera-matrix self.image_transform = np.array([[0.25,0,0.25-0.5],[0,0.25,0.25-0.5],[0,0,1]]) # self.calibration = gtsam.Cal3_S2(fx=156.3, fy=236,s=0,u0=320, v0=240).matrix() self.calibration = gtsam.Cal3_S2(fx=333.4, fy=314.7,s=0,u0=303.6, v0=247.6).matrix() self.calibration = np.dot(self.image_transform,self.calibration) self.distortion = np.array([-0.282548, 0.054412, -0.001882, 0.004796, 0.000000]) # self.cal = gtsam.Cal3_S2(fx=156.3, fy=236,s=0,u0=320, v0=240) # self.cal = gtsam.Cal3_S2(fx=19.54, fy=29.5,s=0,u0=39.625, v0=29.625) self.cal = gtsam.Cal3_S2(fx=83.35, fy=78.675,s=0,u0=75.65, v0=61.65) self.keypoint_list = [] self.descriptor_list = [] self.correspondence_table = [] self.img_pose=[] self.landmark=[]
def test_insertProjectionFactors(self): """Test insertProjectionFactors.""" graph = gtsam.NonlinearFactorGraph() gtsam.utilities.insertProjectionFactors( graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]), gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2()) self.assertEqual(graph.size(), 2) graph = gtsam.NonlinearFactorGraph() gtsam.utilities.insertProjectionFactors( graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]), gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2(), gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0))) self.assertEqual(graph.size(), 2)
def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: """"Returns global rotations and unit translation directions between 8 cameras that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata and the unit translations directions between some camera pairs are computed from their global translations. """ fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0 wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(fx, fy, s, u0, v0)) # Rotations of the cameras in the world frame. wRc_values = gtsam.Values() # Normalized translation directions from camera i to camera j # in the coordinate frame of camera i. i_iZj_list = [] for i in range(0, len(wTc_list) - 2): # Add the rotation. wRi = wTc_list[i].rotation() wRc_values.insert(i, wRi) # Create unit translation measurements with next two poses. for j in range(i + 1, i + 3): # Compute the translation from pose i to pose j, in the world reference frame. w_itj = wTc_list[j].translation() - wTc_list[i].translation() # Obtain the translation in the camera i's reference frame. i_itj = wRi.unrotate(w_itj) # Compute the normalized unit translation direction. i_iZj = gtsam.Unit3(i_itj) i_iZj_list.append( gtsam.BinaryMeasurementUnit3( i, j, i_iZj, gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) # Add the last two rotations. wRc_values.insert(len(wTc_list) - 1, wTc_list[-1].rotation()) wRc_values.insert(len(wTc_list) - 2, wTc_list[-2].rotation()) return wRc_values, i_iZj_list
def __init__(self, atrium_map, downsample_width, downsample_height, l2_thresh, distance_thresh): """ A class to estimate the current camera pose with prebuilt map, an image, and a trajectory included the past pose. Args: atrium_map - a map object that includes a N*3 numpy array of gtsam.Point3 and a N*M numpy array of M dimensions descriptors downsample_width - image width downsample factor downsample_height - image height downsample factor l2_thresh - l2 distance threshold of two descriptors distance_thresh - feature coordinate distance threshold """ # Camera settings self.image_width = 640 self.image_height = 480 self.fov_in_degrees = 128 self.calibration = gtsam.Cal3_S2(fx=333.4, fy=314.7,s=0,u0=303.6, v0=247.6) # gtsam.Cal3_S2( # self.fov_in_degrees, self.image_width, self.image_height) self.distortion = np.array([-0.282548, 0.054412, -0.001882, 0.004796, 0.000000]) self.projection = np.array([[226.994629 ,0.000000, 311.982613, 0.000000],[0.000000, 245.874146, 250.410089, 0.000000],[0.000000, 0.000000, 1.000000, 0.000000]]) self.atrium_map = atrium_map self.downsample_w = downsample_width self.downsample_h = downsample_height self.l2_threshold = l2_thresh self.distance_threshold = distance_thresh
def undistort_image(basedir, img_extension): search = os.path.join(basedir, img_extension) img_paths = glob.glob(search) img_paths.sort() print("Number of Images: ", len(img_paths)) maxlen = len(img_paths) if maxlen == 0: raise IOError( 'No images were found (maybe wrong \'image extension\' parameter?)' ) calibration = gtsam.Cal3_S2(fx=333.4, fy=314.7, s=0, u0=303.6, v0=247.6).matrix() distortion = np.array([-0.282548, 0.054412, -0.001882, 0.004796, 0.000000]) projection = np.array([[226.994629, 0.000000, 311.982613, 0.000000], [0.000000, 245.874146, 250.410089, 0.000000], [0.000000, 0.000000, 1.000000, 0.000000]]) # calibration = gtsam.Cal3_S2(fx=343.555173, fy=327.221818,s=0,u0=295.979699, v0=261.530851).matrix() # distortion = np.array([-0.305247, 0.064438, -0.007641, 0.006581, 0.000000]) # projection = np.array([[235.686951, 0.000000, 304.638818, 0.000000],[0.000000, 256.651520, 265.858792, 0.000000],[0.000000, 0.000000, 1.000000, 0.000000]]) for i, img_path in enumerate(img_paths): img = cv2.imread(img_path, 0) h, w = img.shape[:2] newcameramtx, roi = cv2.getOptimalNewCameraMatrix( calibration, distortion, (w, h), 1, (w, h)) # undistort dst = cv2.undistort(img, calibration, distortion, None, projection) # dst = cv2.undistort(img,calibration ,distortion , None, calibration) # cv2.imshow(dst) output_path = '%d_frame_undist.jpg' % i output_path = os.path.join(basedir, output_path) cv2.imwrite(output_path, dst)
def __init__(self,image_directory_path = 'datasets/5x5x8/source_1x5x1/', image_extension = '*.jpg', image_size = (640,480), down_sample = 1, nn_thresh = 0.7, row = 1, col = 6, angles = 1, directions = 1): # def __init__(self,image_directory_path = 'datasets/5x5x8/source/', image_extension = '*.jpg', image_size = (640,480), down_sample = 1, nn_thresh = 0.7, row = 5, col = 5, angles = 8, directions = 3): # def __init__(self,image_directory_path = 'datasets/4.5/mapping/source_images/', image_extension = '*.jpg', image_size = (640,480), down_sample = 1, nn_thresh = 0.7, row = 1, col = 6, angles = 1, directions = 1): self.basedir = image_directory_path self.img_extension = image_extension self.nn_thresh = nn_thresh self.fe = SuperPointFrontend(weights_path="SuperPointPretrainedNetwork/superpoint_v1.pth", nms_dist=4, conf_thresh=0.015, nn_thresh=0.7, cuda=False) self.point_tracker = PointTracker(5, self.nn_thresh) self.img_size = image_size self.scale = down_sample self.cal= gtsam.Cal3_S2(fx=232.0542, fy=252.8620,s=0,u0=325.3452, v0=240.2912) self.calibration = self.cal.matrix() self.keypoint_list = [] self.descriptor_list = [] self.correspondence_table = [] self.row = row self.col = col self.angles = angles self.directions = directions self.img_pose = np.array([]) self.landmark=[] self.landmark_descriptor = []
def undistort_image(basedir,img_extension): """A function to undistort the distorted images.""" search = os.path.join(basedir, img_extension) img_paths = glob.glob(search) img_paths.sort() print("Number of Images: ",len(img_paths)) maxlen = len(img_paths) if maxlen == 0: raise IOError('No images were found (maybe wrong \'image extension\' parameter?)') calibration = gtsam.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]) rectification = np.identity(3) projection = np.array([[240.446564 ,0.000000, 302.423680, 0.000000],[0.000000, 265.140778, 221.096494, 0.000000],[0.000000, 0.000000, 1.000000, 0.000000]]) count = 0 img_idx = 0 for i,img_path in enumerate(img_paths): img = cv2.imread(img_path, 1) if count == 8: count = 0 img_idx += 1 dst = cv2.undistort(img, calibration, distortion, np.eye(3), projection) output_path = '%d_'%img_idx+'%d_frame_undist.jpg'%count output_path = os.path.join(basedir+'source/', output_path) cv2.imwrite(output_path,dst) count += 1
def test_find_dsf(self): """Test functions related to DSF""" # """Test find bad matches.""" data_directory = 'mapping/datasets/dsf_test_data/' num_images = 3 filter_bad_landmarks_enable = False min_obersvation_number = 3 back_end = MappingBackEnd( data_directory, num_images, gtsam.Cal3_S2(), [], [], [], filter_bad_landmarks_enable, min_obersvation_number) bad_matches = back_end.find_bad_matches() self.assertEqual(bad_matches, {(2, 6), (0, 4)}) # """Test create landmark map.""" actual_landmark_map, dsf = back_end.create_landmark_map(False) expected_landmark_map = {(0, 1): [(0, Point2(1, 1)), (1, Point2(2, 1)), (2, Point2(3, 1))], (2, 0): [(2, Point2(0, 0))], (0, 0): [(0, Point2(0, 0))], (0, 5): [(0, Point2(1, 5)), (0, Point2(1, 6)), (2, Point2(3, 6))], (0, 4): [ (0, Point2(1, 4)), (2, Point2(3, 3)), (2, Point2(3, 5))], (1, 0): [(1, Point2(0, 0))], (0, 3): [(0, Point2(1, 3)), (1, Point2(2, 2)), (2, Point2(3, 2))], (0, 2): [(0, Point2(1, 2)), (2, Point2(3, 4))]} self.assert_landmark_map_equal( actual_landmark_map, expected_landmark_map) # """Test generate dsf.""" landmark_representative = dsf.find(gtsam.IndexPair(2, 1)) key = (landmark_representative.i(), landmark_representative.j()) self.assertEqual(key, (0, 1)) # """Test filter bad landmarks.""" actual_landmark_map = back_end.filter_bad_landmarks( expected_landmark_map, dsf, True) expected_landmark_map = [[(0, Point2(1, 1)), (1, Point2(2, 1)), (2, Point2(3, 1))], [ (0, Point2(1, 3)), (1, Point2(2, 2)), (2, Point2(3, 2))]] # self.assert_landmark_map_equal(actual_landmark_map, expected_landmark_map) for i, observations in enumerate(expected_landmark_map): for j, observation in enumerate(observations): self.assertEqual(actual_landmark_map[i][j][0], observation[0]) self.gtsamAssertEquals( actual_landmark_map[i][j][1], observation[1])
def __init__(self, data_directory, num_images=3, delta_z=1): """Construct by reading from a data directory.""" self.basedir = data_directory self.nrimages = num_images # Camera calibration fov, w, h = 60, 1280, 720 self._calibration = gtsam.Cal3_S2(fov, w, h) # Pose distance along z axis # fps = 30 # velocity = 10 # m/s # self.delta_z = velocity / fps self.delta_z = delta_z self._min_landmark_seen = 3 def get_image_features(image_index): """ Load features features - keypoints:A Nx2 list of (x,y). Descriptors: A Nx256 list of desc. """ feat_file = os.path.join(self.basedir, "{0:07}.key".format(image_index)) keypoints, descriptors = load_features_list(feat_file) return ImageFeature(keypoints, descriptors) self._image_features = [ get_image_features(image_index) for image_index in range(self.nrimages) ] self._image_matches = self.create_image_matches() self.image_points, self._landmark_estimates, self.image_poses = self.data_association( )
def load_calibration(): """ calculates the intrinsic camera calibration """ pixel_width = 320; pixel_height = 240 vfov = 45; hfov = 60 fx = (pixel_width/2.0)/math.tan(math.radians(hfov/2.0)) fy = (pixel_height/2.0)/math.tan(math.radians(vfov/2.0)) s = 0.0; cx = pixel_width/2.0; cy = pixel_height/2.0 camera_intrinsics = gtsam.Cal3_S2(fx, fy, s, cx, cy) return camera_intrinsics
def __init__(self, triangle=False, nrCameras=3, K=gtsam.Cal3_S2()): """ 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 test_triangulation_rectify(self): """Estimate spatial point from image measurements using rectification""" rectified = gtsam.Point2Vector([ k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements) ]) shared_cal = gtsam.Cal3_S2() triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) self.gtsamAssertEquals(triangulated, self.origin)
def __init__(self, nrCameras, nrPoints, fov_in_degrees, image_width, image_height): """ Parameters: nrCameras -- Number of cameras nrPoints -- Number of landmarks fov_in_degrees -- Based on the official document of Runcam Swift 2: horizontal FOV = 128, Vertical FOV = 91, Diagonal FOV = 160 image_width, image_height -- camera output image [640,480]. Superpoint can extract features with downsampled images, therefore the output of Superpoint is flexible. """ self.nrCameras = nrCameras self.nrPoints = nrPoints self.calibration = gtsam.Cal3_S2(fov_in_degrees, image_width, image_height)
def test_reprojectionErrors(self): """Test reprojectionErrors.""" pixels = np.asarray([[20, 30], [20, 30]]) I = [1, 2] K = gtsam.Cal3_S2() graph = gtsam.NonlinearFactorGraph() gtsam.utilities.insertProjectionFactors( graph, 0, I, pixels, gtsam.noiseModel.Isotropic.Sigma(2, 0.1), K) values = gtsam.Values() values.insert(0, gtsam.Pose3()) cam = gtsam.PinholeCameraCal3_S2(gtsam.Pose3(), K) gtsam.utilities.insertBackprojections(values, cam, I, pixels, 10) errors = gtsam.utilities.reprojectionErrors(graph, values) np.testing.assert_allclose(errors, np.zeros((2, 2)))
def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4): self.K = K self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras] self.J = [x[:] for x in [[0] * nrPoints] * nrCameras] self.odometry = [gtsam.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 undistort_image(basedir, img_extension): """A function to undistort the distorted images.""" search = os.path.join(basedir, img_extension) img_paths = glob.glob(search) img_paths.sort() print("Number of Images: ", len(img_paths)) maxlen = len(img_paths) if maxlen == 0: raise IOError( 'No images were found (maybe wrong \'image extension\' parameter?)' ) calibration = gtsam.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]) rectification = np.identity(3) projection = np.array([[240.446564, 0.000000, 302.423680, 0.000000], [0.000000, 265.140778, 221.096494, 0.000000], [0.000000, 0.000000, 1.000000, 0.000000]]) # calibration = gtsam.Cal3_S2(fx=331.0165, fy=310.4791,s=0,u0=332.7372, v0=248.5307).matrix() # distortion = np.array([-0.3507, 0.1112, 8.6304e-04, -0.0018, 0.000000]) # rectification = np.identity(3) # calibration = gtsam.Cal3_S2(fx=332.8312, fy=312.3082,s=-1.1423,u0=330.2353, v0=249.5842).matrix() # distortion = np.array([-0.3718, 0.1623, 5.4112e-04, -8.9232e-04, -0.0362]) # rectification = np.identity(3) count = 0 img_idx = 0 for i, img_path in enumerate(img_paths): img = cv2.imread(img_path, 1) # h, w = img.shape[:2] # newcameramtx, roi=cv2.getOptimalNewCameraMatrix(calibration,distortion,(w,h),1,(w,h)) # dst = cv2.undistort(img, calibration, distortion, np.eye(3),projection) dst = cv2.undistort(img, calibration, distortion) output_path = 'frame_%d.jpg' % count output_path = os.path.join(basedir + 'matlab_undist/', output_path) cv2.imwrite(output_path, dst) count += 1
def generate_data(options): """ Generate ground-truth and measurement data. """ K = gtsam.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] = gtsam.Point3(r * cos(theta), r * sin(theta), 0) else: # 3D landmarks as vertices of a cube truth.points = [ gtsam.Point3(10, 10, 10), gtsam.Point3(-10, 10, 10), gtsam.Point3(-10, -10, 10), gtsam.Point3(10, -10, 10), gtsam.Point3(10, 10, -10), gtsam.Point3(-10, 10, -10), gtsam.Point3(-10, -10, -10), gtsam.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 = gtsam.Point3(r * cos(theta), r * sin(theta), height) truth.cameras[i] = gtsam.SimpleCamera.Lookat(t, gtsam.Point3(), gtsam.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 create_map(fignum): calibration = gtsam.Cal3_S2(fx=333.4, fy=314.7,s=0,u0=303.6, v0=247.6) distortion = np.array([-0.282548, 0.054412, -0.001882, 0.004796, 0.000000]) rectification = np.identity(3) projection = np.array([[226.994629 ,0.000000, 311.982613, 0.000000],[0.000000, 245.874146, 250.410089, 0.000000],[0.000000, 0.000000, 1.000000, 0.000000]]) # Create matched feature data and desc data data = Data(5,10,calibration.matrix(),distortion,projection) data.create_feature_data(0) data.create_descriptor_data() data.undistort_feature_points() # Mapping mapping = Mapping(5, 10, calibration, 640, 480) sfm_result = mapping.atrium_sfm(data,30,1.58) atrium_map = mapping.create_atrium_map(sfm_result, data.desc) return sfm_result, atrium_map
def undistort_image(basedir,img_extension): search = os.path.join(basedir, img_extension) img_paths = glob.glob(search) img_paths.sort() print("Number of Images: ",len(img_paths)) maxlen = len(img_paths) if maxlen == 0: raise IOError('No images were found (maybe wrong \'image extension\' parameter?)') calibration = gtsam.Cal3_S2(fx=333.4, fy=314.7,s=0,u0=303.6, v0=247.6).matrix() projection = np.array([[226.994629 ,0.000000, 311.982613, 0.000000],[0.000000, 245.874146, 250.410089, 0.000000],[0.000000, 0.000000, 1.000000, 0.000000]]) distortion = np.array([-0.282548, 0.054412, -0.001882, 0.004796, 0.000000]) for i,img_path in enumerate(img_paths): img = cv2.imread(img_path, 0) dst = cv2.undistort(img,calibration ,distortion , None, calibration) # cv2.imshow(dst) output_path = '%d_frame_undist.jpg'%i output_path = os.path.join(basedir, output_path) cv2.imwrite(output_path,dst)
def __init__(self, fov, width, height, nr_points): """ Key Arguments: calibration - the calibration of a camera poses - a list of the camera poses, each pose is a gtsam.Pose3 object vision_area_list - a list of vision area of each pose, back project the vertices of the image to distance 10m to get its vision area at 10m projected_features - a list of features, each element is a collection of features which are landmarks that projected to the current pose superpoint_features - a list of features, each element is a Synthetic superpoint output features contains key point and normalized descriptor. match_features - a list of features, each element is a matched feature visible_map - a list of Map, each element is a landmark point in the map """ self.calibration = gtsam.Cal3_S2(fov, width, height) self.poses = [] self.past_poses = [] self.vision_area_list = [] self.projected_features = [] self.map_indices = [] self.superpoint_features = [] self.matched_features = [] self.visible_map = []
def __init__(self, pose0=np.eye(4), pose0_to_pose1_range=1.0, K=np.eye(3), relinearizeThreshold=0.1, relinearizeSkip=10, proj_noise_val=1.0): self.graph = NonlinearFactorGraph() # Add a prior on pose x0. This indirectly specifies where the origin is. # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z pose_noise = gtsam.noiseModel_Diagonal.Sigmas( np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) x0factor = PriorFactorPose3(iSAM2Wrapper.get_key('x', 0), gtsam.gtsam.Pose3(pose0), pose_noise) self.graph.push_back(x0factor) # Set scale between pose 0 and pose 1 to Unity x0x1_noise = gtsam.noiseModel_Isotropic.Sigma(1, 0.1) x1factor = RangeFactorPose3(iSAM2Wrapper.get_key('x', 0), iSAM2Wrapper.get_key('x', 1), pose0_to_pose1_range, x0x1_noise) self.graph.push_back(x1factor) iS2params = gtsam.ISAM2Params() iS2params.setRelinearizeThreshold(relinearizeThreshold) iS2params.setRelinearizeSkip(relinearizeSkip) self.isam2 = gtsam.ISAM2(iS2params) self.projection_noise = gtsam.noiseModel_Isotropic.Sigma( 2, proj_noise_val) self.K = gtsam.Cal3_S2(iSAM2Wrapper.CameraMatrix_to_Cal3_S2(K)) #self.opt_params = gtsam.DoglegParams() #self.opt_params.setVerbosity('Error') #self.opt_params.setErrorTol(0.1) self.initial_estimate = gtsam.Values() self.initial_estimate.insert(iSAM2Wrapper.get_key('x', 0), gtsam.gtsam.Pose3(pose0)) self.lm_factor_ids = set()
def setUp(self): # self.calibration = gtsam.Cal3_S2(160, 640, 480).matrix() # self.calibration = gtsam.Cal3_S2(fx=156.3, fy=236,s=1,u0=320, v0=240).matrix() self.image_transform = np.array([[0.25, 0, 0.125 - 0.5], [0, 0.25, 0.125 - 0.5], [0, 0, 1]]) self.calibration = gtsam.Cal3_S2(fx=333.4, fy=327.22, s=0, u0=303.6, v0=247.6).matrix() # self.calibration = gtsam.Cal3_S2(fx=343.55, fy=314.7,s=0,u0=295.97, v0=261.53).matrix() # self.calibration = np.dot(self.image_transform,self.calibration2) self.distortion = np.array( [-0.282548, 0.054412, -0.001882, 0.004796, 0.000000]) # self.distortion = np.array([-0.305247, 0.064438, -0.007641, 0.006581, 0.000000]) self.rectification = np.identity(3) self.projection = np.array( [[226.994629, 0.000000, 311.982613, 0.000000], [0.000000, 245.874146, 250.410089, 0.000000], [0.000000, 0.000000, 1.000000, 0.000000]]) # self.projection = np.array([[235.686951, 0.000000, 304.638818, 0.000000],[0.000000, 256.651520, 265.858792, 0.000000],[0.000000, 0.000000, 1.000000, 0.000000]]) # self.projection = np.dot(self.image_transform,self.projection2) self.calibration_inv = np.linalg.inv(self.calibration) self.R = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) # self.t = np.array([[-0.79],[-1.36832],[0]]) self.t = np.array([[1.58], [0], [0]]) # self.t = np.array([[-0.61],[-0.78832],[0]]) self.t_skew_symmetric = np.array([[0, -self.t[2], self.t[1]], [self.t[2], 0, -self.t[0]], [-self.t[1], self.t[0], 0]]) self.E = np.dot(self.t_skew_symmetric, self.R) self.F = np.dot(np.dot(self.calibration_inv.T, self.E), self.calibration_inv) # http://answers.opencv.org/question/31421/opencv-3-essentialmatrix-and-recoverpose/ # self.P_0 = np.dot(self.calibration,np.hstack((np.identity(3),np.zeros((3,1))))) # self.P_1 = np.dot(self.calibration,np.hstack((self.R.T,-np.dot(self.R.T,self.t)))) self.P_0 = np.hstack((np.identity(3), np.zeros((3, 1)))) self.P_1 = np.hstack((self.R.T, -np.dot(self.R.T, self.t)))
def __init__(self,image_directory_path = 'datasets/4.5/mapping/source_images/', image_extension = '*.jpg', image_size = (640,480), down_sample = 1, nn_thresh = 0.7): self.basedir = image_directory_path self.img_extension = image_extension self.nn_thresh = nn_thresh self.fe = SuperPointFrontend(weights_path="SuperPointPretrainedNetwork/superpoint_v1.pth", nms_dist=4, conf_thresh=0.015, nn_thresh=0.7, cuda=False) self.point_tracker = PointTracker(5, self.nn_thresh) self.img_size = image_size self.scale = down_sample # self.calibration = gtsam.Cal3_S2(fx=240.446564, fy=265.140778,s=0,u0=302.423680, v0=221.096494).matrix() # self.cal = gtsam.Cal3_S2(fx=240.446564, fy=265.140778,s=0,u0=302.423680, v0=221.096494) self.cal = gtsam.Cal3_S2(fx=232.0542, fy=252.8620,s=0,u0=325.3452, v0=240.2912) self.calibration = self.cal.matrix() self.keypoint_list = [] self.descriptor_list = [] self.correspondence_table = [] self.img_pose=[] self.landmark=[]
def __init__(self): fx = Config().fx fy = Config().fy cx = Config().cx cy = Config().cy bf = Config().bf # Create realistic calibration and measurement noise model # format: fx fy skew cx cy baseline baseline = bf / fx self.K_stereo = gt.Cal3_S2Stereo(fx, fy, 0.0, cx, cy, baseline) self.K_mono = gt.Cal3_S2(fx, fy, 0.0, cx, cy) self.deltaMono = np.sqrt(5.991) self.deltaStereo = np.sqrt(7.815) self.depth_threshold = bf / fx * 60 # Create graph container and add factors to it self.graph = gt.NonlinearFactorGraph() # Create initial estimate for camera poses and landmarks self.initialEstimate = gt.Values() # add a constraint on the starting pose # first_pose = gt.Pose3() # self.graph.add(gt.NonlinearEqualityPose3(X(1), first_pose)) self.inv_lvl_sigma2 = np.zeros((8, ), dtype=np.float) for idx in np.arange(8): self.inv_lvl_sigma2[idx] = 1. / 1.2**(2 * idx - 2) # point counter for landmarks and octave container self.counter = 1 self.octave = [] self.is_stereo = []
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 __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4): self.K = K self.cameras = [gtsam.Pose3()] * nrCameras self.points = [gtsam.Point3()] * nrPoints
import unittest import numpy as np import gtsam import gtsam_unstable from gtsam.utils.test_case import GtsamTestCase pose1 = gtsam.Pose3() pose2 = gtsam.Pose3( np.array([[0.9999375, 0.00502487, 0.00998725, 0.1], [-0.00497488, 0.999975, -0.00502487, 0.02], [-0.01001225, 0.00497488, 0.9999375, 1.], [0., 0., 0., 1.]])) point = np.array([2, 0, 15]) point_noise = gtsam.noiseModel.Diagonal.Sigmas(np.ones(2)) cal = gtsam.Cal3_S2() body_p_sensor = gtsam.Pose3() alpha = 0.1 measured = np.array([0.13257015, 0.0004157]) class TestProjectionFactorRollingShutter(GtsamTestCase): def test_constructor(self): ''' test constructor for the ProjectionFactorRollingShutter ''' factor = gtsam_unstable.ProjectionFactorRollingShutter( measured, alpha, point_noise, 0, 1, 2, cal) factor = gtsam_unstable.ProjectionFactorRollingShutter( measured, alpha, point_noise, 0, 1, 2, cal, body_p_sensor) factor = gtsam_unstable.ProjectionFactorRollingShutter(
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()
if __name__ == '__main__': # declare noise estimates ODOM_SIGMA = 0.01 BOX_SIGMA = 3 # define shortcuts for symbols X = lambda i: int(gtsam.symbol(ord('x'), i)) Q = lambda i: int(gtsam.symbol(ord('q'), i)) # create empty graph / estimate graph = gtsam.NonlinearFactorGraph() initial_estimate = gtsam.Values() # define calibration calibration = gtsam.Cal3_S2(525.0, 525.0, 0.0, 160.0, 120.0) # define noise models prior_noise = gtsam.noiseModel_Diagonal.Sigmas( np.array([1e-1] * 6, dtype=np.float)) odometry_noise = gtsam.noiseModel_Diagonal.Sigmas( np.array([ODOM_SIGMA] * 3 + [ODOM_SIGMA] * 3, dtype=np.float)) bbox_noise = gtsam.noiseModel_Diagonal.Sigmas( np.array([BOX_SIGMA] * 4, dtype=np.float)) # define poses poses = [] poses.append( gtsam.SimpleCamera.Lookat(gtsam.Point3(10, 0, 0), gtsam.Point3(), gtsam.Point3(0, 0, 1)).pose()) poses.append(
def IMU_example(): """Run iSAM 2 example with IMU factor.""" # Start with a camera on x-axis looking at origin radius = 30 up = gtsam.Point3(0, 0, 1) target = gtsam.Point3(0, 0, 0) position = gtsam.Point3(radius, 0, 0) camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2()) pose_0 = camera.pose() # Create the set of ground-truth landmarks and poses angular_velocity = math.radians(180) # rad/sec delta_t = 1.0/18 # makes for 10 degrees per step angular_velocity_vector = vector3(0, -angular_velocity, 0) linear_velocity_vector = vector3(radius * angular_velocity, 0, 0) scenario = gtsam.ConstantTwistScenario( angular_velocity_vector, linear_velocity_vector, pose_0) # Create a factor graph newgraph = gtsam.NonlinearFactorGraph() # Create (incremental) ISAM2 solver isam = gtsam.ISAM2() # Create the initial estimate to the solution # Intentionally initialize the variables off from the ground truth initialEstimate = gtsam.Values() # Add a prior on pose x0. This indirectly specifies where the origin is. # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noise = gtsam.noiseModel_Diagonal.Sigmas( np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1])) newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise)) # Add imu priors biasKey = gtsam.symbol(ord('b'), 0) biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(), biasnoise) newgraph.push_back(biasprior) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1) # Calculate with correct initial velocity n_velocity = vector3(0, angular_velocity * radius, 0) velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise) newgraph.push_back(velprior) initialEstimate.insert(V(0), n_velocity) accum = gtsam.PreintegratedImuMeasurements(PARAMS) # Simulate poses and imu measurements, adding them to the factor graph for i in range(80): t = i * delta_t # simulation time if i == 0: # First time add two poses pose_1 = scenario.pose(delta_t) initialEstimate.insert(X(0), pose_0.compose(DELTA)) initialEstimate.insert(X(1), pose_1.compose(DELTA)) elif i >= 2: # Add more poses as necessary pose_i = scenario.pose(t) initialEstimate.insert(X(i), pose_i.compose(DELTA)) if i > 0: # Add Bias variables periodically if i % 5 == 0: biasKey += 1 factor = gtsam.BetweenFactorConstantBias( biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE) newgraph.add(factor) initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias()) # Predict acceleration and gyro measurements in (actual) body frame nRb = scenario.rotation(t).matrix() bRn = np.transpose(nRb) measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity) measuredOmega = scenario.omega_b(t) accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t) # Add Imu Factor imufac = gtsam.ImuFactor( X(i - 1), V(i - 1), X(i), V(i), biasKey, accum) newgraph.add(imufac) # insert new velocity, which is wrong initialEstimate.insert(V(i), n_velocity) accum.resetIntegration() # Incremental solution isam.update(newgraph, initialEstimate) result = isam.calculateEstimate() ISAM2_plot(result) # reset newgraph = gtsam.NonlinearFactorGraph() initialEstimate.clear()