Разложение ProjectionMatrix дает неожиданный результат

Question

У меня есть следующая матрица проекции P:

-375   0    2000  262500
-375  2000    0   262500
 -1    0      0    700

Эта проекционная матрица проецирует трехмерные точки в мм на детектор в пикселях (с 1 пикселем равным 0,5 мм) и строится из внутренней матрицы K и внешней матрицы [R|t] (где R - матрица вращения и t вектор перевода) в соответствии с соотношением P = K [R|t].

     2000    0    375             0   0   1              0
K =    0   2000   375       R =   0   1   0        t =   0
       0     0     1             -1   0   0             700

По некоторым причинам мне нужно разложить P обратно на эти матрицы. Когда я использую decomposeProjectionMatrix, я получаю это в качестве матрицы вращения:

 0   0   0
 0   0   0
-1   0   0

Что для меня не похоже на матрицу вращения.

Более того, когда я строю матрицу проекции из разложения Open CV, я получаю эту матрицу:

-375   0      0   262500
-375   0      0   262500
 -1    0      0    700

Похоже, но это не то же самое.

Мне интересно, делаю ли я что-то неправильно или мне не повезло, и это был один из редких случаев, когда эта функция не срабатывает.

Обратите внимание, что я произвел декомпозицию самостоятельно и получил согласованные результаты, но я бы предпочел использовать функции Open CV как можно больше.

Answer 1

Кажется, проблема в разложении RQ, используемом decomposeProjectionMatrix.Несмотря на то, что первый квадрат матрицы P не является единичным, функция RQDecomp3x3 дает неверные результаты:

    0   0  375              0  0  0
R = 0   0  375         Q =  0  0  0
    0   0   1              -1  0  0

Поэтому обходной путь заключается в использовании самодельной функции (здесь написанной на Python) на основев разделе 2.2 лекций Питера Штурма :

def decomposeP(P):
    import numpy as np
    M = P[0:3,0:3]
    Q = np.eye(3)[::-1]
    P_b = Q @ M @ M.T @ Q
    K_h = Q @ np.linalg.cholesky(P_b) @ Q
    K = K_h / K_h[2,2]
    A = np.linalg.inv(K) @ M
    l = (1/np.linalg.det(A)) ** (1/3)
    R = l * A
    t = l * np.linalg.inv(K) @ P[0:3,3]
    return K, R, t

Я использую матрицу антиидентификации Q для построения нетрадиционной декомпозиции Холецкого U U*, где U является верхнимтреугольные.Этот метод немного отличается от метода Питера Штурма, так как мы используем отношение P = K[R|t], тогда как в лекциях Питера Штурма используется отношение P = K[R|-Rt].

Реализация на C ++, использующая только Open CV, сложнее, так как они этого не делают.реально выставить функцию разложения по Холецкому:

void chol(cv::Mat const& S, cv::Mat& L)
{
    L = cv::Mat::zeros(S.rows, S.rows, cv::DataType<double>::type);
    for (int i = 0; i < S.rows; ++i) {
        for (int j = 0; j <= i ; ++j) {
            double sum = 0;
            for (int k = 0; k < j; ++k)
                sum += L.at<double>(i,k) * L.at<double>(j,k);
            L.at<double>(i,j) = (i == j) ? sqrt(S.at<double>(i,i) - sum) : (S.at<double>(i,j) - sum) / L.at<double>(j,j);
        }
    }
}

void decomposeP(cv::Mat const& P, cv::Mat& K, cv::Mat& R, cv::Mat& t)
{
    cv::Mat M(3, 3, cv::DataType<double>::type);
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            M.at<double>(i, j) = P.at<double>(i ,j);
    cv::Mat Q = cv::Mat::zeros(3, 3, cv::DataType<double>::type);
    Q.at<double>(0, 2) = 1.0;
    Q.at<double>(1, 1) = 1.0;
    Q.at<double>(2, 0) = 1.0;
    cv::Mat O = Q * M * M.t() * Q;
    cv::Mat C;
    chol(O, C);
    cv::Mat B = Q * C * Q;
    K = B / B.at<double>(2,2);
    cv::Mat A = K.inv() * M;
    double l = std::pow((1 / cv::determinant(A)), 1/3);
    R = l * A;
    cv::Mat p(3, 1, cv::DataType<double>::type);
    for (int i = 0; i < 3; ++i)
        p.at<double>(i, 0) = P.at<double>(i ,3);
    t = l * K.inv() * p;
}