(edges, landmarks2D, R_XM, t_XM, p_XM, only_landmarks=False)
| 4 | import pycolmap.cost_functions |
| 5 | |
| 6 | def XM_Ceres_interface(edges, landmarks2D, R_XM, t_XM, p_XM, only_landmarks=False): |
| 7 | N = int(np.max(edges[:, 0])) |
| 8 | M = int(np.max(edges[:, 1])) |
| 9 | |
| 10 | cam = pycolmap.Camera( |
| 11 | model="SIMPLE_PINHOLE", |
| 12 | width=1, |
| 13 | height=1, |
| 14 | params=np.array([1.,0.,0.]), |
| 15 | # easy camera because we have already normalized |
| 16 | camera_id=0, |
| 17 | ) |
| 18 | |
| 19 | prob = pyceres.Problem() |
| 20 | loss = pyceres.TrivialLoss() |
| 21 | |
| 22 | # initial guess |
| 23 | quat = [] |
| 24 | t = [] |
| 25 | points = [] |
| 26 | for i in range(N): |
| 27 | # xyzw |
| 28 | Ri = pycolmap.Rotation3d(R_XM[:,3*i:3*i+3].T) |
| 29 | quat.append(Ri.quat) |
| 30 | t.append(-R_XM[:,3*i:3*i+3].T @ t_XM[:,i]) |
| 31 | |
| 32 | for i in range(M): |
| 33 | points.append(p_XM[:,i]) |
| 34 | |
| 35 | for i in range(N): |
| 36 | index = np.where(edges[:, 0] == i + 1)[0] |
| 37 | if len(index) == 0: |
| 38 | continue |
| 39 | p2d = landmarks2D[index,:] |
| 40 | p2d_index = edges[index,1] |
| 41 | for j in range(len(index)): |
| 42 | p = pycolmap.Point2D(p2d[j],p2d_index[j]-1) |
| 43 | cost = pycolmap.cost_functions.ReprojErrorCost(cam.model, p.xy) |
| 44 | params = [ |
| 45 | quat[i], |
| 46 | t[i], |
| 47 | points[p.point3D_id], |
| 48 | cam.params, |
| 49 | ] |
| 50 | prob.add_residual_block(cost, loss, params) |
| 51 | prob.set_manifold( |
| 52 | quat[i], pyceres.EigenQuaternionManifold() |
| 53 | ) |
| 54 | prob.set_parameter_block_constant(cam.params) |
| 55 | |
| 56 | if only_landmarks: |
| 57 | for i in range(N): |
| 58 | prob.set_parameter_block_constant(quat[i]) |
| 59 | print( |
| 60 | prob.num_parameter_blocks(), |
| 61 | prob.num_parameters(), |
| 62 | prob.num_residual_blocks(), |
| 63 | prob.num_residuals(), |
no test coverage detected