3
$\begingroup$

I'm dealing with a Kalman Filter situation, trying to track points in 3D using cameras, each of which can represent a 3D point as a 2D projection according to:

$$ \begin{bmatrix} u \\ v \end{bmatrix} = \begin{bmatrix} \alpha_x & \gamma & u_0 \\ 0 & \alpha_y & v_0 \end{bmatrix} \cdot R_{3\times3} \cdot \begin{bmatrix} x \\ y \\ z \end{bmatrix} + \begin{bmatrix} \alpha_x & \gamma & u_0 \\ 0 & \alpha_y & v_0 \end{bmatrix} \cdot \vec{T}_{3\times1} $$

Where $u$ and $v$ are the pixel coordinates of a point in camera frame; $ \begin{bmatrix} \alpha_x & \gamma & u_0 \\ 0 & \alpha_y & v_0 \end{bmatrix} $ is the upper two rows of the camera matrix $K$, $R_{3x3}$ is a rotation matrix describing the rotation of the world coordinate frame relative to the camera; and $\vec{T}_{3x1}$ is a translation that describes the position of the world coordinate frame relative to the camera.

I'd really like this to be in the form

$$ \vec{y} = H \vec{x} $$

I've seen this $H$ called the "observation matrix" and in other cases the "Jacobian". But Jacobian would imply a structure like

$$ \begin{bmatrix} \frac{\partial y_1}{\partial x_1} & \frac{\partial y_1}{\partial x_2} & \frac{\partial y_1}{\partial x_3} \\ \frac{\partial y_2}{\partial x_1} & \frac{\partial y_2}{\partial x_2} & \frac{\partial y_2}{\partial x_3} \end{bmatrix} $$

and it's not clear to me that this would actually give you $\vec{y}$ from $\vec{x}$.

So I guess I have a few questions:

  • How can I derive a single $H$ to put my transformation in a form a Kalman filter can handle?

  • Should I use homogeneous coordinates (tack on some 1s) instead? Would the Kalman filter update to the state reliably keep its last entry a 1 then?

  • I could make my $\vec{y} = \begin{bmatrix} u \\ v \end{bmatrix} - \begin{bmatrix} \alpha_x & \gamma & u_0 \\ 0 & \alpha_y & v_0 \end{bmatrix} \cdot \vec{T}_{3\times1}$, but this really isn't what I want.

  • Does keeping the additive term on the right make this nonlinear? $\vec{y} = H_{2\times3} \vec{x} + K_{2\times3}\vec{T}_{3\times1} \rightarrow \vec{y} = \tilde{H}(\vec{x}) $? How does this then relate to the Jacobian?

$\endgroup$

1 Answer 1

1
$\begingroup$

I've made some progress.

First, my camera equation was definitely wrong: I was missing a division by range to object. This video is what caused me to see the light. Really:

$$ \begin{bmatrix} u \\ v \end{bmatrix} = K_{2\times3} \cdot \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix}/z' $$

where

$$ \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix} = R_{3\times3} \cdot \begin{bmatrix} x \\ y \\ z \end{bmatrix} + \vec{T}_{3 \times 1} \tag{1}$$

The primed ($'$) variables represent coordinates of the thing you're trying to project in to camera in the camera's reference frame, and the unprimed variables represent coordinates in the world frame. I'm trying to find $H$ w.r.t. the world frame.

This makes

$$ u = \frac{\alpha_x x'}{z'} + u_0 $$ $$ v = \frac{\alpha_y y'}{z'} + v_0 $$

If you follow the Jacobian equation that I left above in the question (very carefully, taking derivatives w.r.t. $x$ and friends rather than $x'$ and other friends) through a page and a half of calculus and simplifying linear algebra, you eventually end up with

$$ Jacobian_{2\times3} = \frac{KR}{z'} - \frac{K\vec{x'} \otimes R_3}{z'^2} $$

where $ \vec{x'} = \begin{bmatrix} x' \\ y' \\ z' \end{bmatrix} $, $R_3$ is the third row of the $R$ matrix, and $\otimes$ is an outer product.

Note that you can optionally include translation before rotation in equation (1), and it makes no difference to the derivatives because $x$, $y$, and $z$ don't appear in the translation term.

Second, there is a distinction to be made between system evolution and observation functions and the Jacobians. These tend to blur together because in the linear case multiplying by the Jacobian is the same as evaluating the function. Take the example $y_1 = ax_1 + bx_2$, $y_2 = cx_1 + dx_2$. Then we can write the system as:

$$ \begin{bmatrix} y_1 \\ y_2 \end{bmatrix} = \begin{bmatrix} a & b \\ c & d \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \end{bmatrix} $$

But also

$$ \begin{bmatrix} \frac{\partial y_1}{\partial x_1} & \frac{\partial y_1}{\partial x_2} \\ \frac{\partial y_2}{\partial x_1} & \frac{\partial y_2}{\partial x_2} \end{bmatrix} = \begin{bmatrix} a & b \\ c & d \end{bmatrix} $$

This is convenient if you need to both find $\vec{y}$ from $\vec{x}$ and do other things like project covariance matrices around, but in a nonlinear system you do these things by separate methods:

Notice the first line uses $h(x)$, but on subsequent lines we use the Jacobian $H$ evaluated at $x$. Likewise at the update step we use $f(x)$ to evolve the system and $F$ evaluated at $x$ to update covariance.

Now to specifically answer my questions:

  • You don't just get a single $H$; you need both $H(\vec{x})$ and $h(\vec{x})$, evaluated at $\vec{x}$ because the nonlinearity makes the shape vary from place to place.

  • No, don't use homogeneous coordinates. This is actually so nonlinear (thanks to that division by $z'$) that you're definitely going to need to use the Jacobian anyway. Not sure whether a filter could ever be trusted to keep a state variable stationary. If there were no noise in that variable and the update equations were just so, maybe? Irrelevant here now.

  • That subtraction isn't..nope.

  • I think linearity isn't actually violated by that additive term, since that's just an offset. But linearity is violated here after all.

$\endgroup$
2
  • 1
    $\begingroup$ Regarding the last point: This is in fact not a linear map, as linearity means that $f(\lambda_1 x_1 + \lambda_2 x_2)=\lambda_1 f(x_1)+ \lambda_2 f(x_2)$. What you have here is an affine map. $\endgroup$
    – sebhofer
    Nov 28, 2020 at 13:25
  • $\begingroup$ Ah, yes, technically you're right. $\endgroup$ Nov 29, 2020 at 6:04

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.