|
import numpy as np |
|
import torch |
|
|
|
|
|
def cart_to_hom(pts): |
|
""" |
|
:param pts: (N, 3 or 2) |
|
:return pts_hom: (N, 4 or 3) |
|
""" |
|
if isinstance(pts, np.ndarray): |
|
pts_hom = np.concatenate((pts, np.ones([*pts.shape[:-1], 1], dtype=np.float32)), -1) |
|
else: |
|
ones = torch.ones([*pts.shape[:-1], 1], dtype=torch.float32, device=pts.device) |
|
pts_hom = torch.cat((pts, ones), dim=-1) |
|
return pts_hom |
|
|
|
|
|
def hom_to_cart(pts): |
|
return pts[..., :-1] / pts[..., -1:] |
|
|
|
|
|
def canonical_to_camera(pts, pose): |
|
pts = cart_to_hom(pts) |
|
pts = pts @ pose.transpose(-1, -2) |
|
pts = hom_to_cart(pts) |
|
return pts |
|
|
|
|
|
def rect_to_img(K, pts_rect): |
|
from dl_ext.vision_ext.datasets.kitti.structures import Calibration |
|
pts_2d_hom = pts_rect @ K.t() |
|
pts_img = Calibration.hom_to_cart(pts_2d_hom) |
|
return pts_img |
|
|
|
|
|
def calc_pose(phis, thetas, size, radius=1.2): |
|
import torch |
|
def normalize(vectors): |
|
return vectors / (torch.norm(vectors, dim=-1, keepdim=True) + 1e-10) |
|
|
|
device = torch.device('cuda') |
|
thetas = torch.FloatTensor(thetas).to(device) |
|
phis = torch.FloatTensor(phis).to(device) |
|
|
|
centers = torch.stack([ |
|
radius * torch.sin(thetas) * torch.sin(phis), |
|
-radius * torch.cos(thetas) * torch.sin(phis), |
|
radius * torch.cos(phis), |
|
], dim=-1) |
|
|
|
|
|
forward_vector = normalize(centers).squeeze(0) |
|
up_vector = torch.FloatTensor([0, 0, 1]).to(device).unsqueeze(0).repeat(size, 1) |
|
right_vector = normalize(torch.cross(up_vector, forward_vector, dim=-1)) |
|
if right_vector.pow(2).sum() < 0.01: |
|
right_vector = torch.FloatTensor([0, 1, 0]).to(device).unsqueeze(0).repeat(size, 1) |
|
up_vector = normalize(torch.cross(forward_vector, right_vector, dim=-1)) |
|
|
|
poses = torch.eye(4, dtype=torch.float, device=device).unsqueeze(0).repeat(size, 1, 1) |
|
poses[:, :3, :3] = torch.stack((right_vector, up_vector, forward_vector), dim=-1) |
|
poses[:, :3, 3] = centers |
|
return poses |
|
|
