How do I estimate the pose of a camera looking at a flat horizontal surface?

Question

I have a camera looking down on a flat horizontal surface, Z=0.

The position of the camera with respect to the surface: $[XC,YC,ZC]$ is unknown.

Also the orientation of the camera with respect to the surface is unknown. Ideally the camera should look perfectly vertically on the surface: $\vec{dir}=[0,0,-1]$. However, unfortunately the camera may not be mounted perfectly. As a result the direction vector of the camera is slightly rotated: $\vec{DIR}=T\vec{dir}$ where T is a 3x3 rotation matrix. This rotatation matrix can alternatively be described by the Euler angles (roll, pitch, yaw).

I need to estimate the position, $[XC,YC,ZC]$, and the orientation of the camera, (roll,pitch,yaw).

I was thinking placing N target points (crosses) on the flat surface with known coordinates: $\{(X_i,Y_i,Z_i=0)\}$.

These targets could then be identified in the image taken by the camera. The spatial pixel of a target, $(x_{image},y_{image})$, corresponds to a horizontal and vertical angle from the center pixel: $(\theta x_i,\theta y_i)$.

Which algorithm should I use to estimate the position and orientation of the camera? Is such an algorithm perhaps implemented in some library (e.g OpenCV)?

On my own I have come up with the idea to trace a ray from each target pixel $(\theta x_i,\theta y_i)$ to the horizontal surface. The result is a set of estimated positions: $\{(XE_i,YI_i)\}$. Further I would define an error function: $$ SE(XC,YC,ZC,roll,pitch,yaw)=\frac{1}{N}\Sigma_{i=1}^N(XE_i - X_i)^2 + (YE_i - Y_i)^2 $$

I would then start by assuming $XC=YC=ZC=0$ and T being the identity matrix (roll=pitch=yaw=0). Next I would use gradient descent to find the position and orientation that minimizes the error.

Answer 1

Your problem is called pose estimation. The OpenCV function is called solvePnP(). Be aware that the intrinsic camera matrix must be known in advance.

If you don't have the intrinsic camera matrix, you can use calibrateCamera() to find camera matrix and pose simultaneously. This – unfortunately – requires calibration points in three-dimensional space. They cannot be all in the same plane. Multiple flat-plane shots from different angles are also acceptable, so you can wiggle the flat-plane calibration target in front of the camera (or the other way around.) calibrateCamera() reports both camera matrix and pose, but you can split the calibration into two steps if you want. First find the camera matrix by wiggling a target in front of the camera running calibrateCamera(), then mount the camera in its final position and estimate the pose using solvePnP().

Maybe you don't need camera position and orientation. Maybe all you want is to rectify the perspective as if the camera was looking straight from above? In this case, you can perform a perspective transform using getPerspectiveTransform() and warpPerspective(). You don't need out-of-plane calibration points or prior knowledge about the camera matrix, but it won't tell you how far away the camera is. The rectification transform will only work correctly for objects in the calibration plane, not for objects above or below.

You can roll your own algorithm, but the same limitations apply. If you only have one pose and all points are in a single plane, you cannot tell how far away the camera is. You need prior knowledge about the camera or out-of-plane points or multiple poses. Or you don't actually care how far away the camera is.

Answer 2

All of this is written assuming that you have already calibrated the camera for its intrinsic matrix. If you have not done this, then you need to do so beforehand, or you need to look to the answer from @RainerP.

1. I don't know if OpenCV has or doesn't have an algorithm to do this for you -- I would be surprised if it doesn't have anything, and surprised if what they had was just exactly what you want.
2. Doing some sort of optimization to the match of the image to a model is correct, unless there's a reasonably easy and direct algebraic solution that goes from image points to camera placement. If OpenCV offers a canned solution, it'll use one of these two approaches.
3. Even if there is an algebraic solution, doing the optimization isn't wrong, especially if the algebraic solution isn't tidy.

Answer 3

I deleted my previous answer because I was glued in some convoluted calculations and the question was answered with a nice on-the-shelf software answer. However since then I continued to look into this problem just for the sport of it! I hope people will have as much fun as I had doing this.

The origin where the camera would be if the world was perfect will be the perfect origin and the position it should be will give us the perfect reference frame. What I'll call the pointing vectors would be unit vectors pointing from the camera origin (somewhere at the back of the camera) to what I'll call the targets (for instance crosses on the table) of which we'll know the positions in the perfect reference frame.

The $u_i$ vectors are the pointing vectors estimates computed with the camera position estimate ($u_i = \frac{target\_i\_position-camera\_position}{\vert\vert target\_i\_position-camera\_position \vert\vert}$) in the perfect reference frame and the $v_i$ are the measured pointing vectors in the reference frame of the camera.

Once you have a $u_i$ computed with the position, you only have to project (or rotate) it to the camera's frame with a rotation matrix $R$ to get a prediction of what you actually measured. It leads to the following least squares problem which is called Wahba's problem : $$ J(attitude, position) = \sum_{i=1}^{n}\vert\vert v_i-R(attitude)\times u_i(position) \vert\vert² $$

Usually this problem is used to determine the attitude (i.e orientation) of a satellite by sensing stars that are so remote that the kind of position variation you might see in an orbit wont affect the pointing vectors. Fortunately for us the targets here are not millions of light years away and their position will affect our loss function.

The main problem now before handing it to your favorite optimizer is to understand a way to enforce the fact that the rotation matrix is a rotation matrix. The way I found to do this is to represent the attitude with quaternions. To ensure that a quaternion $q$ represents an attitude you just have to enforce $\vert\vert q \vert\vert^{2} = 1$. I tried the nonlinear contraint before I figured out that I could let the quaternion roam free in $\mathbb{R}^{4}$ and just normalize $q$ just before performing the rotation, leading to a unconstrained optimization problem.

Here is the code I used if you want to play around with it :

import numpy as np
from scipy.spatial.transform import Rotation
from scipy.optimize import minimize


# takes vectors input of shape 3 x n
def rotate(quat, vectors):
    r = Rotation.from_quat(quat.flatten())
    return np.atleast_2d(r.apply(vectors.T)).T


def normalize_columns(mat):
    return np.divide(mat, np.linalg.norm(mat, axis=0))


def compute_pointing_vectors(quat, pos, target_pos):
    return normalize_columns(rotate(quat, pos - target_pos))


def problem_set_up(nb_targets, position_magnitude, noise_magnitude, seed=4242):
    np.random.seed(seed)

    target_pos = position_magnitude * 2 * (np.random.rand(3, nb_targets) - 0.5)
    true_pos = 2 * (position_magnitude / 10) * (np.random.rand(3, 1) - 0.5)

    true_quat = normalize_columns(np.random.rand(4, 1) - 0.5)
    if true_quat[0][0] < 0:
        true_quat = -true_quat
    true_pointing_vectors = compute_pointing_vectors(true_quat, true_pos, target_pos)

    measured_pointing_vectors = true_pointing_vectors + np.random.normal(
        0, noise_magnitude, true_pointing_vectors.shape
    )

    return target_pos, measured_pointing_vectors, true_quat, true_pos


def soft_wahba_loss(X, target_pos, measured_pointing_vectors):
    alpha_1, alpha_2, alpha_3, alpha_4, x, y, z = X

    quat = normalize_columns(np.array([[alpha_1], [alpha_2], [alpha_3], [alpha_4]]))
    pos = np.array([[x], [y], [z]])

    pointing_vectors_estimates = compute_pointing_vectors(quat, pos, target_pos)

    return (np.square(pointing_vectors_estimates - measured_pointing_vectors)).mean()


def extract_result(res):
    pos = np.atleast_2d(res.x[4:]).T
    quat = np.atleast_2d(normalize_columns(res.x[0:4])).T
    if quat[0][0] < 0:
        quat = -quat

    return quat, pos


nb_targets = 5
position_magnitude = 100
noise_magnitude = 0  # 1e-3
seed = np.random.randint(0, 1000000)
target_pos, measured_pointing_vectors, true_quat, true_pos = problem_set_up(
    nb_targets, position_magnitude, noise_magnitude, seed=seed
)

X0 = np.array([0, 0, 0, 1, 100, 100, 100])
fun = lambda X: soft_wahba_loss(X, target_pos, measured_pointing_vectors)


options = {"gtol": 1e-15}
res = minimize(fun, X0, method="L-BFGS-B", options=options)

quat_estimate, pos_estimate = extract_result(res)

print("with the seed : " + str(seed))
print("the true quaternion is :")
print(true_quat)
print("the quaternion estimate is :")
print(quat_estimate)
print(
    "the l2 distance between the two is :"
    + str(np.linalg.norm(true_quat - quat_estimate))
)

print("the true position is : ")
print(true_pos)
print("the position estimate is :")
print(pos_estimate)
print(
    "the l2 distance between the two is : "
    + str(np.linalg.norm(true_pos - pos_estimate))
)

The results are interesting. As expected, you need to have at least three targets to extract position and attitude. The attitude is determined relatively precisely but the position is not so accurately computed. In fact the number of targets (above 4 or 5) and the noise level has very little effect on the position determination, which is quite funny.

I believe it is due to the fact that the error function is relatively insensitive to the position, therefore when you're close enough the gradient vanishes and the error stagnates, leading to algorithm termination. You'll have a similar phenomenon if your initial guess is away from the real camera position (like [1000, 1000, 1000]), the gradient w.r.t the position is barely existing (which is not too crazy here if you think about it), hence the algorithm stops immediately. I guess that would be why Wahba's loss is an attitude determination loss and not a position determination loss.

I hope you enjoyed!