def objective_reproj(self, p2d, p3d, w, K, theta):
"""Weighted squared reprojection error
f(p2d, p3d, w, K, y) = sum_{i=1}^n
w_i * ||pi(p3d_i, K, y) - p2d_i||_2^2
where pi(p, K, y) = h2i(K * (R(y) * p + t(y)))
where h2i(x) = [x1 / x3, x2 / x3]
"""
p3d_projected = geo.project_points_by_theta(p3d, theta, K)
z2 = torch.sum((p3d_projected - p2d)**2, dim=-1)
return torch.einsum('bn,bn->b', (w, z2))
def objective_reproj_huber(self, p2d, p3d, w, K, theta):
"""Weighted Huber reprojection error
f(p2d, p3d, w, K, y) = sum_{i=1}^n
w_i * rho(pi(p3d_i, K, y) - p2d_i, alpha)
where rho(z, alpha) = / 0.5 z^2 for |z| <= alpha
\ alpha * (|z| - 0.5 * alpha) else
and pi(p, K, y) = h2i(K * (R(y) * p + t(y)))
where h2i(x) = [x1 / x3, x2 / x3]
"""
def huber(z2, alpha=1.0):
return torch.where(z2 <= alpha ** 2, 0.5 * z2, alpha * (
z2.sqrt() - 0.5 * alpha))
p3d_projected = geo.project_points_by_theta(p3d, theta, K)
z2 = torch.sum((p3d_projected - p2d) ** 2, dim=-1)
return torch.einsum('bn,bn->b', (w, huber(z2, self.alpha)))