import torch import numpy as np from anydata.geometry.cameras.base import CameraBase from anydata.utils.data import pixel_grid, interpolate_image def polyval(P, x): if type(x) == torch.Tensor: P = torch.Tensor(P).to(x.device) if type(P) == torch.Tensor: npol = P.shape[1] val = torch.zeros_like(x) for i in range(npol - 1): val = val * x + P[:, [i]] * x val += P[:, [npol - 1]] return val else: return np.polyval(P, x) class CameraOmniMMT(CameraBase): """Omnidirectional camera using Scaramuzza polynomial model (OmniMMT variant). K: [B,25] raw params (poly, inv_poly, center, affine, max_fov).""" def __init__(self, K, hw, *args, **kwargs): if isinstance(hw, torch.Tensor): hw = hw.shape[-2:] self._K = torch.tensor([ [hw[1] / 2, 0.0, hw[1] / 2], [0.0, hw[0] / 2, hw[0] / 2], [0.0, 0.0, 1.0], ], device=K.device, dtype=K.dtype).unsqueeze(0).repeat(K.shape[0], 1, 1) super().__init__(hw, *args, **kwargs) self.geometry = 'omni' self.Komni = K.clone() self.poly = torch.flip(K[:, 1:7], [1]) self.inv_poly = torch.flip(K[:, 8:19], [1]) self.xc, self.yc = K[:, [19]], K[:, [20]] self.c, self.d, self.e = K[:, [21]], K[:, [22]], K[:, [23]] self.max_theta = torch.deg2rad(K[:, [24]]) / 2.0 self.xc = self.xc.unsqueeze(1) self.yc = self.yc.unsqueeze(1) self.full_hw = (512, 512) # NOTE(bvh): override CameraBase methods since we must pass Komni, not synthetic _K def __getitem__(self, idx): if isinstance(idx, int): idx = slice(idx, idx + 1) return type(self)(K=self.Komni[idx], hw=self._hw, Tcw=self._Tcw[idx]) def clone(self): return type(self)(K=self.Komni.clone(), hw=self._hw, Tcw=self._Tcw.clone() if self._Tcw is not None else None) @staticmethod def from_list(cams): K = torch.cat([c.Komni for c in cams], 0) Tcw = torch.cat([c.Tcw for c in cams], 0) return type(cams[0])(K=K, hw=cams[0].hw, Tcw=Tcw) def no_translation(self): Tcw = self._Tcw.clone() Tcw[:, :3, -1] = 0 return type(self)(K=self.Komni, hw=self._hw, Tcw=Tcw) def pixelToRay(self, grid): """Convert pixel coordinates to ray directions using Scaramuzza polynomial. grid: [B,2or3,N]. Returns [B,3,N] (not normalized).""" b, c, hw = grid.shape if c == 3: grid = grid[:, :2] x = grid[:, 1, :].reshape((b, 1, -1)) - self.xc y = grid[:, 0, :].reshape((b, 1, -1)) - self.yc grid = torch.cat((x, y), axis=1) invdet = 1.0 / (self.c - self.d * self.e) # A_inv = invdet * torch.tensor([[ # [ 1, -self.d], # [-self.e, self.c]]], device=grid.device, dtype=grid.dtype) A_inv = torch.stack([invdet[i] * torch.tensor([[ [ 1, -self.d[i]], [-self.e[i], self.c[i]]]], device=grid.device, dtype=grid.dtype) for i in range(b)], 0).squeeze(1) grid = torch.bmm(A_inv, grid) x = grid[:, 1, :].reshape((b, 1, -1)) y = grid[:, 0, :].reshape((b, 1, -1)) rho = torch.sqrt(x * x + y * y).squeeze(1) z = polyval(self.poly, rho).reshape((b, 1, -1)) theta = torch.atan2(rho, -z) rays = torch.cat((x, y, -z), axis=1) return rays # # P can be torch.Tensor # def rayToPixel(self, P, out_theta=False, max_theta=None): # if max_theta is None: max_theta = self.max_theta # norm = sqrt(P[0,:]**2 + P[1,:]**2) + EPS # theta = atan2(-P[2,:], norm) # rho = polyval(self.inv_poly, theta) # # max_theta check : theta is the angle from xy-plane in ocam, # # thus add pi/2 to compute the angle from the optical axis. # theta = theta + np.pi / 2 # # flip axis # x = P[1,:] / norm * rho # y = P[0,:] / norm * rho # x2 = x * self.c + y * self.d + self.xc # y2 = x * self.e + y + self.yc # x2 = x2.reshape((1, -1)) # y2 = y2.reshape((1, -1)) # out = concat((y2, x2), axis=0) # out[:,theta.squeeze() > max_theta] = -1.0 # if out_theta: # return out, theta # else: # return out def lift(self, grid=None, depth=None): """Lift using Scaramuzza omni projection. Computes rays at full_hw, resizes if needed. grid: [B,3,N], depth: [B,1,N]. Returns [B,3,N].""" b = depth.shape[0] resize = depth.shape[-1] != self.full_hw[0] * self.full_hw[1] if resize: grid = pixel_grid(self.full_hw, b=b, with_ones=True, device=self.device).view(b, 3, -1) rays = self.pixelToRay(grid) rays[torch.isnan(rays)] = 0.0 rays = torch.nn.functional.normalize(rays, dim=1, p=2) if resize: rays = rays.view(b, 3, *self.full_hw) rays = interpolate_image(rays, self.hw) rays = rays.view(b, 3, -1) rays = torch.nn.functional.normalize(rays, dim=1, p=2) xyz = rays * depth return xyz.float() def reconstruct_euclidean(self, depth, to_world=False, grid=None, scene_flow=None, world_scene_flow=None): if depth is None: return None b, _, h, w = depth.shape points = self.origin + self.rays * depth.view(depth.shape[0], 1, -1) return points.view(b, 3, h, w) # def get_center(self): # return self.Tcw.T[:, :3, -1] # def get_origin(self, flatten=False): # orig = self.get_center().view(len(self), 3, 1, 1).repeat(1, 1, *self.hw) # if flatten: # orig = orig.reshape(len(self), 3, -1).permute(0, 2, 1) # return orig def get_plucker(self, flatten=False, with_orig=True): """Plucker ray coordinates. Returns [B,6,H,W] (or [B,9,H,W] with_orig), [B,HW,6/9] if flatten.""" b = len(self) ones = torch.ones((b, 1, *self.hw), dtype=self.dtype, device=self.device) rays = self.no_translation().reconstruct_depth_map(ones, to_world=True) rays = rays / torch.norm(rays, dim=1).unsqueeze(1) orig = self.get_center().view(b, 3, 1, 1).repeat(1, 1, *self.hw) orig = orig.view(b, 3, -1).permute(0, 2, 1) rays = rays.view(b, 3, -1).permute(0, 2, 1) cross = torch.cross(orig, rays, dim=-1) if with_orig: plucker = torch.cat((rays, cross, orig), dim=-1) else: plucker = torch.cat((rays, cross), dim=-1) # NOTE(bvh): When you scale the translation component self.pose[:, 0:3, 3] by a scalar s, # then cross and orig are also scaled by s, but rays stay the same. if not flatten: plucker = plucker.permute(0, 2, 1).reshape(b, -1, *self.hw) return plucker # import torch # import numpy as np # from easydict import EasyDict as Edict # from omnimvs.utils.geometry import applyTransform, inverseTransform # from vidar.geometry.cameras.base import CameraBase # def polyval(P, x): # if type(x) == torch.Tensor: P = torch.Tensor(P).to(x.device) # if type(P) == torch.Tensor: # npol = P.shape[1] # val = torch.zeros_like(x) # for i in range(npol-1): # val = val * x + P[:, i] * x # val += P[:, -1] # return val # else: # return np.polyval(P, x) # class CameraOmni(CameraBase): # def __init__(self, K, *args, **kwargs): # self.height ,self.width = (1024, 1024) # self._K = torch.tensor([ # [self.width / 2, 0.0, self.width / 2], # [self.height / 2, 0.0, self.height / 2], # [0.0, 0.0, 1.0], # ]).unsqueeze(0) # super().__init__(*args, **kwargs) # # self.poly = K[:, 1:6] # # self.inv_poly = K[:, 7:17] # # self.xc, self.yc = K[:, [17]], K[:, [18]] # # self.c, self.d, self.e = K[:, [19]], K[:, [20]], K[:, [21]] # # self.max_theta = torch.deg2rad(K[:, [22]]) / 2.0 # conf = Edict(K) # camera_config = conf.cameras # cfg = camera_config[0] # self.poly = cfg['poly'][-1:0:-1] # self.inv_poly = cfg['inv_poly'][-1:0:-1] # self.xc, self.yc = cfg['center'] # self.c, self.d, self.e = cfg['affine'] # self.max_theta = np.deg2rad(cfg['max_fov']) / 2.0 # self.poly = torch.tensor(self.poly).unsqueeze(0).float() # self.inv_poly = torch.tensor(self.inv_poly).unsqueeze(0).float() # self.xc = torch.tensor(self.xc).unsqueeze(0).float() # self.yc = torch.tensor(self.yc).unsqueeze(0).float() # self.c = torch.tensor(self.c).unsqueeze(0).float() # self.d = torch.tensor(self.d).unsqueeze(0).float() # self.e = torch.tensor(self.e).unsqueeze(0).float() # self.max_theta = torch.tensor(self.max_theta).unsqueeze(0).float() # print() # print('!!!!!!!!!!!!!!!!!!!!!!') # print(self.poly) # print(self.inv_poly) # print(self.xc, self.yc) # print(self.c, self.d, self.e) # print(self.max_theta) # print('!!!!!!!!!!!!!!!!!!!!!!') # print() # self.cam2rig = np.array(cfg['pose']).reshape((6, 1)) # self.rig2cam = inverseTransform(self.cam2rig) # from scipy.spatial.transform import Rotation as R # vec = torch.tensor(self.cam2rig).unsqueeze(0)[..., 0] # rot = torch.tensor(R.from_rotvec(vec[:, :3]).as_matrix()) # trans = vec[:, 3:].unsqueeze(-1) # T = torch.cat([rot, trans], -1) # T = torch.cat([T, torch.tensor([[[0.0, 0.0, 0.0, 1.0]]])], 1) # # self.height, self.width = cfg['image_size'] # # p can be torch.Tensor # def pixelToRay2(self, p, out_theta=False, max_theta=None): # if max_theta is None: max_theta = self.max_theta # # flip axis # x = p[1,:].reshape((1, -1)) - self.xc # y = p[0,:].reshape((1, -1)) - self.yc # p = concat((x, y), axis=0) # invdet = 1.0 / (self.c - self.d * self.e) # A_inv = invdet * np.array([ # [ 1, -self.d], # [-self.e, self.c]]) # p = A_inv.dot(p) # # flip axis # x = p[1,:].reshape((1, -1)) # y = p[0,:].reshape((1, -1)) # rho = sqrt(x * x + y * y) # z = polyval(self.poly, rho).reshape((1, -1)) # # theta is angle from the optical axis. # theta = atan2(rho, -z) # out = concat((x, y, -z), axis=0) # out[:,theta.squeeze() > max_theta] = np.nan # if out_theta: # return out, theta # else: # return out # # end pixelToRay # # p can be torch.Tensor # def pixelToRay(self, p, out_theta=False, max_theta=None): # if max_theta is None: max_theta = self.max_theta # b, _, h, w = p.shape # p = torch.tensor(p).view(b, 2, h * w) # # flip axis # x = p[:, 1, :].reshape((b, 1, -1)) - self.xc # y = p[:, 0, :].reshape((b, 1, -1)) - self.yc # p = torch.cat((x, y), axis=1) # invdet = 1.0 / (self.c - self.d * self.e) # A_inv = invdet * torch.tensor([[ # [ 1, -self.d], # [-self.e, self.c]]]) # print('GGGGGGGGGGGGGG2', p.shape, p.mean()) # p = torch.bmm(A_inv, p) # print('GGGGGGGGGGGGGG2', p.shape, p.mean()) # # flip axis # x = p[:, 1, :].reshape((b, 1, -1)) # y = p[:, 0, :].reshape((b, 1, -1)) # print('GGGGGGGGGGGGGG2', x.shape, x.mean(), y.shape, y.mean()) # rho = torch.sqrt(x * x + y * y) # print('!!!!', self.poly) # z = polyval(self.poly, rho).reshape((b, 1, -1)) # print('GGGGGGGGGGGGGG2', rho.shape, rho.mean(), z.shape, z.mean()) # # theta is angle from the optical axis. # theta = torch.atan2(rho, -z) # print('GGGGGGGGGGGGGGtheta', theta.shape, theta.mean()) # out = torch.cat((x, y, -z), axis=1) # out[:, :, theta.squeeze() > max_theta] = np.nan # print('GGGGGGGGGGGGGG2', out.shape, out.mean()) # out = out.view(b, -1, h, w) # return out # # P can be torch.Tensor # def rayToPixel(self, P, out_theta=False, max_theta=None): # if max_theta is None: max_theta = self.max_theta # norm = sqrt(P[0,:]**2 + P[1,:]**2) + EPS # theta = atan2(-P[2,:], norm) # rho = polyval(self.inv_poly, theta) # # max_theta check : theta is the angle from xy-plane in ocam, # # thus add pi/2 to compute the angle from the optical axis. # theta = theta + np.pi / 2 # # flip axis # x = P[1,:] / norm * rho # y = P[0,:] / norm * rho # x2 = x * self.c + y * self.d + self.xc # y2 = x * self.e + y + self.yc # x2 = x2.reshape((1, -1)) # y2 = y2.reshape((1, -1)) # out = concat((y2, x2), axis=0) # out[:,theta.squeeze() > max_theta] = -1.0 # if out_theta: # return out, theta # else: # return out # # end rayToPixel # @property # def device(self): # return 'cpu' # @staticmethod # def from_list(cams): # K = torch.cat([cam.K for cam in cams], 0) # Twc = torch.cat([cam.Twc.T for cam in cams], 0) # return CameraRays(K=K, Twc=Twc, hw=cams[0].hw) # @staticmethod # def from_dict(K, hw, Twc=None, broken=False): # if broken: # return {key: CameraRays( # K=K[key] if key in K else K[(0, key[1])], # hw=hw[key], Twc=val # ) for key, val in Twc.items()} # else: # return {key: CameraRays( # K=K[key] if key in K else K[0], # hw=hw[key], Twc=val # ) for key, val in Twc.items()} # def lift(self, grid=None, depth=None): # width, height = (1024, 1024) # xs, ys = np.meshgrid(range(width), range(height)) # p = np.concatenate((xs.reshape((1, -1)), ys.reshape((1, -1))), axis=0) # p = torch.tensor(p).unsqueeze(0).view(1, 2, height, width).float() # print('rays2a', p.shape, p.mean()) # rays = self.pixelToRay(p) # print('rays2b', rays.shape, rays.mean()) # rays = torch.tensor(rays) # print('rays2c', rays.shape, rays.mean()) # rays[torch.isnan(rays)] = 0.0 # print('rays2d', rays.shape, rays.mean()) # rays = torch.nn.functional.normalize(rays, dim=1, p=2) # print('rays2e', rays.shape, rays.mean()) # rays = rays.float().view(1, 3, -1) # print('rays2f', rays.shape, rays.mean()) # xyz = rays * depth # # xyz = applyTransform(self.cam2rig, xyz) # xyz = xyz.view(1, 3, height * width) # return xyz # def reconstruct_euclidean(self, depth, to_world=False, grid=None, scene_flow=None, world_scene_flow=None): # if depth is None: # return None # b, _, h, w = depth.shape # points = self.origin + self.rays * depth.view(depth.shape[0], 1, -1) # return points.view(b, 3, h, w)