来自视图矩阵（和返回）的平移和旋转向量

Question

问题

我有一个相机的视图矩阵（作为 4x4 矩阵），我想从中提取旋转和平移向量，然后返回（从 rot 和 transl 向量到视图矩阵）。

我找到了一些论坛可以做到这一点，但它们不起作用。

我该怎么做？

背景

对于增强现实应用程序，我根据失真参数和solvePnP 计算视图矩阵。该方法有效：我在图像平面中找到了一些点（通过单击它们），并且通过了解现实世界中的点，我可以估计相机位置。

点击过程有一些错误（点击不准确），所以我想通过手动修改平移和旋转向量来优化相机位置。

所以我正在寻找一种方法从矩阵中获取向量，然后从向量返回到矩阵。

测试

我现在做的是：

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

答案取决于您是使用列向量还是行向量来通过矩阵转换向量。假设您有一个矩阵M 和一个向量v，并且您将其转换为v' = Mv，则您使用的是列向量。相反，如果您有 v' = vM，则您正在使用行向量。

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

对于旋转，请尝试将您的矩阵乘以向量 [1 0 0 0]。这将告诉您“x 轴”映射到的位置。对于[0 1 0 0]（y 轴）和[0 0 1 0]（z 轴）也是如此。无论您使用的是列向量还是行向量，相关条目都是 4x4 矩阵左上角的 3x3 子矩阵。行向量和列向量之间唯一变化的是映射轴是子矩阵中的列向量还是行向量。

希望对您有所帮助。