def test_kapture_write_read(self): kdata_expected = kapture.Kapture() kdata_expected.sensors = kapture.Sensors() kdata_expected.trajectories = kapture.Trajectories() kdata_expected.rigs = kapture.Rigs() csv.kapture_to_dir(self._tempdir.name, kdata_expected) kdata_actual = csv.kapture_from_dir(self._tempdir.name)
def test_init_wifi(self): records_wifi = kapture.RecordsWifi() timestamp0, timestamp1 = 0, 1 device_id0, device_id1 = 'AC01324954_WIFI', 'AC01324955_WIFI' bssid, ssid = '68:72:51:80:52:df', 'M1X_PicoM2' rssi = -33 freq = 2417 scan_time_start, scan_time_end = 1555398770280, 1555398770290 # assign wifi_data = kapture.RecordWifi({bssid: kapture.RecordWifiSignal(ssid=ssid, rssi=rssi, frequency=freq, scan_time_start=scan_time_start, scan_time_end=scan_time_end)}) records_wifi[timestamp0, device_id0] = wifi_data kapture_data = kapture.Kapture(records_wifi=records_wifi) self.assertEqual(1, len(kapture_data.records_wifi.keys())) self.assertEqual(1, len(kapture_data.records_wifi.key_pairs())) self.assertIn(timestamp0, kapture_data.records_wifi) self.assertIn(device_id0, kapture_data.records_wifi[timestamp0]) self.assertIn((timestamp0, device_id0), kapture_data.records_wifi) self.assertEqual(wifi_data, kapture_data.records_wifi[timestamp0, device_id0]) kapture_data.records_wifi[timestamp1, device_id1] = wifi_data self.assertEqual(2, len(kapture_data.records_wifi.keys())) self.assertEqual(2, len(kapture_data.records_wifi.key_pairs())) kapture_data.records_wifi[timestamp0][device_id1] = wifi_data self.assertEqual(2, len(kapture_data.records_wifi.keys())) self.assertEqual(3, len(kapture_data.records_wifi.key_pairs()))
def test_kapture_format_version_from_disk(self): kapture_data = kapture.Kapture() kapture_data.sensors = kapture.Sensors() csv.kapture_to_dir(self._tempdir.name, kapture_data) version = csv.kapture_format_version(self._tempdir.name) self.assertEqual(csv.current_format_version(), version, "We have the current version")
def test_init_wifi(self): records_wifi = kapture.RecordsWifi() timestamp0, timestamp1 = 0, 1 device_id0, device_id1 = 'AC01324954_WIFI', 'AC01324955_WIFI' bssid = '68:72:51:80:52:df' rssi = -33 freq = 2417 scan_time = 1555398770280 visible_name = 'M1X_PicoM2' # assign wifi_data = {} wifi_data[bssid] = kapture.RecordWifi(rssi, freq, scan_time, visible_name) records_wifi[timestamp0, device_id0] = wifi_data kapture_data = kapture.Kapture(records_wifi=records_wifi) self.assertEqual(1, len(kapture_data.records_wifi.keys())) self.assertEqual(1, len(kapture_data.records_wifi.key_pairs())) self.assertIn(timestamp0, kapture_data.records_wifi) self.assertIn(device_id0, kapture_data.records_wifi[timestamp0]) self.assertIn((timestamp0, device_id0), kapture_data.records_wifi) self.assertEqual(wifi_data, kapture_data.records_wifi[timestamp0, device_id0]) kapture_data.records_wifi[timestamp1, device_id1] = wifi_data self.assertEqual(2, len(kapture_data.records_wifi.keys())) self.assertEqual(2, len(kapture_data.records_wifi.key_pairs())) kapture_data.records_wifi[timestamp0][device_id1] = wifi_data self.assertEqual(2, len(kapture_data.records_wifi.keys())) self.assertEqual(3, len(kapture_data.records_wifi.key_pairs()))
def generate_priors_for_reconstruction( kapture_data: kapture.Kapture, database: COLMAPDatabase, path_to_priors_for_reconstruction: str) -> None: """ Generate priors for a reconstruction using cameras, images and trajectories. :param kapture_data: kapture data :param database: colmap database. :param path_to_priors_for_reconstruction: path to the priors file """ colmap_camera_ids = get_colmap_camera_ids_from_db( database, kapture_data.records_camera) colmap_image_ids = get_colmap_image_ids_from_db(database) kapture_data_copy = kapture.Kapture( sensors=kapture_data.sensors, records_camera=kapture_data.records_camera, trajectories=kapture_data.trajectories, rigs=kapture_data.rigs) # in priors, do not copy keypoints, points3d export_to_colmap_txt( path_to_priors_for_reconstruction, kapture_data_copy, "", # kapture_data_copy do not have binaries so path is irrelevant colmap_camera_ids, colmap_image_ids)
def test_kapture_write(self): kdata = kapture.Kapture() # test it is not writing files for undefined parts csv.kapture_to_dir(self._tempdir.name, kdata) self.assertFalse( path.exists(path.join(self._tempdir.name, 'sensors', 'sensors.txt'))) self.assertFalse( path.exists( path.join(self._tempdir.name, 'sensors', 'trajectories.txt'))) self.assertFalse( path.exists(path.join(self._tempdir.name, 'sensors', 'rigs.txt'))) # test it is actually writing files for parts kdata.sensors = kapture.Sensors() kdata.trajectories = kapture.Trajectories() kdata.rigs = kapture.Rigs() csv.kapture_to_dir(self._tempdir.name, kdata) self.assertTrue( path.exists(path.join(self._tempdir.name, 'sensors', 'sensors.txt'))) self.assertTrue( path.exists( path.join(self._tempdir.name, 'sensors', 'trajectories.txt'))) self.assertTrue( path.exists(path.join(self._tempdir.name, 'sensors', 'rigs.txt')))
def test_init_camera(self): timestamp0, timestamp1 = 0, 1 device_id0, device_id1 = 'cam0', 'cam1' records_camera = kapture.RecordsCamera() records_camera[timestamp0, device_id0] = 'cam0/image000.jpg' kapture_data = kapture.Kapture(records_camera=records_camera) self.assertEqual(1, len(kapture_data.records_camera.keys())) self.assertEqual(1, len(kapture_data.records_camera.key_pairs())) self.assertIn(timestamp0, kapture_data.records_camera) self.assertIn(device_id0, kapture_data.records_camera[timestamp0]) self.assertIn((timestamp0, device_id0), kapture_data.records_camera) self.assertEqual('cam0/image000.jpg', kapture_data.records_camera[timestamp0, device_id0]) records_camera[timestamp1, device_id0] = 'cam0/image001.jpg' self.assertEqual(2, len(kapture_data.records_camera.keys())) self.assertEqual(2, len(kapture_data.records_camera.key_pairs())) kapture_data.records_camera[timestamp0][ device_id1] = 'cam1/image000.jpg' self.assertEqual(2, len(kapture_data.records_camera.keys())) self.assertEqual(3, len(kapture_data.records_camera.key_pairs())) records_camera[timestamp1][device_id1] = 'cam1/image001.jpg' self.assertEqual(2, len(kapture_data.records_camera.keys())) self.assertEqual(4, len(kapture_data.records_camera.key_pairs())) self.assertEqual('cam0/image000.jpg', kapture_data.records_camera[timestamp0, device_id0]) self.assertEqual('cam1/image000.jpg', kapture_data.records_camera[timestamp0, device_id1]) self.assertEqual('cam0/image001.jpg', kapture_data.records_camera[timestamp1, device_id0]) self.assertEqual('cam1/image001.jpg', kapture_data.records_camera[timestamp1, device_id1]) self.assertNotIn((timestamp1, 'cam2'), kapture_data.records_camera) self.assertNotIn((2, device_id0), kapture_data.records_camera)
def import_colmap_database(colmap_database_filepath: str, kapture_dir_path: str, no_geometric_filtering: bool = False, skip_reconstruction: bool = False, keypoint_name: str = 'SIFT', descriptor_name: str = 'SIFT') -> kapture.Kapture: """ Converts colmap database file to kapture data. If kapture_dir_path is given, it creates keypoints, descriptors, matches files (if any). :param colmap_database_filepath: path to colmap database file. :param kapture_dir_path: path to kapture directory. Is used to store keypoints, descriptors and matches files. If not given (None), is equivalent to skip_reconstruction == True. :param no_geometric_filtering: :param keypoint_name: name of the keypoints detector (by default, in colmap, its SIFT, but can be imported) :param descriptor_name: name of the keypoints descriptor (by default, in colmap, its SIFT, but can be imported) :param skip_reconstruction: skip the import of the kapture/reconstruction part, ie. Keypoints, Descriptors, Matches. :return: kapture object """ kapture_data = kapture.Kapture() logger.debug(f'loading colmap database {colmap_database_filepath}') db = COLMAPDatabase.connect(colmap_database_filepath) # Generate cameras logger.debug('parsing cameras in database.') kapture_data.sensors = get_cameras_from_database(db) # Images, Trajectories logger.debug('parsing images and trajectories in database.') kapture_data.records_camera, kapture_data.trajectories = get_images_and_trajectories_from_database( db) if kapture_dir_path is not None and not skip_reconstruction: os.makedirs(kapture_dir_path, exist_ok=True) # keypoints logger.debug('parsing keypoints in database...') kapture_data.keypoints = get_keypoints_from_database( db, kapture_data.records_camera, kapture_dir_path, keypoint_name) # descriptors logger.debug('parsing descriptors in database...') kapture_data.descriptors = get_descriptors_from_database( db, kapture_data.records_camera, kapture_dir_path, descriptor_name) # matches logger.debug('parsing matches in database...') kapture_data.matches = get_matches_from_database( db, kapture_data.records_camera, kapture_dir_path, no_geometric_filtering) db.close() return kapture_data
def import_image_folder( images_path: str, kapture_path: str, force_overwrite_existing: bool = False, images_import_method: TransferAction = TransferAction.skip) -> None: """ Imports the images of a folder to a kapture. This creates only images and cameras. :param images_path: path to directory containing the images. :param kapture_path: path to kapture root directory. :param force_overwrite_existing: Silently overwrite kapture files if already exists. :param images_import_method: choose how to import actual image files. """ os.makedirs(kapture_path, exist_ok=True) delete_existing_kapture_files(kapture_path, force_erase=force_overwrite_existing) cameras = kapture.Sensors() images = kapture.RecordsCamera() file_list = [ os.path.relpath(os.path.join(dirpath, filename), images_path) for dirpath, dirs, filenames in os.walk(images_path) for filename in filenames ] file_list = sorted(file_list) logger.info('starting conversion...') for n, filename in enumerate(file_list): # test if file is a valid image try: # lazy load with Image.open(path.join(images_path, filename)) as im: width, height = im.size model_params = [width, height] except (OSError, PIL.UnidentifiedImageError): # It is not a valid image: skip it logger.info(f'Skipping invalid image file {filename}') continue camera_id = f'sensor{n}' images[(n, camera_id)] = path_secure(filename) # don't forget windows cameras[camera_id] = kapture.Camera(kapture.CameraType.UNKNOWN_CAMERA, model_params) # import (copy) image files. logger.info('import image files ...') filename_list = [f for _, _, f in kapture.flatten(images)] import_record_data_from_dir_auto(images_path, kapture_path, filename_list, images_import_method) # pack into kapture format imported_kapture = kapture.Kapture(sensors=cameras, records_camera=images) logger.info('writing imported data...') kapture_to_dir(kapture_path, imported_kapture)
def import_colmap_from_reconstruction_files(reconstruction_dirpath: str, kapture_dirpath: Optional[str], skip: Set[Type[Union[kapture.Keypoints, kapture.Points3d, kapture.Observations]]] ) -> kapture.Kapture: """ Converts colmap reconstruction files to kapture data. If kapture_dirpath is given, keypoints files are created, and potentially their observations. :param reconstruction_dirpath: :param kapture_dirpath: path to kapture directory. Is used to store keypoints files. If not given (None), keypoints are automatically skipped. :param skip: can skip independently : Keypoints, Points3d or Observations. Note that Points3d and Observations are in the same file, so you should skip both to gain its reading. :return: kapture object """ logger.debug(f'loading colmap reconstruction from:\n\t"{reconstruction_dirpath}"') if skip: logger.debug(f'loading colmap reconstruction skipping {", ".join(s.__name__ for s in skip)}') kapture_data = kapture.Kapture() reconstruction_file_paths = (path.join(reconstruction_dirpath, filename) for filename in ['cameras.txt', 'images.txt', 'points3D.txt']) colmap_cameras_filepath, colmap_images_filepath, colmap_points3d_filepath = reconstruction_file_paths proceed_keypoints = kapture.Keypoints not in skip and kapture_dirpath is not None proceed_points3d = kapture.Points3d not in skip and path.exists(colmap_points3d_filepath) proceed_observations = kapture.Observations not in skip and path.exists(colmap_points3d_filepath) if path.exists(colmap_cameras_filepath): logging.debug(f'parsing cameras from:\n\t"{path.basename(colmap_cameras_filepath)}"') kapture_data.sensors = import_from_colmap_cameras_txt(colmap_cameras_filepath) if path.exists(colmap_images_filepath): logging.debug(f'loading images from:\n\t"{path.basename(colmap_images_filepath)}"') kapture_dirpath_for_keypoints = kapture_dirpath if proceed_keypoints else None images, trajectories, keypoints = import_from_colmap_images_txt( colmap_images_filepath, kapture_dirpath_for_keypoints) kapture_data.records_camera = images kapture_data.trajectories = trajectories kapture_data.keypoints = keypoints if proceed_points3d or proceed_observations: assert kapture_data.records_camera is not None image_id_2_names = {ts: image_name for ts, cam_id, image_name in kapture.flatten(kapture_data.records_camera, True)} logger.debug(f'parsing 3d points and observations from:\n\t"{path.basename(colmap_points3d_filepath)}"') points3d, observations = import_from_colmap_points3d_txt(colmap_points3d_filepath, image_id_2_names) kapture_data.points3d = points3d if proceed_points3d else None kapture_data.observations = observations if proceed_observations else None return kapture_data
def test_init(self): lidar0 = kapture.Sensor('lidar', []) cam0 = kapture.Sensor('camera', []) sensors = kapture.Sensors() sensors['cam0'] = cam0 kapture_data = kapture.Kapture(sensors=sensors) self.assertEqual(sensors, kapture_data.sensors) self.assertEqual(sensors['cam0'], kapture_data.sensors['cam0']) # assign sensors = kapture_data.sensors self.assertIsInstance(sensors, kapture.Sensors) kapture_data.sensors = sensors kapture_data.sensors['lidar0'] = lidar0
def test_init_camera(self): timestamp0, timestamp1 = 0, 1 device_id0, device_id1 = 'cam0', 'cam1' record_cam0_image0 = 'cam0/image000.jpg' record_cam0_image1 = 'cam0/image001.jpg' record_cam1_image0 = 'cam1/image000.jpg' record_cam1_image1 = 'cam1/image001.jpg' # Test insertions records_camera = kapture.RecordsCamera() records_camera[timestamp0, device_id0] = record_cam0_image0 kapture_data = kapture.Kapture(records_camera=records_camera) self.assertEqual(1, len(kapture_data.records_camera.keys())) self.assertEqual(1, len(kapture_data.records_camera.key_pairs())) self.assertIn(timestamp0, kapture_data.records_camera) self.assertIn(device_id0, kapture_data.records_camera[timestamp0]) self.assertIn((timestamp0, device_id0), kapture_data.records_camera) self.assertEqual(record_cam0_image0, kapture_data.records_camera[timestamp0, device_id0]) records_camera[timestamp1, device_id0] = record_cam0_image1 self.assertEqual(2, len(kapture_data.records_camera.keys())) self.assertEqual(2, len(kapture_data.records_camera.key_pairs())) kapture_data.records_camera[timestamp0][ device_id1] = record_cam1_image0 self.assertEqual(2, len(kapture_data.records_camera.keys())) self.assertEqual(3, len(kapture_data.records_camera.key_pairs())) records_camera[timestamp1][device_id1] = record_cam1_image1 self.assertEqual(2, len(kapture_data.records_camera.keys())) self.assertEqual(4, len(kapture_data.records_camera.key_pairs())) self.assertEqual(record_cam0_image0, kapture_data.records_camera[timestamp0, device_id0]) self.assertEqual(record_cam1_image0, kapture_data.records_camera[timestamp0, device_id1]) self.assertEqual(record_cam0_image1, kapture_data.records_camera[timestamp1, device_id0]) self.assertEqual(record_cam1_image1, kapture_data.records_camera[timestamp1, device_id1]) self.assertNotIn((timestamp1, 'cam2'), kapture_data.records_camera) self.assertNotIn((2, device_id0), kapture_data.records_camera) self.assertEqual(kapture_data.records_camera.sensors_ids, {device_id0, device_id1}) # Test deletion del kapture_data.records_camera[(timestamp0, device_id0)] self.assertEqual(2, len(kapture_data.records_camera.keys())) self.assertEqual(3, len(kapture_data.records_camera.key_pairs())) del kapture_data.records_camera[(timestamp0, device_id1)] self.assertEqual(1, len(kapture_data.records_camera.keys())) self.assertEqual(2, len(kapture_data.records_camera.key_pairs()))
def save_to_kapture(self, trajectory_rig_id: Optional[str] = None) -> None: """ Save the data in kapture format. :param trajectory_rig_id: the rig identifier of the trajectory points """ # Convert pose info to trajectories if len(self.poses_info) > 0 and trajectory_rig_id is None: raise ValueError("Must provide rig identifier for trajectory") trajectories = kapture.Trajectories() if len( self.poses_info) > 0 else None for pose_info in self.poses_info: t = pose_info.timestamp.to_nsec() ros_translation = pose_info.pose6d.position translation = [ ros_translation.x, ros_translation.y, ros_translation.z ] ros_rotation = pose_info.pose6d.orientation rotation = np.quaternion(ros_rotation.w, ros_rotation.x, ros_rotation.y, ros_rotation.z) # Transform the pose from the ROS body coordinate system defined here # https://www.ros.org/reps/rep-0103.html#axis-orientation # to the Kapture coordinate system # ros pose seems to be the inverse of the extrinsic matrix # i.e world position and rig orientation with respect to the world axis pose6d = kapture.PoseTransform.compose([ pose_kapture_from_ros, kapture.PoseTransform(rotation, translation).inverse(), pose_ros_from_kapture ]) trajectories[(t, trajectory_rig_id)] = pose6d self.logger.info(f'Saving {len(list(flatten(trajectories)))} poses') # Convert image info to kapture image records_camera = kapture.RecordsCamera() for image_info in self.images_info: t = image_info.timestamp.to_nsec() records_camera[(t, image_info.camera_name)] = image_info.filename self.logger.info( f'Saving {len(list(flatten(records_camera)))} camera records') kapture_data = kapture.Kapture(rigs=self._rigs, sensors=self._sensors, records_camera=records_camera, trajectories=trajectories) self.logger.info(f'Saving to kapture {self._kapture_path}') kcsv.kapture_to_dir(self._kapture_path, kapture_data) self.logger.info('Done')
def test_as_dict(self): kapture_data = kapture.Kapture() # test empty members = kapture_data.as_dict() self.assertEqual(members, {}) members = kapture_data.as_dict(keep_none=True) self.assertEqual(len(members), 18) self.assertTrue(all(member is None for member in members.values())) # test sensors only kapture_data.sensors = kapture.Sensors({'cam0': kapture.Sensor('camera', [])}) members = kapture_data.as_dict() self.assertEqual(len(members), 1) self.assertEqual(members, {'sensors': kapture_data.sensors}) members = kapture_data.as_dict(keep_none=True) self.assertEqual(len(members), 18) self.assertEqual(members['sensors'], kapture_data.sensors) self.assertTrue(all(member is None for name, member in members.items() if name != 'sensors'))
def import_openmvg_sfm_data_json( sfm_data_json: Dict[str, Union[str, Dict]], kapture_images_path: str, image_action=TransferAction.skip) -> kapture.Kapture: """ Imports an openMVG sfm_data json structure to a kapture object. Also copy, move or link the images files if necessary. :param sfm_data_json: the openmvg JSON parsed as a dictionary :param kapture_images_path: top directory to create the kapture images tree :param image_action: action to apply on images: link, copy, move or do nothing. :return: the constructed kapture object """ data_root_path: str = '' if sfm_data_json[JSON_KEY.ROOT_PATH]: data_root_path = sfm_data_json[JSON_KEY.ROOT_PATH] elif image_action == TransferAction.skip: logger.warning('No root_path in sfm_data.') else: # It is needed to execute an action with the image file raise ValueError( f"Missing root_path to do image action '{image_action.name}'") openmvg_images_dir = path.basename(data_root_path) # Imports all the data from the json file to kapture objects kapture_cameras = import_openmvg_cameras(sfm_data_json) device_identifiers = {int: str} # Pose id -> device id timestamp_for_pose = {int: int} # Pose id -> timestamp # Imports the images as records_camera, but also fill in the devices_identifiers and timestamp_for_pose dictionaries records_camera = import_openmvg_images(sfm_data_json, image_action, kapture_images_path, openmvg_images_dir, data_root_path, device_identifiers, timestamp_for_pose) trajectories = import_openmvg_trajectories(sfm_data_json, device_identifiers, timestamp_for_pose) kapture_data = kapture.Kapture(sensors=kapture_cameras, records_camera=records_camera, trajectories=trajectories) return kapture_data
def sub_kapture_from_img_list(kdata, kdata_path, img_list, pairs): trajectories = kapture.Trajectories() sensors = kapture.Sensors() records = kapture.RecordsCamera() keypoints = kapture.Keypoints(kdata.keypoints._tname, kdata.keypoints._dtype, kdata.keypoints._dsize) if kdata.descriptors != None: descriptors = kapture.Descriptors(kdata.descriptors._tname, kdata.descriptors._dtype, kdata.descriptors._dsize) else: descriptors = None matches = kapture.Matches() timestamp_sensor_id_from_image_name = { img_name: (timestamp, sensor_id) for timestamp, sensor_id, img_name in kapture.flatten( kdata.records_camera) } for img in img_list: timestamp, sensor_id = timestamp_sensor_id_from_image_name[img] pose = kdata.trajectories[timestamp][sensor_id] sensors[sensor_id] = kdata.sensors[sensor_id] records[timestamp, sensor_id] = img trajectories[timestamp, sensor_id] = pose keypoints.add(img) if kdata.descriptors != None: descriptors.add(img) for i in pairs: image_matches_filepath = get_matches_fullpath((i[0], i[1]), kdata_path) if os.path.exists(image_matches_filepath): matches.add(i[0], i[1]) matches.normalize() return kapture.Kapture(sensors=sensors, trajectories=trajectories, records_camera=records, descriptors=descriptors, keypoints=keypoints, matches=matches)
def sub_kapture_from_img_list(kdata, img_list, pairs, keypoints_type, descriptors_type): trajectories = kapture.Trajectories() sensors = kapture.Sensors() records = kapture.RecordsCamera() keypoints = kapture.Keypoints(kdata.keypoints[keypoints_type].type_name, kdata.keypoints[keypoints_type].dtype, kdata.keypoints[keypoints_type].dsize) if kdata.descriptors is not None and descriptors_type in kdata.descriptors: descriptors = kapture.Descriptors(kdata.descriptors[descriptors_type].type_name, kdata.descriptors[descriptors_type].dtype, kdata.descriptors[descriptors_type].dsize, kdata.descriptors[descriptors_type].keypoints_type, kdata.descriptors[descriptors_type].metric_type) else: descriptors = None matches = kapture.Matches() timestamp_sensor_id_from_image_name = {img_name: (timestamp, sensor_id) for timestamp, sensor_id, img_name in kapture.flatten(kdata.records_camera)} for img in img_list: timestamp, sensor_id = timestamp_sensor_id_from_image_name[img] sensors[sensor_id] = kdata.sensors[sensor_id] records[timestamp, sensor_id] = img if (timestamp, sensor_id) in kdata.trajectories: pose = kdata.trajectories[timestamp][sensor_id] trajectories[timestamp, sensor_id] = pose keypoints.add(img) if kdata.descriptors is not None: descriptors.add(img) for i in pairs: if i in kdata.matches[keypoints_type]: matches.add(i[0], i[1]) matches.normalize() return kapture.Kapture(sensors=sensors, trajectories=trajectories, records_camera=records, descriptors={descriptors_type: descriptors}, keypoints={keypoints_type: keypoints}, matches={keypoints_type: matches})
def __init__(self, path, reset=False, img_format=IMG_FORMAT_JPG, min_pt3d_obs=3, min_pt3d_ratio=0.2, jpg_qlt=95, scale=1.0): self.path = path self.default_cam = ('1', self._IMAGE_FOLDER) self.default_kp_type = 'gftt' self.img_format = img_format self.min_pt3d_obs = min_pt3d_obs self.min_pt3d_ratio = min_pt3d_ratio self.jpg_qlt = jpg_qlt self.scale = scale if os.path.exists(self.path): self.kapture = kapture_from_dir(self.path) if reset: shutil.rmtree(self.path) if not os.path.exists(self.path): self.kapture = kt.Kapture()
def pycolmap_localize_from_loaded_data( kapture_data: kapture.Kapture, kapture_path: str, tar_handlers: TarCollection, kapture_query_data: kapture.Kapture, output_path: str, pairsfile_path: str, max_error: float, min_inlier_ratio: float, min_num_iterations: int, max_num_iterations: int, confidence: float, keypoints_type: Optional[str], duplicate_strategy: DuplicateCorrespondencesStrategy, rerank_strategy: RerankCorrespondencesStrategy, write_detailed_report: bool, force: bool) -> None: """ Localize images using pycolmap. :param kapture_data: loaded kapture data (incl. points3d) :param kapture_path: path to the kapture to use :param tar_handlers: collection of pre-opened tar archives :param kapture_data: loaded kapture data (mapping and query images) :param output_path: path to the write the localization results :param pairsfile_path: pairs to use :param max_error: RANSAC inlier threshold in pixel :param min_inlier_ratio: abs_pose_options.ransac_options.min_inlier_ratio :param min_num_iterations: abs_pose_options.ransac_options.min_num_trials :param max_num_iterations: abs_pose_options.ransac_options.max_num_trials :param confidence: abs_pose_options.ransac_options.confidence :param keypoints_type: types of keypoints (and observations) to use :param duplicate_strategy: strategy to handle duplicate correspondences (either kpt_id and/or pt3d_id) :param rerank_strategy: strategy to reorder pairs before handling duplicate correspondences :param write_detailed_report: if True, write a json file with inliers, reprojection error for each query :param force: Silently overwrite kapture files if already exists """ assert has_pycolmap if not (kapture_data.records_camera and kapture_data.sensors and kapture_data.keypoints and kapture_data.matches and kapture_data.points3d and kapture_data.observations): raise ValueError('records_camera, sensors, keypoints, matches, ' 'points3d, observations are mandatory for map+query') if not (kapture_query_data.records_camera and kapture_query_data.sensors): raise ValueError('records_camera, sensors are mandatory for query') if keypoints_type is None: keypoints_type = try_get_only_key_from_collection( kapture_data.keypoints) assert keypoints_type is not None assert keypoints_type in kapture_data.keypoints assert keypoints_type in kapture_data.matches if kapture_data.rigs is not None and kapture_data.trajectories is not None: # make sure, rigs are not used in trajectories. logger.info('remove rigs notation.') rigs_remove_inplace(kapture_data.trajectories, kapture_data.rigs) kapture_data.rigs.clear() if kapture_query_data.trajectories is not None: logger.warning( "Input query data contains trajectories: they will be ignored") kapture_query_data.trajectories.clear() os.umask(0o002) os.makedirs(output_path, exist_ok=True) delete_existing_kapture_files(output_path, force_erase=force) # load pairsfile pairs = {} with open(pairsfile_path, 'r') as fid: table = kapture.io.csv.table_from_file(fid) for img_query, img_map, _ in table: if img_query not in pairs: pairs[img_query] = [] pairs[img_query].append(img_map) kapture_data.matches[keypoints_type].normalize() keypoints_filepaths = keypoints_to_filepaths( kapture_data.keypoints[keypoints_type], keypoints_type, kapture_path, tar_handlers) obs_for_keypoints_type = { point_id: per_keypoints_type_subdict[keypoints_type] for point_id, per_keypoints_type_subdict in kapture_data.observations.items() if keypoints_type in per_keypoints_type_subdict } point_id_from_obs = { (img_name, kp_id): point_id for point_id in obs_for_keypoints_type.keys() for img_name, kp_id in obs_for_keypoints_type[point_id] } query_images = [(timestamp, sensor_id, image_name) for timestamp, sensor_id, image_name in kapture.flatten( kapture_query_data.records_camera)] # kapture for localized images + pose trajectories = kapture.Trajectories() for timestamp, sensor_id, image_name in tqdm( query_images, disable=logging.getLogger().level >= logging.CRITICAL): if image_name not in pairs: continue # N number of correspondences # points2D - Nx2 array with pixel coordinates # points3D - Nx3 array with world coordinates points2D = [] points3D = [] keypoints_filepath = keypoints_filepaths[image_name] kapture_keypoints_query = image_keypoints_from_file( filepath=keypoints_filepath, dsize=kapture_data.keypoints[keypoints_type].dsize, dtype=kapture_data.keypoints[keypoints_type].dtype) query_cam = kapture_query_data.sensors[sensor_id] assert isinstance(query_cam, kapture.Camera) col_cam_id, width, height, params, _ = get_colmap_camera(query_cam) cfg = { 'model': CAMERA_MODEL_NAME_ID[col_cam_id][0], 'width': int(width), 'height': int(height), 'params': params } points2D, _, points3D, stats = get_correspondences( kapture_data, keypoints_type, kapture_path, tar_handlers, image_name, pairs[image_name], point_id_from_obs, kapture_keypoints_query, None, duplicate_strategy, rerank_strategy) # compute absolute pose # inlier_threshold - RANSAC inlier threshold in pixels # answer - dictionary containing the RANSAC output ret = pycolmap.absolute_pose_estimation(points2D, points3D, cfg, max_error, min_inlier_ratio, min_num_iterations, max_num_iterations, confidence) # add pose to output kapture if ret['success'] and ret['num_inliers'] > 0: pose = kapture.PoseTransform(ret['qvec'], ret['tvec']) if write_detailed_report: num_2dpoints = len(points2D) points2D_final, K, distortion = get_camera_matrix_from_kapture( np.array(points2D, dtype=np.float), query_cam) points2D_final = list(points2D_final.reshape( (num_2dpoints, 2))) inliers = np.where(ret['inliers'])[0].tolist() reprojection_error = compute_reprojection_error( pose, ret['num_inliers'], inliers, points2D_final, points3D, K, distortion) cache = { "num_correspondences": len(points3D), "num_inliers": inliers, "inliers": ret['inliers'], "reprojection_error": reprojection_error, "stats": stats } cache_path = os.path.join(output_path, f'pycolmap_cache/{image_name}.json') save_to_json(cache, cache_path) trajectories[timestamp, sensor_id] = pose kapture_data_localized = kapture.Kapture( sensors=kapture_query_data.sensors, trajectories=trajectories, records_camera=kapture_query_data.records_camera, rigs=kapture_query_data.rigs) kapture.io.csv.kapture_to_dir(output_path, kapture_data_localized)
def import_robotcar_seasons( robotcar_path: str, kapture_path: str, force_overwrite_existing: bool = False, images_import_method: TransferAction = TransferAction.skip, skip_reconstruction: bool = False, rig_collapse: bool = False, use_colmap_intrinsics: bool = False, import_v1: bool = False) -> None: """ Read the RobotCar Seasons data, creates several kaptures with training and query data. :param robotcar_path: path to the robotcar top directory :param kapture_path: path to the kapture top directory :param force_overwrite_existing: Silently overwrite kapture files if already exists. :param images_import_method: choose how to import actual image files. :param skip_reconstruction: if True, will skip the reconstruction part from the training data :param rig_collapse: if True, will collapse the rig """ kapture_path = path.join(kapture_path, "base") os.makedirs(kapture_path, exist_ok=True) cameras = import_robotcar_cameras(path.join(robotcar_path, 'intrinsics')) rigs = import_robotcar_rig(path.join(robotcar_path, 'extrinsics')) logger.info("Importing test data") # Test data image_pattern = re.compile( r'(?P<condition>.+)/(?P<camera>\w+)/(?P<timestamp>\d+)\.jpg') queries_path = path.join(robotcar_path, '3D-models', 'individual', 'queries_per_location') kapture_imported_query = {} for root, dirs, files in os.walk(queries_path): for query_file in files: records_camera = kapture.RecordsCamera() # Get list of query images with open(path.join(queries_path, query_file)) as f: for line in f: matches = image_pattern.match(line) image_path = line.strip() if not matches: logger.warning(f"Error matching line in {image_path}") continue matches = matches.groupdict() timestamp = int(matches['timestamp']) camera = str(matches['camera']) condition = str(matches['condition']) records_camera[timestamp, camera] = image_path (query_name, _) = query_file.split('.') kapture_test = kapture.Kapture(sensors=cameras, rigs=rigs, records_camera=records_camera) kapture_imported_query[int( query_name.split('_')[-1])] = kapture_test # Reference map data logger.info("Importing reference map") colmap_reconstructions_path = path.join(robotcar_path, '3D-models', 'individual', 'colmap_reconstructions') kapture_imported_mapping = {} for root, dirs, files in os.walk(colmap_reconstructions_path): for colmap_reconstruction in dirs: (loc_id, _) = colmap_reconstruction.split('_') kapture_reconstruction_dir = path.join(kapture_path, f"{int(loc_id):02d}", "mapping") delete_existing_kapture_files(kapture_reconstruction_dir, force_erase=force_overwrite_existing) logger.info(f'Converting reconstruction {loc_id} to kapture ...') kapture_reconstruction_data = import_robotcar_colmap_location( robotcar_path, path.join(colmap_reconstructions_path, colmap_reconstruction), kapture_reconstruction_dir, rigs, skip_reconstruction) # replace intrinsics with the ones found in the text files if not use_colmap_intrinsics: kapture_reconstruction_data.sensors = cameras kapture_imported_mapping[int(loc_id)] = kapture_reconstruction_data if not import_v1: queries_per_location = { image_name: (ts, cam_id, loc_id) for loc_id, kdata_test in kapture_imported_query.items() for ts, cam_id, image_name in kapture.flatten(kdata_test.records_camera) } kapture_imported_training = {} # stores kapture for each submap # read robotcar_v2_train.txt v2_train_data = read_robotcar_v2_train(robotcar_path) for image_name, pose in v2_train_data.items(): ts, cam_id, loc_id = queries_per_location[image_name] assert cam_id == 'rear' # create kapture object for submap if it doesn't exist if loc_id not in kapture_imported_training: kapture_loc_id = kapture.Kapture(sensors=cameras, rigs=rigs) kapture_loc_id.records_camera = kapture.RecordsCamera() kapture_loc_id.trajectories = kapture.Trajectories() kapture_imported_training[loc_id] = kapture_loc_id kapture_imported_training[loc_id].records_camera[ ts, cam_id] = image_name kapture_imported_training[loc_id].trajectories[ts, cam_id] = pose matches = image_pattern.match(image_name) if not matches: logger.warning(f"Error matching line in {image_name}") continue matches = matches.groupdict() condition = str(matches['condition']) timestamp = str(matches['timestamp']) camera = str(matches['camera']) # added left and right images in records_camera left_image_name = condition + '/' + 'left' + '/' + timestamp + '.jpg' right_image_name = condition + '/' + 'right' + '/' + timestamp + '.jpg' kapture_imported_training[loc_id].records_camera[ ts, 'left'] = left_image_name kapture_imported_training[loc_id].records_camera[ ts, 'right'] = right_image_name # remove entries from query del kapture_imported_query[loc_id].records_camera[ts][cam_id] del kapture_imported_query[loc_id].records_camera[ts]['left'] del kapture_imported_query[loc_id].records_camera[ts]['right'] del kapture_imported_query[loc_id].records_camera[ts] # all remaining query images are kept; reading robotcar_v2_test.txt is not necessary # apply rig collapse if rig_collapse: logger.info('replacing camera poses with rig poses.') for kdata_mapping in kapture_imported_mapping.values(): kapture.rigs_recover_inplace(kdata_mapping.trajectories, rigs, 'rear') for kdata_training in kapture_imported_training.values(): kapture.rigs_recover_inplace(kdata_training.trajectories, rigs, 'rear') # IO operations robotcar_image_path = path.join(robotcar_path, "images") for loc_id, kdata_query in kapture_imported_query.items(): loc_id_str = f"{loc_id:02d}" logger.info(f'writing test data: {loc_id_str}') kapture_test_dir = path.join(kapture_path, loc_id_str, "query") delete_existing_kapture_files(kapture_test_dir, force_erase=force_overwrite_existing) if not kdata_query.records_camera: # all images were removed continue kapture_to_dir(kapture_test_dir, kdata_query) query_images = [ f for _, _, f in kapture.flatten(kdata_query.records_camera) ] import_record_data_from_dir_auto(robotcar_image_path, kapture_test_dir, query_images, images_import_method) for loc_id, kdata_mapping in kapture_imported_mapping.items(): loc_id_str = f"{loc_id:02d}" logger.info(f'writing mapping data: {loc_id_str}') kapture_reconstruction_dir = path.join(kapture_path, f"{loc_id:02d}", "mapping") delete_existing_kapture_files(kapture_reconstruction_dir, force_erase=force_overwrite_existing) kapture_to_dir(kapture_reconstruction_dir, kdata_mapping) mapping_images = [ f for _, _, f in kapture.flatten(kdata_mapping.records_camera) ] import_record_data_from_dir_auto(robotcar_image_path, kapture_reconstruction_dir, mapping_images, images_import_method) for loc_id, kdata_training in kapture_imported_training.items(): loc_id_str = f"{loc_id:02d}" logger.info(f'writing training data: {loc_id_str}') kapture_training_dir = path.join(kapture_path, f"{loc_id:02d}", "training") delete_existing_kapture_files(kapture_training_dir, force_erase=force_overwrite_existing) kapture_to_dir(kapture_training_dir, kdata_training) mapping_images = [ f for _, _, f in kapture.flatten(kdata_training.records_camera) ] import_record_data_from_dir_auto(robotcar_image_path, kapture_training_dir, mapping_images, images_import_method)
def merge_keep_ids( kapture_list: List[ kapture. Kapture], # noqa: C901: function a bit long but not too complex skip_list: List[Type], data_paths: List[str], kapture_path: str, images_import_method: TransferAction) -> kapture.Kapture: """ Merge multiple kapture while keeping ids (sensor_id) identical in merged and inputs. :param kapture_list: list of kapture to merge. :param skip_list: optional types not to merge. sensors and rigs are unskippable :param data_paths: list of path to root path directory in same order as mentioned in kapture_list. :param kapture_path: directory root path to the merged kapture. :param images_import_method: method to transfer image files :return: merged kapture """ merged_kapture = kapture.Kapture() # get the union of all sensors new_sensors = merge_sensors( [every_kapture.sensors for every_kapture in kapture_list]) # if merge_sensors returned an empty object, keep merged_kapture.sensors to None merged_kapture.sensors = get_new_if_not_empty(new_sensors, merged_kapture.sensors) # get the union of all rigs new_rigs = merge_rigs( [every_kapture.rigs for every_kapture in kapture_list]) # if merge_rigs returned an empty object, keep merged_kapture.rigs to None merged_kapture.rigs = get_new_if_not_empty(new_rigs, merged_kapture.rigs) # all fields below can be skipped with skip_list # we do not assign the properties when the merge evaluate to false, we keep it as None if kapture.Trajectories not in skip_list: new_trajectories = merge_trajectories( [every_kapture.trajectories for every_kapture in kapture_list]) merged_kapture.trajectories = get_new_if_not_empty( new_trajectories, merged_kapture.trajectories) if kapture.RecordsCamera not in skip_list: new_records_camera = merge_records_camera( [every_kapture.records_camera for every_kapture in kapture_list]) merged_kapture.records_camera = get_new_if_not_empty( new_records_camera, merged_kapture.records_camera) merge_records_data( [[ image_name for _, _, image_name in kapture.flatten( every_kapture.records_camera) ] if every_kapture.records_camera is not None else [] for every_kapture in kapture_list], [ get_image_fullpath(data_path, image_filename=None) for data_path in data_paths ], kapture_path, images_import_method) if kapture.RecordsLidar not in skip_list: new_records_lidar = merge_records_lidar( [every_kapture.records_lidar for every_kapture in kapture_list]) merged_kapture.records_lidar = get_new_if_not_empty( new_records_lidar, merged_kapture.records_lidar) if kapture.RecordsWifi not in skip_list: new_records_wifi = merge_records_wifi( [every_kapture.records_wifi for every_kapture in kapture_list]) merged_kapture.records_wifi = get_new_if_not_empty( new_records_wifi, merged_kapture.records_wifi) if kapture.RecordsBluetooth not in skip_list: new_records_bluetooth = merge_records_bluetooth([ every_kapture.records_bluetooth for every_kapture in kapture_list ]) merged_kapture.records_bluetooth = get_new_if_not_empty( new_records_bluetooth, merged_kapture.records_bluetooth) if kapture.RecordsGnss not in skip_list: new_records_gnss = merge_records_gnss( [every_kapture.records_gnss for every_kapture in kapture_list]) merged_kapture.records_gnss = get_new_if_not_empty( new_records_gnss, merged_kapture.records_gnss) if kapture.RecordsAccelerometer not in skip_list: new_records_accelerometer = merge_records_accelerometer([ every_kapture.records_accelerometer for every_kapture in kapture_list ]) merged_kapture.records_accelerometer = get_new_if_not_empty( new_records_accelerometer, merged_kapture.records_accelerometer) if kapture.RecordsGyroscope not in skip_list: new_records_gyroscope = merge_records_gyroscope([ every_kapture.records_gyroscope for every_kapture in kapture_list ]) merged_kapture.records_gyroscope = get_new_if_not_empty( new_records_gyroscope, merged_kapture.records_gyroscope) if kapture.RecordsMagnetic not in skip_list: new_records_magnetic = merge_records_magnetic( [every_kapture.records_magnetic for every_kapture in kapture_list]) merged_kapture.records_magnetic = get_new_if_not_empty( new_records_magnetic, merged_kapture.records_magnetic) # for the reconstruction, except points and observations, the files are copied with shutil.copy # if kapture_path evaluates to False, all copies will be skipped (but classes will be filled normally) if kapture.Keypoints not in skip_list: keypoints = [every_kapture.keypoints for every_kapture in kapture_list] keypoints_not_none = [k for k in keypoints if k is not None] if len(keypoints_not_none) > 0: new_keypoints = merge_keypoints(keypoints, data_paths, kapture_path) merged_kapture.keypoints = get_new_if_not_empty( new_keypoints, merged_kapture.keypoints) if kapture.Descriptors not in skip_list: descriptors = [ every_kapture.descriptors for every_kapture in kapture_list ] descriptors_not_none = [k for k in descriptors if k is not None] if len(descriptors_not_none) > 0: new_descriptors = merge_descriptors(descriptors, data_paths, kapture_path) merged_kapture.descriptors = get_new_if_not_empty( new_descriptors, merged_kapture.descriptors) if kapture.GlobalFeatures not in skip_list: global_features = [ every_kapture.global_features for every_kapture in kapture_list ] global_features_not_none = [ k for k in global_features if k is not None ] if len(global_features_not_none) > 0: new_global_features = merge_global_features( global_features, data_paths, kapture_path) merged_kapture.global_features = get_new_if_not_empty( new_global_features, merged_kapture.global_features) if kapture.Matches not in skip_list: matches = [every_kapture.matches for every_kapture in kapture_list] new_matches = merge_matches(matches, data_paths, kapture_path) merged_kapture.matches = get_new_if_not_empty(new_matches, merged_kapture.matches) if kapture.Points3d not in skip_list and kapture.Observations not in skip_list: points_and_obs = [(every_kapture.points3d, every_kapture.observations) for every_kapture in kapture_list] new_points, new_observations = merge_points3d_and_observations( points_and_obs) merged_kapture.points3d = get_new_if_not_empty(new_points, merged_kapture.points3d) merged_kapture.observations = get_new_if_not_empty( new_observations, merged_kapture.observations) elif kapture.Points3d not in skip_list: points = [every_kapture.points3d for every_kapture in kapture_list] new_points = merge_points3d(points) merged_kapture.points3d = get_new_if_not_empty(new_points, merged_kapture.points3d) return merged_kapture
def import_7scenes(d7scenes_path: str, kapture_dir_path: str, force_overwrite_existing: bool = False, images_import_method: TransferAction = TransferAction.skip, partition: Optional[str] = None ) -> None: """ Imports RGB-D Dataset 7-Scenes dataset and save them as kapture. :param d7scenes_path: path to the 7scenes sequence root path :param kapture_dir_path: path to kapture top directory :param force_overwrite_existing: Silently overwrite kapture files if already exists. :param images_import_method: choose how to import actual image files. :param partition: if specified = 'mapping' or 'query'. Requires d7scenes_path/TestSplit.txt or TrainSplit.txt to exists. """ os.makedirs(kapture_dir_path, exist_ok=True) delete_existing_kapture_files(kapture_dir_path, force_erase=force_overwrite_existing) logger.info('loading all content ...') d7s_filename_re = re.compile(r'((?P<sequence>.+)/)?frame-(?P<frame_id>\d{6})\.(?P<suffix>\w*)\.(?P<ext>\w*)') # populate all relevant files d7s_filenames = (path_secure(path.relpath(path.join(dp, fn), d7scenes_path)) for dp, _, fs in os.walk(d7scenes_path) for fn in fs) logger.info('populating 7-scenes files ...') d7s_filenames = {filename: d7s_filename_re.search(filename).groupdict() for filename in sorted(d7s_filenames) if d7s_filename_re.search(filename)} # reorg as shot[seq, id] = {color: , depth: , pose: , ...} shots = {} for timestamp, (filename, file_attribs) in enumerate(d7s_filenames.items()): shot_id = (file_attribs.get('sequence'), file_attribs['frame_id']) shots.setdefault(shot_id, {})[file_attribs['suffix']] = filename # fake timestamps for timestamp, shot_id in enumerate(shots): shots[shot_id]['timestamp'] = timestamp # if given, filter partition if partition is not None: # read the authors split file partition_filepath = path.join(d7scenes_path, PARTITION_FILENAMES[partition]) if not path.isfile(partition_filepath): raise FileNotFoundError(f'partition file is missing: {partition_filepath}.') with open(partition_filepath, 'rt') as file: split_sequences = [f'seq-{int(seq.strip()[len("sequence"):]):02}' for seq in file.readlines()] assert len(split_sequences) > 0 # filter out shots = {(seq, frame): shot for (seq, frame), shot in shots.items() if seq in split_sequences} if len(shots) == 0: raise FileNotFoundError('no file found: make sure the path to 7scenes sequence is valid.') # eg. shots['seq-01', '000000'] = # { # 'color': 'seq-01/frame-000000.color.jpg', # 'depth': 'seq-01/frame-000000.depth.png', # 'pose': 'seq-01/frame-000000.pose.txt', # 'timestamp': 0} # images + depth maps logger.info('populating image and depth maps files ...') snapshots = kapture.RecordsCamera() depth_maps = kapture.RecordsDepth() for shot in shots.values(): snapshots[shot['timestamp'], RGB_SENSOR_ID] = shot['color'] kapture_depth_map_filename = shot['depth'][:-len('.png')] # kapture depth files are not png depth_maps[shot['timestamp'], DEPTH_SENSOR_ID] = kapture_depth_map_filename kapture_registered_depth_map_filename = shot['depth'][:-len('.png')] + '.reg' # kapture depth files are not png depth_maps[shot['timestamp'], REG_DEPTH_SENSOR_ID] = kapture_registered_depth_map_filename # poses logger.info('import poses files ...') trajectories = kapture.Trajectories() for shot in shots.values(): pose_filepath = path.join(d7scenes_path, shot['pose']) pose_mat = np.loadtxt(pose_filepath) # camera-to-world, 4×4 matrix in homogeneous coordinates rotation_mat = pose_mat[0:3, 0:3] position_vec = pose_mat[0:3, 3] rotation_quat = quaternion.from_rotation_matrix(rotation_mat) pose_world_from_cam = kapture.PoseTransform(r=rotation_quat, t=position_vec) pose_cam_from_world = pose_world_from_cam.inverse() trajectories[shot['timestamp'], RGBD_SENSOR_ID] = pose_cam_from_world # sensors """ From authors: The RGB and depth camera have not been calibrated and we can’t provide calibration parameters at the moment. The recorded frames correspond to the raw, uncalibrated camera images. In the KinectFusion pipeline we used the following default intrinsics for the depth camera: Principle point (320,240), Focal length (585,585). ---- We use the extr. kinect camera parameters from https://projet.liris.cnrs.fr/voir/activities-dataset/kinect-calibration.html. """ sensors = kapture.Sensors() # camera_type = kapture.CameraType.OPENCV # camera_params = [640, 480, 5.2161910696979987e+02, 5.2132946256749767e+02, 3.1755491910920682e+02, 2.5921654718027673e+02, # 2.5673002693536984e-01, -9.3976085633794137e-01, -1.8605549188751580e-03, -2.2232238578189420e-03] # w, h, f, cx, cy, k1, k2, p1, p2, k3 camera_type = kapture.CameraType.SIMPLE_PINHOLE # camera_params = [640, 480, 5.2161910696979987e+02, 5.2132946256749767e+02, 3.1755491910920682e+02, 2.5921654718027673e+02] # w, h, fx, fy, cx, cy camera_params = [640, 480, 525, 320, 240] # w, h, f, cx, cy sensors[RGB_SENSOR_ID] = kapture.Camera( name=RGB_SENSOR_ID, camera_type=camera_type, camera_params=camera_params ) # depth_camera_type = kapture.CameraType.OPENCV # depth_camera_params = [640, 480, 5.8818670481438744e+02, 5.8724220649505514e+02, 3.1076280589210484e+02, 2.2887144980135292e+02, # -1.8932947734719333e-01, 1.1358015104098631e+00, -4.4260345347128536e-03, -5.4869578635708153e-03, -2.2460143607712921e+00] # w, h, f, cx, cy, k1, k2, p1, p2, k3 depth_camera_type = kapture.CameraType.SIMPLE_PINHOLE # depth_camera_params = [640, 480, 5.8818670481438744e+02, 5.8724220649505514e+02, 3.1076280589210484e+02, 2.2887144980135292e+02] # w, h, fx, fy, cx, cy depth_camera_params = [640, 480, 585, 320, 240] # w, h, f, cx, cy sensors[DEPTH_SENSOR_ID] = kapture.Camera( name=DEPTH_SENSOR_ID, camera_type=depth_camera_type, camera_params=depth_camera_params, sensor_type='depth' ) sensors[REG_DEPTH_SENSOR_ID] = kapture.Camera( name=REG_DEPTH_SENSOR_ID, camera_type=depth_camera_type, camera_params=camera_params, sensor_type='depth' ) # bind camera and depth sensor into a rig R = np.array([[9.9996518012567637e-01, 2.6765126468950343e-03, -7.9041012313000904e-03], [-2.7409311281316700e-03, 9.9996302803027592e-01, -8.1504520778013286e-03], [7.8819942130445332e-03, 8.1718328771890631e-03, 9.9993554558014031e-01]]) T = np.array([-2.5558943178152542e-02, 1.0109636268061706e-04, 2.0318321729487039e-03]) Rt = np.vstack((np.hstack((R, T.reshape(3, 1))), np.array([0, 0, 0, 1]))) logger.info('building rig with camera and depth sensor ...') rigs = kapture.Rigs() rigs[RGBD_SENSOR_ID, RGB_SENSOR_ID] = kapture.PoseTransform(quaternion.from_rotation_matrix(R), T) rigs[RGBD_SENSOR_ID, REG_DEPTH_SENSOR_ID] = kapture.PoseTransform(quaternion.from_rotation_matrix(R), T) rigs[RGBD_SENSOR_ID, DEPTH_SENSOR_ID] = kapture.PoseTransform() # import (copy) image files. logger.info('copying image files ...') image_filenames = [f for _, _, f in kapture.flatten(snapshots)] import_record_data_from_dir_auto(d7scenes_path, kapture_dir_path, image_filenames, images_import_method) # import (copy) depth map files. logger.info('converting depth files ...') depth_map_filenames = kapture.io.records.records_to_filepaths(depth_maps, kapture_dir_path) hide_progress = logger.getEffectiveLevel() > logging.INFO for depth_map_filename, depth_map_filepath_kapture in tqdm(depth_map_filenames.items(), disable=hide_progress): if '.reg' in depth_map_filename: continue depth_map_filepath_7scenes = path.join(d7scenes_path, depth_map_filename + '.png') depth_map = np.array(Image.open(depth_map_filepath_7scenes)) # change invalid depth from 65535 to 0 depth_map[depth_map == 65535] = 0 # depth maps is in mm in 7scenes, convert it to meters depth_map = depth_map.astype(np.float32) * 1.0e-3 kapture.io.records.records_depth_to_file(depth_map_filepath_kapture, depth_map) # register depth to rgb reg_depth_map = register_depth(get_K(depth_camera_type, depth_camera_params), get_K(camera_type, camera_params), Rt, depth_map, camera_params[0], camera_params[1]) kapture.io.records.records_depth_to_file(depth_map_filepath_kapture + '.reg', reg_depth_map) # pack into kapture format imported_kapture = kapture.Kapture( records_camera=snapshots, records_depth=depth_maps, rigs=rigs, trajectories=trajectories, sensors=sensors) logger.info('writing imported data ...') kapture_to_dir(kapture_dir_path, imported_kapture)
def run_colmap_gv_from_loaded_data(kapture_none_matches: kapture.Kapture, kapture_colmap_matches: kapture.Kapture, kapture_none_matches_dirpath: str, kapture_colmap_matches_dirpath: str, tar_handlers_none_matches: Optional[TarCollection], tar_handlers_colmap_matches: Optional[TarCollection], colmap_binary: str, keypoints_type: Optional[str], skip_list: List[str], force: bool): logger.info('run_colmap_gv...') if not (kapture_none_matches.records_camera and kapture_none_matches.sensors and kapture_none_matches.keypoints and kapture_none_matches.matches): raise ValueError('records_camera, sensors, keypoints, matches are mandatory') # COLMAP does not fully support rigs. if kapture_none_matches.rigs is not None and kapture_none_matches.trajectories is not None: # make sure, rigs are not used in trajectories. logger.info('remove rigs notation.') rigs_remove_inplace(kapture_none_matches.trajectories, kapture_none_matches.rigs) # Set fixed name for COLMAP database colmap_db_path = os.path.join(kapture_colmap_matches_dirpath, 'colmap.db') if 'delete_existing' not in skip_list: safe_remove_file(colmap_db_path, force) if keypoints_type is None: keypoints_type = try_get_only_key_from_collection(kapture_none_matches.matches) assert keypoints_type is not None assert keypoints_type in kapture_none_matches.keypoints assert keypoints_type in kapture_none_matches.matches if 'matches_importer' not in skip_list: logger.debug('compute matches difference.') if kapture_colmap_matches.matches is not None and keypoints_type in kapture_colmap_matches.matches: colmap_matches = kapture_colmap_matches.matches[keypoints_type] else: colmap_matches = kapture.Matches() matches_to_verify = kapture.Matches(kapture_none_matches.matches[keypoints_type].difference(colmap_matches)) kapture_data_to_export = kapture.Kapture(sensors=kapture_none_matches.sensors, trajectories=kapture_none_matches.trajectories, records_camera=kapture_none_matches.records_camera, keypoints={ keypoints_type: kapture_none_matches.keypoints[keypoints_type] }, matches={ keypoints_type: matches_to_verify }) # creates a new database with matches logger.debug('export matches difference to db.') colmap_db = COLMAPDatabase.connect(colmap_db_path) database_extra.kapture_to_colmap(kapture_data_to_export, kapture_none_matches_dirpath, tar_handlers_none_matches, colmap_db, keypoints_type, None, export_two_view_geometry=False) # close db before running colmap processes in order to avoid locks colmap_db.close() logger.debug('run matches_importer command.') colmap_lib.run_matches_importer_from_kapture_matches( colmap_binary, colmap_use_cpu=True, colmap_gpu_index=None, colmap_db_path=colmap_db_path, kapture_matches=matches_to_verify, force=force ) if 'import' not in skip_list: logger.debug('import verified matches.') os.umask(0o002) colmap_db = COLMAPDatabase.connect(colmap_db_path) kapture_data = kapture.Kapture() kapture_data.records_camera, _ = get_images_and_trajectories_from_database(colmap_db) kapture_data.matches = { keypoints_type: get_matches_from_database(colmap_db, kapture_data.records_camera, kapture_colmap_matches_dirpath, tar_handlers_colmap_matches, keypoints_type, no_geometric_filtering=False) } colmap_db.close() if kapture_colmap_matches.matches is None: kapture_colmap_matches.matches = {} if keypoints_type not in kapture_colmap_matches.matches: kapture_colmap_matches.matches[keypoints_type] = kapture.Matches() kapture_colmap_matches.matches[keypoints_type].update(kapture_data.matches[keypoints_type]) if 'delete_db' not in skip_list: logger.debug('delete intermediate colmap db.') os.remove(colmap_db_path)
def import_extended_cmu_seasons( cmu_path: str, top_kaptures_path: str, slice_range: List[int], import_all_files: bool = False, force_overwrite_existing: bool = False, images_import_method: TransferAction = TransferAction.skip) -> None: """ Import extended CMU data to kapture. Will make training and query kaptures for every CMU slice. :param cmu_path: path to the top directory of the CMU dataset files :param top_kaptures_path: top directory for the kaptures to create :param slice_range: range of CMU slices to import :param import_all_files: if Tre, will import all files :param force_overwrite_existing: Silently overwrite kapture files if already exists. :param images_import_method: choose how to import actual image files. """ os.makedirs(top_kaptures_path, exist_ok=True) cameras = import_extended_cmu_seasons_intrinsics( path.join(cmu_path, 'intrinsics.txt')) for slice_n in slice_range: # prepare paths slice_path = os.path.join(cmu_path, f'slice{slice_n}') training_images_path = os.path.join(slice_path, 'database') query_images_path = os.path.join(slice_path, 'query') gt_trajectories_path = os.path.join( slice_path, f'ground-truth-database-images-slice{slice_n}.txt') query_image_list = os.path.join(slice_path, f'test-images-slice{slice_n}.txt') query_gt_path = os.path.join(slice_path, 'camera-poses') query_gt_list = [ os.path.join(query_gt_path, f) for f in os.listdir(query_gt_path) ] # Import training images kapture_training_path = path.join(top_kaptures_path, f'slice{slice_n}', "mapping") delete_existing_kapture_files(kapture_training_path, force_overwrite_existing) training_records_camera, training_trajectories = import_extended_cmu_seasons_images( gt_trajectories_path) training_kapture = kapture.Kapture( sensors=cameras, records_camera=training_records_camera, trajectories=training_trajectories) if import_all_files: _add_images_from_folder(training_images_path, training_kapture) kapture_to_dir(kapture_training_path, training_kapture) # finally import images if images_import_method != TransferAction.skip: filename_list = [ f for _, _, f in kapture.flatten(training_kapture.records_camera) ] logger.info(f'importing {len(filename_list)} image files ...') import_record_data_from_dir_auto(training_images_path, kapture_training_path, filename_list, images_import_method) # Import query images kapture_query_path = path.join(top_kaptures_path, f'slice{slice_n}', "query") delete_existing_kapture_files(kapture_query_path, force_erase=force_overwrite_existing) query_records_camera, query_trajectories = import_extended_cmu_seasons_images( query_image_list) query_kapture = kapture.Kapture(sensors=cameras, records_camera=query_records_camera, trajectories=query_trajectories) # import query gt when possible query_gt_kapture = [] for query_gt_path in query_gt_list: query_gt_records_camera, query_gt_trajectories = import_extended_cmu_seasons_images( query_gt_path) query_gt_kapture.append( kapture.Kapture(sensors=cameras, records_camera=query_gt_records_camera, trajectories=query_gt_trajectories)) data_to_merge = [query_kapture] + query_gt_kapture query_kapture = merge_keep_ids( data_to_merge, skip_list=[], data_paths=["" for _ in range(len(data_to_merge))], kapture_path="", images_import_method=TransferAction.skip) if import_all_files: _add_images_from_folder(query_images_path, query_kapture) kapture_to_dir(kapture_query_path, query_kapture) # finally import images if images_import_method != TransferAction.skip: filename_list = [ f for _, _, f in kapture.flatten(query_kapture.records_camera) ] logger.info(f'importing {len(filename_list)} image files ...') import_record_data_from_dir_auto(query_images_path, kapture_query_path, filename_list, images_import_method)
def import_7scenes(d7scenes_path: str, kapture_dir_path: str, force_overwrite_existing: bool = False, images_import_method: TransferAction = TransferAction.skip, partition: Optional[str] = None ) -> None: """ Imports RGB-D Dataset 7-Scenes dataset and save them as kapture. :param d7scenes_path: path to the 7scenes sequence root path :param kapture_dir_path: path to kapture top directory :param force_overwrite_existing: Silently overwrite kapture files if already exists. :param images_import_method: choose how to import actual image files. :param partition: if specified = 'mapping' or 'query'. Requires d7scenes_path/TestSplit.txt or TrainSplit.txt to exists. """ os.makedirs(kapture_dir_path, exist_ok=True) delete_existing_kapture_files(kapture_dir_path, force_erase=force_overwrite_existing) logger.info('loading all content ...') d7s_filename_re = re.compile(r'((?P<sequence>.+)/)?frame-(?P<frame_id>\d{6})\.(?P<suffix>\w*)\.(?P<ext>\w*)') # populate all relevant files d7s_filenames = (path_secure(path.relpath(path.join(dp, fn), d7scenes_path)) for dp, _, fs in os.walk(d7scenes_path) for fn in fs) logger.info('populating 7-scenes files ...') d7s_filenames = {filename: d7s_filename_re.search(filename).groupdict() for filename in sorted(d7s_filenames) if d7s_filename_re.search(filename)} # reorg as shot[seq, id] = {color: , depth: , pose: , ...} shots = {} for timestamp, (filename, file_attribs) in enumerate(d7s_filenames.items()): shot_id = (file_attribs.get('sequence'), file_attribs['frame_id']) shots.setdefault(shot_id, {})[file_attribs['suffix']] = filename # fake timestamps for timestamp, shot_id in enumerate(shots): shots[shot_id]['timestamp'] = timestamp # if given, filter partition if partition is not None: # read the authors split file partition_filepath = path.join(d7scenes_path, PARTITION_FILENAMES[partition]) if not path.isfile(partition_filepath): raise FileNotFoundError(f'partition file is missing: {partition_filepath}.') with open(partition_filepath, 'rt') as file: split_sequences = [f'seq-{int(seq.strip()[len("sequence"):]):02}' for seq in file.readlines()] assert len(split_sequences) > 0 # filter out shots = {(seq, frame): shot for (seq, frame), shot in shots.items() if seq in split_sequences} if len(shots) == 0: raise FileNotFoundError('no file found: make sure the path to 7scenes sequence is valid.') # eg. shots['seq-01', '000000'] = # { # 'color': 'seq-01/frame-000000.color.jpg', # 'depth': 'seq-01/frame-000000.depth.png', # 'pose': 'seq-01/frame-000000.pose.txt', # 'timestamp': 0} # images + depth maps logger.info('populating image and depth maps files ...') snapshots = kapture.RecordsCamera() depth_maps = kapture.RecordsDepth() for shot in shots.values(): snapshots[shot['timestamp'], RGB_SENSOR_ID] = shot['color'] kapture_depth_map_filename = shot['depth'][:-len('.png')] # kapture depth files are not png depth_maps[shot['timestamp'], DEPTH_SENSOR_ID] = kapture_depth_map_filename # poses logger.info('import poses files ...') trajectories = kapture.Trajectories() for shot in shots.values(): pose_filepath = path.join(d7scenes_path, shot['pose']) pose_mat = np.loadtxt(pose_filepath) # camera-to-world, 4×4 matrix in homogeneous coordinates rotation_mat = pose_mat[0:3, 0:3] position_vec = pose_mat[0:3, 3] rotation_quat = quaternion.from_rotation_matrix(rotation_mat) pose_world_from_cam = kapture.PoseTransform(r=rotation_quat, t=position_vec) pose_cam_from_world = pose_world_from_cam.inverse() trajectories[shot['timestamp'], RGBD_SENSOR_ID] = pose_cam_from_world # sensors """ From authors: The RGB and depth camera have not been calibrated and we can’t provide calibration parameters at the moment. The recorded frames correspond to the raw, uncalibrated camera images. In the KinectFusion pipeline we used the following default intrinsics for the depth camera: Principle point (320,240), Focal length (585,585). """ sensors = kapture.Sensors() camera_type = kapture.CameraType.SIMPLE_PINHOLE camera_params = [640, 480, 585, 320, 240] # w, h, f, cx, cy sensors[RGB_SENSOR_ID] = kapture.Camera( name=RGB_SENSOR_ID, camera_type=camera_type, camera_params=camera_params ) sensors[DEPTH_SENSOR_ID] = kapture.Camera( name=DEPTH_SENSOR_ID, camera_type=camera_type, camera_params=camera_params, sensor_type='depth' ) # bind camera and depth sensor into a rig logger.info('building rig with camera and depth sensor ...') rigs = kapture.Rigs() rigs[RGBD_SENSOR_ID, RGB_SENSOR_ID] = kapture.PoseTransform() rigs[RGBD_SENSOR_ID, DEPTH_SENSOR_ID] = kapture.PoseTransform() # import (copy) image files. logger.info('copying image files ...') image_filenames = [f for _, _, f in kapture.flatten(snapshots)] import_record_data_from_dir_auto(d7scenes_path, kapture_dir_path, image_filenames, images_import_method) # import (copy) depth map files. logger.info('converting depth files ...') depth_map_filenames = kapture.io.records.records_to_filepaths(depth_maps, kapture_dir_path) hide_progress = logger.getEffectiveLevel() > logging.INFO for depth_map_filename, depth_map_filepath_kapture in tqdm(depth_map_filenames.items(), disable=hide_progress): depth_map_filepath_7scenes = path.join(d7scenes_path, depth_map_filename + '.png') depth_map = np.array(Image.open(depth_map_filepath_7scenes)) # change invalid depth from 65535 to 0 depth_map[depth_map == 65535] = 0 # depth maps is in mm in 7scenes, convert it to meters depth_map = depth_map.astype(np.float32) * 1.0e-3 kapture.io.records.records_depth_to_file(depth_map_filepath_kapture, depth_map) # pack into kapture format imported_kapture = kapture.Kapture( records_camera=snapshots, records_depth=depth_maps, rigs=rigs, trajectories=trajectories, sensors=sensors) logger.info('writing imported data ...') kapture_to_dir(kapture_dir_path, imported_kapture)
def import_robotcar_seasons( robotcar_path: str, # noqa: C901: function a bit long but not too complex kapture_path: str, force_overwrite_existing: bool = False, images_import_method: TransferAction = TransferAction.skip, import_feature_db: bool = False, skip_reconstruction: bool = False, rig_collapse: bool = False, use_colmap_intrinsics: bool = False, import_v1: bool = False) -> None: """ Read the RobotCar Seasons data, creates several kaptures with training and query data. :param robotcar_path: path to the robotcar top directory :param kapture_path: path to the kapture top directory :param force_overwrite_existing: Silently overwrite kapture files if already exists. :param images_import_method: choose how to import actual image files. :param import_feature_db: if True, will import the features from the database :param skip_reconstruction: if True, will skip the reconstruction part from the training data :param rig_collapse: if True, will collapse the rig :param use_colmap_intrinsics: if True, will use the colmap intrinsics :param import_v1: if True, will use the version 1 of the format """ os.makedirs(kapture_path, exist_ok=True) cameras = import_robotcar_cameras(path.join(robotcar_path, 'intrinsics')) rigs = import_robotcar_rig(path.join(robotcar_path, 'extrinsics')) logger.info("Importing test data") # Test data image_pattern = re.compile( r'(?P<condition>.+)/(?P<camera>\w+)/(?P<timestamp>\d+)\.jpg') queries_path = path.join(robotcar_path, '3D-models', 'individual', 'queries_per_location') kapture_imported_query = {} for root, dirs, files in os.walk(queries_path): for query_file in files: records_camera = kapture.RecordsCamera() # Get list of query images with open(path.join(queries_path, query_file)) as f: for line in f: matches = image_pattern.match(line) image_path = line.strip() if not matches: logger.warning(f"Error matching line in {image_path}") continue matches = matches.groupdict() timestamp = int(matches['timestamp']) camera = str(matches['camera']) # condition = str(matches['condition']) : not used ? records_camera[timestamp, camera] = image_path (query_name, _) = query_file.split('.') kapture_test = kapture.Kapture(sensors=cameras, rigs=rigs, records_camera=records_camera) kapture_imported_query[int( query_name.split('_')[-1])] = kapture_test # Training data logger.info("Importing training data") colmap_reconstructions_path = path.join(robotcar_path, '3D-models', 'individual', 'colmap_reconstructions') kapture_imported_training = {} for root, dirs, files in os.walk(colmap_reconstructions_path): for colmap_reconstruction in dirs: (loc_id, _) = colmap_reconstruction.split('_') kapture_reconstruction_dir = path.join(kapture_path, f"{int(loc_id):02d}", "mapping") delete_existing_kapture_files(kapture_reconstruction_dir, force_overwrite_existing) logger.info(f'Converting reconstruction {loc_id} to kapture ...') kapture_reconstruction_data = import_robotcar_colmap_location( robotcar_path, path.join(colmap_reconstructions_path, colmap_reconstruction), kapture_reconstruction_dir, rigs, skip_reconstruction) # replace intrinsics with the ones found in the text files if not use_colmap_intrinsics: kapture_reconstruction_data.sensors = cameras kapture_imported_training[int( loc_id)] = kapture_reconstruction_data if not import_v1: _import_robotcar_v2_train(robotcar_path, kapture_imported_query, kapture_imported_training, image_pattern) # apply rig collapse if rig_collapse: logger.info('replacing camera poses with rig poses.') for kapture_mapping in kapture_imported_training.values(): kapture.rigs_recover_inplace(kapture_mapping.trajectories, rigs, ['rear']) # IO operations robotcar_image_path = path.join(robotcar_path, "images") for loc_id, kapture_query in kapture_imported_query.items(): loc_id_str = f"{loc_id:02d}" logger.info(f'writing test data: {loc_id_str}') kapture_test_dir = path.join(kapture_path, loc_id_str, "query") delete_existing_kapture_files(kapture_test_dir, force_overwrite_existing) if not kapture_query.records_camera: # all images were removed continue kapture_to_dir(kapture_test_dir, kapture_query) query_images = [ f for _, _, f in kapture.flatten(kapture_query.records_camera) ] import_record_data_from_dir_auto(robotcar_image_path, kapture_test_dir, query_images, images_import_method) for loc_id, kapture_mapping in kapture_imported_training.items(): loc_id_str = f"{loc_id:02d}" logger.info(f'writing mapping data: {loc_id_str}') kapture_reconstruction_dir = path.join(kapture_path, f"{loc_id:02d}", "mapping") kapture_to_dir(kapture_reconstruction_dir, kapture_mapping) mapping_images = [ f for _, _, f in kapture.flatten(kapture_mapping.records_camera) ] import_record_data_from_dir_auto(robotcar_image_path, kapture_reconstruction_dir, mapping_images, images_import_method) if import_feature_db: _import_colmap_overcast_reference(robotcar_path, kapture_path, force_overwrite_existing)
def test_evaluation(self): position = [1.658, 0, 0] position_a = [2.658, 0, 0] position_b = [1.758, 0, 0] position_c = [10.1, 0, 0] position_d = [2., 0, 0] position_e = [6.658, 0, 0] rotation = quaternion.from_euler_angles(np.deg2rad(110.0), 0, 0) rotation_a = quaternion.from_euler_angles(np.deg2rad(111.0), 0, 0) rotation_b = quaternion.from_euler_angles(np.deg2rad(108.0), 0, 0) rotation_c = quaternion.from_euler_angles(np.deg2rad(10.0), 0, 0) rotation_d = quaternion.from_euler_angles(np.deg2rad(110.0), 0, 0) pose_gt = kapture.PoseTransform(r=rotation, t=position).inverse() pose_a = kapture.PoseTransform(r=rotation_a, t=position_a).inverse() pose_b = kapture.PoseTransform(r=rotation_b, t=position_b).inverse() pose_c = kapture.PoseTransform(r=rotation_c, t=position_c).inverse() pose_d = kapture.PoseTransform(r=rotation_d, t=position_d).inverse() pose_e = kapture.PoseTransform(r=None, t=[-x for x in position_e]) kdata = kapture.Kapture(sensors=kapture.Sensors(), records_camera=kapture.RecordsCamera(), trajectories=kapture.Trajectories()) kdata.sensors['cam0'] = kapture.Camera( kapture.CameraType.UNKNOWN_CAMERA, [25, 13]) kdata.records_camera[(0, 'cam0')] = 'a' kdata.records_camera[(1, 'cam0')] = 'b' kdata.records_camera[(2, 'cam0')] = 'c' kdata.records_camera[(3, 'cam0')] = 'd' kdata.records_camera[(4, 'cam0')] = 'e' kdata.trajectories[(0, 'cam0')] = pose_a kdata.trajectories[(1, 'cam0')] = pose_b kdata.trajectories[(2, 'cam0')] = pose_c kdata.trajectories[(3, 'cam0')] = pose_d kdata2 = copy.deepcopy(kdata) kdata2.trajectories[(4, 'cam0')] = pose_e kdata2.records_camera[(5, 'cam0')] = 'f' kdata_gt = copy.deepcopy(kdata2) kdata_gt.trajectories[(0, 'cam0')] = pose_gt kdata_gt.trajectories[(1, 'cam0')] = pose_gt kdata_gt.trajectories[(2, 'cam0')] = pose_gt kdata_gt.trajectories[(3, 'cam0')] = pose_gt kdata_gt.trajectories[(4, 'cam0')] = pose_gt kdata_gt.trajectories[(5, 'cam0')] = pose_gt kdata_list = [kdata, kdata2, kdata_gt] intersection = {'a', 'b', 'c', 'd', 'e'} result1 = evaluate(kdata, kdata_gt, intersection) self.assertEqual(len(result1), 5) self.assertEqual(result1[0][0], 'a') self.assertAlmostEqual(result1[0][1], 1.0) self.assertAlmostEqual(result1[0][2], 1.0) self.assertEqual(result1[1][0], 'b') self.assertAlmostEqual(result1[1][1], 0.1) self.assertAlmostEqual(result1[1][2], 2.0) self.assertEqual(result1[2][0], 'c') self.assertAlmostEqual(result1[2][1], 8.442) self.assertAlmostEqual(result1[2][2], 100.0) self.assertEqual(result1[3][0], 'd') self.assertAlmostEqual(result1[3][1], 0.342) self.assertAlmostEqual(result1[3][2], 0.0) self.assertEqual(result1[4][0], 'e') self.assertTrue(math.isnan(result1[4][1])) self.assertTrue(math.isnan(result1[4][2])) result2 = evaluate(kdata2, kdata_gt, intersection) self.assertEqual(len(result2), 5) self.assertEqual(result2[0][0], 'a') self.assertAlmostEqual(result2[0][1], 1.0) self.assertAlmostEqual(result2[0][2], 1.0) self.assertEqual(result2[1][0], 'b') self.assertAlmostEqual(result2[1][1], 0.1) self.assertAlmostEqual(result2[1][2], 2.0) self.assertEqual(result2[2][0], 'c') self.assertAlmostEqual(result2[2][1], 8.442) self.assertAlmostEqual(result2[2][2], 100.0) self.assertEqual(result2[3][0], 'd') self.assertAlmostEqual(result2[3][1], 0.342) self.assertAlmostEqual(result2[3][2], 0.0) self.assertEqual(result2[4][0], 'e') self.assertAlmostEqual(result2[4][1], 5.0) self.assertTrue(math.isnan(result2[4][2])) bins1 = fill_bins(result1, [(0.9, 5), (10, 105)]) self.assertEqual(len(bins1), 2) self.assertEqual(bins1[0][0], 0.9) self.assertEqual(bins1[0][1], 5) self.assertEqual(bins1[0][2], 2) self.assertEqual(bins1[1][0], 10) self.assertEqual(bins1[1][1], 105) self.assertEqual(bins1[1][2], 4) bins2 = fill_bins(result1, [(0.9, 5), (10, 105)]) self.assertEqual(len(bins2), 2) self.assertEqual(bins2[0][0], 0.9) self.assertEqual(bins2[0][1], 5) self.assertEqual(bins2[0][2], 2) self.assertEqual(bins2[1][0], 10) self.assertEqual(bins2[1][1], 105) self.assertEqual(bins2[1][2], 4) bins3 = fill_bins(result2, [(0.9, math.nan), (10, math.nan)]) self.assertEqual(len(bins3), 2) self.assertEqual(bins3[0][0], 0.9) self.assertTrue(math.isnan(bins3[0][1])) self.assertEqual(bins3[0][2], 2) self.assertEqual(bins3[1][0], 10) self.assertTrue(math.isnan(bins3[1][1])) self.assertEqual(bins3[1][2], 5) bins4 = fill_bins(result2, [(0.9, -1), (10, -1)]) self.assertEqual(len(bins4), 2) self.assertEqual(bins4[0][0], 0.9) self.assertEqual(bins4[0][1], -1) self.assertEqual(bins4[0][2], 2) self.assertEqual(bins4[1][0], 10) self.assertEqual(bins4[1][1], -1) self.assertEqual(bins4[1][2], 5)
def kapture_from_dir( kapture_dirpath: str, matches_pairsfile_path: Optional[str] = None, skip_list: List[Type[Union[kapture.Rigs, kapture.Trajectories, kapture.RecordsCamera, kapture.RecordsLidar, kapture.RecordsWifi, kapture.RecordsGnss, kapture.Keypoints, kapture.Descriptors, kapture.GlobalFeatures, kapture.Matches, kapture.Points3d, kapture.Observations, ]]] = [] ) -> kapture.Kapture: """ Reads and return kapture data from directory. :param kapture_dirpath: kapture directory root path :param matches_pairsfile_path: text file in the csv format; where each line is image_name1, image_name2, score :param skip_list: Input option for expert only. Skip the load of specified parts. :return: kapture data read """ if not path.isdir(kapture_dirpath): raise FileNotFoundError(f'No kapture directory {kapture_dirpath}') csv_filepaths = {dtype: path.join(kapture_dirpath, filename) for dtype, filename in CSV_FILENAMES.items()} data_dirpaths = {dtype: path.join(kapture_dirpath, dir_name) for dtype, dir_name in kapture.io.features.FEATURES_DATA_DIRNAMES.items()} # keep only those in load_only and that exists kapture_data_paths = {**data_dirpaths, **csv_filepaths} # make sure files take precedence over dirs kapture_loadable_data = { kapture_type for kapture_type in KAPTURE_LOADABLE_TYPES if kapture_type not in skip_list and path.exists(kapture_data_paths[kapture_type]) } kapture_data = kapture.Kapture() # sensors sensor_ids = None sensors_file_path = csv_filepaths[kapture.Sensors] if sensors_file_path: logger.debug(f'loading sensors {sensors_file_path} ...') kapture_data.__version__ = get_version_from_csv_file(sensors_file_path) kapture_data.sensors = sensors_from_file(sensors_file_path) sensor_ids = set(kapture_data.sensors.keys()) if kapture_data.sensors is not None else set() if sensor_ids is None: # no need to continue, everything else depends on sensors raise FileNotFoundError(f'File {sensors_file_path} is missing or empty in {kapture_dirpath}') # rigs if kapture.Rigs in kapture_loadable_data: rigs_file_path = csv_filepaths[kapture.Rigs] logger.debug(f'loading rigs {rigs_file_path} ...') assert sensor_ids is not None kapture_data.rigs = rigs_from_file(rigs_file_path, sensor_ids) # update sensor_ids with rig_id sensor_ids.update(kapture_data.rigs.keys()) # trajectories if kapture.Trajectories in kapture_loadable_data: trajectories_file_path = csv_filepaths[kapture.Trajectories] logger.debug(f'loading trajectories {trajectories_file_path} ...') assert sensor_ids is not None kapture_data.trajectories = trajectories_from_file(trajectories_file_path, sensor_ids) # records camera if kapture.RecordsCamera in kapture_loadable_data: records_camera_file_path = csv_filepaths[kapture.RecordsCamera] logger.debug(f'loading images {records_camera_file_path} ...') assert kapture_data.sensors is not None camera_sensor_ids = set([sensor_id for sensor_id in kapture_data.sensors.keys() if kapture_data.sensors[sensor_id].sensor_type == 'camera']) kapture_data.records_camera = records_camera_from_file(csv_filepaths[kapture.RecordsCamera], camera_sensor_ids) # records lidar if kapture.RecordsLidar in kapture_loadable_data: records_lidar_file_path = csv_filepaths[kapture.RecordsLidar] logger.debug(f'loading lidar {records_lidar_file_path} ...') assert kapture_data.sensors is not None lidar_sensor_ids = set([sensor_id for sensor_id in kapture_data.sensors.keys() if kapture_data.sensors[sensor_id].sensor_type == 'lidar']) assert lidar_sensor_ids is not None kapture_data.records_lidar = records_lidar_from_file(records_lidar_file_path, lidar_sensor_ids) # records Wifi if kapture.RecordsWifi in kapture_loadable_data: records_wifi_file_path = csv_filepaths[kapture.RecordsWifi] logger.debug(f'loading wifi {records_wifi_file_path} ...') assert kapture_data.sensors is not None wifi_sensor_ids = set([sensor_id for sensor_id in kapture_data.sensors.keys() if kapture_data.sensors[sensor_id].sensor_type == 'wifi']) assert wifi_sensor_ids is not None kapture_data.records_wifi = records_wifi_from_file(records_wifi_file_path, wifi_sensor_ids) # records GNSS if kapture.RecordsGnss in kapture_loadable_data: records_gnss_file_path = csv_filepaths[kapture.RecordsGnss] logger.debug(f'loading GNSS {records_gnss_file_path} ...') assert kapture_data.sensors is not None epsg_codes = {sensor_id: sensor.sensor_params[0] for sensor_id, sensor in kapture_data.sensors.items() if sensor.sensor_type == 'gnss'} if len(epsg_codes) > 0: kapture_data.records_gnss = records_gnss_from_file(records_gnss_file_path, epsg_codes) else: logger.warning('no declared GNSS sensors: all GNSS data will be ignored') # features image_filenames = set(image_name for _, _, image_name in kapture.flatten(kapture_data.records_camera)) \ if kapture_data.records_camera is not None else set() # keypoints if kapture.Keypoints in kapture_loadable_data: logger.debug(f'loading keypoints {data_dirpaths[kapture.Keypoints]} ...') assert kapture_data.records_camera is not None kapture_data.keypoints = keypoints_from_dir(kapture_dirpath, image_filenames) # descriptors if kapture.Descriptors in kapture_loadable_data: logger.debug(f'loading descriptors {data_dirpaths[kapture.Descriptors]} ...') assert kapture_data.records_camera is not None kapture_data.descriptors = descriptors_from_dir(kapture_dirpath, image_filenames) # global_features if kapture.GlobalFeatures in kapture_loadable_data: logger.debug(f'loading global features {data_dirpaths[kapture.GlobalFeatures]} ...') assert kapture_data.records_camera is not None kapture_data.global_features = global_features_from_dir(kapture_dirpath, image_filenames) # matches if kapture.Matches in kapture_loadable_data: logger.debug(f'loading matches {data_dirpaths[kapture.Matches]} ...') assert kapture_data.records_camera is not None kapture_data.matches = matches_from_dir(kapture_dirpath, image_filenames, matches_pairsfile_path) # points3d if kapture.Points3d in kapture_loadable_data: points3d_file_path = csv_filepaths[kapture.Points3d] logger.debug(f'loading points 3d {points3d_file_path} ...') kapture_data.points3d = points3d_from_file(points3d_file_path) # observations if kapture.Observations in kapture_loadable_data: observations_file_path = csv_filepaths[kapture.Observations] logger.debug(f'loading observations {observations_file_path} ...') assert kapture_data.keypoints is not None assert kapture_data.points3d is not None kapture_data.observations = observations_from_file(observations_file_path, kapture_data.keypoints) return kapture_data
def merge_remap(kapture_list: List[kapture.Kapture], skip_list: List[Type], data_paths: List[str], kapture_path: str, images_import_method: TransferAction) -> kapture.Kapture: """ Merge multiple kapture while keeping ids (sensor_id) identical in merged and inputs. :param kapture_list: list of kapture to merge. :param skip_list: input optional types to not merge. sensors and rigs are unskippable :param data_paths: list of path to root path directory in same order as mentioned in kapture_list. :param kapture_path: directory root path to the merged kapture. :return: """ merged_kapture = kapture.Kapture() # find new sensor ids / rig ids sensor_offset = 0 rigs_offset = 0 sensors_mapping = [] rigs_mapping = [] for every_kapture in kapture_list: if every_kapture.sensors is not None: sensors_mapping.append( get_sensors_mapping(every_kapture.sensors, sensor_offset)) sensor_offset += len(every_kapture.sensors) else: sensors_mapping.append({}) if every_kapture.rigs is not None: rigs_mapping.append( get_rigs_mapping(every_kapture.rigs, rigs_offset)) rigs_offset += len(every_kapture.rigs) else: rigs_mapping.append({}) # concatenate all sensors with the remapped ids new_sensors = merge_sensors( [a_kapture.sensors for a_kapture in kapture_list], sensors_mapping) if new_sensors: # if merge_sensors returned an empty object, keep merged_kapture.sensors to None merged_kapture.sensors = new_sensors # concatenate all rigs with the remapped ids new_rigs = merge_rigs([a_kapture.rigs for a_kapture in kapture_list], rigs_mapping, sensors_mapping) if new_rigs: # if merge_rigs returned an empty object, keep merged_kapture.rigs to None merged_kapture.rigs = new_rigs # all fields below can be skipped with skip_list # we do not assign the properties when the merge evaluate to false, we keep it as None if kapture.Trajectories not in skip_list: new_trajectories = merge_trajectories( [a_kapture.trajectories for a_kapture in kapture_list], rigs_mapping, sensors_mapping) if new_trajectories: merged_kapture.trajectories = new_trajectories if kapture.RecordsCamera not in skip_list: new_records_camera = merge_records_camera( [a_kapture.records_camera for a_kapture in kapture_list], sensors_mapping) if new_records_camera: merged_kapture.records_camera = new_records_camera merge_records_data( [[ image_name for _, _, image_name in kapture.flatten( every_kapture.records_camera) ] if every_kapture.records_camera is not None else [] for every_kapture in kapture_list], [ get_image_fullpath(data_path, image_filename=None) for data_path in data_paths ], kapture_path, images_import_method) if kapture.RecordsLidar not in skip_list: new_records_lidar = merge_records_lidar( [a_kapture.records_lidar for a_kapture in kapture_list], sensors_mapping) if new_records_lidar: merged_kapture.records_lidar = new_records_lidar if kapture.RecordsWifi not in skip_list: new_records_wifi = merge_records_wifi( [a_kapture.records_wifi for a_kapture in kapture_list], sensors_mapping) if new_records_wifi: merged_kapture.records_wifi = new_records_wifi if kapture.RecordsGnss not in skip_list: new_records_gnss = merge_records_gnss( [a_kapture.records_gnss for a_kapture in kapture_list], sensors_mapping) if new_records_gnss: merged_kapture.records_gnss = new_records_gnss # for the reconstruction, except points and observations, the files are copied with shutil.copy # if kapture_path evaluates to False, all copies will be skipped (but classes will be filled normally) if kapture.Keypoints not in skip_list: keypoints = [a_kapture.keypoints for a_kapture in kapture_list] keypoints_not_none = [k for k in keypoints if k is not None] if len(keypoints_not_none) > 0: new_keypoints = merge_keypoints(keypoints, data_paths, kapture_path) if new_keypoints: merged_kapture.keypoints = new_keypoints if kapture.Descriptors not in skip_list: descriptors = [a_kapture.descriptors for a_kapture in kapture_list] descriptors_not_none = [k for k in descriptors if k is not None] if len(descriptors_not_none) > 0: new_descriptors = merge_descriptors(descriptors, data_paths, kapture_path) if new_descriptors: merged_kapture.descriptors = new_descriptors if kapture.GlobalFeatures not in skip_list: global_features = [ a_kapture.global_features for a_kapture in kapture_list ] global_features_not_none = [ k for k in global_features if k is not None ] if len(global_features_not_none) > 0: new_global_features = merge_global_features( global_features, data_paths, kapture_path) if new_global_features: merged_kapture.global_features = new_global_features if kapture.Matches not in skip_list: matches = [a_kapture.matches for a_kapture in kapture_list] new_matches = merge_matches(matches, data_paths, kapture_path) if new_matches: merged_kapture.matches = new_matches if kapture.Points3d not in skip_list and kapture.Observations not in skip_list: points_and_obs = [(a_kapture.points3d, a_kapture.observations) for a_kapture in kapture_list] new_points, new_observations = merge_points3d_and_observations( points_and_obs) if new_points: merged_kapture.points3d = new_points if new_observations: merged_kapture.observations = new_observations elif kapture.Points3d not in skip_list: points = [a_kapture.points3d for a_kapture in kapture_list] new_points = merge_points3d(points) if new_points: merged_kapture.points3d = new_points return merged_kapture
def pyransaclib_localize_from_loaded_data( kapture_data: kapture.Kapture, kapture_path: str, tar_handlers: TarCollection, kapture_query_data: kapture.Kapture, output_path: str, pairsfile_path: str, inlier_threshold: float, number_lo_steps: int, min_num_iterations: int, max_num_iterations: int, refine_poses: bool, keypoints_type: Optional[str], duplicate_strategy: DuplicateCorrespondencesStrategy, rerank_strategy: RerankCorrespondencesStrategy, write_detailed_report: bool, force: bool) -> None: """ Localize images using pyransaclib. :param kapture_data: loaded kapture data (incl. points3d) :param kapture_path: path to the kapture to use :param tar_handlers: collection of pre-opened tar archives :param kapture_data: loaded kapture data (mapping and query images) :param output_path: path to the write the localization results :param pairsfile_path: pairs to use :param inlier_threshold: RANSAC inlier threshold in pixel :param number_lo_steps: number of local optimization iterations in LO-MSAC. Use 0 to use MSAC :param min_num_iterations: minimum number of ransac loops :param max_num_iterations: maximum number of ransac loops :param refine_poses: refine poses with pycolmap :param keypoints_type: types of keypoints (and observations) to use :param force: Silently overwrite kapture files if already exists. """ assert has_pyransaclib if refine_poses: assert has_pycolmap if not (kapture_data.records_camera and kapture_data.sensors and kapture_data.keypoints and kapture_data.matches and kapture_data.points3d and kapture_data.observations): raise ValueError('records_camera, sensors, keypoints, matches, ' 'points3d, observations are mandatory for map+query') if not (kapture_query_data.records_camera and kapture_query_data.sensors): raise ValueError('records_camera, sensors are mandatory for query') if keypoints_type is None: keypoints_type = try_get_only_key_from_collection( kapture_data.keypoints) assert keypoints_type is not None assert keypoints_type in kapture_data.keypoints assert keypoints_type in kapture_data.matches if kapture_data.rigs is not None and kapture_data.trajectories is not None: # make sure, rigs are not used in trajectories. logger.info('remove rigs notation.') rigs_remove_inplace(kapture_data.trajectories, kapture_data.rigs) kapture_data.rigs.clear() if kapture_query_data.trajectories is not None: logger.warning( "Input query data contains trajectories: they will be ignored") kapture_query_data.trajectories.clear() os.umask(0o002) os.makedirs(output_path, exist_ok=True) delete_existing_kapture_files(output_path, force_erase=force) # load pairsfile pairs = {} with open(pairsfile_path, 'r') as fid: table = kapture.io.csv.table_from_file(fid) for img_query, img_map, _ in table: if img_query not in pairs: pairs[img_query] = [] pairs[img_query].append(img_map) kapture_data.matches[keypoints_type].normalize() keypoints_filepaths = keypoints_to_filepaths( kapture_data.keypoints[keypoints_type], keypoints_type, kapture_path, tar_handlers) obs_for_keypoints_type = { point_id: per_keypoints_type_subdict[keypoints_type] for point_id, per_keypoints_type_subdict in kapture_data.observations.items() if keypoints_type in per_keypoints_type_subdict } point_id_from_obs = { (img_name, kp_id): point_id for point_id in obs_for_keypoints_type.keys() for img_name, kp_id in obs_for_keypoints_type[point_id] } query_images = [(timestamp, sensor_id, image_name) for timestamp, sensor_id, image_name in kapture.flatten( kapture_query_data.records_camera)] # kapture for localized images + pose trajectories = kapture.Trajectories() progress_bar = tqdm(total=len(query_images), disable=logging.getLogger().level >= logging.CRITICAL) for timestamp, sensor_id, image_name in query_images: if image_name not in pairs: continue keypoints_filepath = keypoints_filepaths[image_name] kapture_keypoints_query = image_keypoints_from_file( filepath=keypoints_filepath, dsize=kapture_data.keypoints[keypoints_type].dsize, dtype=kapture_data.keypoints[keypoints_type].dtype) query_cam = kapture_query_data.sensors[sensor_id] assert isinstance(query_cam, kapture.Camera) num_keypoints = kapture_keypoints_query.shape[0] kapture_keypoints_query, K, distortion = get_camera_matrix_from_kapture( kapture_keypoints_query, query_cam) kapture_keypoints_query = kapture_keypoints_query.reshape( (num_keypoints, 2)) cv2_keypoints_query = np.copy(kapture_keypoints_query) if np.count_nonzero(distortion) > 0: epsilon = np.finfo(np.float64).eps stop_criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 500, epsilon) cv2_keypoints_query = cv2.undistortPointsIter( cv2_keypoints_query, K, distortion, R=None, P=K, criteria=stop_criteria) cv2_keypoints_query = cv2_keypoints_query.reshape((num_keypoints, 2)) # center keypoints for i in range(cv2_keypoints_query.shape[0]): cv2_keypoints_query[i, 0] = cv2_keypoints_query[i, 0] - K[0, 2] cv2_keypoints_query[i, 1] = cv2_keypoints_query[i, 1] - K[1, 2] kpts_query = kapture_keypoints_query if ( refine_poses or write_detailed_report) else None points2D, points2D_undistorted, points3D, stats = get_correspondences( kapture_data, keypoints_type, kapture_path, tar_handlers, image_name, pairs[image_name], point_id_from_obs, kpts_query, cv2_keypoints_query, duplicate_strategy, rerank_strategy) # compute absolute pose # inlier_threshold - RANSAC inlier threshold in pixels # answer - dictionary containing the RANSAC output ret = pyransaclib.ransaclib_localization(image_name, K[0, 0], K[1, 1], points2D_undistorted, points3D, inlier_threshold, number_lo_steps, min_num_iterations, max_num_iterations) # add pose to output kapture if ret['success'] and ret['num_inliers'] > 0: pose = kapture.PoseTransform(ret['qvec'], ret['tvec']) if refine_poses: inlier_mask = np.zeros((len(points2D), ), dtype=bool) inlier_mask[ret['inliers']] = True inlier_mask = inlier_mask.tolist() col_cam_id, width, height, params, _ = get_colmap_camera( query_cam) cfg = { 'model': CAMERA_MODEL_NAME_ID[col_cam_id][0], 'width': int(width), 'height': int(height), 'params': params } ret_refine = pycolmap.pose_refinement(pose.t_raw, pose.r_raw, points2D, points3D, inlier_mask, cfg) if ret_refine['success']: pose = kapture.PoseTransform(ret_refine['qvec'], ret_refine['tvec']) logger.debug( f'{image_name} refinement success, new pose: {pose}') if write_detailed_report: reprojection_error = compute_reprojection_error( pose, ret['num_inliers'], ret['inliers'], points2D, points3D, K, distortion) cache = { "num_correspondences": len(points3D), "num_inliers": ret['num_inliers'], "inliers": ret['inliers'], "reprojection_error": reprojection_error, "stats": stats } cache_path = os.path.join( output_path, f'pyransaclib_cache/{image_name}.json') save_to_json(cache, cache_path) trajectories[timestamp, sensor_id] = pose progress_bar.update(1) progress_bar.close() kapture_data_localized = kapture.Kapture( sensors=kapture_query_data.sensors, trajectories=trajectories, records_camera=kapture_query_data.records_camera, rigs=kapture_query_data.rigs) kapture.io.csv.kapture_to_dir(output_path, kapture_data_localized)