opencv - 来自 View 矩阵的平移和旋转矢量(以及反向)

Question

题
我有我的相机的视点矩阵(作为4x4矩阵)，我想从中提取旋转和平移 vector ，然后从中提取(从腐烂和平移 vector 到视点矩阵)。
我已经找到一些这样做的论坛，但它们不起作用。
我怎样才能做到这一点？
背景
对于增强现实应用程序，我根据失真参数和solvePnP计算 View 矩阵。该方法有效:我在图像平面中找到了一些点(单击它们)，并且知道了现实世界中的点，因此可以估计摄像机的位置。
单击过程存在一些错误(单击不完全)，所以我想通过手动修改平移和旋转 vector 来优化相机位置。
因此，我正在寻找一种从矩阵中获取 vector ，然后再从 vector 返回到矩阵的方法。
测验
到目前为止，我正在做的是:

void matrix2vector(cv::Mat n) { // n is the pose matrix
    this->position[0] = n.at<float>(3);
    this->position[1] = n.at<float>(7);
    this->position[2] = n.at<float>(11);

    float rx, ry, rz;
    rx = atan2(n.at<float>(9), n.at<float>(10));
    ry = atan2(-n.at<float>(8), 
               sqrt( n.at<float>(9)*n.at<float>(9) + n.at<float>(10)*n.at<float>(10) ));
    rz = atan2(n.at<float>(4), n.at<float>(0));
    this->angle[0] = rx * RAD2DEG;
    this->angle[1] = ry * RAD2DEG;
    this->angle[2] = rz * RAD2DEG;
}

void vector2matrix() {
    _viewMatrix =  cv::Mat::eye(4, 4, CV_32F);
    rotateX(_viewMatrix, _viewMatrix, -angle[0]);
    rotateY(_viewMatrix, _viewMatrix, -angle[1]);
    rotateZ(_viewMatrix, _viewMatrix, -angle[2]);
    _viewMatrix.at<float>(3) = position[0];
    _viewMatrix.at<float>(7) = position[1];
    _viewMatrix.at<float>(11) = position[2];
}

旋转方法在​​哪里:

inline
void rotateX(cv::Mat &src, cv::Mat &dst, float angleDegree) {
    dst = src.clone();

    float c = cosf(angleDegree * DEG2RAD);
    float s = sinf(angleDegree * DEG2RAD);
    float m4 = src.at<float>(4),
          m5 = src.at<float>(5),
          m6 = src.at<float>(6),
          m7 = src.at<float>(7),
          m8 = src.at<float>(8),
          m9 = src.at<float>(9),
          m10= src.at<float>(10),
          m11= src.at<float>(11);

    dst.at<float>(4) = m4 * c + m8 *-s;
    dst.at<float>(5) = m5 * c + m9 *-s;
    dst.at<float>(6) = m6 * c + m10*-s;
    dst.at<float>(7) = m7 * c + m11*-s;
    dst.at<float>(8) = m4 * s + m8 * c;
    dst.at<float>(9) = m5 * s + m9 * c;
    dst.at<float>(10)= m6 * s + m10* c;
    dst.at<float>(11)= m7 * s + m11* c;
}

inline
void rotateY(cv::Mat &src, cv::Mat &dst, float angleDegree)
{
    float c = cosf(angleDegree * DEG2RAD);
    float s = sinf(angleDegree * DEG2RAD);
    float m0 = src.at<float>(0),
          m1 = src.at<float>(1),
          m2 = src.at<float>(2),
          m3 = src.at<float>(3),
          m8 = src.at<float>(8),
          m9 = src.at<float>(9),
          m10= src.at<float>(10),
          m11= src.at<float>(11);

    dst.at<float>(0) = m0 * c + m8 * s;
    dst.at<float>(1) = m1 * c + m9 * s;
    dst.at<float>(2) = m2 * c + m10* s;
    dst.at<float>(3) = m3 * c + m11* s;
    dst.at<float>(8) = m0 *-s + m8 * c;
    dst.at<float>(9) = m1 *-s + m9 * c;
    dst.at<float>(10)= m2 *-s + m10* c;
    dst.at<float>(11)= m3 *-s + m11* c;
}

inline
void rotateZ(cv::Mat &src, cv::Mat &dst, float angleDegree)
{
    float c = cosf(angleDegree * DEG2RAD);
    float s = sinf(angleDegree * DEG2RAD);
    float m0 = src.at<float>(0),
          m1 = src.at<float>(1),
          m2 = src.at<float>(2),
          m3 = src.at<float>(3),
          m4 = src.at<float>(4),
          m5 = src.at<float>(5),
          m6 = src.at<float>(6),
          m7 = src.at<float>(7);

    dst.at<float>(0) = m0 * c + m4 *-s;
    dst.at<float>(1) = m1 * c + m5 *-s;
    dst.at<float>(2) = m2 * c + m6 *-s;
    dst.at<float>(3) = m3 * c + m7 *-s;
    dst.at<float>(4) = m0 * s + m4 * c;
    dst.at<float>(5) = m1 * s + m5 * c;
    dst.at<float>(6) = m2 * s + m6 * c;
    dst.at<float>(7) = m3 * s + m7 * c;
}

Answer 1

答案取决于您使用的是列 vector 还是行 vector 来通过矩阵转换 vector 。假设您有一个矩阵M和一个 vector v，并通过v' = Mv对其进行了转换，则使用列 vector 。相反，如果您有v' = vM，则使用的是行 vector 。

要查找翻译，只需检查 vector [0 0 0 1]的结尾。如果您使用列 vector ，则会发现矩阵的最后一列是转换所在的位置。最后一列的前三个元素将是转换 vector (x，y，z)。同样，在行 vector 中，您会发现矩阵的最后一行是转换所在的位置。矩阵最后一行的前三个元素将是转换 vector (x，y，z)。

要进行旋转，请尝试将矩阵乘以 vector [1 0 0 0]。这将告诉您“x轴”的映射位置。对于[0 1 0 0](y轴)和[0 0 1 0](z轴)类似。无论您使用的是列 vector 还是行 vector ，相关的条目都是4x4矩阵左上方的3x3子矩阵。行 vector 和列 vector 之间唯一发生变化的是映射轴是子矩阵中的列 vector 还是行 vector 。

希望能有所帮助。