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_triangulation_skipped(self): """Estimate spatial point from image measurements""" triangulated = gtsam.triangulatePoint3(self.cameras, self.measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(triangulated, self.origin)
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 triangulate_two_view_correspondences( cls, camera_i1: gtsfm_types.CAMERA_TYPE, camera_i2: gtsfm_types.CAMERA_TYPE, keypoints_i1: Keypoints, keypoints_i2: Keypoints, corr_idxs: np.ndarray, ) -> Tuple[List[SfmTrack], List[int]]: """Triangulate 2-view correspondences to form 3d tracks. Args: camera_i1: camera for 1st view. camera_i2: camera for 2nd view. keypoints_i1: keypoints for 1st view. keypoints_i2: keypoints for 2nd view. corr_idxs: indices of corresponding keypoints. Returns: Triangulated 3D points as tracks. """ camera_set = (CameraSetCal3Bundler() if isinstance( camera_i1, PinholeCameraCal3Bundler) else CameraSetCal3Fisheye()) camera_set.append(camera_i1) camera_set.append(camera_i2) tracks_3d: List[SfmTrack] = [] valid_indices: List[int] = [] for j, (idx1, idx2) in enumerate(corr_idxs): track_2d = Point2Vector() track_2d.append(keypoints_i1.coordinates[idx1]) track_2d.append(keypoints_i2.coordinates[idx2]) try: triangulated_pt = gtsam.triangulatePoint3( camera_set, track_2d, rank_tol=SVD_DLT_RANK_TOL, optimize=False) track_3d = SfmTrack(triangulated_pt) track_3d.addMeasurement(0, track_2d[0]) track_3d.addMeasurement(1, track_2d[1]) tracks_3d.append(track_3d) valid_indices.append(j) except RuntimeError: pass # logger.error(e) return tracks_3d, valid_indices
def test_distinct_Ks(self): """ Tests triangulation with individual Cal3_S2 calibrations """ # two camera parameters K1 = (1500, 1200, 0, 640, 480) K2 = (1600, 1300, 0, 650, 440) measurements, cameras = self.generate_measurements(Cal3_S2, PinholeCameraCal3_S2, (K1, K2), camera_set=CameraSetCal3_S2) triangulated_landmark = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
def test_distinct_Ks_Bundler(self) -> None: """Tests triangulation with individual Cal3Bundler calibrations""" # two camera parameters K1 = (1500, 0, 0, 640, 480) K2 = (1600, 0, 0, 650, 440) measurements, cameras = self.generate_measurements( calibration=Cal3Bundler, camera_model=PinholeCameraCal3Bundler, cal_params=(K1, K2), camera_set=CameraSetCal3Bundler) triangulated_landmark = gtsam.triangulatePoint3(cameras, measurements, rank_tol=1e-9, optimize=True) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
def execute_ransac_variant(self, track_2d: SfmTrack2d) -> np.ndarray: """ Execute RANSAC algorithm to find best subset 2d measurements for a 3d point. RANSAC chooses one of 3 different sampling schemes to execute. Args: track: feature track with N 2d measurements in separate images Returns: best_inliers: boolean array of length N. Indices of measurements are set to true if they correspond to the best RANSAC hypothesis """ # Generate all possible matches measurement_pairs = self.generate_measurement_pairs(track_2d) # limit the number of samples to the number of available pairs num_hypotheses = min(self.num_ransac_hypotheses, len(measurement_pairs)) # Sampling samples = self.sample_ransac_hypotheses(track_2d, measurement_pairs, num_hypotheses) # Initialize the best output containers best_num_votes = 0 best_error = MAX_TRACK_REPROJ_ERROR best_inliers = np.zeros(len(track_2d.measurements), dtype=bool) for sample_idxs in samples: k1, k2 = measurement_pairs[sample_idxs] i1, uv1 = track_2d.measurements[k1] i2, uv2 = track_2d.measurements[k2] # check for unestimated cameras if self.track_camera_dict.get( i1) is None or self.track_camera_dict.get(i2) is None: logger.warning( "Unestimated cameras found at indices {} or {}. Skipping them." .format(i1, i2)) continue camera_estimates = CameraSetCal3Bundler() camera_estimates.append(self.track_camera_dict.get(i1)) camera_estimates.append(self.track_camera_dict.get(i2)) img_measurements = Point2Vector() img_measurements.append(uv1) img_measurements.append(uv2) # triangulate point for track try: triangulated_pt = gtsam.triangulatePoint3( camera_estimates, img_measurements, rank_tol=SVD_DLT_RANK_TOL, optimize=True, ) except RuntimeError: # TODO: handle cheirality exception properly? logger.info( "Cheirality exception from GTSAM's triangulatePoint3() likely due to outlier, skipping track" ) continue errors = self.compute_track_reprojection_errors( track_2d.measurements, triangulated_pt) # The best solution should correspond to the one with most inliers # If the inlier number are the same, check the average error of inliers is_inlier = errors < self.reproj_error_thresh # tally the number of votes inlier_errors = errors[is_inlier] if inlier_errors.size > 0: # only tally error over the inlier measurements avg_error = inlier_errors.mean() num_votes = is_inlier.astype(int).sum() if (num_votes > best_num_votes) or (num_votes == best_num_votes and avg_error < best_error): best_num_votes = num_votes best_error = avg_error best_inliers = is_inlier return best_inliers
def triangulate( self, track_2d: SfmTrack2d ) -> Tuple[Optional[SfmTrack], Optional[float], bool]: """Triangulates 3D point according to the configured triangulation mode. Args: track: feature track from which measurements are to be extracted Returns: track with inlier measurements and 3D landmark. None returned if triangulation fails or has high error. avg_track_reproj_error: reprojection error of 3d triangulated point to each image plane Note: this may be "None" if the 3d point could not be triangulated successfully due to a cheirality exception or insufficient number of RANSAC inlier measurements is_cheirality_failure: boolean representing whether the selected 2d measurements lead to a cheirality exception upon triangulation """ if self.mode in [ TriangulationParam.RANSAC_SAMPLE_UNIFORM, TriangulationParam.RANSAC_SAMPLE_BIASED_BASELINE, TriangulationParam.RANSAC_TOPK_BASELINES, ]: best_inliers = self.execute_ransac_variant(track_2d) elif self.mode == TriangulationParam.NO_RANSAC: best_inliers = np.ones(len(track_2d.measurements), dtype=bool) # all marked as inliers inlier_idxs = (np.where(best_inliers)[0]).tolist() is_cheirality_failure = False if len(inlier_idxs) < 2: return None, None, is_cheirality_failure inlier_track = track_2d.select_subset(inlier_idxs) camera_track, measurement_track = self.extract_measurements( inlier_track) try: triangulated_pt = gtsam.triangulatePoint3( camera_track, measurement_track, rank_tol=SVD_DLT_RANK_TOL, optimize=True, ) except RuntimeError: is_cheirality_failure = True return None, None, is_cheirality_failure # compute reprojection errors for each measurement reproj_errors = self.compute_track_reprojection_errors( inlier_track.measurements, triangulated_pt) # all the measurements should have error < threshold if not np.all(reproj_errors < self.reproj_error_thresh): return None, reproj_errors.mean(), is_cheirality_failure track_3d = SfmTrack(triangulated_pt) for i, uv in inlier_track.measurements: track_3d.add_measurement(i, uv) avg_track_reproj_error = reproj_errors.mean() return track_3d, avg_track_reproj_error, is_cheirality_failure
def triangulate( self, track_2d: SfmTrack2d ) -> Tuple[Optional[SfmTrack], Optional[float], TriangulationExitCode]: """Triangulates 3D point according to the configured triangulation mode. Args: track: feature track from which measurements are to be extracted Returns: track with inlier measurements and 3D landmark. None returned if triangulation fails or has high error. avg_track_reproj_error: reprojection error of 3d triangulated point to each image plane Note: this may be "None" if the 3d point could not be triangulated successfully due to a cheirality exception or insufficient number of RANSAC inlier measurements is_cheirality_failure: boolean representing whether the selected 2d measurements lead to a cheirality exception upon triangulation """ # Check if we will run RANSAC, or not. if self.options.mode in [ TriangulationSamplingMode.RANSAC_SAMPLE_UNIFORM, TriangulationSamplingMode.RANSAC_SAMPLE_BIASED_BASELINE, TriangulationSamplingMode.RANSAC_TOPK_BASELINES, ]: best_inliers = self.execute_ransac_variant(track_2d) elif self.options.mode == TriangulationSamplingMode.NO_RANSAC: best_inliers = np.ones(len(track_2d.measurements), dtype=bool) # all marked as inliers # Verify we have at least 2 inliers. inlier_idxs = (np.where(best_inliers)[0]).tolist() if len(inlier_idxs) < 2: return None, None, TriangulationExitCode.INLIERS_UNDERCONSTRAINED # Extract keypoint measurements corresponding to inlier indices. inlier_track = track_2d.select_subset(inlier_idxs) track_cameras, track_measurements = self.extract_measurements( inlier_track) # Exit if we do not have at least 2 measurements in cameras with estimated poses. if track_cameras is None: return None, None, TriangulationExitCode.POSES_UNDERCONSTRAINED # Triangulate and check for cheirality failure from GTSAM. try: triangulated_pt = gtsam.triangulatePoint3( track_cameras, track_measurements, rank_tol=SVD_DLT_RANK_TOL, optimize=True, ) except RuntimeError: return None, None, TriangulationExitCode.CHEIRALITY_FAILURE # Compute reprojection errors for each measurement. reproj_errors, avg_track_reproj_error = reproj_utils.compute_point_reprojection_errors( self.track_camera_dict, triangulated_pt, inlier_track.measurements) # Check that all measurements are within reprojection error threshold. if not np.all( reproj_errors.flatten() < self.options.reproj_error_threshold): return None, avg_track_reproj_error, TriangulationExitCode.EXCEEDS_REPROJ_THRESH # Create a gtsam.SfmTrack with the triangulated 3d point and associated 2d measurements. track_3d = SfmTrack(triangulated_pt) for i, uv in inlier_track.measurements: track_3d.addMeasurement(i, uv) return track_3d, avg_track_reproj_error, TriangulationExitCode.SUCCESS
def test_triangulation_robust_three_poses(self) -> None: """Ensure triangulation with a robust model works.""" sharedCal = Cal3_S2(1500, 1200, 0, 640, 480) # landmark ~5 meters infront of camera landmark = Point3(5, 0.5, 1.2) pose1 = Pose3(UPRIGHT, Point3(0, 0, 1)) pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0)) pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -0.1)) camera1 = PinholeCameraCal3_S2(pose1, sharedCal) camera2 = PinholeCameraCal3_S2(pose2, sharedCal) camera3 = PinholeCameraCal3_S2(pose3, sharedCal) z1: Point2 = camera1.project(landmark) z2: Point2 = camera2.project(landmark) z3: Point2 = camera3.project(landmark) poses = gtsam.Pose3Vector([pose1, pose2, pose3]) measurements = Point2Vector([z1, z2, z3]) # noise free, so should give exactly the landmark actual = gtsam.triangulatePoint3(poses, sharedCal, measurements, rank_tol=1e-9, optimize=False) self.assertTrue(np.allclose(landmark, actual, atol=1e-2)) # Add outlier measurements[0] += Point2(100, 120) # very large pixel noise! # now estimate does not match landmark actual2 = gtsam.triangulatePoint3(poses, sharedCal, measurements, rank_tol=1e-9, optimize=False) # DLT is surprisingly robust, but still off (actual error is around 0.26m) self.assertTrue(np.linalg.norm(landmark - actual2) >= 0.2) self.assertTrue(np.linalg.norm(landmark - actual2) <= 0.5) # Again with nonlinear optimization actual3 = gtsam.triangulatePoint3(poses, sharedCal, measurements, rank_tol=1e-9, optimize=True) # result from nonlinear (but non-robust optimization) is close to DLT and still off self.assertTrue(np.allclose(actual2, actual3, atol=0.1)) # Again with nonlinear optimization, this time with robust loss model = gtsam.noiseModel.Robust.Create( gtsam.noiseModel.mEstimator.Huber.Create(1.345), gtsam.noiseModel.Unit.Create(2)) actual4 = gtsam.triangulatePoint3(poses, sharedCal, measurements, rank_tol=1e-9, optimize=True, model=model) # using the Huber loss we now have a quite small error!! nice! self.assertTrue(np.allclose(landmark, actual4, atol=0.05))