def retrieve_view_projection_transforms(image, mesh, group=None):
rows = image.shape[0]
cols = image.shape[1]
max_d = max(rows, cols)
camera_matrix = np.array([[max_d, 0, cols / 2.0], [0, max_d, rows / 2.0],
[0, 0, 1.0]])
distortion_coeffs = np.zeros(4)
converged, r_vec, t_vec = cv2.solvePnP(
mesh.landmarks[group].lms.points,
image.landmarks[group].lms.points[:, ::-1], camera_matrix,
distortion_coeffs)
rotation_matrix = cv2.Rodrigues(r_vec)[0]
h_camera_matrix = np.eye(4)
h_camera_matrix[:3, :3] = camera_matrix
c = Homogeneous(h_camera_matrix)
t = Translation(t_vec.ravel())
r = Rotation(rotation_matrix)
view_t_flipped = r.compose_before(t)
view_t = view_t_flipped.compose_before(axes_flip_t)
proj_t = Homogeneous(
weak_projection_matrix(image.width, image.height,
view_t_flipped.apply(mesh)))
return view_t, proj_t, r
def rotation_from_3d_ccw_angle_around_y(theta, degrees=True):
r"""
Convenience constructor for 3D CCW rotations around the y axis
Parameters
----------
theta : `float`
The angle of rotation about the origin
degrees : `bool`, optional
If ``True`` theta is interpreted as a degree. If ``False``, theta is
interpreted as radians.
Returns
-------
rotation : :map:`Rotation`
A 3D rotation transform.
"""
if degrees:
# convert to radians
theta = theta * np.pi / 180.0
return Rotation(np.array([[np.cos(theta), 0,
np.sin(theta)], [0, 1, 0],
[-np.sin(theta), 0,
np.cos(theta)]]),
skip_checks=True)
def test_basic_2d_rotation_axis_angle():
rotation_matrix = np.array([[0, 1],
[-1, 0]])
rotation = Rotation(rotation_matrix)
axis, angle = rotation.axis_and_angle_of_rotation()
assert_allclose(axis, np.array([0, 0, 1]))
assert_allclose((90 * np.pi)/180, angle)
def test_3d_rotation_as_vector():
a = np.sqrt(3.0) / 2.0
b = 0.5
# this is a rotation of -30 degrees about the x axis
rotation_matrix = np.array([[1, 0, 0], [0, a, b], [0, -b, a]])
rotation = Rotation(rotation_matrix)
assert_allclose(np.round(rotation.as_vector()[2:]), np.array([0., 0.]))
def test_3d_rotation_inverse_eye():
a = np.sqrt(3.0) / 2.0
b = 0.5
# this is a rotation of -30 degrees about the x axis
rotation_matrix = np.array([[1, 0, 0], [0, a, b], [0, -b, a]])
rotation = Rotation(rotation_matrix)
transformed = rotation.compose_before(rotation.pseudoinverse())
assert_allclose(np.eye(4), transformed.h_matrix, atol=1e-15)
def test_basic_3d_rotation_axis_angle():
a = np.sqrt(3.0) / 2.0
b = 0.5
# this is a rotation of -30 degrees about the x axis
rotation_matrix = np.array([[1, 0, 0], [0, a, b], [0, -b, a]])
rotation = Rotation(rotation_matrix)
axis, angle = rotation.axis_and_angle_of_rotation()
assert_allclose(axis, np.array([1, 0, 0]))
assert_allclose((-30 * np.pi) / 180, angle)
def test_align_2d_rotation():
rotation_matrix = np.array([[0, 1], [-1, 0]])
rotation = Rotation(rotation_matrix)
source = PointCloud(np.array([[0, 1], [1, 1], [-1, -5], [3, -5]]))
target = rotation.apply(source)
# estimate the transform from source and target
estimate = AlignmentRotation(source, target)
# check the estimates is correct
assert_allclose(rotation.h_matrix, estimate.h_matrix, atol=1e-14)
def test_rotation_compose_before_homog():
# can't do this inplace - so should just give transform chain
rotation = Rotation(np.array([[1, 0],
[0, 1]]))
homog = Homogeneous(np.array([[0, 1, 0],
[1, 0, 0],
[0, 0, 1]]))
res = rotation.compose_before(homog)
assert(type(res) == Homogeneous)
def test_align_2d_rotation_set_h_matrix_raises_notimplemented_error():
rotation_matrix = np.array([[0, 1], [-1, 0]])
rotation = Rotation(rotation_matrix)
source = PointCloud(np.array([[0, 1], [1, 1], [-1, -5], [3, -5]]))
target = rotation.apply(source)
# estimate the transform from source and source
estimate = AlignmentRotation(source, source)
# and set the target
estimate.set_h_matrix(rotation.h_matrix)
def test_basic_3d_rotation():
a = np.sqrt(3.0) / 2.0
b = 0.5
# this is a rotation of -30 degrees about the x axis
rotation_matrix = np.array([[1, 0, 0], [0, a, b], [0, -b, a]])
rotation = Rotation(rotation_matrix)
starting_vector = np.array([0, 1, 0])
transformed = rotation.apply(starting_vector)
assert_allclose(np.array([0, a, -b]), transformed)
def _align_mean_shape_with_bbox(self, bbox):
# Convert 3D landmarks to 2D by removing the Z axis
template_shape = PointCloud(self.mm.landmarks.points[:, [1, 0]])
# Rotation that flips over x axis
rot_matrix = np.eye(template_shape.n_dims)
rot_matrix[0, 0] = -1
template_shape = Rotation(rot_matrix,
skip_checks=True).apply(template_shape)
# Align the 2D landmarks' bbox with the provided bbox
return AlignmentSimilarity(template_shape.bounding_box(),
bbox).apply(template_shape)
def retrieve_camera_matrix(image, mesh, group=None, initialize=True):
import cv2
drop_h = Homogeneous(np.eye(4)[:3])
flip_xy_yx = Homogeneous(np.array([[0, 1, 0], [1, 0, 0], [0, 0, 1]]))
rows = image.shape[0]
cols = image.shape[1]
max_d = max(rows, cols)
camera_matrix = np.array([[max_d, 0, cols / 2.0], [0, max_d, rows / 2.0],
[0, 0, 1.0]])
distortion_coeffs = np.zeros(4)
# Initial guess for rotation/translation.
if initialize:
r_vec = np.array([[-2.7193267], [-0.14545351], [-0.34661788]])
t_vec = np.array([[0.], [0.], [280.]])
converged, r_vec, t_vec = cv2.solvePnP(
mesh.landmarks[group].lms.points,
image.landmarks[group].lms.points[:, ::-1], camera_matrix,
distortion_coeffs, r_vec, t_vec, 1)
else:
converged, r_vec, t_vec = cv2.solvePnP(
mesh.landmarks[group].lms.points,
image.landmarks[group].lms.points[:, ::-1], camera_matrix,
distortion_coeffs)
rotation_matrix = cv2.Rodrigues(r_vec)[0]
h_camera_matrix = np.eye(4)
h_camera_matrix[:3, :3] = camera_matrix
t_vec = t_vec.ravel()
if t_vec[2] < 0:
print('Position has a negative value in z-axis')
c = Homogeneous(h_camera_matrix)
t = Translation(t_vec)
r = Rotation(rotation_matrix)
view_t = r.compose_before(t)
proj_t = c.compose_before(drop_h).compose_before(flip_xy_yx)
return view_t, c, proj_t
def align_2d_3d(points_3d,
points_image,
image_shape,
focal_length=None,
distortion_coeffs=None):
try:
import cv2
except ImportError:
raise MenpoMissingDependencyError('opencv3')
height, width = image_shape
# Create camera matrix
focal_length = (max(height, width)
if focal_length is None else focal_length)
c_x = width / 2.0
c_y = height / 2.0
camera_matrix = np.array([[focal_length, 0, c_x], [0, focal_length, c_y],
[0, 0, 1.0]])
# If distortion coefficients are None, set them to zero
if distortion_coeffs is None:
distortion_coeffs = np.zeros(4)
# Estimate the camera pose given the 3D sparse pointcloud on the
# mesh, its 2D projection on the image, the camera matrix and the
# distortion coefficients
lm2d = points_image.points[:, ::-1].copy()
converged, r_vec, t_vec = cv2.solvePnP(points_3d.points, lm2d,
camera_matrix, distortion_coeffs)
if not converged:
warnings.warn('cv2.SolvePnP did not converge to a solution')
# Create rotation and translation transform objects from the vectors
# acquired at the previous step
rotation_matrix = cv2.Rodrigues(r_vec)[0]
r = Rotation(rotation_matrix)
t = Translation(t_vec.ravel())
return r, t, focal_length
def init_from_2d_projected_shape(
cls,
points_3d,
points_image,
image_shape,
focal_length=None,
distortion_coeffs=None,
):
from menpo3d.correspond import solve_pnp # Avoid circular import
model_view_t, _ = solve_pnp(
points_image,
points_3d,
pinhole_intrinsic_matrix(
image_shape[0], image_shape[1], focal_length=focal_length
),
distortion_coefficients=distortion_coeffs,
)
rotation = Rotation(model_view_t.h_matrix[:3, :3])
translation = Translation(model_view_t.h_matrix[:3, -1])
return OrthographicCamera(
rotation, translation, OrthographicProjection(focal_length, image_shape)
)
def test_rotation2d_n_parameters_raises_notimplementederror():
rot_matrix = np.eye(2)
t = Rotation(rot_matrix)
with raises(NotImplementedError):
t.n_parameters
def solve_pnp(
points_2d,
points_3d,
intrinsic_matrix,
distortion_coefficients=None,
pnp_method=SOLVEPNP_ITERATIVE,
n_iterations=100,
reprojection_error=8.0,
initial_transform=None,
):
"""
Use OpenCV to solve the Perspective-N-Point problem (PnP). Uses Ransac PnP as this
typically provides better results. The image and mesh must both have the same
landmark group name attached.
Note the intrinsic matrix (if given) must be in "OpenCV" space and thus
has the "x" and "y" axes flipped w.r.t the menpo norm. E.g. the intrinsic matrix
is defined as follows:
[fx, 0, cx, 0]
[ 0, fy, cy, 0]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
Parameters
----------
points_2d : :map`Pointcloud` or subclass
The 2D points in the image to solve the PnP problem with.
points_3d : :map`Pointcloud` or subclass
The 3D points to solve the PnP problem with
group : str, optional
The name of the landmark group
intrinsic_matrix : :map`Homogeneous`
The intrinsic matrix - if the intrinsic matrix is unknow please see
usage of pinhole_intrinsic_matrix()
distortion_coefficients : ``(D,)`` `ndarray`
The distortion coefficients (if not given assumes 0 coefficients). See the
OpenCV documentation for the distortion coefficient types that are supported.
pnp_method : int
The OpenCV PNP method e.g. cv2.SOLVEPNP_ITERATIVE or otherwise
n_iterations : int
The number of iterations to perform
reprojection_error : float
The maximum reprojection error to allow for a point to be considered an
inlier.
initial_transform : :map`Homogeneous`
The initialization for the cv2.SOLVEPNP_ITERATIVE method. Compatible
with the returned model transformation returned by this method.
Returns
-------
model_view_t : :map`Homogeneous`
The combined ModelView transform. Can be used to place the 3D points
in "eye space".
proj_t : :map`Homogeneous`
A transform that can be used to project the input 3D points
back into the image
"""
import cv2
if distortion_coefficients is None:
distortion_coefficients = np.zeros(4)
r_vec = t_vec = None
if initial_transform is not None:
if pnp_method != cv2.SOLVEPNP_ITERATIVE:
raise ValueError(
"Initial estimates can only be given to SOLVEPNP_ITERATIVE")
else:
r_vec = cv2.Rodrigues(initial_transform.h_matrix[:3, :3])[0]
t_vec = initial_transform.h_matrix[:3, -1].ravel()
converged, r_vec, t_vec, _ = cv2.solvePnPRansac(
points_3d.points,
points_2d.points[:, ::-1],
intrinsic_matrix.h_matrix[:3, :3],
distortion_coefficients,
flags=pnp_method,
iterationsCount=n_iterations,
reprojectionError=reprojection_error,
useExtrinsicGuess=r_vec is not None,
rvec=r_vec,
tvec=t_vec,
)
if not converged:
raise ValueError("cv2.solvePnPRansac failed to converge")
rotation = Rotation(cv2.Rodrigues(r_vec)[0])
translation = Translation(t_vec.ravel())
model_view_t = rotation.compose_before(translation)
proj_t = intrinsic_matrix.compose_before(
flip_xy_yx()).compose_before(_drop_h)
return model_view_t, proj_t
def test_basic_2d_rotation():
rotation_matrix = np.array([[0, 1],
[-1, 0]])
rotation = Rotation(rotation_matrix)
assert_allclose(np.array([0, -1]), rotation.apply(np.array([1, 0])))
def test_rotation3d_n_parameters_raises_notimplementederror():
rot_matrix = np.eye(3)
t = Rotation(rot_matrix)
# Throws exception
t.n_parameters
def test_rotation_set_h_matrix_raises_notimplementederror():
r = Rotation(np.array([[1, 0], [0, 1]]))
r.set_h_matrix(r.h_matrix)
