Align point clouds of two overlapping chunks: point_map2 → point_map1 coordinate frame. Args: point_map1: (B, H, W, 3) world-coordinate points of the overlapping region in the first chunk conf1: (B, H, W) confidence point_map2: (B, H, W, 3) world-coordinate points of the
(point_map1, conf1, point_map2, conf2,
conf_threshold=None, use_weighted=True)
| 80 | |
| 81 | |
| 82 | def align_overlapping_chunks(point_map1, conf1, point_map2, conf2, |
| 83 | conf_threshold=None, use_weighted=True): |
| 84 | """Align point clouds of two overlapping chunks: point_map2 → point_map1 coordinate frame. |
| 85 | |
| 86 | Args: |
| 87 | point_map1: (B, H, W, 3) world-coordinate points of the overlapping region in the first chunk |
| 88 | conf1: (B, H, W) confidence |
| 89 | point_map2: (B, H, W, 3) world-coordinate points of the overlapping region in the second chunk |
| 90 | conf2: (B, H, W) confidence |
| 91 | conf_threshold: confidence threshold; computed automatically when None |
| 92 | use_weighted: whether to use weighted SIM3 |
| 93 | |
| 94 | Returns: |
| 95 | s, R, t: SIM(3) transform parameters |
| 96 | """ |
| 97 | b = min(point_map1.shape[0], point_map2.shape[0]) |
| 98 | |
| 99 | if conf_threshold is None: |
| 100 | conf_threshold = min(np.median(conf1), np.median(conf2)) * 0.1 |
| 101 | |
| 102 | aligned_pts1 = [] |
| 103 | aligned_pts2 = [] |
| 104 | all_weights = [] |
| 105 | |
| 106 | for i in range(b): |
| 107 | mask1 = conf1[i] > conf_threshold |
| 108 | mask2 = conf2[i] > conf_threshold |
| 109 | valid_mask = mask1 & mask2 |
| 110 | |
| 111 | # Exclude NaN/Inf points |
| 112 | valid_mask = valid_mask & np.isfinite(point_map1[i]).all(axis=-1) |
| 113 | valid_mask = valid_mask & np.isfinite(point_map2[i]).all(axis=-1) |
| 114 | |
| 115 | idx = np.where(valid_mask) |
| 116 | if len(idx[0]) == 0: |
| 117 | continue |
| 118 | |
| 119 | pts1 = point_map1[i][idx] |
| 120 | pts2 = point_map2[i][idx] |
| 121 | |
| 122 | aligned_pts1.append(pts1) |
| 123 | aligned_pts2.append(pts2) |
| 124 | |
| 125 | if use_weighted: |
| 126 | combined_conf = np.sqrt(conf1[i][idx] * conf2[i][idx]) |
| 127 | all_weights.append(combined_conf) |
| 128 | |
| 129 | if len(aligned_pts1) == 0: |
| 130 | print("[WARNING] No matching point pairs found, using identity transform") |
| 131 | return 1.0, np.eye(3), np.zeros(3) |
| 132 | |
| 133 | all_pts1 = np.concatenate(aligned_pts1, axis=0) |
| 134 | all_pts2 = np.concatenate(aligned_pts2, axis=0) |
| 135 | |
| 136 | print(f" [SIM3] {all_pts1.shape[0]} corresponding points") |
| 137 | |
| 138 | if use_weighted and all_weights: |
| 139 | weights = np.concatenate(all_weights, axis=0) |
no test coverage detected