diff --git a/src/apps/sfm_map_creator/ui.py b/src/apps/sfm_map_creator/ui.py index 59dad4c..e9ad484 100644 --- a/src/apps/sfm_map_creator/ui.py +++ b/src/apps/sfm_map_creator/ui.py @@ -57,6 +57,7 @@ def __init__( interactive: bool = True, debug: bool = True, ): + """TODO: Clean up.""" QtWidgets.QMainWindow.__init__(self, parent) try: self._init_ui() @@ -64,6 +65,9 @@ def __init__( print(f"UI setup failed! e={e}.") raise + self._mapper_thread = None + self._ipython_thread = None + self._image_update_signal.connect(self._show_image) self._state = _ExecutionState() @@ -73,10 +77,14 @@ def __init__( self._output_path = Path(output_path) # TODO: remove hardcoded data type. - calibration_data = kitti.KittiCalibration(Path(self._calibration_path)) - camera_calibration0 = calibration_data.stereo_calibration.calibrations[0] - self._output_path = self._output_path / str(uuid.uuid4()) - os.makedirs(self._output_path) + try: + calibration_data = kitti.KittiCalibration(Path(self._calibration_path)) + camera_calibration0 = calibration_data.stereo_calibration.calibrations[0] + self._output_path = self._output_path / str(uuid.uuid4()) + os.makedirs(self._output_path) + except FileNotFoundError as e: + print(e) + raise # Mapper self._debug = debug @@ -91,7 +99,6 @@ def __init__( ), ), ) - self._mapper_thread = None self._prev_frame_count = 0 # Monitor @@ -99,7 +106,6 @@ def __init__( self._reset_monitor() # IPython - self._ipython_thread = None if interactive: self._ipython_thread = threading.Thread(target=self._start_ipython) self._ipython_thread.start() @@ -138,7 +144,7 @@ def _run(self): self._mapper_thread.start() def __del__(self): - if self._mapper_thread.is_alive(): + if self._mapper_thread is not None and self._mapper_thread.is_alive(): self._mapper_thread.join() if self._ipython_thread is not None and self._ipython_thread.is_alive(): self._ipython_thread.join() @@ -148,7 +154,39 @@ def _run_mapping(self): while True: if not self._step_run(): break - time.sleep(0.01) + time.sleep(0.001) + print("Start map optimization.") + self._update_viewer() + + def _update_viewer(self): + if self._debug: + # NOTE: The points are estimated in reference frame. + frames = self._mapper.reconstruction.frames + if len(frames) < 2: + return + ref_frame = frames[-2] + points3d = ref_frame.points3d + if points3d is None or len(points3d) == 0: + return + + pose_G = ref_frame.pose_G + image_points = ref_frame.camera.project_points(points3d) + image_points = image_points.round().astype(np.int32) + + colors = ref_frame.image.data[image_points[:, 1], image_points[:, 0]] + self._plotter.add_points( + pose_G @ points3d, + scalars=colors, + rgb=True, + point_size=5.0, + ) + _plot_pose(self._plotter, pose_G) + else: + points = self._mapper.reconstruction.get_landmark_positions_G() + if len(points): + self._plotter.add_points( + points, point_size=5.0, color="g", render_points_as_spheres=True + ) def _update(self): """ @@ -156,47 +194,13 @@ def _update(self): """ if self._mapper.state.frame_count != self._prev_frame_count: self._prev_frame_count = self._mapper.state.frame_count - - if self._debug: - # NOTE: The points are estimated in reference frame. - frames = self._mapper.reconstruction.frames - if len(frames) < 2: - return - ref_frame = frames[-2] - points3d = ref_frame.points3d - if points3d is None or len(points3d) == 0: - return - - pose_G = ref_frame.pose_G - image_points = ref_frame.camera.project_points(points3d) - image_points = image_points.round().astype(np.int32) - - colors = ( - ref_frame.image.data[image_points[:, 1], image_points[:, 0]] - .reshape(-1, 3) - .astype(np.float) - ) - - self._plotter.add_points( - pose_G @ points3d, - # render_points_as_spheres=True, - scalars=colors, - rgb=True, - point_size=5.0, - ) - - _plot_pose(self._plotter, pose_G) - - else: - points = self._mapper.reconstruction.get_landmark_positions_G() - if len(points): - self._plotter.add_points( - points, point_size=5.0, color="g", render_points_as_spheres=True - ) + self._update_viewer() def _step_run(self): self._state.mode = _ExecutionMode.Step r = self._mapper.step_run() + self._mapper.bundle_adjustment() + self._update_viewer() self._image_update_signal.emit() return r diff --git a/src/mvg/mapping/common/frame.py b/src/mvg/mapping/common/frame.py index c049661..0fa5d55 100644 --- a/src/mvg/mapping/common/frame.py +++ b/src/mvg/mapping/common/frame.py @@ -20,6 +20,13 @@ class Frame: image: Optional[Image] = None points3d: Optional[np.ndarray] = None + def __repr__(self) -> str: + num_points3d = len(self.points3d) if self.points3d is not None else None + return ( + f"Frame(id={self.id}, #keypoints={len(self.keypoints)}, " + f"#points3d={num_points3d}, pose_G={self.pose_G})" + ) + def has_point(self, point: np.ndarray): # TODO return True diff --git a/src/mvg/mapping/common/reconstruction.py b/src/mvg/mapping/common/reconstruction.py index de5f4ce..65e5d01 100644 --- a/src/mvg/mapping/common/reconstruction.py +++ b/src/mvg/mapping/common/reconstruction.py @@ -1,7 +1,7 @@ import uuid from dataclasses import dataclass from pathlib import Path -from typing import List, Optional +from typing import Dict, List, Tuple import numpy as np import plyfile @@ -13,10 +13,8 @@ class Landmark: id: uuid.UUID pose_G: SE3 - descriptor: np.ndarray - frame_id: uuid.UUID - num_matches: Optional[int] = -1 # TODO - num_observations: Optional[int] = -1 # TODO + descriptor: np.ndarray # TODO: Compute a representative from the observations + observations: Dict[int, Tuple[Frame, Dict]] class Reconstruction: @@ -24,9 +22,14 @@ def __init__(self): # TODO: Use pandas.DataFrame self._landmarks_G: List[Landmark] = list() self._frames: List[Frame] = list() + self._frame_index_lookup = dict() def add_frame(self, frame: Frame): self._frames.append(frame) + self._frame_index_lookup[frame.id] = len(self._frames) - 1 + + def get_frame_index_from_id(self, frame_id: uuid.UUID): + return self._frame_index_lookup.get(frame_id) def add_landmark_G(self, landmark: Landmark): self._landmarks_G.append(landmark) @@ -55,10 +58,15 @@ def landmarks_G(self) -> np.ndarray: def dump(self, path: Path): points = self.get_landmark_positions_G() - filepath = path / "reconstruction.ply" - print(f"Writing to {filepath}.") + model_filepath = path / "reconstruction.ply" + print(f"Writing model to {model_filepath}.") vertices = np.array( [tuple(e) for e in points], dtype=[("x", " 1.0 - num_valid_points = np.count_nonzero(inlier_mask) - - if num_valid_points > max_num_valid_points: - max_num_valid_points = num_valid_points - best_T_RL = T_RL_candidates[i] - best_points3d_L = points3d_L - best_inlier_mask = inlier_mask - - return best_T_RL, best_points3d_L, best_inlier_mask - @staticmethod def _estimate_stereo_pose(frame_R: Frame, frame_C: Frame): keypoints_R = frame_R.keypoints @@ -122,19 +84,22 @@ def _estimate_stereo_pose(frame_R: Frame, frame_C: Frame): E_CR = frame_R.camera.K.as_matrix().T @ F_CR @ frame_C.camera.K.as_matrix() R1_CR, R2_CR, t_C = decompose_essential_matrix(E_RL=E_CR) - rel_pose_CR, points3d_R, inlier_mask_R = VisualOdometry._disambiguate_camera_pose( + rel_pose_CR, points3d_R, cheirality_inlier_mask = cheirality_check( + image_points_L=matched_points_R[fundamental_inlier_mask], + image_points_R=matched_points_C[fundamental_inlier_mask], R1_RL=R1_CR, R2_RL=R2_CR, t_R=t_C, - camera_matrix=frame_R.camera.K.as_matrix(), - image_points_L=matched_points_R[fundamental_inlier_mask], - image_points_R=matched_points_C[fundamental_inlier_mask], + K=frame_R.camera.K, ) return ( rel_pose_CR, - points3d_R[inlier_mask_R], - descriptors_R[query_indices][fundamental_inlier_mask][inlier_mask_R], + points3d_R[cheirality_inlier_mask], + matched_points_R[fundamental_inlier_mask][cheirality_inlier_mask], + matched_points_C[fundamental_inlier_mask][cheirality_inlier_mask], + descriptors_R[query_indices][fundamental_inlier_mask][cheirality_inlier_mask], + descriptors_C[train_indices][fundamental_inlier_mask][cheirality_inlier_mask], ) def _initialize_reconstruction(self, reconstruction: Reconstruction): @@ -143,7 +108,16 @@ def _initialize_reconstruction(self, reconstruction: Reconstruction): frame_R = reconstruction.frames[-2] frame_C = reconstruction.frames[-1] - rel_pose_CR, points3d_R, descriptors_R = self._estimate_stereo_pose(frame_R, frame_C) + + ( + rel_pose_CR, + points3d_R, + observations_R, + observations_C, + descriptors_R, + descriptors_C, + ) = self._estimate_stereo_pose(frame_R, frame_C) + is_in_range = self._get_points3d_range_mask(points3d_R) points3d_R = points3d_R[is_in_range] descriptors_R = descriptors_R[is_in_range] @@ -156,7 +130,20 @@ def _initialize_reconstruction(self, reconstruction: Reconstruction): id=uuid.uuid4(), pose_G=SE3.from_rotvec_pose(np.r_[0.0, 0.0, 0.0, point3d_G]), descriptor=descriptors_R[i], - frame_id=frame_R.id, + observations=OrderedDict( + { + frame_R.id: dict( + frame=frame_R, + observation=observations_R[i], + descriptor=descriptors_R[i], + ), + frame_C.id: dict( + frame=frame_C, + observation=observations_C[i], + descriptor=descriptors_C[i], + ), + } + ), ) ) @@ -208,9 +195,9 @@ def _get_image_point_visibility_mask(image_points): return is_x_in_view & is_y_in_view @staticmethod - def _get_points3d_range_mask(points3d, max_val=20.0): + def _get_points3d_range_mask(points3d, max_val=40.0): is_x_in_range = np.abs(points3d[:, 0]) < max_val - is_y_in_range = np.abs(points3d[:, 1]) < max_val + is_y_in_range = np.abs(points3d[:, 1]) < max_val * 0.5 is_z_in_range = (0.01 < points3d[:, 2]) & (points3d[:, 2] < max_val) return is_x_in_range & is_y_in_range & is_z_in_range @@ -228,7 +215,7 @@ def _filter_visible_landmarks_in_frame_R(landmarks_G: np.ndarray, frame_R: Frame def _filter_consistent_landmarks_in_frame_R( landmarks_G: np.ndarray, frame_R: Frame, - reprojection_error_threshold=2.0, + reprojection_error_threshold=1.0, ): """Filter out the landmarks that have consistent descriptors in frame (R). @@ -284,7 +271,9 @@ def _generate_new_landmarks( rel_pose_CR: SE3, frame_R: Frame, frame_C: Frame, - map_points_G: np.ndarray, + reconstruction: Reconstruction, + flow_optical_error_threshold=1.0, + point3d_distance_threshold=1.0, ): # Update new frame pose. rel_pose_RC = rel_pose_CR.inv() @@ -299,7 +288,8 @@ def _generate_new_landmarks( predicted_keypoints_C, _ = OpticalFlowLK.track( frame_R.image.data, frame_C.image.data, keypoints_R ) - flow_mask = np.linalg.norm(predicted_keypoints_C - keypoints_C, axis=1) < 1.0 + tracking_error = np.linalg.norm(predicted_keypoints_C - keypoints_C, axis=1) + flow_mask = tracking_error < flow_optical_error_threshold keypoints_R = keypoints_R[flow_mask] keypoints_C = keypoints_C[flow_mask] @@ -310,12 +300,35 @@ def _generate_new_landmarks( new_points3d_G = frame_R.pose_G @ new_points3d_R new_landmarks_G = [] - new_descriptors = frame_R.descriptors[query_indices][flow_mask][is_in_range] + descriptors_in_range_R = frame_R.descriptors[query_indices][flow_mask][is_in_range] + descriptors_in_range_C = frame_C.descriptors[train_indices][flow_mask][is_in_range] + + keypoints_in_range_R = keypoints_R[is_in_range] + keypoints_in_range_C = keypoints_C[is_in_range] + + # Data association + tree = KDTree(reconstruction.get_landmark_positions_G()) + # FIXME: Need to fix the scale to set this correctly + dists, indices = tree.query(new_points3d_G, distance_upper_bound=point3d_distance_threshold) + observed_mask = ~np.isinf(dists) + print(f" - {'Found observed landmarks':35s}: {np.count_nonzero(observed_mask):5d}.") - tree = KDTree(map_points_G) - dists, _ = tree.query(new_points3d_G) for i in range(len(new_points3d_G)): - if dists[i] < 0.2: + new_observations = { + frame_R.id: dict( + frame=frame_R, + observation=keypoints_in_range_R[i], + descriptor=descriptors_in_range_R[i], + ), + frame_C.id: dict( + frame=frame_C, + observation=keypoints_in_range_C[i], + descriptor=descriptors_in_range_C[i], + ), + } + if observed_mask[i]: + # FIXME: Don't care about the efficiency for now. The frame might be in the dict already. + reconstruction.landmarks_G[indices[i]].observations.update(new_observations) continue new_point3d_G = new_points3d_G[i] new_landmark_pose_G = SE3.from_rotvec_pose(np.r_[np.zeros(3), new_point3d_G]) @@ -323,8 +336,8 @@ def _generate_new_landmarks( Landmark( id=uuid.uuid4(), pose_G=new_landmark_pose_G, - descriptor=new_descriptors[i], - frame_id=frame_R.id, + descriptor=descriptors_in_range_R[i], + observations=OrderedDict(new_observations), ) ) return new_landmarks_G @@ -343,7 +356,7 @@ def _add_frame(self, reconstruction: Reconstruction, frame: Frame): print(f"Translation too large: {translation_mag:7.3f}!") return False new_landmarks_G = self._generate_new_landmarks( - rel_pose_CR, frame_R, frame_C, reconstruction.get_landmark_positions_G() + rel_pose_CR, frame_R, frame_C, reconstruction ) reconstruction.extend_landmarks_G(new_landmarks_G) return True diff --git a/src/mvg/mapping/mapper/sfm.py b/src/mvg/mapping/mapper/sfm.py index da447aa..f0ac546 100644 --- a/src/mvg/mapping/mapper/sfm.py +++ b/src/mvg/mapping/mapper/sfm.py @@ -7,8 +7,15 @@ import numpy as np from mvg import features, streamer +from mvg.basic import SE3 from mvg.camera import Camera -from mvg.mapping.common import frame, reconstruction, visual_odometry +from mvg.mapping.common import frame, visual_odometry +from mvg.mapping.common.reconstruction import Reconstruction +from scipy.optimize import least_squares +from scipy.sparse import lil_matrix + +_N_CAMERA_PARAM = 6 +_N_POINT_PARAM = 3 class IncrementalSFM: @@ -70,7 +77,7 @@ def _ensure_reconstruction(self): with self._reconstruction_lock: if self._reconstruction is None: print("Creating reconstruction (map) object...") - self._reconstruction = reconstruction.Reconstruction() + self._reconstruction = Reconstruction() @property def cache(self): @@ -118,6 +125,98 @@ def step_run(self) -> bool: self.state.frame_count += 1 return True + @staticmethod + def _residual(x, reconstruction: Reconstruction, camera: Camera): + # FIXME: This is a quick implementation, need to optimize it. + poses_G, points3d_G = IncrementalSFM._decompose_parameters(x, len(reconstruction.frames)) + residuals = [] + for landmark_index, landmark_G in enumerate(reconstruction.landmarks_G): + point3d_G = points3d_G[landmark_index] + for frame_id, observation in landmark_G.observations.items(): + pose_G = poses_G[reconstruction.get_frame_index_from_id(frame_id)] + reprojected_image_point = camera.project_points([pose_G.inv() @ point3d_G])[0] + residuals.append(reprojected_image_point - observation["observation"]) + return np.asarray(residuals).reshape(-1) + + @staticmethod + def _compose_parameters(reconstruction: Reconstruction): + return np.r_[ + np.reshape([frame.pose_G.as_rotvec_pose() for frame in reconstruction.frames], -1), + np.reshape([landmark_G.pose_G.t for landmark_G in reconstruction.landmarks_G], -1), + ] + + @staticmethod + def _decompose_parameters(x, num_frames): + poses_G = [ + SE3.from_rotvec_pose(x[i : i + _N_CAMERA_PARAM]) + for i in range(0, num_frames * _N_CAMERA_PARAM, _N_CAMERA_PARAM) + ] + points3d_G = x[num_frames * _N_CAMERA_PARAM :].reshape(-1, _N_POINT_PARAM) + return (poses_G, points3d_G) + + @staticmethod + def _get_jac_sparsity(reconstruction: Reconstruction): + num_landmarks = len(reconstruction.landmarks_G) + num_frames = len(reconstruction.frames) + n = num_frames * _N_CAMERA_PARAM + num_landmarks * _N_POINT_PARAM + A = [] + frame_indices = [] + landmark_indices = [] + for landmark_index, landmark_G in enumerate(reconstruction.landmarks_G): + for frame_id, _ in landmark_G.observations.items(): + frame_indices.append(reconstruction.get_frame_index_from_id(frame_id)) + landmark_indices.append(landmark_index) + frame_indices = np.asarray(frame_indices, dtype=np.int32) + landmark_indices = np.asarray(landmark_indices, dtype=np.int32) + m = frame_indices.size * 2 + i = np.arange(frame_indices.size) + A = lil_matrix((m, n), dtype=int) + for j in range(_N_CAMERA_PARAM): + A[2 * i, frame_indices * _N_CAMERA_PARAM + j] = 1 + A[2 * i + 1, frame_indices * _N_CAMERA_PARAM + j] = 1 + for j in range(_N_POINT_PARAM): + A[2 * i, num_frames * _N_CAMERA_PARAM + landmark_indices * _N_POINT_PARAM + j] = 1 + A[2 * i + 1, num_frames * _N_CAMERA_PARAM + landmark_indices * _N_POINT_PARAM + j] = 1 + return A + + def bundle_adjustment(self): + result = least_squares( + fun=self._residual, + x0=self._compose_parameters(self._reconstruction), + jac_sparsity=self._get_jac_sparsity(self._reconstruction), + kwargs=dict(reconstruction=self._reconstruction, camera=self._camera), + x_scale="jac", + ftol=1e-4, + method="trf", + loss="huber", + verbose=1, + ) + if result["success"]: + poses_G, points_G = self._decompose_parameters( + result["x"], len(self._reconstruction.frames) + ) + for i, f in enumerate(self._reconstruction.frames): + f.pose_G = poses_G[i] + for i, landmark_G in enumerate(self._reconstruction.landmarks_G): + landmark_G.pose_G.t = points_G[i] + observation_residuals = np.linalg.norm(result["fun"].reshape(-1, 2), axis=1) + res_index = 0 + to_preserve = [] + for landmark_index, _ in enumerate(self._reconstruction.landmarks_G): + dists = [] + for _ in enumerate(landmark_G.observations.values()): + dists.append(observation_residuals[res_index]) + res_index += 1 + if np.asarray(dists).mean() < 2.0 and np.asarray(dists).std() < 2.0: + to_preserve.append(landmark_index) + print( + f" - {'# preversed landmarks':35s}: {len(to_preserve)} / {len(self._reconstruction.landmarks_G)}." + ) + # FIXME + self._reconstruction._landmarks_G = np.asarray(self._reconstruction._landmarks_G)[ + to_preserve + ].tolist() + def run(self): """TODO: virtualize this function.""" self._ensure_reconstruction() @@ -128,10 +227,8 @@ def run(self): i = 0 while True: frame_data = self._streamer.get() - if frame_data is None: break - image = None if isinstance(self._streamer, streamer.FeatureFileStreamer): keypoints, descriptors = frame_data @@ -142,7 +239,6 @@ def run(self): frame_data.keypoints = keypoints frame_data.descriptors = descriptors image = frame_data - f = frame.Frame( id=uuid.uuid4(), timestamp=-1, @@ -151,17 +247,15 @@ def run(self): camera=self._camera, image=image, ) - succeeded = self._vo.add_frame(reconstruction=self._reconstruction, frame=f) - print( f"Added {i:>5d}-th frame, id={f.id}, " f"current map size: {len(self._reconstruction.landmarks_G):5d}." ) - if not succeeded: break - + if i > 1: + self.bundle_adjustment() i += 1 except KeyboardInterrupt: diff --git a/src/mvg/stereo.py b/src/mvg/stereo.py index ba7481e..e3dc986 100644 --- a/src/mvg/stereo.py +++ b/src/mvg/stereo.py @@ -2,25 +2,25 @@ """This module implement stereo vision related algorithms.""" -from math import pi, sqrt from dataclasses import dataclass +from itertools import product +from math import pi, sqrt from typing import Optional import cv2 -from itertools import product - +import numpy as np from scipy.optimize import least_squares from scipy.spatial.transform import Rotation -import numpy as np - -from mvg.homography import Homography2d from mvg.basic import ( + SE3, SkewSymmetricMatrix3d, get_isotropic_scaling_matrix_2d, - homogenize, get_line_points_in_image, + homogenize, ) +from mvg.camera import CameraMatrix +from mvg.homography import Homography2d class Fundamental: @@ -315,47 +315,6 @@ def _plot_line(ax, lines, width, height): plt.tight_layout() plt.show() - # @staticmethod - # def _assert_symbolic_A(): - # """This function is used for manual validation.""" - # import sympy as sym - - # x_L = np.array(np.r_[sym.symbols("x1 y1"), 1.0]).reshape(-1, 1) - # x_R = np.array(np.r_[sym.symbols("x2 y2"), 1.0]).reshape(-1, 1) - - # F_RL = [] - # for row in range(1, 4): - # for col in range(1, 4): - # F_RL.append(sym.Symbol(f"f{row}{col}")) - # F_RL = np.asarray(F_RL).reshape(3, 3) - - # eqn1 = sym.Matrix(x_R.T @ F_RL @ x_L) - # eqn2 = sym.Matrix(x_L.T @ F_RL.T @ x_R) - - # A = np.kron(x_R, x_L) - # f = F_RL.reshape(-1) - # Af = A.T @ f - # eqn3 = sym.Matrix(Af) - - # subs = dict( - # x1=1, - # x2=2, - # y1=3, - # y2=4, - # f11=1, - # f12=2, - # f13=3, - # f21=4, - # f22=5, - # f23=6, - # f31=7, - # f32=8, - # f33=9, - # ) - - # assert eqn1.subs(subs) == eqn2.subs(subs) - # assert eqn3.subs(subs) == eqn1.subs(subs) - def decompose_essential_matrix(*, E_RL: np.ndarray): """Decompose essential matrix describing the change of frame from (L) to (R).""" @@ -381,6 +340,44 @@ def decompose_essential_matrix(*, E_RL: np.ndarray): return R1_RL, R2_RL, t_R +def cheirality_check( + image_points_L: np.ndarray, + image_points_R: np.ndarray, + R1_RL: Rotation, + R2_RL: Rotation, + t_R: np.ndarray, + K: CameraMatrix, +): + P1 = np.hstack([K.as_matrix(), np.zeros((3, 1))]) + camera_matrix = K.as_matrix() + T_candidates_RL = [ + SE3.from_rotmat_tvec(R1_RL, t_R), + SE3.from_rotmat_tvec(R1_RL, -t_R), + SE3.from_rotmat_tvec(R2_RL, t_R), + SE3.from_rotmat_tvec(R2_RL, -t_R), + ] + + best_T_RL = None + best_points_3d_L = None + best_inlier_mask = None + best_num_valid_points = -1 + + for T_RL in T_candidates_RL: + P2 = camera_matrix @ T_RL.as_augmented_matrix() + points_3d_L = triangulate(P1, P2, image_points_L, image_points_R) + + inlier_mask = (points_3d_L[:, 2] > 0.0) & ((T_RL @ points_3d_L)[:, 2] > 0.0) + num_valid_points = np.count_nonzero(inlier_mask) + + if num_valid_points > best_num_valid_points: + best_num_valid_points = num_valid_points + best_T_RL = T_RL + best_points_3d_L = points_3d_L + best_inlier_mask = inlier_mask + + return best_T_RL, best_points_3d_L, best_inlier_mask + + def triangulate(P1, P2, points1, points2): """Use projection matrices and pixels coordinates to find out the 3D points. diff --git a/src/mvg/streamer.py b/src/mvg/streamer.py index 89ac79c..fef86b4 100644 --- a/src/mvg/streamer.py +++ b/src/mvg/streamer.py @@ -23,7 +23,7 @@ def __init__(self, path: str, buffer_size: int = 10): def _get_frame_reader(self): filenames = sorted(os.listdir(self._path)) - for filename in filenames[::2]: + for filename in filenames[:3:1]: # FIXME: only support Kitti images. if len(filename) != 14 or not filename.endswith(".png"): continue