def setUp(self): self._rigs = kapture.Rigs() looking_not_straight = quaternion.from_rotation_vector( [0, np.deg2rad(5.), 0]) self._rigs['rig', 'cam1'] = kapture.PoseTransform( t=[-10, 0, 0], r=looking_not_straight).inverse() self._rigs['rig', 'cam2'] = kapture.PoseTransform( t=[+10, 0, 0], r=looking_not_straight.inverse()).inverse() self._trajectories_rigs = kapture.Trajectories() for timestamp, ratio in enumerate(np.linspace(0., 1., num=8)): looking_around = quaternion.from_rotation_vector( [0, np.deg2rad(360. * ratio), 0]) self._trajectories_rigs[timestamp, 'rig'] = kapture.PoseTransform( t=[0, 0, -100.], r=looking_around) self._trajectories_cams = kapture.Trajectories() for timestamp, rig_id, pose_rig_from_world in kapture.flatten( self._trajectories_rigs, is_sorted=True): for rig_id2, cam_id, pose_cam_from_rig in kapture.flatten( self._rigs): assert rig_id == rig_id pose_cam_from_world = kapture.PoseTransform.compose( [pose_cam_from_rig, pose_rig_from_world]) self._trajectories_cams[timestamp, cam_id] = pose_cam_from_world
def get_interpolated_pose(kdata_map: kapture.Kapture, kdata_query: kapture.Kapture, weights: Dict[str, List[Tuple[str, float]]]): """ compute the approximated pose for all query images given the precomputed weights :param kdata_map: map images + their poses as kapture data :type kdata_map: kapture.Kapture :param kdata_query: query images as kapture data :type kdata_query: kapture.Kapture :param weights: weights for the pose interpolation :type weights: Dict[str, List[Tuple[str, float]]] """ output_trajectories = kapture.Trajectories() assert kdata_map.trajectories is not None assert kdata_map.records_camera is not None reverse_map_records_camera = {image_name: (timestamp, sensor_id) for timestamp, sensor_id, image_name in kapture.flatten(kdata_map.records_camera)} if kdata_map.rigs is not None: input_trajectories = kapture.rigs_remove(kdata_map.trajectories, kdata_map.rigs) else: input_trajectories = kdata_map.trajectories assert kdata_query.records_camera is not None reverse_query_records_camera = {image_name: (timestamp, sensor_id) for timestamp, sensor_id, image_name in kapture.flatten(kdata_query.records_camera)} for query_image_name, weighted_map_images in weights.items(): pose_inv_list = [input_trajectories[reverse_map_records_camera[name]].inverse() for name, _ in weighted_map_images] weight_list = [w for _, w in weighted_map_images] output_trajectories[reverse_query_records_camera[query_image_name]] = average_pose_transform_weighted( pose_inv_list, weight_list ).inverse() return output_trajectories
def _import_trajectories(silda_dir_path, image_name_to_ids, hide_progress_bars) -> kapture.Trajectories: logger.info('Processing trajectories ...') trajectories = kapture.Trajectories() with open(path.join(silda_dir_path, 'silda-train-poses.txt')) as file: lines = file.readlines() lines = (line.rstrip().split() for line in lines) extrinsics = { line[0]: np.array(line[1:8], dtype=np.float) for line in lines } for silda_image_name, pose_params in tqdm(extrinsics.items(), disable=hide_progress_bars): # Silda poses are 7-dim vectors with the rotation quaternion, # and the translation vector. The order needs to be: # qw,qx,qy,qz,tx,ty,tz # The parameters should be described in terms of camera to world transformations if silda_image_name not in image_name_to_ids: # if this is not referenced: means its part of the corpus to be ignored. continue pose = kapture.PoseTransform(pose_params[0:4], pose_params[4:7]).inverse() timestamp, cam_id = image_name_to_ids[silda_image_name] trajectories[timestamp, cam_id] = pose # if query, trajectories is empty, so juste do not save it if len(trajectories) == 0: trajectories = None return trajectories
def test_type_checking(self): traj = kapture.Trajectories() valid_ts, valid_id, valid_pose = 0, 'cam0', kapture.PoseTransform() invalid_ts, invalid_id, invalid_pose = '0', float(0), 'pose' self.assertRaises(TypeError, traj.__setitem__, (invalid_ts, valid_id), valid_pose) self.assertRaises(TypeError, traj.__setitem__, (valid_ts, invalid_id), valid_pose) self.assertRaises(TypeError, traj.__setitem__, (valid_ts, valid_id), invalid_pose) self.assertRaises(TypeError, traj.__setitem__, (invalid_ts, invalid_id), invalid_pose) self.assertRaises(TypeError, traj.__setitem__, invalid_ts, {valid_id: valid_pose}) self.assertRaises(TypeError, traj.__setitem__, valid_ts, {invalid_id: valid_pose}) self.assertRaises(TypeError, traj.__setitem__, valid_ts, {valid_id: invalid_pose}) self.assertRaises(TypeError, traj.__setitem__, invalid_ts, valid_pose) self.assertRaises(TypeError, traj.__contains__, invalid_ts, valid_id) self.assertRaises(TypeError, traj.__contains__, valid_ts, invalid_id) self.assertRaises(TypeError, traj.__contains__, invalid_ts, invalid_id) self.assertRaises(TypeError, traj.__delitem__, invalid_ts) self.assertRaises(TypeError, traj.__delitem__, (valid_ts, invalid_id))
def test_rig_remove(self): rigs = kapture.Rigs() rigs['rig0', 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[100, 0, 0]) rigs['rig0', 'cam1'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[-100, 0, 0]) trajectories = kapture.Trajectories() trajectories[0, 'rig0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 0]) trajectories[1, 'rig0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 10]) trajectories[2, 'rig0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 20]) trajectories_ = kapture.rigs_remove(trajectories, rigs) # timestamps should be unchanged self.assertEqual(trajectories_.keys(), trajectories.keys()) self.assertNotEqual(trajectories_.key_pairs(), trajectories.key_pairs()) self.assertEqual(len(trajectories_.key_pairs()), len(trajectories.key_pairs()) * len(rigs.key_pairs())) self.assertIn((0, 'cam0'), trajectories_.key_pairs()) self.assertIn((0, 'cam1'), trajectories_.key_pairs()) self.assertIn((2, 'cam0'), trajectories_.key_pairs()) self.assertIn((2, 'cam1'), trajectories_.key_pairs()) self.assertAlmostEqual(trajectories_[2, 'cam1'].t_raw, [-100.0, 0.0, 20.0]) self.assertAlmostEqual(trajectories_[2, 'cam1'].r_raw, [1.0, 0.0, 0.0, 0.0])
def convert_results_to_kapture(query_path: str, results: str, outpath: str): """ convert file with name qw qx qy qz tx ty tz to kapture """ skip_heavy_useless = [ kapture.Trajectories, kapture.RecordsLidar, kapture.RecordsWifi, kapture.Keypoints, kapture.Descriptors, kapture.GlobalFeatures, kapture.Matches, kapture.Points3d, kapture.Observations ] kapture_query = kapture_from_dir(query_path, skip_list=skip_heavy_useless) inverse_records_camera = { image_name: (timestamp, sensor_id) for timestamp, sensor_id, image_name in kapture.flatten( kapture_query.records_camera) } trajectories = kapture.Trajectories() with open(results) as fid: lines = fid.readlines() lines = [line.rstrip().split() for line in lines if line != '\n'] for line in lines: image_name = line[0] rotation = quaternion.quaternion(float(line[1]), float(line[2]), float(line[3]), float(line[4])) translation = [float(line[5]), float(line[6]), float(line[7])] timestamp, sensor_id = inverse_records_camera[image_name] trajectories[timestamp, sensor_id] = kapture.PoseTransform(rotation, translation) kapture_query.trajectories = trajectories kapture_to_dir(outpath, kapture_query)
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 merge_trajectories( trajectories_list: List[Optional[kapture.Trajectories]], rig_mappings: List[Dict[str, str]], sensor_mappings: List[Dict[str, str]]) -> kapture.Trajectories: """ Merge several trajectories list into one list with new identifiers for the rigs and the sensors. :param trajectories_list: list of trajectories to merge :param rig_mappings: mapping of the rigs identifiers to their new identifiers :param sensor_mappings: mapping of the sensor identifiers to their new identifiers :return: merged trajectories """ assert len(trajectories_list) > 0 assert len(trajectories_list) == len(rig_mappings) assert len(trajectories_list) == len(sensor_mappings) merged_trajectories = kapture.Trajectories() for trajectories, rig_mapping, sensor_mapping in zip( trajectories_list, rig_mappings, sensor_mappings): if trajectories is None: continue for timestamp, sensor_id, pose in kapture.flatten(trajectories): if sensor_id in rig_mapping: new_sensor_id = rig_mapping[sensor_id] else: new_sensor_id = sensor_mapping[sensor_id] merged_trajectories[(timestamp, new_sensor_id)] = pose return merged_trajectories
def test_trajectories_write(self): pose1 = kapture.PoseTransform(r=[1.0, 0.0, 0.0, 0.0], t=[0.0, 0.0, 0.0]) pose2 = kapture.PoseTransform(r=[0.5, 0.5, 0.5, 0.5], t=[4., 2., -2.]) content_expected = [ csv.KAPTURE_FORMAT_1 + kapture_linesep, '# timestamp, device_id, qw, qx, qy, qz, tx, ty, tz' + kapture_linesep, ' 0, cam0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0' + kapture_linesep, ' 0, cam1, 0.5, 0.5, 0.5, 0.5, 4.0, 2.0, -2.0' + kapture_linesep, ' 100, cam2, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0' + kapture_linesep ] trajectories = kapture.Trajectories() timestamp1, timestamp2 = 0, 100 sensor_id1, sensor_id2, sensor_id3 = 'cam0', 'cam1', 'cam2' trajectories[(timestamp1, sensor_id1)] = pose1 trajectories[(timestamp1, sensor_id2)] = pose2 trajectories[(timestamp2, sensor_id3)] = pose1 csv.trajectories_to_file(self._temp_filepath, trajectories) with open(self._temp_filepath, 'rt') as f: content_actual = f.readlines() self.assertListEqual(content_actual, content_expected)
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_import(self): colmap_rigs = [{ "cameras": [{ "camera_id": 0, "image_prefix": "camL" }, { "camera_id": 1, "image_prefix": "camR" }], "ref_camera_id": 0 }] rigs_kapture, reconstructed_images, reconstructed_trajectories = import_colmap_rig_json( rigs_colmap=colmap_rigs) self.assertEqual([('rig0', get_camera_kapture_id_from_colmap_id(0)), ('rig0', get_camera_kapture_id_from_colmap_id(1))], rigs_kapture.key_pairs()) self.assertIsNone(reconstructed_images) self.assertIsNone(reconstructed_trajectories) # the records images = kapture.RecordsCamera() images[0, get_camera_kapture_id_from_colmap_id(0)] = 'camL/0000.jpg' images[1, get_camera_kapture_id_from_colmap_id(1)] = 'camR/0000.jpg' images[2, get_camera_kapture_id_from_colmap_id(0)] = 'camL/0001.jpg' images[3, get_camera_kapture_id_from_colmap_id(1)] = 'camR/0001.jpg' rigs_kapture, reconstructed_images, reconstructed_trajectories = import_colmap_rig_json( rigs_colmap=colmap_rigs, images=images) # check timestamps has been recovered. self.assertEqual([(0, get_camera_kapture_id_from_colmap_id(0)), (0, get_camera_kapture_id_from_colmap_id(1)), (1, get_camera_kapture_id_from_colmap_id(0)), (1, get_camera_kapture_id_from_colmap_id(1))], reconstructed_images.key_pairs()) # trajectories trajectories = kapture.Trajectories() trajectories[ 0, get_camera_kapture_id_from_colmap_id(0)] = kapture.PoseTransform() trajectories[ 1, get_camera_kapture_id_from_colmap_id(1)] = kapture.PoseTransform() trajectories[ 2, get_camera_kapture_id_from_colmap_id(0)] = kapture.PoseTransform() trajectories[ 3, get_camera_kapture_id_from_colmap_id(1)] = kapture.PoseTransform() rigs_kapture, reconstructed_images, reconstructed_trajectories = import_colmap_rig_json( rigs_colmap=colmap_rigs, images=images, trajectories=trajectories) self.assertEqual([(0, get_camera_kapture_id_from_colmap_id(0)), (0, get_camera_kapture_id_from_colmap_id(1)), (1, get_camera_kapture_id_from_colmap_id(0)), (1, get_camera_kapture_id_from_colmap_id(1))], reconstructed_trajectories.key_pairs())
def import_extended_cmu_seasons_images( image_list_file_path: str ) -> Tuple[kapture.RecordsCamera, kapture.Trajectories]: """ Read image list, name.jpg or name.jpg qw qx qy qz cx cy cz :param image_list_file_path: path to the image list file :return: kapture images and trajectories """ records_camera = kapture.RecordsCamera() trajectories = kapture.Trajectories() # name.jpg qw qx qy qz cx cy cz # or # name.jpg with open(image_list_file_path) as fin: table = fin.readlines() # remove comment lines table = (line for line in table if not line.startswith('#')) # remove empty lines table = (line for line in table if line.strip()) # trim trailing EOL table = (line.rstrip("\n\r") for line in table) # split space table = (re.split(r'\s+', line) for line in table) # remove empty split table = ([s for s in line if s] for line in table) image_pattern = re.compile(ECMU_IMAGE_PATTERN) for line in table: image_name = line[0] timestamp, camera_id = _parse_image_name(image_name, image_pattern) if camera_id is None or timestamp is None: continue records_camera[(timestamp, camera_id)] = image_name if len(line) > 1: # also contains trajectory qw, qx, qy, qz, cx, cy, cz = line[1:] quaternion_array = float_array_or_none([qw, qx, qy, qz]) assert quaternion_array is not None center_array = float_array_or_none([cx, cy, cz]) assert center_array is not None rotation = quaternion.from_float_array(quaternion_array) # C = -R^T * t -> t = -R * C translation = np.matmul( quaternion.as_rotation_matrix(rotation), -1 * np.array(center_array, dtype=np.float)) pose = kapture.PoseTransform(r=rotation, t=translation) trajectories[(timestamp, camera_id)] = pose # if no trajectories were added (query), prefer None if not trajectories: trajectories = None return records_camera, trajectories
def test_timestamps_length(self): trajectories = kapture.Trajectories() self.assertEqual(trajectories.timestamp_length(), -1) trajectories[1614362592378, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 20]) trajectories[1614362592634, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 0]) trajectories[1614362592378, 'cam1'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[10, 0, 20]) trajectories[1614362593123, 'cam1'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[10, 0, 30]) self.assertEqual(trajectories.timestamp_length(), 13) # Check that if we have timestamps of different precision, we can not compute a common length trajectories[1614362594, 'lidar0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 0]) self.assertEqual(trajectories.timestamp_length(), -1)
def test_pose_interpolation(self): trajectories = kapture.Trajectories() trajectories[1614362592000, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 0]) trajectories[1614362592000, 'cam1'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 10, 0]) trajectories[1614362592500, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 10]) trajectories[1614362593000, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 20]) trajectories[1614362593000, 'cam1'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 10, 20]) trajectories[1614362593500, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 30]) trajectories[1614362594000, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 40]) trajectories[1614362594500, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 50]) trajectories[1614362595000, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 60]) trajectories[1614362595500, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 70]) self.assertEqual(trajectories.timestamp_length(), 13) pose = trajectories.intermediate_pose(1614362592500, 'cam2', 1000000) self.assertIsNone(pose, "unknown device") pose = trajectories.intermediate_pose(1614362593000, 'cam0', 1000000) self.assertEqual(pose, kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 20]), "existing pose") pose = trajectories.intermediate_pose(1614362500000, 'cam0', 1000000) self.assertIsNone(pose, "time too far in past") pose = trajectories.intermediate_pose(1614362600000, 'cam0', 1000000) self.assertIsNone(pose, "time too far in future") pose = trajectories.intermediate_pose(1614362595250, 'cam0', 1000000) self.assertEqual(pose, kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 65])) pose = trajectories.intermediate_pose(1614362592500, 'cam1', 1000000) self.assertEqual(pose, kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 10, 10])) pose = trajectories.intermediate_pose(1614362595250, 'cam1', 1000000) self.assertIsNone(pose, "not enough pose for cam1")
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_remove(self): trajectories = kapture.Trajectories() trajectories[0, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 0]) trajectories[1, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 10]) trajectories[2, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 20]) trajectories[2, 'cam1'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[10, 0, 20]) self.assertEqual(len(trajectories), 3) self.assertEqual(len(trajectories[2]), 2) del trajectories[2, 'cam0'] self.assertEqual(len(trajectories), 3) self.assertEqual(len(trajectories[2]), 1) del trajectories[1] self.assertEqual(len(trajectories), 2) del trajectories[2, 'cam1'] self.assertEqual(len(trajectories), 1)
def test_timestamps_list(self): trajectories = kapture.Trajectories() trajectories[2, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 20]) trajectories[0, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 0]) trajectories[2, 'cam1'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[10, 0, 20]) self.assertEqual(trajectories.timestamps_sorted_list(), [0, 2]) trajectories[1, 'cam0'] = kapture.PoseTransform(r=[1, 0, 0, 0], t=[0, 0, 10]) self.assertEqual(len(trajectories), 3) self.assertEqual(trajectories.timestamps_sorted_list(), [0, 1, 2]) del trajectories[2, 'cam0'] self.assertEqual(trajectories.timestamps_sorted_list(), [0, 1, 2]) del trajectories[1] self.assertEqual(trajectories.timestamps_sorted_list(), [0, 2]) del trajectories[2, 'cam1'] self.assertEqual(trajectories.timestamps_sorted_list(), [0, ])
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 merge_trajectories( trajectories_list: List[Optional[kapture.Trajectories]] ) -> kapture.Trajectories: """ Merge several trajectories lists. For trajectory point with the same timestamp and sensor identifier, keep only the first one. :param trajectories_list: list of trajectories :return: merged trajectories """ assert len(trajectories_list) > 0 merged_trajectories = kapture.Trajectories() for trajectories in trajectories_list: if trajectories is None: continue for timestamp, sensor_id, pose in kapture.flatten(trajectories): if (timestamp, sensor_id) in merged_trajectories: continue merged_trajectories[(timestamp, sensor_id)] = pose return merged_trajectories
def get_images_and_trajectories_from_database( database: COLMAPDatabase ) -> Tuple[kapture.RecordsCamera, kapture.Trajectories]: """ Creates records_camera and trajectories from colmap images table In trajectories, timestamps are made up from colmap image id. :param database: colmap database :return: kapture records_camera and trajectories """ logging.info('parsing images ...') kapture_images = kapture.RecordsCamera() kapture_trajectories = kapture.Trajectories() hide_progressbar = logger.getEffectiveLevel() > logging.INFO for image_id, name, camera_id, prior_qw, prior_qx, prior_qy, prior_qz, prior_tx, prior_ty, prior_tz \ in tqdm(database.execute('SELECT image_id, name, camera_id, ' 'prior_qw, prior_qx, prior_qy, prior_qz, ' 'prior_tx, prior_ty, prior_tz FROM images;' ), disable=hide_progressbar): # images timestamp = int(image_id) camera_id = get_camera_kapture_id_from_colmap_id(camera_id) kapture_images[timestamp, camera_id] = name # trajectories prior_q = [prior_qw, prior_qx, prior_qy, prior_qz] prior_t = [prior_tx, prior_ty, prior_tz] # do not register the pose part if its invalid. is_undefined = all(v is None for v in prior_q + prior_t) if is_undefined: # just ignore continue prior_pose = kapture.PoseTransform(prior_q, prior_t) kapture_trajectories[timestamp, camera_id] = prior_pose if len(kapture_trajectories) == 0: # if there is no pose at all, just don't bother. kapture_trajectories = None return kapture_images, kapture_trajectories
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 _import_trajectories(input_json, device_identifiers, timestamp_for_pose): trajectories = kapture.Trajectories() if input_json.get(EXTRINSICS): extrinsics = input_json[EXTRINSICS] logger.info(f'Importing {len(extrinsics)} extrinsics -> trajectories') for pose in extrinsics: pose_id = pose[KEY] center = pose[VALUE][CENTER] rotation = pose[VALUE][ROTATION] kap_translation = -1 * np.matmul(rotation, center) kap_pose = kapture.PoseTransform( quaternion.from_rotation_matrix(rotation), kap_translation) timestamp = timestamp_for_pose.get(pose_id) if timestamp is None: logger.warning(f'Missing timestamp for extrinsic {pose_id}') continue device_id = device_identifiers.get(pose_id) if device_id is None: logger.warning(f'Missing device for extrinsic {pose_id}') continue trajectories[(timestamp, device_id)] = kap_pose # tuple of int,str -> 6D pose return trajectories
def test_init(self): timestamp1 = 0 timestamp2 = 1 device_id = 'cam0' pose = kapture.PoseTransform() traj = kapture.Trajectories() # pair assignment traj[(timestamp1, device_id)] = pose # dict assignment traj[timestamp2] = {device_id: pose} self.assertEqual(2, len(traj)) self.assertEqual(traj[timestamp1], traj[timestamp2]) self.assertEqual(traj[(timestamp1, device_id)], traj[(timestamp2, device_id)]) # test __contains__ self.assertIn(timestamp1, traj) self.assertIn((timestamp1, device_id), traj) self.assertIn(timestamp2, traj) self.assertIn((timestamp1, device_id), traj) self.assertNotIn((timestamp1, 'cam1'), traj) self.assertNotIn((2, device_id), traj)
def trajectories_from_file(filepath: str, device_ids: Optional[Set[str]] = None) -> kapture.Trajectories: """ Reads trajectories from CSV file. :param filepath: input file path :param device_ids: input set of valid device ids (rig or sensor). If the trajectories contains unknown devices, they will be ignored. If no device_ids given, everything is loaded. :return: trajectories """ trajectories = kapture.Trajectories() with open(filepath) as file: table = table_from_file(file) # timestamp, device_id, qw, qx, qy, qz, tx, ty, tz for timestamp, device_id, qw, qx, qy, qz, tx, ty, tz in table: if device_ids is not None and device_id not in device_ids: # just ignore continue rotation = float_array_or_none([qw, qx, qy, qz]) trans = float_array_or_none([tx, ty, tz]) pose = kapture.PoseTransform(rotation, trans) trajectories[(int(timestamp), str(device_id))] = pose return trajectories
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 import_robotcar_colmap_location( robotcar_path: str, colmap_reconstruction_fullpath: path, kapture_path: str, rigs: kapture.Rigs, skip_reconstruction: bool) -> kapture.Kapture: """ Import robotcar data for one location from colmap reconstruction :param robotcar_path: path to the robotcar top directory :param colmap_reconstruction_fullpath: path to the colmap reconstruction directory :param kapture_path: path to the kapture top directory :param rigs: kapture rigs to modify :param skip_reconstruction: if True, will not add the reconstruction :return: a kapture object """ # First, import Colmap reconstruction for given location kapture_data = import_colmap( kapture_dirpath=kapture_path, colmap_reconstruction_dirpath=colmap_reconstruction_fullpath, colmap_images_dirpath=path.join(robotcar_path, "images"), skip_reconstruction=skip_reconstruction, images_import_strategy=TransferAction.skip ) # since filenames are incorrect # Post processing: # - use correct names for cameras # - model was built with PNG files, but we have JPG # - recover proper timestamps # - recover rig # Fix sensors.txt camera_mapping = { 'cam_00001': 'left', 'cam_00002': 'rear', 'cam_00003': 'right' } new_cameras = kapture.Sensors() for cam_id in kapture_data.sensors: new_cameras[camera_mapping[cam_id]] = kapture_data.sensors[cam_id] kapture_data.sensors = new_cameras if not skip_reconstruction: # Fix keypoints # Need to rename .png.kpt to .jpg.kpt files and that's all for root, dirs, files in os.walk(kapture_path): for file in files: if file.endswith('.png.kpt'): os.rename( path.join(root, file), path.join(root, file.replace(".png.kpt", ".jpg.kpt"))) # observations.txt: png -> jpg new_observations = kapture.Observations() for point3d_idx in kapture_data.observations: for image_path, keypoint_id in kapture_data.observations[ point3d_idx]: new_observations.add(point3d_idx, image_path.replace(".png", ".jpg"), int(keypoint_id)) kapture_data.observations = new_observations # records_camera.txt # timestamps, png->jpg new_records_camera = kapture.RecordsCamera() records_camera_pattern = re.compile(r'.*/(?P<timestamp>\d+)\.png') ts_mapping = {} for ts, shot in kapture_data.records_camera.items(): for cam_id, image_path in shot.items(): matches = records_camera_pattern.match(image_path) if not matches: continue matches = matches.groupdict() timestamp = int(matches['timestamp']) ts_mapping[ts] = timestamp new_path = image_path.replace(".png", ".jpg") new_records_camera[timestamp, camera_mapping[cam_id]] = new_path kapture_data.records_camera = new_records_camera # trajectories.txt new_trajectories = kapture.Trajectories() # First recover timestamps and camera names for ts, sensor_id in sorted(kapture_data.trajectories.key_pairs()): new_trajectories[ ts_mapping[ts], camera_mapping[sensor_id]] = kapture_data.trajectories[ts, sensor_id] kapture_data.trajectories = new_trajectories kapture_data.rigs = rigs return kapture_data
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 colmap_localize_from_loaded_data(kapture_data: kapture.Kapture, kapture_path: str, tar_handlers: Optional[TarCollection], colmap_path: str, input_database_path: str, input_reconstruction_path: str, colmap_binary: str, keypoints_type: Optional[str], use_colmap_matches_importer: bool, image_registrator_options: List[str], skip_list: List[str], force: bool) -> None: """ Localize images on a colmap model with the kapture data. :param kapture_data: kapture data to use :param kapture_path: path to the kapture to use :param tar_handler: collection of preloaded tar archives :param colmap_path: path to the colmap build :param input_database_path: path to the map colmap.db :param input_database_path: path to the map colmap.db :param input_reconstruction_path: path to the map reconstruction folder :param colmap_binary: path to the colmap binary executable :param keypoints_type: type of keypoints, name of the keypoints subfolder :param use_colmap_matches_importer: bool, :param image_registrator_options: options for the image registrator :param skip_list: list of steps to skip :param force: Silently overwrite kapture files if already exists. """ os.makedirs(colmap_path, exist_ok=True) if not (kapture_data.records_camera and kapture_data.sensors and kapture_data.keypoints and kapture_data.matches): raise ValueError('records_camera, sensors, keypoints, matches are mandatory') if kapture_data.trajectories: logger.warning("Input data contains trajectories: they will be ignored") kapture_data.trajectories.clear() else: kapture_data.trajectories = kapture.Trajectories() # COLMAP does not fully support rigs. 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() # Prepare output # Set fixed name for COLMAP database colmap_db_path = path.join(colmap_path, 'colmap.db') image_list_path = path.join(colmap_path, 'images.list') reconstruction_path = path.join(colmap_path, "reconstruction") if 'delete_existing' not in skip_list: safe_remove_file(colmap_db_path, force) safe_remove_file(image_list_path, force) safe_remove_any_path(reconstruction_path, force) os.makedirs(reconstruction_path, exist_ok=True) # Copy colmap db to output if not os.path.exists(colmap_db_path): shutil.copy(input_database_path, colmap_db_path) # find correspondences between the colmap db and the kapture data images_all = {image_path: (ts, cam_id) for ts, shot in kapture_data.records_camera.items() for cam_id, image_path in shot.items()} colmap_db = COLMAPDatabase.connect(colmap_db_path) colmap_image_ids = database_extra.get_colmap_image_ids_from_db(colmap_db) colmap_images = database_extra.get_images_from_database(colmap_db) colmap_db.close() # dict ( kapture_camera -> colmap_camera_id ) colmap_camera_ids = {images_all[image_path][1]: colmap_cam_id for image_path, colmap_cam_id in colmap_images if image_path in images_all} images_to_add = {image_path: value for image_path, value in images_all.items() if image_path not in colmap_image_ids} flatten_images_to_add = [(ts, kapture_cam_id, image_path) for image_path, (ts, kapture_cam_id) in images_to_add.items()] if 'import_to_db' not in skip_list: logger.info("Step 1: Add precomputed keypoints and matches to colmap db") 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 cameras_to_add = kapture.Sensors() for _, (_, kapture_cam_id) in images_to_add.items(): if kapture_cam_id not in colmap_camera_ids: kapture_cam = kapture_data.sensors[kapture_cam_id] cameras_to_add[kapture_cam_id] = kapture_cam colmap_db = COLMAPDatabase.connect(colmap_db_path) colmap_added_camera_ids = database_extra.add_cameras_to_database(cameras_to_add, colmap_db) colmap_camera_ids.update(colmap_added_camera_ids) colmap_added_image_ids = database_extra.add_images_to_database_from_flatten( colmap_db, flatten_images_to_add, kapture_data.trajectories, colmap_camera_ids) colmap_image_ids.update(colmap_added_image_ids) colmap_image_ids_reversed = {v: k for k, v in colmap_image_ids.items()} # colmap_id : name # add new features colmap_keypoints = database_extra.get_keypoints_set_from_database(colmap_db, colmap_image_ids_reversed) keypoints_all = kapture_data.keypoints[keypoints_type] keypoints_to_add = {name for name in keypoints_all if name not in colmap_keypoints} keypoints_to_add = kapture.Keypoints(keypoints_all.type_name, keypoints_all.dtype, keypoints_all.dsize, keypoints_to_add) database_extra.add_keypoints_to_database(colmap_db, keypoints_to_add, keypoints_type, kapture_path, tar_handlers, colmap_image_ids) # add new matches colmap_matches = kapture.Matches(database_extra.get_matches_set_from_database(colmap_db, colmap_image_ids_reversed)) colmap_matches.normalize() matches_all = kapture_data.matches[keypoints_type] matches_to_add = kapture.Matches({pair for pair in matches_all if pair not in colmap_matches}) # print(list(matches_to_add)) database_extra.add_matches_to_database(colmap_db, matches_to_add, keypoints_type, kapture_path, tar_handlers, colmap_image_ids, export_two_view_geometry=not use_colmap_matches_importer) colmap_db.close() if use_colmap_matches_importer: logger.info('Step 2: Run geometric verification') logger.debug('running colmap matches_importer...') if keypoints_type is None: keypoints_type = try_get_only_key_from_collection(kapture_data.matches) assert keypoints_type is not None assert keypoints_type in kapture_data.matches # compute two view geometry 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=kapture_data.matches[keypoints_type], force=force) else: logger.info('Step 2: Run geometric verification - skipped') if 'image_registrator' not in skip_list: logger.info("Step 3: Run image_registrator") # run image_registrator colmap_lib.run_image_registrator( colmap_binary, colmap_db_path, input_reconstruction_path, reconstruction_path, image_registrator_options ) # run model_converter if 'model_converter' not in skip_list: logger.info("Step 4: Export reconstruction results to txt") colmap_lib.run_model_converter( colmap_binary, reconstruction_path, reconstruction_path )
def import_colmap_rig_json( rigs_colmap: list, images: Optional[kapture.RecordsCamera] = None, trajectories: Optional[kapture.Trajectories] = None ) -> Tuple[kapture.Rigs, kapture.RecordsCamera, Optional[kapture.Trajectories]]: """ Build a kapture rig from colmap json file. :param rigs_colmap: colmap data describing the rig. :param images: input/output camera recordings: timestamps are modified to match :param trajectories: input/output trajectories: timestamps are modified to match :return: rigs, images and trajectories """ rigs_kapture = kapture.Rigs() # camera_id (kapture) -> file prefix camera_prefixes = {} """ rigs_colmap [{ "cameras": [ {"camera_id": 1, "image_prefix": "leftraw/"}, {"camera_id": 2, "image_prefix": "rightraw/"} ], "ref_camera_id": 1 }] """ for rig_idx_colmap, rig_colmap in enumerate(rigs_colmap): rig_id_kapture = f'rig{rig_idx_colmap}' # make up a rig ID from its idx in colmap. for cam_colmap in rig_colmap['cameras']: # colmap_cam_id -> kapture_cam_id camera_id_colmap = cam_colmap['camera_id'] camera_id_kapture = get_camera_kapture_id_from_colmap_id( camera_id_colmap) camera_prefixes[camera_id_kapture] = cam_colmap['image_prefix'] # colmap does not store rig geometry, but only the fact there is one. pose_unknown = kapture.PoseTransform(r=None, t=None) rigs_kapture[rig_id_kapture, camera_id_kapture] = pose_unknown reconstructed_images = None reconstructed_trajectories = None if images: # image_filepath => (prefix, suffix) filepath_to_split_fix = {} # if there are images, modify their timestamps to match # first pass: gather actual timestamps from suffix # camera_suffixes = set() for timestamp, camera_id_kapture, image_filepath in kapture.flatten( images): if camera_id_kapture not in camera_prefixes: raise KeyError( 'unknown camera_id {}'.format(camera_id_kapture)) camera_prefix = camera_prefixes[camera_id_kapture] if not image_filepath.startswith(camera_prefix): raise ValueError('inconsistent camera name') filepath_to_split_fix[image_filepath] = ( image_filepath[0:len(camera_prefix)], image_filepath[len(camera_prefix):]) suffixes = sorted(set(suf for _, suf in filepath_to_split_fix.values())) suffix_to_timestamp = { suffix: idx for idx, suffix in enumerate(suffixes) } idx_to_timestamp = { colmap_idx: suffix_to_timestamp[filepath_to_split_fix[filepath][1]] for colmap_idx, _, filepath in kapture.flatten(images) } # second pass: reconstruct images with timestamp (frame number) instead of colmap idx reconstructed_images = kapture.RecordsCamera() for colmap_idx, camera_id_kapture, image_filepath in kapture.flatten( images): timestamp = idx_to_timestamp[colmap_idx] reconstructed_images[timestamp, camera_id_kapture] = image_filepath # third pass: [optional] reconstruct trajectories if trajectories: reconstructed_trajectories = kapture.Trajectories() for colmap_idx, camera_id_kapture, pose in kapture.flatten( trajectories): timestamp = idx_to_timestamp[colmap_idx] reconstructed_trajectories[timestamp, camera_id_kapture] = pose return rigs_kapture, reconstructed_images, reconstructed_trajectories
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)