From P to K[R|t]

SLAM

字数统计: 819阅读时长: 3 min

 2020/07/01 

From P to K[R|t]

写在最前，本文参考链接：

[1] https://math.stackexchange.com/questions/1640695/rq-decomposition

[2] https://leohart.wordpress.com/2010/07/23/rq-decomposition-from-qr-decomposition/

[3] http://ksimek.github.io/2012/08/14/decompose/

[4] MVG p163

在从投影矩阵P中分解得到K[R|t]时，首先会进行RQ分解得到K和R，然后很容易就可由inv(K)xp4（P的第四列）得到t。具体可以参考[4]，下面主要是记录一些细节部分。

数据归一化

常规操作，详见[4]（略）。

RQ分解

QR分解一般程序库都有实现，那么如何从QR分解得到RQ分解？下面直接粘贴网上原答案（参考[1]）： \(\newcommand\iddots{\mathinner{ \kern1mu\raise1pt{.} \kern2mu\raise4pt{.} \kern2mu\raise7pt{\Rule{0pt}{7pt}{0pt}.} \kern1mu }}\)

This concerns \(RQ\) decompositions of square matrices.

Suppose you have a \(n \times n\) matrix \(A\) and want to compute the RQ decomposition. If you know how to compute QR decompositions, you will just need a few transpositions / row-column permutations.

Note that given the matrix \(P := \begin{bmatrix} & & 1\\ & \iddots & \\ 1 & & \\ \end{bmatrix}\) (this is different to the camera matrix mentioned in the question), we get that \(AP\) reverses the order of columns of \(A\) and \(PA\) reverses the order of rows. Also note that \(P^T = P\) and \(PP = E_n\), so \(P^{-1} = P = P^T\), in particular \(P\) is orthogonal.

Consider the following algorithm:

i.) Compute \(\tilde A := PA\) (i.e. reverse rows of \(A\))

ii.) Compute decomposition of \(\tilde A ^T = \tilde Q \tilde R\)

iii.) Set \(Q := P \tilde Q^T\) (i.e. reverse rows of \(\tilde Q^T\), note that \(Q\) is orthogonal)

iv.) Set \(R := P \tilde R^T P\)

In step iv.) the following happens: \(\tilde R\) is an upper triangular matrix. By transposing it, it becomes a lower triangular matrix. So we reverse rows and columns and obtain again an upper triangular matrix \(R\). See sketch (start with lower triangular, reverse rows, then revere columns).

\[\begin{bmatrix} * & \cdot & \cdot \\ * & * & \cdot \\ * & * & * \end{bmatrix} \to \begin{bmatrix} * & * & * \\ * & * & \cdot \\ * & \cdot & \cdot \end{bmatrix} \to \begin{bmatrix} * & * & * \\ \cdot & * & * \\ \cdot & \cdot & * \\ \end{bmatrix} \]

Altogether \(R\) and \(Q\) yields our decomposition:

\[RQ = (P \tilde R^T P)(P \tilde Q^T) = P \tilde R^T \tilde Q^T = P(\tilde Q \tilde R)^T = P(\tilde A ^T)^T = P \tilde A = PPA = A\]

代码实现部分，直接粘贴网上代码（参考[2, 3]）：

function [R Q] = rq(M)
    [Q,R] = qr(flipud(M)')
    R = flipud(R');
    R = fliplr(R);

    Q = Q';   
    Q = flipud(Q);

除了这种方式外，MVG附录中还提供了另一种RQ分解方式，OpenCV就是使用的这种（参考[1]）。

K归一化

分解出的K会包含尺度项（可正可负），也就是说K(2,2)不是1，因此需要对K归一化。

奇异性

RQ分解会有奇异性，也就是说把R的任意一列乘上-1，然后对应Q的那一行乘上-1后，结果不变。因此在相机标定问题中，通过强制分解出的K对角线为正数，可以消除不满足的解，具体可参考[3]。

此外，还需对R的行列式进行约束。如果det(R)=-1，需要对R和t都乘上-1才行。

虽然OpenCV中提供了decomposeProjectionMatrix函数，但也需要去做这一步约束才行。

BA优化

一般DLT只适合提供个初值，后面接一个BA效果会更好。而且可以在BA阶段加一些额外的参数进行约束，如畸变系数等。也可以约束skew为0、约束cx和cy的值、约束fx=fy等等，非常灵活方便。

个人理解错误的地方还请不吝赐教，转载请标明出处

原文作者：GJGJH

原文链接：https://gjgjh.github.io/%E5%88%86%E8%A7%A3%E6%8A%95%E5%BD%B1%E7%9F%A9%E9%98%B5P.html

发表日期：July 1st 2020, 10:44:30 pm

更新日期：February 2nd 2022, 7:37:38 pm

Next Post

pybind11
Previous Post

C++17相关

CATALOG

1. From P to K[R|t]