def transform_point_cloud(self, point_cloud: np.ndarray) -> np.ndarray:
"""Apply the SE(2) transformation to point_cloud.
Args:
point_cloud: np.ndarray of shape (N, 2).
Returns:
transformed_point_cloud: np.ndarray of shape (N, 2).
Raises:
ValueError: if point_cloud does not have the required shape.
"""
assert_np_array_shape(point_cloud, (None, 2))
num_points = point_cloud.shape[0]
homogeneous_pts = np.hstack([point_cloud, np.ones((num_points, 1))])
transformed_point_cloud = homogeneous_pts.dot(self.transform_matrix.T)
return transformed_point_cloud[:, :2]
def __init__(self, rotation: np.ndarray, translation: np.ndarray) -> None:
"""Initialize.
Args:
rotation: np.ndarray of shape (2,2).
translation: np.ndarray of shape (2,1).
Raises:
ValueError: if rotation or translation do not have the required shapes.
"""
assert_np_array_shape(rotation, (2, 2))
assert_np_array_shape(translation, (2,))
self.rotation = rotation
self.translation = translation
self.transform_matrix = np.eye(3)
self.transform_matrix[:2, :2] = self.rotation
self.transform_matrix[:2, 2] = self.translation
def __init__(self, R: np.ndarray, t: np.ndarray, s: Union[int, float]) -> None:
"""Initialize from rotation R, translation t, and scale s.
Args:
R: array of shape (2x2) representing 2d rotation matrix
t: array of shape (2,) representing 2d translation
s: scaling factor
"""
assert_np_array_shape(R, (2, 2))
assert_np_array_shape(t, (2,))
assert isinstance(s, float) or isinstance(s, int)
if np.isclose(s, 0.0):
raise ZeroDivisionError("3x3 matrix formation would require division by zero")
self.R_ = R.astype(np.float32)
self.t_ = t.astype(np.float32)
self.s_ = float(s)
def transform_from(self, point_cloud: np.ndarray) -> np.ndarray:
"""Transform point cloud such that if they are in frame A,
and our Sim(3) transform is defines as bSa, then we get points
back in frame B:
p_b = bSa * p_a
Action on a point p is s*(R*p+t).
Args:
point_cloud: Nx2 array representing 2d points in frame A
Returns:
transformed_point_cloud: Nx2 array representing 2d points in frame B
"""
assert_np_array_shape(point_cloud, (None, 2))
# (2,2) x (2,N) + (2,1) = (2,N) -> transpose
transformed_point_cloud = (self.R_ @ point_cloud.T + self.t_.reshape(2, 1)).T
# now scale points
return transformed_point_cloud * self.s_
def transform_from(self, point_cloud: np.ndarray) -> np.ndarray:
"""Transform point cloud such that if they are in frame A,
and our Sim(3) transform is defines as bSa, then we get points
back in frame B:
p_b = bSa * p_a
Action on a point p is s*(R*p+t).
Args:
point_cloud: Nx2 array representing 2d points in frame A
Returns:
transformed_point_cloud: Nx2 array representing 2d points in frame B
"""
if not point_cloud.ndim == 2:
raise ValueError("Input point cloud is not 2-dimensional.")
assert_np_array_shape(point_cloud, (None, 2))
# (2,2) x (2,N) + (2,1) = (2,N) -> transpose
transformed_point_cloud = (point_cloud @ self.R_.T) + self.t_
# now scale points
return transformed_point_cloud * self.s_
