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)
# Adding New axis to the matched points. This is done so that the points have a dimension of 1 x N x 2. # This dimension is required because the next opencv function used in line 70 takes these points as the input with this # particular dimension of 1 x N x 2 matched_L_na = matched_L[:, np.newaxis, :] matched_R_na = matched_R[:, np.newaxis, :] # Undistorting the matched key points # The next step is to multiply the matched keypoints with the inverse camera matrix, undidtort the points and then # multiply again with the camera matrix. # In the case of lens distortion, the equations are non-linear and depend on 3 to 8 parameters (k1 to k6, p1 and p2). # Hence, it would normally require a non-linear solving algorithm (e.g. Newton's method, Levenberg-Marquardt algorithm, # etc) to inverse such a model and estimate the undistorted coordinates from the distorted ones. # And this is what is used behind function undistortPoints, with tuned parameters making the optimization fast but a # little inaccurate. # The explaination of the function and its input arguments are given in the report undist_pts_L = cv.undistortPointsIter(matched_L_na, k, dist, R = None, P = newK, criteria=crit_cal) undist_pts_R = cv.undistortPointsIter(matched_R_na, k, dist, R = None, P = newK, criteria=crit_cal) print('Shape of the returned Undistorted points: ', undist_pts_L.shape) # Computing the Fundamental Matrix. # The fundamental matrix maps points in one stereo image to epipolar lines in the other. It can be computed usingcorresponding # points in two images recovered in the feature matching stage. In particular, cv2.findFundamentalMat() implements just # this approach. # The undistorted keypoints are used for calculating the fundamental matrix. This not only reverses the effect of lens # distortion, but also transforms the coordinates to normalized image coordinates. Normalized image coordinates # (not to be confused with the normalization done in the 8-point algorithm) are camera agnostic coordinates that do # not depend on any of the camera intrinsic parameters. They represent the angle of the bearing vector to the point # in the real world. For example, a normalized image coordinate of (1, 0) would correspond to a bearing angle of 45
# N x 1 x 2 matched_kps_left = [kp_left[mat.queryIdx].pt for mat in matches] matched_kps_right = [kp_right[mat.trainIdx].pt for mat in matches] matched_kps_left = np.array(matched_kps_left) matched_kps_right = np.array(matched_kps_right) matched_kps_left = matched_kps_left[:, np.newaxis, :] matched_kps_right = matched_kps_right[:, np.newaxis, :] criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6) # Undistort the image points pts_left_undistorted = cv2.undistortPointsIter(matched_kps_left, cameraMatrix, distCoeffs, R=None, P=newK, criteria=criteria) pts_right_undistorted = cv2.undistortPointsIter(matched_kps_right, cameraMatrix, distCoeffs, R=None, P=newK, criteria=criteria) # Compute the fundamental matrix F, mask = cv2.findFundamentalMat(pts_left_undistorted, pts_right_undistorted, method=cv2.RANSAC, ransacReprojThreshold=0.5, confidence=.99)
def create_3D_model_from_depth_from_loaded_data( kdata: kapture.Kapture, input_path: str, tar_handlers: TarCollection, output_path: str, keypoints_type: Optional[str], depth_sensor_id: str, topk: int, method: Method, cellsizes: List[str], force: bool): """ Create 3D model from a kapture dataset that has registered depth data Assumes the kapture data is already loaded """ logger.info(f'create 3D model using depth data') if os.path.exists(output_path) and not force: print(f'outpath already exists, use --force to overwrite') return -1 if kdata.rigs is not None: assert kdata.trajectories is not None kapture.rigs_remove_inplace(kdata.trajectories, kdata.rigs) if keypoints_type is None: keypoints_type = try_get_only_key_from_collection(kdata.keypoints) assert keypoints_type is not None assert kdata.keypoints is not None assert keypoints_type in kdata.keypoints if method == Method.voxelgrid: vg = VoxelGrid(cellsizes) # add all 3D points to map that correspond to a keypoint logger.info('adding points from scan to kapture') points3d = [] observations = kapture.Observations() progress_bar = tqdm(total=len( list(kapture.flatten(kdata.records_camera, is_sorted=True))), disable=logger.level >= logging.CRITICAL) for timestamp, sensor_id, sensing_filepath in kapture.flatten( kdata.records_camera, is_sorted=True): logger.info( f'total 3d points: {len(points3d)}, processing {sensing_filepath}') # check if images have a pose if timestamp not in kdata.trajectories: logger.info('{} does not have a pose. skipping ...'.format( sensing_filepath)) continue # check if depth map exists depth_map_record = '' if timestamp in kdata.records_depth: if depth_sensor_id is None: depth_id = sensor_id + '_depth' else: depth_id = depth_sensor_id if depth_id in kdata.records_depth[timestamp]: depth_map_record = kdata.records_depth[timestamp][depth_id] depth_map_size = tuple( [int(x) for x in kdata.sensors[depth_id].camera_params[0:2]]) depth_path = get_depth_map_fullpath(input_path, depth_map_record) if not os.path.exists(depth_path): logger.info('no 3D data found for {}. skipping ...'.format( sensing_filepath)) continue depth_map = depth_map_from_file(depth_path, depth_map_size) img = Image.open(get_image_fullpath(input_path, sensing_filepath)).convert('RGB') assert img.size[0] == depth_map_size[0] assert img.size[1] == depth_map_size[1] kps_raw = load_keypoints(keypoints_type, input_path, sensing_filepath, kdata.keypoints[keypoints_type].dtype, kdata.keypoints[keypoints_type].dsize, tar_handlers) _, camera_sensor_C, camera_dist = get_camera_matrix_from_kapture( np.zeros((1, 0, 2), dtype=np.float64), kdata.sensors[sensor_id]) cv2_keypoints, depth_sensor_C, depth_dist = get_camera_matrix_from_kapture( kps_raw, kdata.sensors[depth_id]) assert np.isclose(depth_sensor_C, camera_sensor_C).all() assert np.isclose(depth_dist, camera_dist).all() if np.count_nonzero(camera_dist) > 0: epsilon = np.finfo(np.float64).eps stop_criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 500, epsilon) undistorted_cv2_keypoints = cv2.undistortPointsIter( cv2_keypoints, camera_sensor_C, camera_dist, R=None, P=camera_sensor_C, criteria=stop_criteria) else: undistorted_cv2_keypoints = cv2_keypoints cv2_keypoints = cv2_keypoints.reshape((kps_raw.shape[0], 2)) undistorted_cv2_keypoints = undistorted_cv2_keypoints.reshape( (kps_raw.shape[0], 2)) points3d_img = [] rgb_img = [] kp_idxs = [] for idx_kp, kp in enumerate(cv2_keypoints[0:topk]): u = round(kp[0]) v = round(kp[1]) undist_kp = undistorted_cv2_keypoints[idx_kp] undist_u = round(undist_kp[0]) undist_v = round(undist_kp[1]) if u >= 0 and u < depth_map_size[ 0] and v >= 0 and v < depth_map_size[1]: if depth_map[v, u] == 0: continue pt3d = project_kp_to_3D(undist_u, undist_v, depth_map[v, u], depth_sensor_C[0, 2], depth_sensor_C[1, 2], depth_sensor_C[0, 0], depth_sensor_C[1, 1]) points3d_img.append(pt3d) rgb_img.append(img.getpixel((u, v))) kp_idxs.append(idx_kp) # transform to world coordinates (pt3d from a depth map is in camera coordinates) # we use sensor_id here because we assume that the image and the corresponding depthmap have the same pose # and sometimes, the pose might only be provided for the images cam_to_world = kdata.trajectories[timestamp][sensor_id].inverse() if len(points3d_img) == 0: continue points3d_img = cam_to_world.transform_points(np.array(points3d_img)) for idx_kp, pt3d, rgb in zip(kp_idxs, points3d_img, rgb_img): if not np.isnan(pt3d).any(): # apply transform (alignment) if method == Method.voxelgrid: assert vg is not None if not vg.exists(pt3d): # add 3D point points3d.append(list(pt3d) + list(rgb)) # add observation observations.add( len(points3d) - 1, keypoints_type, sensing_filepath, idx_kp) vg.add(pt3d, len(points3d) - 1, sensing_filepath) else: ret = vg.append(pt3d, sensing_filepath) if ret is not None: observations.add(ret[0], keypoints_type, sensing_filepath, idx_kp) elif method == Method.all: # add 3D point points3d.append(list(pt3d) + list(rgb)) # add observation observations.add( len(points3d) - 1, keypoints_type, sensing_filepath, idx_kp) # save_3Dpts_to_ply(points3d, os.path.join(output_path, 'map.ply')) progress_bar.update(1) progress_bar.close() kdata.points3d = kapture.Points3d(np.array(points3d)) kdata.observations = observations logger.info('saving ...') kapture_to_dir(output_path, kdata) # save_3Dpts_to_ply(points3d, os.path.join(output_path, 'map.ply')) logger.info('all done')
i1_udist = img_undistort(f_camera, i1)[1] i2_udist = img_undistort(f_camera, i2)[1] i1_udistcp = img_undistort(f_camera, i1)[1] i2_udistcp = img_undistort(f_camera, i2)[1] # # Undistort matched kps, adopt the procedure shown in class by prof.Peters cdt_kp1 = cdt_kp1[:, np. newaxis, :] # Possibly convert kp array format to 40(n) x 1 x 2 cdt_kp2 = cdt_kp2[:, np. newaxis, :] # I have to use subpix cdt of kps (no 'int'), otherwise I will meet depth error criteria_cal = (cv.TERM_CRITERIA_EPS + cv.TermCriteria_MAX_ITER, 30, 1e-6) cdt_kp1_udist = cv.undistortPointsIter(cdt_kp1, new_kmat, dist, R=None, P=new_kmat, criteria=criteria_cal) cdt_kp2_udist = cv.undistortPointsIter(cdt_kp2, new_kmat, dist, R=None, P=new_kmat, criteria=criteria_cal) # It seems that printed cdts are reasonable # print(cdt_kp1_udist) # print(cdt_kp2_udist) # auxiliary function to draw undistorted key pts from coordinates i1_udist_temp = img_undistort(f_camera, i1)[1] i2_udist_temp = img_undistort(f_camera, i2)[1]