Loop closure optimizer for sequences of Sim3 transformations Input: - sequential_transforms: List[Tuple[float, np.ndarray, np.ndarray]] Each element is (s, R, t), where s is scalar scale, R is [3,3] rotation matrix, t is [3,] translation vector - loop_constraints: List[
| 32 | |
| 33 | |
| 34 | class Sim3LoopOptimizer: |
| 35 | """ |
| 36 | Loop closure optimizer for sequences of Sim3 transformations |
| 37 | |
| 38 | Input: |
| 39 | - sequential_transforms: List[Tuple[float, np.ndarray, np.ndarray]] |
| 40 | Each element is (s, R, t), where s is scalar scale, R is [3,3] rotation matrix, |
| 41 | t is [3,] translation vector |
| 42 | - loop_constraints: List[Tuple[int, int, Tuple[float, np.ndarray, np.ndarray]]] |
| 43 | Each element is (i, j, (s, R, t)), representing a loop closure constraint |
| 44 | from frame i to frame j |
| 45 | |
| 46 | Output: |
| 47 | - Optimized sequential_transforms |
| 48 | """ |
| 49 | |
| 50 | def __init__(self, config, device="cpu"): |
| 51 | self.device = device |
| 52 | self.config = config |
| 53 | self.solve_system_version = self.config["Loop"]["SIM3_Optimizer"][ |
| 54 | "lang_version" |
| 55 | ] # choose between 'python' and 'cpp' |
| 56 | |
| 57 | if not cpp_version: |
| 58 | self.solve_system_version = "python" |
| 59 | |
| 60 | def numpy_to_pypose_sim3(self, s: float, R_mat: np.ndarray, t_vec: np.ndarray) -> pp.Sim3: |
| 61 | """Convert numpy s,R,t to pypose Sim3""" |
| 62 | q = R.from_matrix(R_mat).as_quat() # [x,y,z,w] |
| 63 | # pypose requires [t, q, s] format |
| 64 | data = np.concatenate([t_vec, q, np.array([s])]) |
| 65 | return pp.Sim3(torch.from_numpy(data).float().to(self.device)) |
| 66 | |
| 67 | def pypose_sim3_to_numpy(self, sim3: pp.Sim3) -> Tuple[float, np.ndarray, np.ndarray]: |
| 68 | """Convert pypose Sim3 to numpy s,R,t""" |
| 69 | data = sim3.data.cpu().numpy() |
| 70 | t = data[:3] |
| 71 | q = data[3:7] # [x,y,z,w] |
| 72 | s = data[7] |
| 73 | R_mat = R.from_quat(q).as_matrix() |
| 74 | return s, R_mat, t |
| 75 | |
| 76 | def sequential_to_absolute_poses( |
| 77 | self, sequential_transforms: List[Tuple[float, np.ndarray, np.ndarray]] |
| 78 | ) -> torch.Tensor: |
| 79 | """ |
| 80 | Convert sequential relative transforms to absolute pose sequence |
| 81 | S_01, S_12, S_23, ... -> T_0, T_1, T_2, T_3, ... |
| 82 | Where T_i is the transform from world coordinate to frame i |
| 83 | """ |
| 84 | len(sequential_transforms) + 1 |
| 85 | poses = [] |
| 86 | |
| 87 | identity = pp.Sim3( |
| 88 | torch.tensor([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0], device=self.device) |
| 89 | ) |
| 90 | poses.append(identity) |
| 91 |
no outgoing calls
no test coverage detected