计算几何 – 求解两个三维向量之间的三维旋转矩阵

1 求解两个三维向量之间的3D旋转矩阵

1.1 方法1

先求解两个三维向量之间的夹角作为旋转角度，然后通过求解两个三维向量之间的叉乘向量作为旋转轴，最后通过旋转角度和旋转轴获取两个向量之间的旋转矩阵。

#include <iostream>
#include <Eigen/Dense>

int main()
{
    Eigen::Matrix3d rotation_matrix;

    Eigen::Vector3d vector_before{ 0, -1, 0 };
    Eigen::Vector3d vector_after{ 0.707, -0.707, 0 };

    // 求解两个向量之间的夹角
    double tem = vector_before.dot(vector_after);
    double tep = sqrt(vector_before.dot(vector_before) * vector_after.dot(vector_after));
    double angle = acos(tem / tep);

    if (isnan(angle))//acos取值范围[-1,1],若超出范围则越界，输出-1.#IND00
    {
        angle = acos(tep / tem);
    }

    // 求解两个向量之间的旋转轴
    Eigen::Vector3d axis = vector_before.cross(vector_after);

    // 求解旋转矩阵
    Eigen::Affine3d transform = Eigen::Affine3d::Identity();
    transform.translation() << 0, 0, 0;
    transform.rotate(Eigen::AngleAxisd(angle, axis.normalized()));

    std::cout << "Rotation Matrix: " << std::endl << transform.matrix() << std::endl;

    return 0;
}

输出：

Rotation Matrix:
 0.707107 -0.707107         0         0
 0.707107  0.707107         0         0
        0         0         1         0
        0         0         0         1

1.2 方法2

#include <iostream>

#include <Eigen/Dense>

Eigen::Matrix3d GetRotationMatrixBetweenTwo3DVector1(Eigen::Vector3d vector_before, Eigen::Vector3d vector_after)
{
    Eigen::Matrix3d rotation_matrix;

    rotation_matrix = Eigen::Quaterniond::FromTwoVectors(vector_before, vector_after).toRotationMatrix();

    return rotation_matrix;
}


int main()
{

    Eigen::Vector3d vector_before{ 0, -1, 0 };
    Eigen::Vector3d vector_after{ 0.707, -0.707, 0 };

    Eigen::Matrix3d rotation_matrix;

    rotation_matrix = Eigen::Quaterniond::FromTwoVectors(vector_before, vector_after).toRotationMatrix();

    std::cout << "Rotation Matrix: " << std::endl << rotation_matrix << std::endl;
    std::cout << "Result Point" << std::endl << rotation_matrix * vector_before << std::endl;

    return 0;
}

输出

Rotation Matrix:
 0.707107 -0.707107         0
 0.707107  0.707107         0
        0         0         1
Result Point
 0.707107
-0.707107
        0