12
$\begingroup$

I am trying to calculate Image to World model for my thesis dealing with road lanes. As a disclaimer I have to say that linear algebra is not my strong suite.

The idea is - given that I know yield, pitch, and position of the camera - I can translate image pixels to real world coordinates which will be useful in road recognition algorithm.

I managed to get working Camera Pinhole Perspective projection. Here are the matrices used

Extrinsic Matrix

Translates to camera position and rotates accordingly

$$\begin{pmatrix} &1 &0 &0 &-cx \\ &0 &1 &0 &-cy \\ &0 &0 &1 &-cz \\ &0 &0 &0 &1 \end{pmatrix} \begin{pmatrix} &1 &0 &0 &0 \\ &0 &\cos(\text{yaw}) &-\sin(\text{yaw}) &0 \\ &0 &\sin(\text{yaw}) &\cos(\text{yaw}) &0 \\ &0 &0 &0 &1 \end{pmatrix} \begin{pmatrix} &\cos(\text{pitch})) &0 &\sin(\text{pitch})) &0 \\ &0 &1 &0 &0 \\ &-\sin(\text{pitch}) &0 &\cos(\text{pitch}) &0 \\ &0 &0 &0 &1 \end{pmatrix} $$

Projection

$f$ is the focal length of the camera.

Based on https://en.wikipedia.org/wiki/3D_projection

$$\begin{pmatrix} Fx\\ Fy\\ Fz\\ Fw \end{pmatrix} = \begin{pmatrix} &1 &0 &1/f &0 \\ &0 &1 &1/f &0 \\ &0 &0 &1 &0 \\ &0 &0 &1/f &0 \end{pmatrix} \begin{pmatrix} dx\\ dy\\ dz\\ 1 \end{pmatrix}$$

$$p = \begin{pmatrix} Fx/Fw\\ Fy/Fw\\ 1 \end{pmatrix}$$

Intrinsic Matrix Scales to pixel units and moves origin to center. $w$ is the width of screen and $W$ is the width of the sensor. Similarily with height

$Fx = w/W$

$Fy = h/H$

$$\begin{pmatrix} &Fx &0 &w/2 \\ &0 &Fy &h/2 \\ &0 &0 &1 \\ \end{pmatrix} $$

In typical projection I first multiply 3d point with extrinsic matrix, then project it using Projection matrix and then apply Intrinsic matrix.

But how can I reverse the process? I can use assumption that all points lie on the road plane (Y == 0). Yet I am not sure how to fit it with all these matrixes. I know I can invert Intrinsic and Extrinsic Matrix, but I can't do it with the projection matrix, because is singular.

Any lead would be useful. Thanks

$\endgroup$
  • $\begingroup$ Restricting the reverse mapping to a particular plane is a hugely useful assumption. See my updated answer, below. $\endgroup$ – amd Mar 11 '16 at 3:31
  • 1
    $\begingroup$ There's a whole subject called Multiple View Geometry about inverting this mapping. $\endgroup$ – littleO Mar 11 '16 at 3:34
7
$\begingroup$

The location on the image plane will give you a ray on which the object lies. You’ll need to use other information to determine where along this ray the object actually is, though. That information is lost when the object is projected onto the image plane. Assuming that the object is somewhere on the road plane is a huge simplification. Now, instead of trying to find the inverse of a perspective mapping, you only need to find a perspective projection of the image plane onto the road. That’s a fairly straightforward construction similar to the one used to derive the original perspective projection.

Start by working in camera-relative coordinates. A point $\mathbf p_i$ on the image plane has coordinates $(x_i,y_i,f)^T$. The original projection maps all points on the ray $\mathbf p_i t$ onto this point. Now, we’re assuming that the road is a plane, so it can be represented by an equation of the form $\mathbf n\cdot(\mathbf p_o-\mathbf r)=0$, where $\mathbf n$ is a normal to the plane and $\mathbf r$ is some known point on it. We seek the intersection of the ray and this plane, which will satisfy $\mathbf n\cdot(\mathbf p_i t-\mathbf r)=0$. Solving for $t$ and substituting gives $$\mathbf p_o = {\mathbf n\cdot \mathbf r \over \mathbf n\cdot \mathbf p_i}\mathbf p_i.$$ Moving to homogeneous coordinates, this mapping is the linear transformation represented by the matrix $$ M = \pmatrix{1&0&0&0 \\ 0&1&0&0 \\ 0&0&1&0 \\ {n_x \over \mathbf n\cdot\mathbf r} & {n_y \over \mathbf n\cdot\mathbf r} & {n_z \over \mathbf n\cdot\mathbf r} & 0}, $$ i.e., $$ \mathbf p_o = M\pmatrix{x_i \\ y_i \\ f \\ 1}. $$ Once you have this, it should be obvious how to complete the mapping back to world coordinates.

All that’s left is to find the parameters $\mathbf n$ and $\mathbf r$ that describe the road plane in camera coordinates. That’s also pretty simple. Since we’re taking the road to be the plane $y=0$ in world coordinates, its normal there is $(0,1,0)^T$. As for a known point on the road, the origin will do. Another reasonable choice is the point at which the camera’s optical axis meets the road, since the the camera-relative coordinates of that point will be of the form $(0,0,z)^T$. Convert both of these into camera-relative coordinates, and you’re done.

Note that you don’t necessarily need to know anything about the camera to compute a perspective transformation that will map from the image plane to the road plane. If you can somehow find four pairs of non-colinear points, i.e., a pair of quadrilaterals, that correspond to each other on these two planes, a planar perspective transformation that relates them can be computed fairly easily. See here for details. Essentially, you calibrate the camera view by matching a region of the image to a known region in the road plane.


Update 2018.10.22: If you have the complete camera matrix $P$, which you do, there’s a fairly straightforward way to construct the back-mapping to points on the road with a few matrix operations. We choose a coordinate system for the road plane, which gives us a $4\times3$ matrix $M$ that maps from these plane coordinates to world coordinates, i.e., $\mathbf X = M\mathbf x$. The image of this point is $PM\mathbf x$. If $PM$ is invertible, which it will be unless the camera center is on the road plane, the matrix $(PM)^{-1}$ maps from image to plane coordinates, and so the back-mapping from image to world coordinates on the road is $M(PM)^{-1}$. For the plane $Y=0$, a natural choice for $M$ is $$M=\begin{bmatrix}1&0&0\\0&0&0\\0&1&0\\0&0&1\end{bmatrix},$$ which simply inserts a $Y$-coordinate of zero to obtain world coordinates. You can adjust the origin of this coordinate system by changing the last column of $M$.

$\endgroup$
  • $\begingroup$ Thanks, this was very helpful. I spent some time working it in my model and it looks very promising. Just one thing that wasn't immediately clear for me. After multiplying by the M matrix is necessary to homogenise the coordinates by dividing it by the 4th row. $\endgroup$ – Tomáš Kohout Mar 11 '16 at 17:09
  • $\begingroup$ @TomášKohout Yes, you do need to transform back to $\mathbb R^3$ at some point, but since the camera-to-world mapping is also in homogeneous coordinates, I’d leave doing that until the very end. $\endgroup$ – amd Mar 11 '16 at 19:05
  • $\begingroup$ One more thing that I realised after a while (you actually mentioned it) was that I need to not only adjust translation of the plane, but when the pitch and yaw is changed also adjust the normal vector of the plane. Otherwise the projected lines would become skewed. This can be done by multiplying the normal vector by the rotational matrix. Anyway, now it works great. Thanks again! $\endgroup$ – Tomáš Kohout Mar 12 '16 at 0:52
  • $\begingroup$ @TomášKohout Glad it worked. You can multiply a homogeneous coordinate transformation matrix by an arbitrary non-zero scalar and still have the same transformation. Using $(\mathbf n\cdot\mathbf r)M$ might make things simpler. If you represent the normal as $(0,1,0,0)^T$, you can just multiply it by the world-to-camera matrix instead of pulling the rotations out separately; this also lets you compute $\mathbf n\cdot\mathbf r$ using the homogeneous vectors. Finally, I believe that dot product is just $-c_y$, so you may not even have to transform $\mathbf r$ at all. $\endgroup$ – amd Mar 12 '16 at 3:21

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.