def generate(cls, camera: PerspectiveCamera, true_pose_w_c: SE3, true_points_w: np.ndarray):
"""Generate a 2D-3D measurement
:param camera: A PerspectiveCamera representing the camera that performed the measurement.
:param true_pose_w_c: The true pose of the camera in the world frame.
:param true_points_w: The true world points.
:return: The generated measurements
"""
num_points = true_points_w.shape[1]
# Generate observations in pixels.
u = camera.project_to_pixel(true_pose_w_c.inverse() * true_points_w)
covs_u = [np.diag(np.array([2, 2]) ** 2)] * num_points # Same for all observations.
# Add noise according to uncertainty.
for c in range(num_points):
u[:2, [c]] = u[:2, [c]] + np.random.multivariate_normal(np.zeros(2), covs_u[c]).reshape(-1, 1)
# Construct measurement.
return cls(camera, true_pose_w_c, u, covs_u)
def __init__(self, camera: PerspectiveCamera, pose_w_c: SE3, u: np.ndarray, covs_u: list):
"""Constructs the 2D-3D measurement
:param camera: A PerspectiveCamera representing the camera that performed the measurement.
:param pose_w_c: The pose of the camera in the world frame.
:param u: A 2xn matrix of n pixel observations corresponding to each and every 3D world point state.
:param covs_u: A list of covariance matrices representing the uncertainty in each pixel observation.
"""
self.camera = camera
self.pose_w_c = pose_w_c
self.pose_c_w = pose_w_c.inverse()
# Transform to the normalised image plane.
self.xn = camera.pixel_to_normalised(u)
# Propagate uncertainty, and precompute square root of information matrices.
self.num = u.shape[1]
self.covs = [np.identity(2)] * self.num
self.sqrt_inv_covs = [np.identity(2)] * self.num
for c in range(self.num):
self.covs[c] = self.camera.pixel_cov_to_normalised_com(covs_u[c])
self.sqrt_inv_covs[c] = scipy.linalg.sqrtm(scipy.linalg.inv(self.covs[c]))