def skew_sym(x):
b = x.shape[:-1]
x, y, z = x.unbind(dim=-1)
o = torch.zeros_like(x)
return torch.stack([o, -z, y, z, o, -x, -y, x, o], dim=-1).view(*b, 3, 3)
# get distance in 3.3
def point_to_dist(X):
d = torch.linalg.norm(X, dim=-1, keepdim=True)
return d
# get ray distance in 3.3
def point_to_ray_dist(X, jacobian=False):
b = X.shape[:-1]
d = point_to_dist(X)
d_inv = 1.0 / d
r = d_inv * X
rd = torch.cat((r, d), dim=-1) # Dim 4
if not jacobian:
return rd
else:
d_inv_2 = d_inv**2
I = torch.eye(3, device=X.device, dtype=X.dtype).repeat(*b, 1, 1)
dr_dX = d_inv.unsqueeze(-1) * (
I - d_inv_2.unsqueeze(-1) * (X.unsqueeze(-1) @ X.unsqueeze(-2))
)
dd_dX = r.unsqueeze(-2)
drd_dX = torch.cat((dr_dX, dd_dX), dim=-2)
return rd, drd_dX
# from depth to W pointmap
def constrain_points_to_ray(img_size, Xs, K):
uv = get_pixel_coords(Xs.shape[0], img_size, device=Xs.device, dtype=Xs.dtype).view(
*Xs.shape[:-1], 2
)
Xs = backproject(uv, Xs[..., 2:3], K)
return Xs
def act_Sim3(X: lietorch.Sim3, pC: torch.Tensor, jacobian=False):
pW = X.act(pC)
if not jacobian:
return pW
dpC_dt = torch.eye(3, device=pW.device).repeat(*pW.shape[:-1], 1, 1)
dpC_dR = -skew_sym(pW)
dpc_ds = pW.reshape(*pW.shape[:-1], -1, 1)
return pW, torch.cat([dpC_dt, dpC_dR, dpc_ds], dim=-1) # view(-1, mdim)
def decompose_K(K):
fx = K[..., 0, 0]
fy = K[..., 1, 1]
cx = K[..., 0, 2]
cy = K[..., 1, 2]
return fx, fy, cx, cy 分析代码