姿态矩阵与姿态向量的相互转化

罗德里格斯公式

转轴 n 是矩阵 R 特征值 1 对应的特征向量。求解此方程,再归一化,就得到了旋转轴。

CV_EXPORTS_W void Rodrigues( InputArray src, OutputArray dst, OutputArray jacobian = noArray() );

参数说明:

• src——为输入的旋转向量（3x1或者1x3）或者旋转矩阵（3x3）。该参数向量表示其旋转的角度，用向量长度表示。

• dst——为输出的旋转矩阵（3x3）或者旋转向量（3x1或者1x3）。

• jacobian——为可选的输出雅可比矩阵（3x9或者9x3），是输入与输出数组的偏导数。

举例

CMakeLists.txt

project(RotationMatrix)

# 使用Qt的模块
set(QT Core Widgets)
find_package(Qt5 COMPONENTS REQUIRED ${QT})

# 使用opencv的模块
find_package(OpenCV REQUIRED )

add_executable(RotationMatrix RotationMatrix.cpp)

target_link_libraries(RotationMatrix ${OpenCV_LIBS})

qt5_use_modules(RotationMatrix ${QT})

RotationMatrix.cpp

#include <stdio.h>
#include <QDebug>
#include <opencv2/opencv.hpp>
using namespace cv;


int main(int argc, char *argv[])
{
    // 旋转向量
    double posture[3] = {1, 0, 0};
    cv::Mat rxyz = cv::Mat(1, 3, CV_64F, posture);
    cv::Mat rotateMat = cv::Mat(3, 3, CV_64F);
    // 1x3旋转向量->3x3旋转矩阵
    cv::Rodrigues(rxyz, rotateMat);
    qDebug() << "1x3旋转向量->3x3旋转矩阵";
    qDebug() << rotateMat.at<double>(0, 0) << rotateMat.at<double>(0, 1) << rotateMat.at<double>(0, 2);
    qDebug() << rotateMat.at<double>(1, 0) << rotateMat.at<double>(1, 1) << rotateMat.at<double>(1, 2);
    qDebug() << rotateMat.at<double>(2, 0) << rotateMat.at<double>(2, 1) << rotateMat.at<double>(2, 2);

    // 3x3姿态矩阵->1x3旋转向量
    cv::Mat rxyz2 = cv::Mat(1, 3, CV_64F);
    cv::Rodrigues(rotateMat, rxyz2);
    qDebug() << "3x3旋转矩阵->1x3旋转向量";
    qDebug() << rxyz2.at<double>(0, 0) << rxyz2.at<double>(0, 1) << rxyz2.at<double>(0, 2);


    return 0;
}

结果