def encode(self, ex_rois, gt_rois): #输入的是ROI坐标,x1y1 x2y2 angle五个参数
ex_widths = ex_rois[:, 2] - ex_rois[:, 0] #计算宽度
ex_heights = ex_rois[:, 3] - ex_rois[:, 1] #计算高度
ex_widths = torch.clamp(ex_widths, min=1) #宽度最小为1
ex_heights = torch.clamp(ex_heights, min=1) #高度最小为1
ex_ctr_x = ex_rois[:, 0] + 0.5 * ex_widths #计算中心坐标xy
ex_ctr_y = ex_rois[:, 1] + 0.5 * ex_heights
ex_thetas = ex_rois[:, 4] #角度
gt_widths = gt_rois[:, 2] - gt_rois[:, 0]
gt_heights = gt_rois[:, 3] - gt_rois[:, 1]
gt_widths = torch.clamp(gt_widths, min=1)
gt_heights = torch.clamp(gt_heights, min=1) #获得gt宽和高
gt_ctr_x = gt_rois[:, 0] + 0.5 * gt_widths #获得gt的中心坐标
gt_ctr_y = gt_rois[:, 1] + 0.5 * gt_heights
gt_thetas = gt_rois[:, 4] #获得gt的角度
wx, wy, ww, wh, wt = self.weights
targets_dx = wx * (gt_ctr_x - ex_ctr_x) / ex_widths
targets_dy = wy * (gt_ctr_y - ex_ctr_y) / ex_heights
targets_dw = ww * torch.log(gt_widths / ex_widths)
targets_dh = wh * torch.log(gt_heights / ex_heights)
targets_dt = wt * (torch.tan(gt_thetas / 180.0 * np.pi) -
torch.tan(ex_thetas / 180.0 * np.pi))
targets = torch.stack(
(targets_dx, targets_dy, targets_dw, targets_dh, targets_dt),
dim=1)
return targets #计算偏移
def get_pitch_est(pitch_bins_low_device_batch, pitch_bins_mid_device_batch, pitch_bins_high_device_batch, output_pitch, batchsize, H_batch, vfov_estim, f_estim):
# fixPitchMidOnly
# all_pitch_bins_centers = torch.cat((
# torch.from_numpy(pitch_bins_low).float().to(device),
# # 2*torch.tan(vfov_estim[idx]/2)*(torch.arange(0, 193).float().to(device)/192 - 0.5),
# torch.arange(0, 193).float().to(device)/192,
# torch.from_numpy(pitch_bins_high).float().to(device),
# torch.Tensor([np.pi/6]).float().to(device)))
# return all_pitch_bins_centers
# pitch_bins_low_device_batch = pitch_bins_low_device.unsqueeze(0).repeat(batchsize, 1)
# pitch_bins_mid_device_batch = pitch_bins_mid_device.unsqueeze(0).repeat(batchsize, 1)
# pitch_bins_high_device_batch = pitch_bins_high_device.unsqueeze(0).repeat(batchsize, 1)
# print(H_batch.shape, f_estim.shape)
half_fov_batch = (vfov_estim / 2.).unsqueeze(-1)
pitch_bins_low_device_batch_v0 = 0.5 - torch.tan(- pitch_bins_low_device_batch) * f_estim.unsqueeze(-1) / H_batch.unsqueeze(-1)
pitch_bins_high_device_batch_v0 = 0.5 + torch.tan(pitch_bins_high_device_batch) * f_estim.unsqueeze(-1) / H_batch.unsqueeze(-1)
pitch_bins_mid_device_batch_v0 = pitch_bins_mid_device_batch
# expectation_pitch_bins_low_device_batch_v0 = pitch_bins_low_device_batch_v0 * output_pitch[:, :pitch_bins_low_device_batch.shape[0]]
low_dims = pitch_bins_low_device_batch.shape[1] # 31
mid_dims = pitch_bins_mid_device_batch.shape[1]
high_dims = pitch_bins_high_device_batch.shape[1] # 32
# print(output_pitch[:, :low_dims].shape, output_pitch[:, -high_dims:].shape, output_pitch[:, low_dims:(low_dims+mid_dims)].shape)
expectation_pitch_bins_low_device_batch_v0 = (torch.exp(nn.functional.log_softmax(output_pitch[:, :low_dims], dim=1))*pitch_bins_low_device_batch_v0).sum(dim=1)
expectation_pitch_bins_mid_device_batch_v0 = (torch.exp(nn.functional.log_softmax(output_pitch[:, low_dims:(low_dims+mid_dims)], dim=1))*pitch_bins_mid_device_batch).sum(dim=1)
expectation_pitch_bins_high_device_batch_v0 = (torch.exp(nn.functional.log_softmax(output_pitch[:, -high_dims:], dim=1))*pitch_bins_high_device_batch_v0).sum(dim=1)
# print(pitch_bins_low_device_batch_v0.detach().cpu().numpy())
# print(pitch_bins_mid_device_batch.detach().cpu().numpy())
# print(pitch_bins_high_device_batch_v0.detach().cpu().numpy())
# expectation_batch_v0 = (torch.argmax(output_pitch, dim=1) < 31).float() * expectation_pitch_bins_low_device_batch_v0 + \
# (torch.argmax(output_pitch, dim=1) >=224).float() * expectation_pitch_bins_high_device_batch_v0 + \
# ((torch.argmax(output_pitch, dim=1) >= 31).float() * (torch.argmax(output_pitch, dim=1) < 224).float()) * expectation_pitch_bins_mid_device_batch_v0
expectation_batch_v0 = expectation_pitch_bins_mid_device_batch_v0
return expectation_batch_v0
def perspectivey_grid(output_size,
ulim=(1, 8),
vlim=(-0.99 * np.pi / 2, 0.99 * np.pi / 2),
out=None,
device=None):
"""Vertical perspective coordinate system.
Args:
output_size: (int, int), number of sampled values for the v-coordinate and u-coordinate respectively
ulim: (float, float), y^{-1} "radial" coordinate limits
vlim: (float, float), angular coordinate limits
out: torch.FloatTensor, output tensor
device: string or torch.device, device for torch.tensor
Returns:
torch.FloatTensor, shape (output_size[0], output_size[1], 2), tensor where entry (i,j) gives the
(x, y)-coordinates of the grid point.
"""
nv, nu = output_size
urange = torch.linspace(ulim[0], ulim[1], nu, device=device)
vrange = torch.linspace(vlim[0], vlim[1], nv // 2, device=device)
vs, us = torch.meshgrid([vrange, urange])
yl = -1 / us.flip([1])
yr = 1 / us
xl = -yl * torch.tan(vs)
xr = yr * torch.tan(vs)
if nv % 2 == 0:
xs = torch.cat([xl, xr])
ys = torch.cat([yl, yr])
else:
xs = torch.cat([xl, xl.narrow(0, xl.shape[0] - 1, 1), xr])
ys = torch.cat([yl, yl.narrow(0, yl.shape[0] - 1, 1), yr])
return torch.stack([xs, ys], 2, out=out)
def get_affine_matrix2d(translations: torch.Tensor, center: torch.Tensor, scale: torch.Tensor, angle: torch.Tensor,
sx: Optional[torch.Tensor] = None, sy: Optional[torch.Tensor] = None) -> torch.Tensor:
r"""Composes affine matrix Bx3x3 from the components
Returns:
torch.Tensor: params to be passed to the affine transformation.
"""
transform: torch.Tensor = get_rotation_matrix2d(center, -angle, scale)
transform[..., 2] += translations # tx/ty
# pad transform to get Bx3x3
transform_h = convert_affinematrix_to_homography(transform)
if sx is not None:
x, y = torch.split(center, 1, dim=-1)
x = x.view(-1)
y = y.view(-1)
sx_tan = torch.tan(sx) # type: ignore
sy_tan = torch.tan(sy) # type: ignore
zeros = torch.zeros_like(sx) # type: ignore
ones = torch.ones_like(sx) # type: ignore
shear_mat = torch.stack([ones, -sx_tan, sx_tan * x, # type: ignore # noqa: E241
-sy_tan, ones + sx_tan * sy_tan, sy_tan * (-sx_tan * x + y)], # noqa: E241
dim=-1).view(-1, 2, 3)
shear_mat = convert_affinematrix_to_homography(shear_mat)
transform_h = transform_h @ shear_mat
return transform_h
def get_shear_matrix2d(center, sx=None, sy=None):
"""
Composes shear matrix Bx4x4 from the components.
"""
sx = torch.tensor([0.]).repeat(center.size(0)) if sx is None else sx
sy = torch.tensor([0.]).repeat(center.size(0)) if sy is None else sy
x, y = torch.split(center, 1, dim=-1)
x, y = x.view(-1), y.view(-1)
sx_tan = torch.tan(sx)
sy_tan = torch.tan(sy)
ones = torch.ones_like(sx)
shear_mat = torch.stack([
ones, -sx_tan, sx_tan * y, -sy_tan, ones + sx_tan * sy_tan, sy_tan *
(sx_tan * y + x)
],
dim=-1).view(-1, 2, 3)
shear_mat = convert_affinematrix_to_homography(shear_mat)
return shear_mat
def estimate_3d_position(self, x, eps=1e-7):
"""Estimate 3D position
"""
# 1. depth prediction
cent_depth = torch.sigmoid(self.predict_depths(x)) # [B,1,M]
cent_depth = self.alpha * cent_depth + self.beta # Rescale for each dataset
# 2. angle prediction
cent_angle = self.predict_angles(x) # [B,2,M]
# - angle_theta: range=[-pi/2,pi/2], polar angle in spherical coords
cent_angle_theta = torch.tanh(cent_angle[:, :1, :]).clamp(min=-1 + eps,
max=1 - eps)
cent_angle_theta = cent_angle_theta * math.pi / 2.0 # [B,1,M]
# - angle_phi: [0,2pi], azimuthal angle in spherical coords
cent_angle_phi = cent_angle[:, 1:, :] # [B,1,M]
# 3. Calculate plane equation params from centroid's depth & angle
face_alpha = -torch.mul(torch.tan(cent_angle_theta),
torch.cos(cent_angle_phi)) # [B,1,M]
face_beta = -torch.mul(torch.tan(cent_angle_theta),
torch.sin(cent_angle_phi)) # [B,1,M]
face_depth_params = torch.cat((face_alpha, face_beta, cent_depth),
1) # [B,3,M]
face_depth_params = face_depth_params.transpose(1, 2) # [B,M,3]
return face_depth_params
def encode(self, ex_rois, gt_rois):
ex_widths = ex_rois[:, 2] - ex_rois[:, 0]
ex_heights = ex_rois[:, 3] - ex_rois[:, 1]
ex_widths = torch.clamp(ex_widths, min=1)
ex_heights = torch.clamp(ex_heights, min=1)
ex_ctr_x = ex_rois[:, 0] + 0.5 * ex_widths
ex_ctr_y = ex_rois[:, 1] + 0.5 * ex_heights
ex_thetas = ex_rois[:, 4]
gt_widths = gt_rois[:, 2] - gt_rois[:, 0]
gt_heights = gt_rois[:, 3] - gt_rois[:, 1]
gt_widths = torch.clamp(gt_widths, min=1)
gt_heights = torch.clamp(gt_heights, min=1)
gt_ctr_x = gt_rois[:, 0] + 0.5 * gt_widths
gt_ctr_y = gt_rois[:, 1] + 0.5 * gt_heights
gt_thetas = gt_rois[:, 4]
wx, wy, ww, wh, wt = self.weights
targets_dx = wx * (gt_ctr_x - ex_ctr_x) / ex_widths
targets_dy = wy * (gt_ctr_y - ex_ctr_y) / ex_heights
targets_dw = ww * torch.log(gt_widths / ex_widths)
targets_dh = wh * torch.log(gt_heights / ex_heights)
targets_dt = wt * (torch.tan(gt_thetas / 180.0 * np.pi) -
torch.tan(ex_thetas / 180.0 * np.pi))
targets = torch.stack(
(targets_dx, targets_dy, targets_dw, targets_dh, targets_dt),
dim=1)
return targets
def _homography_floor_svd(self,
top_corners: torch.Tensor, # in [-1, 1]
bottom_corners: torch.Tensor, # in [-1, 1]
floor_z: float=-1.6,
):
b, N, _ = top_corners.size()
u = bottom_corners[:, :, 0] * np.pi
v = bottom_corners[:, :, 1] * (-0.5 * np.pi)
c = floor_z / torch.tan(v)
x = c * torch.sin(u)
y = -c * torch.cos(u)
floor_xy = torch.stack([x, y], dim=-1)
scale = self._get_scale_all(floor_xy)
scale = scale / 2.0
centroid = floor_xy.mean(dim=1)
c = torch.linalg.norm(floor_xy, ord=2, dim=-1)
v = top_corners[:, :, 1] * (-0.5 * np.pi)
ceil_z = (c * torch.tan(v)).mean(dim=1, keepdim=True)
ceil_z = ceil_z.unsqueeze(1).expand(b, 4, 1).contiguous()
floor_xy = floor_xy - centroid.unsqueeze(1)
inds = torch.sort(torch.atan2(floor_xy[..., 0], floor_xy[..., 1] + 1e-12))[1]
axes = self.cuboid_axes[:, inds.squeeze(), :]
homography = kornia.get_perspective_transform(floor_xy, axes)
homogeneous = torch.cat([floor_xy, torch.ones_like(floor_xy[..., -1:])], dim=2)
xformed = (homography @ homogeneous.transpose(1, 2)).transpose(1, 2)
xformed = xformed[:, :, :2] / xformed[:, :, 2].unsqueeze(-1)
rect_floor_xy = xformed * scale.unsqueeze(1) + centroid.unsqueeze(1)
original_xy = floor_xy + centroid.unsqueeze(1)
R, t, s = self._svd(rect_floor_xy, original_xy[:, inds.squeeze(), :])
rect_floor_xy = self._transform_points(rect_floor_xy, R, t, s)
bottom_points = torch.cat([rect_floor_xy, floor_z * torch.ones_like(c.unsqueeze(-1))], dim=-1)
top_points = torch.cat([rect_floor_xy, ceil_z], dim=-1)
return top_points, bottom_points
def run_experiments(self, input_data: torch.Tensor):
super().run_experiments(input_data)
if not self._include_derivatives:
return torch.tan(input_data)
else:
return torch.cat(
(torch.tan(input_data),
torch.sqrt(torch.tensor(1.) / torch.cos(input_data)**2)), 0)
def tensor_xi(x):
def theta_t(xxx):
return torch.asin(torch.sin(xxx) / 1.474)
theta_plus = x + theta_t(x)
theta_minus = x - theta_t(x)
return 2*(torch.sin(theta_minus) ** 2)/(torch.sin(theta_minus)**2 + torch.sin(theta_plus)**2) + \
2*(torch.tan(theta_minus) ** 2)/(torch.tan(theta_minus)**2 + torch.tan(theta_plus)**2)
def tensor_zeta(x, phi_perp, phi_0):
def theta_t(xxx):
return torch.asin(torch.sin(xxx) / 1.474)
theta_plus = x + theta_t(x)
theta_minus = x - theta_t(x)
return 2 * (torch.sin(theta_minus) ** 2) / (torch.sin(theta_minus) ** 2 + torch.sin(theta_plus) ** 2) * (torch.cos(phi_0 - phi_perp))**2 + \
2 * (torch.tan(theta_minus) ** 2) / (torch.tan(theta_minus) ** 2 + torch.tan(theta_plus) ** 2) * (torch.sin(phi_0 - phi_perp))**2
def forward(self,x): x = torch.tensor([x],dtype=torch.float32) x = x.unsqueeze(0) o = torch.tan(self.l1(x)) o = torch.tan(self.l2(o)) o = torch.tan(self.l3(o)) o = torch.tan(self.l4(o)) return o
def get_shear_matrix2d(center: torch.Tensor,
sx: Optional[torch.Tensor] = None,
sy: Optional[torch.Tensor] = None):
r"""Composes shear matrix Bx4x4 from the components.
Note: Ordered shearing, shear x-axis then y-axis.
.. math::
\begin{bmatrix}
1 & b \\
a & ab + 1 \\
\end{bmatrix}
Args:
center: shearing center coordinates of (x, y).
sx: shearing degree along x axis.
sy: shearing degree along y axis.
Returns:
params to be passed to the affine transformation with shape :math:`(B, 3, 3)`.
Examples:
>>> rng = torch.manual_seed(0)
>>> sx = torch.randn(1)
>>> sx
tensor([1.5410])
>>> center = torch.tensor([[0., 0.]]) # Bx2
>>> get_shear_matrix2d(center, sx=sx)
tensor([[[ 1.0000, -33.5468, 0.0000],
[ -0.0000, 1.0000, 0.0000],
[ 0.0000, 0.0000, 1.0000]]])
.. note::
This function is often used in conjuntion with :func:`warp_affine`, :func:`warp_perspective`.
"""
sx = torch.tensor([0.0]).repeat(center.size(0)) if sx is None else sx
sy = torch.tensor([0.0]).repeat(center.size(0)) if sy is None else sy
x, y = torch.split(center, 1, dim=-1)
x, y = x.view(-1), y.view(-1)
sx_tan = torch.tan(sx) # type: ignore
sy_tan = torch.tan(sy) # type: ignore
ones = torch.ones_like(sx) # type: ignore
shear_mat = torch.stack(
[
ones,
-sx_tan,
sx_tan * y, # type: ignore # noqa: E241
-sy_tan,
ones + sx_tan * sy_tan,
sy_tan * (sx_tan * y + x), # noqa: E241
],
dim=-1,
).view(-1, 2, 3)
shear_mat = convert_affinematrix_to_homography(shear_mat)
return shear_mat
def dynModelBlend(self, x, u):
blend_ratio = (x[:, 3] - 0.3) / (0.2)
lambda_blend = np.min([np.max([blend_ratio, 0]), 1])
# blend_max = torch.max(torch.cat([blend_ratio.view(-1,1), torch.zeros(blend_ratio.size(0),1)],dim=1),dim=1)
# blend_min = torch.min(torch.cat([blend_max.values.view(-1, 1), torch.ones(blend_max.values.size(0), 1)], dim=1), dim=1)
# lambda_blend = blend_min.values
if lambda_blend < 1:
v_x = x[:, 3]
v_y = x[:, 4]
x_kin = torch.cat(
[x[:, 0:3],
torch.sqrt(v_x * v_x + v_y * v_y).reshape(-1, 1)],
dim=1)
k1 = self.dxkin(x_kin, u).to(self.device)
k2 = self.dxkin(x_kin + self.Ts / 2. * k1, u).to(self.device)
k3 = self.dxkin(x_kin + self.Ts / 2. * k2, u).to(self.device)
k4 = self.dxkin(x_kin + self.Ts * k3, u).to(self.device)
x_kin_state = x_kin + self.Ts * (k1 / 6. + k2 / 3. + k3 / 3. +
k4 / 6.).to(self.device)
delta = u[:, 1]
beta = torch.atan(self.l_r * torch.tan(delta) /
(self.l_f + self.l_r))
v_x_state = x_kin_state[:, 3] * torch.cos(beta) # V*cos(beta)
v_y_state = x_kin_state[:, 3] * torch.sin(beta) # V*sin(beta)
yawrate_state = v_x_state * torch.tan(delta) / (self.l_f +
self.l_r)
x_kin_full = torch.cat([
x_kin_state[:, 0:3],
v_x_state.view(-1, 1),
v_y_state.view(-1, 1),
yawrate_state.view(-1, 1)
],
dim=1)
if lambda_blend == 0:
return x_kin_full
if lambda_blend > 0:
k1 = self.dxCurve(x, u).to(self.device)
k2 = self.dxCurve(x + self.Ts / 2. * k1, u).to(self.device)
k3 = self.dxCurve(x + self.Ts / 2. * k2, u).to(self.device)
k4 = self.dxCurve(x + self.Ts * k3, u).to(self.device)
x_dyn = x + self.Ts * (k1 / 6. + k2 / 3. + k3 / 3. + k4 / 6.).to(
self.device)
if lambda_blend == 1:
return x_dyn
return x_dyn * lambda_blend + (1 - lambda_blend) * x_kin_full
def _homography_joint_svd(self,
top_corners: torch.Tensor, # in [-1, 1]
bottom_corners: torch.Tensor, # in [-1, 1]
floor_z: float=-1.6,
ceil_z: float=1.6,
):
b, N, _ = top_corners.size()
floor_u = bottom_corners[:, :, 0] * np.pi
floor_v = bottom_corners[:, :, 1] * (-0.5 * np.pi)
floor_c = floor_z / torch.tan(floor_v)
floor_x = floor_c * torch.sin(floor_u)
floor_y = -floor_c * torch.cos(floor_u)
floor_xy = torch.stack([floor_x, floor_y], dim=-1)
floor_scale = self._get_scale_all(floor_xy)
floor_scale = floor_scale / 2.0
floor_ceil_c = torch.linalg.norm(floor_xy, ord=2, dim=-1)
floor_ceil_v = top_corners[:, :, 1] * (-0.5 * np.pi)
floor_ceil_z = (floor_ceil_c * torch.tan(floor_ceil_v)).mean(dim=1, keepdim=True)
floor_ceil_z = floor_ceil_z.unsqueeze(1).expand(b, 4, 1).contiguous()
ceil_u_t = top_corners[:, :, 0] * np.pi
ceil_v_t = top_corners[:, :, 1] * (-0.5 * np.pi)
ceil_c = ceil_z / torch.tan(ceil_v_t)
ceil_x = ceil_c * torch.sin(ceil_u_t)
ceil_y = -ceil_c * torch.cos(ceil_u_t)
ceil_xy = torch.stack([ceil_x, ceil_y], dim=-1)
ceil_floor_c = torch.linalg.norm(ceil_xy, ord=2, dim=-1)
ceil_v_b = bottom_corners[:, :, 1] * (-0.5 * np.pi)
ceil_floor_z = (ceil_floor_c * torch.tan(ceil_v_b)).mean(dim=1, keepdim=True)
fix_ceil = -ceil_z / ceil_floor_z
ceil_z_fix = ceil_z * fix_ceil
ceil_z_fix = ceil_z_fix.unsqueeze(1).expand(b, 4, 1).contiguous()
ceil_floor_fixed_c = ceil_z_fix.squeeze(-1) / torch.tan(ceil_v_t)
ceil_x = ceil_floor_fixed_c * torch.sin(ceil_u_t)
ceil_y = -ceil_floor_fixed_c * torch.cos(ceil_u_t)
ceil_xy = torch.stack([ceil_x, ceil_y], dim=-1)
ceil_scale = self._get_scale_all(ceil_xy)
ceil_scale = ceil_scale / 2.0
joint_xy = 0.5 * (floor_xy + ceil_xy)
joint_scale = 0.5 * (floor_scale + ceil_scale)
joint_centroid = joint_xy.mean(dim=1)
joint_xy = joint_xy - joint_centroid.unsqueeze(1)
inds = torch.sort(torch.atan2(joint_xy[..., 0], joint_xy[..., 1] + 1e-12))[1]
axes = self.cuboid_axes[:, inds.squeeze(), :]
homography = kornia.get_perspective_transform(joint_xy, axes)
homogeneous = torch.cat([joint_xy, torch.ones_like(joint_xy[..., -1:])], dim=2)
xformed = (homography @ homogeneous.transpose(1, 2)).transpose(1, 2)
xformed = xformed[:, :, :2] / xformed[:, :, 2].unsqueeze(-1)
rect_joint_xy = xformed * joint_scale.unsqueeze(1) + joint_centroid.unsqueeze(1)
original_xy = joint_xy + joint_centroid.unsqueeze(1)
R, t, s = self._svd(rect_joint_xy, original_xy[:, inds.squeeze(), :])
rect_joint_xy = self._transform_points(rect_joint_xy, R, t, s)
bottom_points = torch.cat([rect_joint_xy, floor_z * torch.ones_like(floor_c.unsqueeze(-1))], dim=-1)
top_points = torch.cat([rect_joint_xy, ceil_z_fix], dim=-1)
return top_points, bottom_points
def Txyzpers(h_fov, v_fov, u, v, out_hw, in_rot):
out = torch.ones((*out_hw, 3), dtype=float)
x_max = torch.tan(torch.tensor([h_fov / 2])).item()
y_max = torch.tan(torch.tensor([v_fov / 2])).item()
x_rng = torch.linspace(-x_max, x_max, out_hw[1], dtype=float)
y_rng = torch.linspace(-y_max, y_max, out_hw[0], dtype=float)
out[..., :2] = torch.stack(torch.meshgrid(x_rng, -y_rng), -1).permute(1, 0, 2)
Rx = Trotation_matrix(v, torch.tensor([1, 0, 0], dtype=float))
Ry = Trotation_matrix(u, torch.tensor([0, 1, 0], dtype=float))
dots = (torch.tensor([[0, 0, 1]], dtype=float) @ Rx) @ Ry
Ri = Trotation_matrix(in_rot, dots[0])
return ((out @ Rx) @ Ry) @ Ri
def get_pitch_est_v0(bins, output_pitch, H_batch, vfov_estim, f_estim):
# v0FixYannick
# half_fov_batch = (vfov_estim / 2.).unsqueeze(-1)
pitch_bins_low_device_batch_v0 = 0.5 - torch.tan(- bins['pitch_bins_low_device_batch']) * f_estim.unsqueeze(-1) / H_batch.unsqueeze(-1)
pitch_bins_high_device_batch_v0 = 0.5 + torch.tan(bins['pitch_bins_high_device_batch']) * f_estim.unsqueeze(-1) / H_batch.unsqueeze(-1)
pitch_bins_mid_device_batch_v0 = bins['pitch_bins_mid_device_batch'].repeat(pitch_bins_low_device_batch_v0.shape[0], 1)
pitch_bins_device_batch_v0 = torch.cat((pitch_bins_low_device_batch_v0, pitch_bins_mid_device_batch_v0, pitch_bins_high_device_batch_v0), dim=1)
# np.set_printoptions(precision=3, suppress=True)
# print(pitch_bins_device_batch_v0[0].detach().cpu().numpy())
expectation_batch_v0 = (torch.exp(nn.functional.log_softmax(output_pitch, dim=1))*pitch_bins_device_batch_v0).sum(dim=1)
return expectation_batch_v0
def spherical_class_to_dirs(x_cls, y_cls, cls_num):
theta = (x_cls.float() + 0.5) / cls_num * 180 - 90
phi = (y_cls.float() + 0.5) / cls_num * 180 - 90
theta = theta.clamp(-90, 90) / 180.0 * np.pi
phi = phi.clamp(-90, 90) / 180.0 * np.pi
tan2_theta = pow(torch.tan(theta), 2)
y = torch.sin(phi)
z = torch.sqrt((1 - y * y) / (1 + tan2_theta))
x = z * torch.tan(theta)
dirs = torch.stack([x, y, z], 1)
dirs = dirs / dirs.norm(p=2, dim=1, keepdim=True)
return dirs
def angle_diff_angle(res_angle, gt_angle):
size = res_angle.size()
res_norm = torch.ones(size[0], 3, size[2], size[3])
gt_norm = torch.ones(size[0], 3, size[2], size[3])
res_norm[:, 0, :, :] = torch.tan(res_angle[:, 0, :, :])
res_norm[:, 1, :, :] = torch.tan(res_angle[:, 1, :, :])
gt_norm[:, 0, :, :] = torch.tan(gt_angle[:, 0, :, :])
gt_norm[:, 1, :, :] = torch.tan(gt_angle[:, 1, :, :])
return angle_diff_norm(res_norm, gt_norm)
def tan(self):
real = self.real.clone()
imag = self.imag.clone()
denom = 1.0 + torch.tan(real)**2 * torch.tanh(imag)**2
a = torch.tan(real) - torch.tan(real) * torch.tanh(imag)**2
b = torch.tanh(imag) + torch.tanh(imag) * torch.tan(real)**2
real = a / denom
imag = b / denom
return self.__graph_copy__(real, imag)
def output2env(self, axisOrig, lambOrig, weightOrig):
bn, _, envRow, envCol = weightOrig.size()
axis = axisOrig
weight = 0.999 * weightOrig
weight = torch.tan(np.pi / 2 * weight)
lambOrig = 0.999 * lambOrig
lamb = torch.tan(np.pi / 2 * lambOrig)
envmaps = self.fromSGtoIm(axis, lamb, weight)
return envmaps, axis, lamb, weight
def project_to_2d(pos, half_fov, w, h):
# pos: BxNx3
# half_fov: Bx1
# w: Bx1
# h: Bx1
# output: BxNx2
n = pos.shape[1]
pos_2d = [(pos[:, :, 0] / (pos[:, :, 2] * torch.tan(torch.deg2rad(half_fov)).repeat(1, n))).unsqueeze(2),
(pos[:, :, 1] / (pos[:, :, 2] * torch.tan(torch.deg2rad(half_fov)).repeat(1, n))).unsqueeze(2)]
pos_2d = torch.cat(pos_2d, dim=2)
x = (w.repeat(1, n) * ((pos_2d[:, :, 0] + 1.0) / 2.0)).long().unsqueeze(2)
y = (h.repeat(1, n) * (1 - ((pos_2d[:, :, 1] + 1.0) / 2.0))).long().unsqueeze(2)
return torch.cat([x, y], dim=2)
def project_to_3d(pos, depth, half_fov, w, h):
# pos: BxNx2
# depth: BxNx1
# half_fov: Bx1
# w: Bx1
# h: Bx1
# output: BxNx3
n = pos.shape[1]
x = (pos[:, :, 0] * 2 / w.repeat(1, n)) - 1.0
y = ((1.0 - (pos[:, :, 1] / h.repeat(1, n))) * 2) - 1.0
pos_3d = [(x * (depth.squeeze(2) * torch.tan(torch.deg2rad(half_fov)).repeat(1, n))).unsqueeze(2),
(y * (depth.squeeze(2) * torch.tan(torch.deg2rad(half_fov)).repeat(1, n))).unsqueeze(2),
depth]
return torch.cat(pos_3d, dim=2)
def dxkin(self, x, u):
fkin = torch.empty(x.size(0), 4)
s = x[:, 0] #progress
d = x[:, 1] #horizontal displacement
mu = x[:, 2] #orientation
v = x[:, 3]
delta = u[:, 1]
kappa = self.getCurvature(s)
beta = torch.atan(self.l_r * torch.tan(delta) / (self.l_f + self.l_r))
fkin[:, 0] = (v * torch.cos(beta + mu)) / (1.0 - kappa * d) # s_dot
fkin[:, 1] = v * torch.sin(beta + mu) # d_dot
fkin[:, 2] = v * torch.sin(beta) / self.l_r - kappa * (
v * torch.cos(beta + mu)) / (1.0 - kappa * d)
slow_ind = v <= 0.1
D_0 = (self.Cr0 + self.Cr2 * v * v) / (self.Cm1 - self.Cm2 * v)
D_slow = torch.max(D_0, u[:, 0])
D_fast = u[:, 0]
D = D_slow * slow_ind + D_fast * (~slow_ind)
fkin[:, 3] = 1 / self.mass_long * (self.Cm1 * D - self.Cm2 * v * D -
self.Cr0 - self.Cr2 * v * v)
return fkin
def _update_x_bwd(
self,
step: int,
state: State,
m: Tensor,
first: bool,
) -> tuple[State, Tensor]:
eps = self.xeps[step]
mb = torch.ones_like(m) - m
s, t, q = self._call_xnet(step, (m * state.x, state.v), first=first)
s = (-eps) * s
q = eps * q
exp_s = torch.exp(s)
exp_q = torch.exp(q)
if self.config.use_ncp:
x1 = 2. * torch.atan(exp_s * torch.tan(state.x / 2.))
x2 = exp_s * eps * (state.v * exp_q + t)
xnew = x1 - x2
xb = (m * state.x) + (mb * xnew)
cterm = torch.cos(state.x / 2.)**2
sterm = (exp_s * torch.sin(state.x / 2.))**2
logdet_ = torch.log(exp_s / (cterm + sterm))
logdet = (mb * logdet_).sum(dim=1)
else:
xnew = exp_s * (state.x - eps * (state.v * exp_q + t))
xb = (m * state.x) + (mb * xnew)
logdet = (mb * s).sum(dim=1)
return State(x=xb, v=state.v, beta=state.beta), logdet
def _update_x_fwd(
self,
step: int,
state: State,
m: Tensor,
first: bool,
) -> tuple[State, Tensor]:
"""Single x update in the forward direction"""
eps = self.xeps[step]
mb = torch.ones_like(m) - m
s, t, q = self._call_xnet(step, (m * state.x, state.v), first=first)
s = eps * s
q = eps * q
exp_s = torch.exp(s)
exp_q = torch.exp(q)
if self.config.use_ncp:
_x = 2 * torch.atan(torch.tan(state.x / 2.) * exp_s)
xnew = _x + eps * (state.v * exp_q + t)
xf = (m * state.x) + (mb * xnew)
cterm = torch.cos(state.x / 2.)**2
sterm = (exp_s * torch.sin(state.x / 2.))**2
logdet_ = torch.log(exp_s / (cterm + sterm))
logdet = (mb * logdet_).sum(dim=1)
else:
xnew = state.x * exp_s + eps * (state.v * exp_q + t)
xf = (m * state.x) + (mb * xnew)
logdet = (mb * s).sum(dim=1)
return State(x=xf, v=state.v, beta=state.beta), logdet
def fov(self, value):
self._fov = value
fov_factor = 1.0 / torch.tan(transform.radians(0.5 * self._fov))
o = torch.ones([1], dtype=torch.float32)
diag = torch.cat([fov_factor, fov_factor, o], 0)
self._cam_to_ndc = torch.diag(diag)
self.ndc_to_cam = torch.inverse(self._cam_to_ndc)
def test_angular_loss(self):
loss_func = AngularLoss(alpha=40)
for dtype in [torch.float16, torch.float32, torch.float64]:
embedding_angles = [0, 20, 40, 60, 80]
embeddings = torch.tensor(
[c_f.angle_to_coord(a) for a in embedding_angles],
requires_grad=True,
dtype=dtype).to(self.device) #2D embeddings
labels = torch.LongTensor([0, 0, 1, 1, 2])
loss = loss_func(embeddings, labels)
loss.backward()
sq_tan_alpha = torch.tan(
torch.tensor(np.radians(40), dtype=dtype).to(self.device))**2
triplets = [(0, 1, 2), (0, 1, 3), (0, 1, 4), (1, 0, 2), (1, 0, 3),
(1, 0, 4), (2, 3, 0), (2, 3, 1), (2, 3, 4), (3, 2, 0),
(3, 2, 1), (3, 2, 4)]
correct_losses = [0, 0, 0, 0]
for a, p, n in triplets:
anchor, positive, negative = embeddings[a], embeddings[
p], embeddings[n]
exponent = 4 * sq_tan_alpha * torch.matmul(
anchor + positive, negative) - 2 * (
1 + sq_tan_alpha) * torch.matmul(anchor, positive)
correct_losses[a] += torch.exp(exponent)
total_loss = 0
for c in correct_losses:
total_loss += torch.log(1 + c)
total_loss /= len(correct_losses)
self.assertTrue(torch.isclose(loss, total_loss))
def backward(ctx, dz):
"""
:return: ``dlog(cosh(z)) = ∂log(cosh(z))/∂Re[z] * Re[dz] + ∂log(cosh(zₙ))/∂Im[z] * Im[dz]``.
"""
z, tanh_x = ctx.saved_tensors
if z.dtype == torch.float32:
complex_type = np.complex64
elif z.dtype == torch.float64:
complex_type = np.complex128
else:
raise TypeError(
"Supported float types are float and double, but got {}.".
format(z.dtype))
x = z.view(-1, 2)[:, 0]
y = z.view(-1, 2)[:, 1]
tan_y = torch.tan(y)
out = torch.empty(z.size(), dtype=z.dtype, requires_grad=False)
_log_cosh_backward_impl(
dz.detach().numpy().view(dtype=complex_type),
out.numpy().view(dtype=complex_type),
tanh_x.numpy(),
tan_y.numpy(),
)
return out
def para_module(h, w, alpha, beta, phi_0):
torch.set_default_tensor_type(torch.cuda.FloatTensor)
batch_num = alpha.size(0)
alpha = torch.stack([torch.stack([alpha] * h, dim=1)] * w, dim=2)
beta = torch.stack([torch.stack([beta] * h, dim=1)] * w, dim=2)
c_x, c_y = w / 2, h / 2
f_x, f_y = w * 1.4, w * 1.4
kk = torch.tan(alpha)
xx, yy = torch.meshgrid(
[torch.linspace(0, w - 1, steps=w),
torch.linspace(0, h - 1, steps=h)])
xx, yy = torch.stack([xx.transpose(0, 1)] * batch_num), torch.stack(
[yy.transpose(0, 1)] * batch_num)
glass_x = (xx - c_x)
glass_y = (yy - c_y)
glass_z = f_x
normal_x, normal_y, normal_z = kk, -torch.sin(beta), torch.cos(beta)
AOI = cal_tensor_aoi_3d(glass_x, glass_y, glass_z, normal_x, normal_y,
normal_z)
AOI[AOI == 0] = 0.000001
PHI_PERP = cal_tensor_phi(glass_x, glass_y, glass_z, normal_x, normal_y,
normal_z)
# phi_out = torch.atan(torch.tan(phi_out))
xi, zeta = tensor_xi(AOI), tensor_zeta(AOI, PHI_PERP, phi_0)
zeta[zeta == xi / 2] += 0.000001
xi, zeta = torch.unsqueeze(xi, 1), torch.unsqueeze(zeta, 1)
return xi, zeta
def fov(self, value):
self._fov = value
fov_factor = 1.0 / torch.tan(transform.radians(0.5 * self._fov))
o = torch.ones([1], dtype=torch.float32)
diag = torch.cat([fov_factor, fov_factor, o], 0)
self._cam_to_ndc = torch.diag(diag)
self.ndc_to_cam = torch.inverse(self._cam_to_ndc)
def gen_perspective_matrix(fov, clip_near, clip_far):
clip_dist = clip_far - clip_near
cot = 1 / torch.tan(radians(fov / 2.0))
z = torch.zeros([1], dtype=torch.float32)
o = torch.ones([1], dtype=torch.float32)
return torch.stack([torch.cat([cot, z, z, z], 0),
torch.cat([ z, cot, z, z], 0),
torch.cat([ z, z, 1 / clip_dist, - clip_near / clip_dist], 0),
torch.cat([ z, z, o, z], 0)])
def forward(self, input):
self.batchgrid = torch.zeros(torch.Size([input.size(0)]) + self.grid.size() )
#print(self.batchgrid.size())
for i in range(input.size(0)):
self.batchgrid[i,:,:,:] = self.grid
self.batchgrid = Variable(self.batchgrid)
#print(self.batchgrid.size())
input_u = input.view(-1,1,1,1).repeat(1,self.height, self.width,1)
#print(input_u.requires_grad, self.batchgrid)
output0 = self.batchgrid[:,:,:,0:1]
output1 = torch.atan(torch.tan(np.pi/2.0*(self.batchgrid[:,:,:,1:2] + self.batchgrid[:,:,:,2:] * input_u[:,:,:,:]))) /(np.pi/2)
#print(output0.size(), output1.size())
output = torch.cat([output0, output1], 3)
return output
def forward(self, depth, trans0, trans1, rotate):
self.batchgrid3d = torch.zeros(torch.Size([depth.size(0)]) + self.grid3d.size())
for i in range(depth.size(0)):
self.batchgrid3d[i] = self.grid3d
self.batchgrid3d = Variable(self.batchgrid3d)
self.batchgrid = torch.zeros(torch.Size([depth.size(0)]) + self.grid.size())
for i in range(depth.size(0)):
self.batchgrid[i] = self.grid
self.batchgrid = Variable(self.batchgrid)
x = self.batchgrid3d[:,:,:,0:1] * depth + trans0.view(-1,1,1,1).repeat(1, self.height, self.width, 1)
y = self.batchgrid3d[:,:,:,1:2] * depth + trans1.view(-1,1,1,1).repeat(1, self.height, self.width, 1)
z = self.batchgrid3d[:,:,:,2:3] * depth
#print(x.size(), y.size(), z.size())
r = torch.sqrt(x**2 + y**2 + z**2) + 1e-5
#print(r)
theta = torch.acos(z/r)/(np.pi/2) - 1
#phi = torch.atan(y/x)
phi = torch.atan(y/(x + 1e-5)) + np.pi * x.lt(0).type(torch.FloatTensor) * (y.ge(0).type(torch.FloatTensor) - y.lt(0).type(torch.FloatTensor))
phi = phi/np.pi
#print(theta.size(), phi.size())
input_u = rotate.view(-1,1,1,1).repeat(1,self.height, self.width,1)
output = torch.cat([theta,phi], 3)
#print(output.size())
output1 = torch.atan(torch.tan(np.pi/2.0*(output[:,:,:,1:2] + self.batchgrid[:,:,:,2:] * input_u[:,:,:,:]))) /(np.pi/2)
output2 = torch.cat([output[:,:,:,0:1], output1], 3)
return output2
def __init__(self,
position,
look_at,
up,
fov,
clip_near,
resolution,
cam_to_ndc = None,
fisheye = False):
assert(position.dtype == torch.float32)
assert(len(position.shape) == 1 and position.shape[0] == 3)
assert(look_at.dtype == torch.float32)
assert(len(look_at.shape) == 1 and look_at.shape[0] == 3)
assert(up.dtype == torch.float32)
assert(len(up.shape) == 1 and up.shape[0] == 3)
if fov is not None:
assert(fov.dtype == torch.float32)
assert(len(fov.shape) == 1 and fov.shape[0] == 1)
assert(isinstance(clip_near, float))
self.position = position
self.look_at = look_at
self.up = up
self._fov = fov
# self.cam_to_world = transform.gen_look_at_matrix(position, look_at, up)
# self.world_to_cam = torch.inverse(self.cam_to_world).contiguous()
if cam_to_ndc is None:
fov_factor = 1.0 / torch.tan(transform.radians(0.5 * fov))
o = torch.ones([1], dtype=torch.float32)
diag = torch.cat([fov_factor, fov_factor, o], 0)
self._cam_to_ndc = torch.diag(diag)
else:
self._cam_to_ndc = cam_to_ndc
self.ndc_to_cam = torch.inverse(self.cam_to_ndc)
self.clip_near = clip_near
self.resolution = resolution
self.fisheye = fisheye
def forward(self, input1, input2):
self.batchgrid3d = torch.zeros(torch.Size([input1.size(0)]) + self.grid3d.size())
for i in range(input1.size(0)):
self.batchgrid3d[i] = self.grid3d
self.batchgrid3d = Variable(self.batchgrid3d)
self.batchgrid = torch.zeros(torch.Size([input1.size(0)]) + self.grid.size())
for i in range(input1.size(0)):
self.batchgrid[i] = self.grid
self.batchgrid = Variable(self.batchgrid)
#print(self.batchgrid3d)
x = torch.sum(torch.mul(self.batchgrid3d, input1[:,:,:,0:4]), 3)
y = torch.sum(torch.mul(self.batchgrid3d, input1[:,:,:,4:8]), 3)
z = torch.sum(torch.mul(self.batchgrid3d, input1[:,:,:,8:]), 3)
#print(x)
r = torch.sqrt(x**2 + y**2 + z**2) + 1e-5
#print(r)
theta = torch.acos(z/r)/(np.pi/2) - 1
#phi = torch.atan(y/x)
phi = torch.atan(y/(x + 1e-5)) + np.pi * x.lt(0).type(torch.FloatTensor) * (y.ge(0).type(torch.FloatTensor) - y.lt(0).type(torch.FloatTensor))
phi = phi/np.pi
input_u = input2.view(-1,1,1,1).repeat(1,self.height, self.width,1)
output = torch.cat([theta,phi], 3)
output1 = torch.atan(torch.tan(np.pi/2.0*(output[:,:,:,1:2] + self.batchgrid[:,:,:,2:] * input_u[:,:,:,:]))) /(np.pi/2)
output2 = torch.cat([output[:,:,:,0:1], output1], 3)
return output2
def icdf(self, value):
self._validate_log_prob_arg(value)
return torch.tan(math.pi * (value - 0.5)) * self.scale + self.loc
