|
| 1 | +import numpy as np |
| 2 | +import ransac |
| 3 | +#from scipy.spatial import KDTree |
| 4 | +from collections import defaultdict |
| 5 | +import models |
| 6 | + |
| 7 | +#NEIGHBORHOOD_RADIUS = 70 |
| 8 | +GRID_SIZE = 50 |
| 9 | + |
| 10 | +class FeaturesMatcher(object): |
| 11 | + |
| 12 | + def __init__(self, detector, **kwargs): |
| 13 | + self._detector = detector |
| 14 | + |
| 15 | + self._params = {} |
| 16 | + # get default values if no value is present in kwargs |
| 17 | + #self._params["num_filtered_percent"] = kwargs.get("num_filtered_percent", 0.25) |
| 18 | + #self._params["filter_rate_cutoff"] = kwargs.get("filter_rate_cutoff", 0.25) |
| 19 | + self._params["ROD_cutoff"] = kwargs.get("ROD_cutoff", 0.92) |
| 20 | + self._params["min_features_num"] = kwargs.get("min_features_num", 40) |
| 21 | + |
| 22 | + # Parameters for the RANSAC |
| 23 | + self._params["model_index"] = kwargs.get("model_index", 3) |
| 24 | + self._params["iterations"] = kwargs.get("iterations", 5000) |
| 25 | + self._params["max_epsilon"] = kwargs.get("max_epsilon", 30.0) |
| 26 | + self._params["min_inlier_ratio"] = kwargs.get("min_inlier_ratio", 0.01) |
| 27 | + self._params["min_num_inlier"] = kwargs.get("min_num_inliers", 7) |
| 28 | + self._params["max_trust"] = kwargs.get("max_trust", 3) |
| 29 | + self._params["det_delta"] = kwargs.get("det_delta", 0.9) |
| 30 | + self._params["max_stretch"] = kwargs.get("max_stretch", 0.25) |
| 31 | + |
| 32 | + self._params["use_regularizer"] = True if "use_regularizer" in kwargs.keys() else False |
| 33 | + self._params["regularizer_lambda"] = kwargs.get("regularizer_lambda", 0.1) |
| 34 | + self._params["regularizer_model_index"] = kwargs.get("regularizer_model_index", 1) |
| 35 | + |
| 36 | + |
| 37 | + def match(self, features_kps1, features_descs1, features_kps2, features_descs2): |
| 38 | + features_kps2 = np.asarray(features_kps2) |
| 39 | + |
| 40 | + # because the sections were already pre-aligned, we only match a feature in section1 to its neighborhood in section2 (according to a grid) |
| 41 | + grid = defaultdict(set) |
| 42 | + |
| 43 | + # build the grid of the feature locations in the second section |
| 44 | + for i, kp2 in enumerate(features_kps2): |
| 45 | + pt_grid = (np.array(kp2.pt) / GRID_SIZE).astype(np.int) |
| 46 | + grid[tuple(pt_grid)].add(i) |
| 47 | + |
| 48 | + match_points = [[], [], []] |
| 49 | + for kp1, desc1 in zip(features_kps1, features_descs1): |
| 50 | + # For each kp1 find the closest points in section2 |
| 51 | + pt_grid = (np.array(kp1.pt) / GRID_SIZE).astype(np.int) |
| 52 | + close_kps2_idxs = set() |
| 53 | + # search in a [-1, -1] -> [1, 1] delta windows (3*3) |
| 54 | + for delta_y in range(-1, 2): |
| 55 | + for delta_x in range(-1, 2): |
| 56 | + delta = np.array([delta_x, delta_y], dtype=np.int) |
| 57 | + delta_grid_loc = tuple(pt_grid + delta) |
| 58 | + if delta_grid_loc in grid.keys(): |
| 59 | + close_kps2_idxs |= grid[delta_grid_loc] |
| 60 | + |
| 61 | + close_kps2_indices = list(close_kps2_idxs) |
| 62 | + close_descs2 = features_descs2[close_kps2_indices] |
| 63 | + matches = self._detector.match(desc1.reshape(1, len(desc1)), close_descs2) |
| 64 | + if len(matches[0]) == 2: |
| 65 | + if matches[0][0].distance < self._params["ROD_cutoff"] * matches[0][1].distance: |
| 66 | + match_points[0].append(kp1.pt) |
| 67 | + match_points[1].append(features_kps2[close_kps2_indices][matches[0][0].trainIdx].pt) |
| 68 | + match_points[2].append(matches[0][0].distance) |
| 69 | + |
| 70 | + |
| 71 | + |
| 72 | + |
| 73 | +# # because the sections were already pre-aligned, we only match a feature in section1 to its neighborhood in section2 |
| 74 | +# features_kps2_pts = [kp.pt for kp in features_kps2] |
| 75 | +# kps2_pts_tree = KDTree(features_kps2_pts) |
| 76 | +# |
| 77 | +# match_points = [[], [], []] |
| 78 | +# for kp1, desc1 in zip(features_kps1, features_descs1): |
| 79 | +# # For each kp1 find the closest points in section2 |
| 80 | +# close_kps2_indices = kps2_pts_tree.query_ball_point(kp1.pt, NEIGHBORHOOD_RADIUS) |
| 81 | +# close_descs2 = features_descs2[close_kps2_indices] |
| 82 | +# matches = self._detector.match(desc1.reshape(1, len(desc1)), close_descs2) |
| 83 | +# if len(matches[0]) == 2: |
| 84 | +# if matches[0][0].distance < self._params["ROD_cutoff"] * matches[0][1].distance: |
| 85 | +# match_points[0].append(kp1.pt) |
| 86 | +# match_points[1].append(features_kps2[close_kps2_indices][matches[0][0].trainIdx].pt) |
| 87 | +# match_points[2].append(matches[0][0].distance) |
| 88 | + |
| 89 | + match_points = (np.array(match_points[0]), np.array(match_points[1]), np.array(match_points[2])) |
| 90 | + |
| 91 | + |
| 92 | +# matches = self._detector.match(features_descs1, features_descs2) |
| 93 | +# |
| 94 | +# good_matches = [] |
| 95 | +# for m, n in matches: |
| 96 | +# #if (n.distance == 0 and m.distance == 0) or (m.distance / n.distance < actual_params["ROD_cutoff"]): |
| 97 | +# if m.distance < self._params["ROD_cutoff"] * n.distance: |
| 98 | +# good_matches.append(m) |
| 99 | +# |
| 100 | +# match_points = ( |
| 101 | +# np.array([features_kps1[m.queryIdx].pt for m in good_matches]), |
| 102 | +# np.array([features_kps2[m.trainIdx].pt for m in good_matches]), |
| 103 | +# np.array([m.distance for m in good_matches]) |
| 104 | +# ) |
| 105 | + |
| 106 | + return match_points |
| 107 | + |
| 108 | + def match_and_filter(self, features_kps1, features_descs1, features_kps2, features_descs2): |
| 109 | + match_points = self.match(features_kps1, features_descs1, features_kps2, features_descs2) |
| 110 | + |
| 111 | + model, filtered_matches = ransac.filter_matches(match_points, match_points, self._params['model_index'], |
| 112 | + self._params['iterations'], self._params['max_epsilon'], self._params['min_inlier_ratio'], |
| 113 | + self._params['min_num_inlier'], self._params['max_trust'], self._params['det_delta'], self._params['max_stretch']) |
| 114 | + |
| 115 | + if model is None: |
| 116 | + return None, None |
| 117 | + |
| 118 | + if self._params["use_regularizer"]: |
| 119 | + regularizer_model, _ = ransac.filter_matches(match_points, match_points, self._params['regularizer_model_index'], |
| 120 | + self._params['iterations'], self._params['max_epsilon'], self._params['min_inlier_ratio'], |
| 121 | + self._params['min_num_inlier'], self._params['max_trust'], self._params['det_delta'], self._params['max_stretch']) |
| 122 | + |
| 123 | + if regularizer_model is None: |
| 124 | + return None, None |
| 125 | + |
| 126 | + result = model.get_matrix() * (1 - self._params["regularizer_lambda"]) + regularizer_model.get_matrix() * self._params["regularizer_lambda"] |
| 127 | + model = models.AffineModel(result) |
| 128 | + |
| 129 | + return model, filtered_matches |
| 130 | + |
0 commit comments