三维旋转变换

R&L_201810012025-12-18 8:09

下面列出了基于各种旋转顺序的组合函数，矩阵公式，以便于使用时直接拷贝，避免代码的编写错误，以及公式推导错误。废话不多说，直接上代码

cpp 复制代码 
//rotation.h
#pragma once
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <vector>
#include <string>
using namespace Eigen;

// 目前只实现加、减、乘符号的解析,不能包含括号
class Polynomial
{
public:
	Polynomial(const std::string& code)
		:mCode(code)
	{};
	~Polynomial() {};

	struct Node
	{
		int _positivity = 1;
		std::string _code;
	};

	std::vector<Node> nodes() const;
	friend Polynomial operator *(const Polynomial& left, const Polynomial& rigth);
	friend Polynomial operator +(const Polynomial& left, const Polynomial& rigth);

	std::string mCode;
};

class Rotation
{
public:
	Rotation() {};
	~Rotation() {};

	// 基本旋转矩阵
	static Matrix3d Rz(double yaw);
	static Matrix3d Rx(double pitch);
	static Matrix3d Ry(double roll);
	static Matrix3d R_zxy(double yaw, double pitch, double roll);
	static Matrix3d R_zyx(double yaw, double pitch, double roll);
	static Matrix3d R_xzy(double yaw, double pitch, double roll);
	static Matrix3d R_xyz(double yaw, double pitch, double roll);
	static Matrix3d R_yxz(double yaw, double pitch, double roll);
	static Matrix3d R_yzx(double yaw, double pitch, double roll);

	static Matrix3d Q_Rz(double yaw);
	static Matrix3d Q_Rx(double pitch);
	static Matrix3d Q_Ry(double roll);
	static Matrix3d Q_R_zxy(double yaw, double pitch, double roll);
	static Matrix3d Q_R_zyx(double yaw, double pitch, double roll);
	static Matrix3d Q_R_xzy(double yaw, double pitch, double roll);
	static Matrix3d Q_R_xyz(double yaw, double pitch, double roll);
	static Matrix3d Q_R_yxz(double yaw, double pitch, double roll);
	static Matrix3d Q_R_yzx(double yaw, double pitch, double roll);

	static Matrix3d rotationAroundAxis(const Vector3d& axis, double angle);
	static Matrix3d rotationAroundAxis2(const Vector3d& axis, double angle);
	static Matrix3d rotationAroundAxis3(const Vector3d& axis, double angle);

	// 必须是方阵
	static std::string MatrixMult(const std::vector<std::string>& left, const std::vector<std::string>& right, int size/*size*size*/);
	static std::vector<std::string> MatrixMultV(const std::vector<std::string>& left, const std::vector<std::string>& right, int size/*size*size*/);

	static std::string MatrixMultToEigenCode3d(const std::vector<std::string>& left, const std::vector<std::string>& right);
	static std::string MatrixMultToEigenCode4d(const std::vector<std::string>& left, const std::vector<std::string>& right);
};
cpp 复制代码 
// rotation.cpp
#include "rotation.h"


/*
    double cy = cos(yaw), sy = sin(yaw);
    double cp = cos(pitch), sp = sin(pitch);
    double cr = cos(roll), sr = sin(roll);
    yaw->Z轴，pitch->X轴，roll->Y轴
*/

// 基本旋转矩阵
Matrix3d Rotation::Rz(double yaw) {
    /*
    Rz = [ cy  -sy   0
           sy   cy   0
           0    0    1 ]
    */
    double cy = cos(yaw), sy = sin(yaw);
    Matrix3d R;
    R << cy, -sy, 0,
        sy, cy, 0,
        0, 0, 1;
    return R;
}

Matrix3d Rotation::Rx(double pitch) {
    /*
    Rx = [ 1    0    0
           0   cp  -sp
           0   sp   cp ]
    */
    double cp = cos(pitch), sp = sin(pitch);
    Matrix3d R;
    R << 1, 0, 0,
        0, cp, -sp,
        0, sp, cp;
    return R;
}

Matrix3d Rotation::Ry(double roll) {
    /*
    Ry = [ cr   0   sr
           0    1    0
           -sr   0   cr ]
    */
    double cr = cos(roll), sr = sin(roll);
    Matrix3d R;
    R << cr, 0, sr,
        0, 1, 0,
        -sr, 0, cr;
    return R;
}

//yaw->Z轴，pitch->X轴，roll->Y轴
Matrix3d Rotation::R_zxy(double yaw, double pitch, double roll) {
    // Rz * Rx * Ry

    double cy = cos(yaw), sy = sin(yaw);
    double cp = cos(pitch), sp = sin(pitch);
    double cr = cos(roll), sr = sin(roll);
   /* Eigen::Matrix3d R;
    R <<
        cy * cr - sy * sp * sr, -sy * cp, cy* sr + sy * sp * cr,
        sy* cr + cy * sp * sr, cy* cp, sy* sr - cy * sp * cr,
        -cp * sr, sp, cp* cr;*/
    Matrix3d R;
    R << cy * cr - sy * sp * sr, -sy * cp, cy* sr + sy * sp * cr, sy* cr + cy * sp * sr, cy* cp, sy* sr - cy * sp * cr, -cp * sr, sp, cp* cr;
    return R;
}

//yaw->Z轴，pitch->X轴，roll->Y轴
Matrix3d Rotation::R_zyx(double yaw, double pitch, double roll) {
    //Rz * Ry* Rx 
    double cy = cos(yaw), sy = sin(yaw);
    double cp = cos(pitch), sp = sin(pitch);
    double cr = cos(roll), sr = sin(roll);

   /* Eigen::Matrix3d R;

    R <<
        cy * cr, cy* sr* sp - sy * cp, cy* sr* cp + sy * sp,
        sy* cr, sy* sr* sp + cy * cp, sy* sr* cp - cy * sp,
        -sr, cr* sp, cr* cp;*/
    Matrix3d R;
    R << cy * cr, -sy * cp + cy * sr * sp, sy* sp + cy * sr * cp, sy* cr, cy* cp + sy * sr * sp, -cy * sp + sy * sr * cp, -sr, cr* sp, cr* cp;

    return R;
}

//yaw->Z轴，pitch->X轴，roll->Y轴
Matrix3d Rotation::R_xzy(double yaw, double pitch, double roll) {

    //Rx * Rz * Ry
    /*
    R = [ cy*cr           -sy    cy*sr           ]
        [ cp*sy*cr+sp*sr  cp*cy  cp*sy*sr-sp*cr  ]
        [ sp*sy*cr-cr*sr  sp*cy  sp*sy*sr+cp*cr  ]
    */
    double cy = cos(yaw), sy = sin(yaw);
    double cp = cos(pitch), sp = sin(pitch);
    double cr = cos(roll), sr = sin(roll);

    //Matrix3d R;
    //R << cy * cr,               -sy,      cy* sr,
    //     cp* sy* cr + sp * sr,  cp* cy,   cp* sy* sr - sp * cr,
    //     sp* sy* cr - cp * sr,  sp* cy,   sp* sy* sr + cp * cr;
    Matrix3d R;
    R << cy * cr, -sy, cy* sr, cp* sy* cr + sp * sr, cp* cy, cp* sy* sr - sp * cr, sp* sy* cr - cp * sr, sp* cy, sp* sy* sr + cp * cr;
    return R;
}

//yaw->Z轴，pitch->X轴，roll->Y轴
Matrix3d Rotation::R_xyz(double yaw, double pitch, double roll) {

    //Rx * Ry * Rz
    /*
    R = [ cr*cy           -cr*sy          sr     ]
        [ cp*sy+sp*sr*cy  cp*cy-sp*sr*sy  -sp*cr ]
        [ sp*sy-cp*sr*cy  sp*cy+cp*sr*sy  cp*cr  ] 
    */

    double cy = cos(yaw), sy = sin(yaw);
    double cp = cos(pitch), sp = sin(pitch);
    double cr = cos(roll), sr = sin(roll);

    //Matrix3d R;
    //R(0, 0) = cr * cy;
    //R(0, 1) = -cr * sy;
    //R(0, 2) = sr;

    //R(1, 0) = cp * sy + sp * sr * cy;
    //R(1, 1) = cp * cy - sp * sr * sy;
    //R(1, 2) = -sp * cr;

    //R(2, 0) = sp * sy - cp * sr * cy;
    //R(2, 1) = sp * cy + cp * sr * sy;
    //R(2, 2) = cp * cr;
    Matrix3d R;
    R << cr * cy, -cr * sy, sr, sp* sr* cy + cp * sy, -sp * sr * sy + cp * cy, -sp * cr, -cp * sr * cy + sp * sy, cp* sr* sy + sp * cy, cp* cr;
    return R;
}

//yaw->Z轴，pitch->X轴，roll->Y轴
Matrix3d Rotation::R_yxz(double yaw, double pitch, double roll) {

    // Ry * Rx * Rz
    /*
    R = [ cy*cr + sy*sp*sr   -cr*sy+sr*sp*cy   sr*cp ]
        [ sy*cp              cy*cp              -sp  ]
        [ -sr*cy+cr*sp*sy   sy*sr + cy*sp*cr   cr*cp ]
    */

    double cy = cos(yaw), sy = sin(yaw);
    double cp = cos(pitch), sp = sin(pitch);
    double cr = cos(roll), sr = sin(roll);

    /*Matrix3d R;
    R(0, 0) = cy * cr + sy * sp * sr;
    R(0, 1) = -cr * sy + sr * sp * cy;
    R(0, 2) = sr * cp;

    R(1, 0) = sy * cp;
    R(1, 1) = cy * cp;
    R(1, 2) = -sp;

    R(2, 0) = -sr * cy + cr * sp * sy;
    R(2, 1) = sy * sr + cy * sp * cr;
    R(2, 2) = cr * cp;*/
    Matrix3d R;
    R << cr * cy + sr * sp * sy, -cr * sy + sr * sp * cy, sr* cp, cp* sy, cp* cy, -sp, -sr * cy + cr * sp * sy, sr* sy + cr * sp * cy, cr* cp;

    return R;
}

//yaw->Z轴，pitch->X轴，roll->Y轴
Matrix3d Rotation::R_yzx(double yaw, double pitch, double roll) {

    //Ry * Rz * Rx

    double cy = cos(yaw), sy = sin(yaw);
    double cp = cos(pitch), sp = sin(pitch);
    double cr = cos(roll), sr = sin(roll);

    //Matrix3d R;
    //R << cr* cy, -cr * sy * cp + sr * sp, cr* sy* sp + sr * cp,
    //    sy, cy* cp, -cy * sp,
    //    -sr * cy, sr* sy* cp + cr * sp, -sr * sy * sp + cr * cp;
    Matrix3d R;
    R << cr * cy, -cr * sy * cp + sr * sp, cr* sy* sp + sr * cp, sy, cy* cp, -cy * sp, -sr * cy, sr* sy* cp + cr * sp, -sr * sy * sp + cr * cp;
    return R;
}

Matrix3d Rotation::Q_Rz(double yaw)
{
    return Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()).toRotationMatrix();
}

Matrix3d Rotation::Q_Rx(double pitch)
{
    return Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitX()).toRotationMatrix();
}

Matrix3d Rotation::Q_Ry(double roll)
{
    return Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitY()).toRotationMatrix();
}

//yaw->Z轴，pitch->X轴，roll->Y轴
Matrix3d Rotation::Q_R_zxy(double yaw, double pitch, double roll)
{
    Eigen::Quaterniond rot = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitY());
    return rot.toRotationMatrix();
}

//yaw->Z轴，pitch->X轴，roll->Y轴
Matrix3d Rotation::Q_R_zyx(double yaw, double pitch, double roll)
{
    Eigen::Quaterniond rot = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitY())*
        Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitX());
    return rot.toRotationMatrix();
}

//yaw->Z轴，pitch->X轴，roll->Y轴
Matrix3d Rotation::Q_R_xzy(double yaw, double pitch, double roll)
{
    Eigen::Quaterniond rot = Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitY());
    return rot.toRotationMatrix();
}

//yaw->Z轴，pitch->X轴，roll->Y轴
Matrix3d Rotation::Q_R_xyz(double yaw, double pitch, double roll)
{
    Eigen::Quaterniond rot = Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitY())*
        Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
    return rot.toRotationMatrix();
}

//yaw->Z轴，pitch->X轴，roll->Y轴
Matrix3d Rotation::Q_R_yxz(double yaw, double pitch, double roll)
{
    Eigen::Quaterniond rot = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitY())*
        Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitX()) *
        Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
    return rot.toRotationMatrix();
}

//yaw->Z轴，pitch->X轴，roll->Y轴
Matrix3d Rotation::Q_R_yzx(double yaw, double pitch, double roll)
{
    Eigen::Quaterniond rot = Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitY())*
        Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitX());
    return rot.toRotationMatrix();
}

// 给定单位向量 u = (x₀, y₀, z₀) 和旋转角度 θ，绕轴 u 旋转 θ 角度的旋转矩阵为：
// R = I + sinθ·[u]× + (1 - cosθ)·[u]ײ
// 其中：
// I 是 3×3 单位矩阵
// [u]× 是向量 u 的叉乘矩阵（反对称矩阵）
// [u]ײ 是 [u]× 的平方


// 使用罗德里格斯公式直接计算
/*
设：
    cθ = cosθ, sθ = sinθ, vθ = 1 - cosθ
    x = x₀, y = y₀, z = z₀
    则旋转矩阵为：
    R = [ cθ + x²·vθ      xy·vθ - z·sθ     xz·vθ + y·sθ ]
        [ xy·vθ + z·sθ   cθ + y²·vθ        yz·vθ - x·sθ ]
        [ xz·vθ - y·sθ   yz·vθ + x·sθ     cθ + z²·vθ    ]

*/
Matrix3d Rotation::rotationAroundAxis(const Vector3d& axis, double angle) {
    // 确保轴是单位向量
    Vector3d u = axis.normalized();

    double x = u.x(), y = u.y(), z = u.z();

    double c = cos(angle);
    double s = sin(angle);
    double v = 1 - c;  // versine = 1 - cos

    Matrix3d R;

    // 第一行
    R(0, 0) = c + x * x * v;
    R(0, 1) = x * y * v - z * s;
    R(0, 2) = x * z * v + y * s;

    // 第二行
    R(1, 0) = y * x * v + z * s;
    R(1, 1) = c + y * y * v;
    R(1, 2) = y * z * v - x * s;

    // 第三行
    R(2, 0) = z * x * v - y * s;
    R(2, 1) = z * y * v + x * s;
    R(2, 2) = c + z * z * v;

    return R;
}

// 使用叉乘矩阵公式
/*
  叉乘矩阵：
  [u]× = [  0  -z   y ]
          [  z   0  -x ]
          [ -y   x   0 ]
  平方：
  [u]ײ = [ -(y²+z²)    xy        xz      ]
           [    xy     -(x²+z²)    yz      ]
           [    xz        yz     -(x²+y²)  ]
*/
Matrix3d Rotation::rotationAroundAxis2(const Vector3d& axis, double angle) {
    // 确保轴是单位向量
    Vector3d u = axis.normalized();

    double c = cos(angle);
    double s = sin(angle);

    // 单位矩阵
    Matrix3d I = Matrix3d::Identity();

    // 叉乘矩阵 [u]×
    Matrix3d U_cross;
    U_cross << 0, -u.z(), u.y(),
        u.z(), 0, -u.x(),
        -u.y(), u.x(), 0;

    // [u]ײ
    Matrix3d U_cross_sq = U_cross * U_cross;

    // 罗德里格斯公式: R = I + sinθ·[u]× + (1-cosθ)·[u]ײ
    return I + s * U_cross + (1 - c) * U_cross_sq;
}

//使用Eigen的AngleAxis类（推荐）
Matrix3d Rotation::rotationAroundAxis3(const Vector3d& axis, double angle) {
    // 使用Eigen内置的AngleAxis类
    AngleAxisd rotation(angle, axis.normalized());
    return rotation.toRotationMatrix();
}

std::vector<Polynomial::Node> Polynomial::nodes() const
{
    static auto split = [](const std::string& str, char delimiter)->std::vector<std::string>
    {
        std::vector<std::string> tokens;
        std::stringstream ss(str);
        std::string token;
        while (std::getline(ss, token, delimiter)) {
            if (token.empty())
                continue;
            tokens.push_back(token);
        }
        return tokens;
    };

    std::vector<Polynomial::Node> nodes;
    if (mCode == "0" || mCode.empty())
        return nodes;
    auto iter1 = mCode.find_last_of('+');
    auto iter2 = mCode.find_last_of('-');
    if (iter1 == std::string::npos && iter2 == std::string::npos)
    {
        Polynomial::Node node;
        node._code = mCode;
        node._positivity = 1;
        nodes.push_back(node);
        return nodes;
    }
    else if (iter1 == std::string::npos && iter2 == 0)
    {
        Polynomial::Node node;
        node._code = mCode.substr(1,-1);
        node._positivity = -1;
        nodes.push_back(node);
        return nodes;
    }

    std::vector<std::string> codes1 = split(mCode, '+');

    // 先处理0
    std::vector<std::string> codes2 = split(codes1[0], '-');

    if (mCode[0] == '-')
    {
        Polynomial::Node node;
        node._code = codes2[0];
        node._positivity = -1;
        nodes.push_back(node);
    }

    for (int i = 1; i < codes2.size(); i++)
    {
        Polynomial::Node node;
        node._code = codes2[i];
        node._positivity = -1;
        nodes.push_back(node);
    }
    // 接着处理后面的
    for (int i = 1; i < codes1.size(); i++)
    {
        std::vector<std::string> codes3 = split(codes1[i], '-');
        Polynomial::Node node;
        node._code = codes3[0];
        node._positivity = 1;
        nodes.push_back(node);
        for (int j = 1; j < codes3.size(); j++)
        {
            Polynomial::Node node;
            node._code = codes3[j];
            node._positivity = -1;
            nodes.push_back(node);
        }
    }
    return nodes;
}

Polynomial operator *(const Polynomial& left, const Polynomial& rigth)
{
    std::vector<Polynomial::Node> lnodes = left.nodes();
    std::vector<Polynomial::Node> rnodes = rigth.nodes();
    if (lnodes.empty() || rnodes.empty())
        return Polynomial("0");

    std::vector<Polynomial::Node> nodes;
    for (int i = 0; i < lnodes.size(); i++)
    {
        for (int j = 0; j < rnodes.size(); j++)
        {
            Polynomial::Node node;
            node._positivity = lnodes[i]._positivity * rnodes[j]._positivity;
            if (lnodes[i]._code == "1")
            {
                node._code = rnodes[j]._code;
            }
            else if (rnodes[i]._code == "1")
            {
                node._code = lnodes[i]._code;
            }
            else
            {
                node._code = lnodes[i]._code + "*" + rnodes[j]._code;
            }
            
            nodes.push_back(node);
        }
    }
    if (nodes.empty())
        return Polynomial("");
    std::string code = "";
    if (nodes[0]._positivity == -1)
        code += "-" + nodes[0]._code;
    else
        code += nodes[0]._code;
    for (int i = 1; i < nodes.size(); i++)
    {
        if (nodes[i]._positivity == -1)
            code += "-" + nodes[0]._code;
        else
            code += "+" +nodes[0]._code;
    }
    return code;
}
Polynomial operator +(const Polynomial& left, const Polynomial& rigth)
{
    if (left.mCode.empty() || left.mCode == "0")
    {
        return Polynomial(rigth.mCode);
    }

    if (rigth.mCode.empty() || rigth.mCode == "0")
    {
        return Polynomial(left.mCode);
    }

    std::string code = left.mCode;
    if (rigth.mCode[0] == '-')
        code += rigth.mCode;
    else
        code += "+" + rigth.mCode;
    return Polynomial(code);
}

std::string Rotation::MatrixMult(const std::vector<std::string>& left, const std::vector<std::string>& right, int size/*size*size*/)
{
    std::vector<std::string> vs = MatrixMultV(left, right, size);
    std::string code = "R = ";
    for (int i = 0; i < size; i++)
    {
        code += "\n["+ vs[i*size];
        for (int j = 1; j < size; j++)
        {
            code += "," + vs[i * size + j];
        }
        code += "]";
    }
    return code;
}

std::vector<std::string> Rotation::MatrixMultV(const std::vector<std::string>& left, const std::vector<std::string>& right, int size/*size*size*/)
{
    std::vector<std::string> r;
    if (left.size() < size * size || right.size() < size * size)
        return r;
    
    for (int i = 0; i < size; ++i)
    {
        std::vector<std::string> lefti; // left第i行的数据
        for (int j = 0; j < size; ++j)
        {
            int index = i * size + j;
            lefti.push_back(left[index]);
        }

        for (int j = 0; j < size; ++j)
        {
            std::vector<std::string> rightj; // right第j列的数据
            for (int k = 0; k < size; k++)
            {
                int index = k * size + j;
                rightj.push_back(right[index]);
            }


            Polynomial rij = Polynomial(lefti[0]) * Polynomial(rightj[0]);
            for (int k = 1; k < size; k++)
            {
                rij = rij + Polynomial(lefti[k]) * Polynomial(rightj[k]);
            }
            r.push_back(rij.mCode);
        }
    }
    return r;
}

std::string Rotation::MatrixMultToEigenCode3d(const std::vector<std::string>& left, const std::vector<std::string>& right)
{
    std::vector<std::string> vs = MatrixMultV(left, right, 3);
    std::string code = "Matrix3d R;\nR << ";
    code += vs[0];
    for (int i = 1; i < 9; i++)
    {
        code += "," + vs[i];
    }
    code += ";";
    return code;
}

std::string Rotation::MatrixMultToEigenCode4d(const std::vector<std::string>& left, const std::vector<std::string>& right)
{
    std::vector<std::string> vs = MatrixMultV(left, right, 4);
    std::string code = "Matrix4d R;\n R << ";
    code += vs[0];
    for (int i = 1; i < 16; i++)
    {
        code += "," + vs[i];
    }
    code += ";";
    return code;
}

下面是我写的测试验证DEMO

cpp 复制代码 
//test.h
#pragma once

#include <vector>

class Test
{
public:
	~Test() {};
	struct datastruct
	{
		datastruct(double yaw, double pitch, double roll)
			:_yaw(yaw)
			, _pitch(pitch)
			, _roll(roll)
		{}

		double _yaw;
		double _pitch;
		double _roll;
	};

	Test();
	void print();
	void doTest();
private:
	std::vector<datastruct> mDataList;
};
cpp 复制代码 
//test.cpp
#include "test.h"
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include "rotation.h"

Test::Test()
{
	// 单位度
	mDataList.push_back({ 220.847, -2.800, 1.521 }); 
	mDataList.push_back({ 207.376, -2.566, 0.506 });
	mDataList.push_back({ 221.472, -2.884, 0.693 });
}
void Test::doTest()
{
	std::vector<std::string> Rx = { "1","0","0",
									"0","cp","-sp",
									"0","sp","cp" };

	std::vector<std::string> Ry = { "cr","0","sr",
								    "0","1","0",
								    "-sr","0","cr" };

	std::vector<std::string> Rz = { "cy","-sy","0",
								    "sy","cy","0",
								    "0","0","1" };
	std::cout << "****** Rx*Ry*Rz: ******\n";
	std::cout << Rotation::MatrixMultToEigenCode3d(Rotation::MatrixMultV(Rx, Ry, 3), Rz);
	std::cout << "\n";
	std::cout << "\n";
	std::cout << Rotation::MatrixMult(Rotation::MatrixMultV(Rx, Ry, 3), Rz,3);

	std::cout << "\n\n****** Rx*Rz*Ry: ******\n";
	std::cout << Rotation::MatrixMultToEigenCode3d(Rotation::MatrixMultV(Rx, Rz, 3), Ry);
	std::cout << "\n";
	std::cout << "\n";
	std::cout << Rotation::MatrixMult(Rotation::MatrixMultV(Rx, Rz, 3), Ry,3);

	std::cout << "\n\n****** Ry*Rx*Rz: ******\n";
	std::cout << Rotation::MatrixMultToEigenCode3d(Rotation::MatrixMultV(Ry, Rx, 3), Rz);
	std::cout << "\n";
	std::cout << "\n";
	std::cout << Rotation::MatrixMult(Rotation::MatrixMultV(Ry, Rx, 3), Rz,3);

	std::cout << "\n\n****** Ry*Rz*Rx: ******\n";
	std::cout << Rotation::MatrixMultToEigenCode3d(Rotation::MatrixMultV(Ry, Rz, 3), Rx);
	std::cout << "\n";
	std::cout << "\n";
	std::cout << Rotation::MatrixMult(Rotation::MatrixMultV(Ry, Rz, 3), Rx,3);

	std::cout << "\n\n****** Rz*Rx*Ry: ******\n";
	std::cout << Rotation::MatrixMultToEigenCode3d(Rotation::MatrixMultV(Rz, Rx, 3), Ry);
	std::cout << "\n";
	std::cout << "\n";
	std::cout << Rotation::MatrixMult(Rotation::MatrixMultV(Rz, Rx, 3), Ry,3);

	std::cout << "\n\n****** Rz*Ry*Rx: ******\n";
	std::cout << Rotation::MatrixMultToEigenCode3d(Rotation::MatrixMultV(Rz, Ry, 3), Rx);
	std::cout << "\n";
	std::cout << "\n";
	std::cout << Rotation::MatrixMult(Rotation::MatrixMultV(Rz, Ry, 3), Rx,3);

	std::cout << "\n";

	std::cout << "\n";

	// 定义PI
	const double PI_CONST = 3.14159265358979323846;
	for (int i = 0; i < mDataList.size(); i++)
	{
		/*std::cout << std::endl;
		std::cout << "@begin --> i = " << i << std::endl;
		std::cout << "_yaw:" << mDataList[i]._yaw << "  _pitch:" << mDataList[i]._pitch << "  _roll:" << mDataList[i]._roll << std::endl;

		double yaw = mDataList[i]._yaw * PI_CONST / 180.0;
		double pitch = mDataList[i]._pitch * PI_CONST / 180.0;
		double roll = mDataList[i]._roll * PI_CONST / 180.0;
		std::cout << "yaw:" << yaw << "  pitch:" << pitch << "  roll:" << roll << std::endl;

		Eigen::Quaterniond rot_o = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitY());
		Eigen::Matrix3d ma_o= rot_o.toRotationMatrix();

		Eigen::Vector3d vd = ma_o.eulerAngles(2, 0, 1);

		double yaw_n = vd[0];
		double pitch_n = vd[1];
		double roll_n = vd[2];
		std::cout << "yaw_n:" << yaw_n << "  pitch_n:" << pitch_n << "  roll_n:" << roll_n << std::endl;

		Eigen::Quaterniond rot_n = Eigen::AngleAxisd(yaw_n, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(pitch_n, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(roll_n, Eigen::Vector3d::UnitY());
		Eigen::Matrix3d ma_n = rot_n.toRotationMatrix();

		std::cout << "矩阵差值的范数（应接近0）：" << (ma_n - ma_o).norm() << std::endl;*/


		std::cout << std::endl;
		std::cout << "@begin --> i = " << i << std::endl;
		std::cout << "_yaw:" << mDataList[i]._yaw << "  _pitch:" << mDataList[i]._pitch << "  _roll:" << mDataList[i]._roll << std::endl;
		double yaw = mDataList[i]._yaw * PI_CONST / 180.0;
		double pitch = mDataList[i]._pitch * PI_CONST / 180.0;
		double roll = mDataList[i]._roll * PI_CONST / 180.0;
		std::cout << "yaw:" << yaw << "  pitch:" << pitch << "  roll:" << roll << std::endl;

		Eigen::Matrix3d Rx1 = Rotation::Rx(pitch);
		Eigen::Matrix3d Ry1 = Rotation::Ry(roll);
		Eigen::Matrix3d Rz1 = Rotation::Rz(yaw);

		Eigen::Matrix3d Rx2 = Rotation::Q_Rx(pitch);
		Eigen::Matrix3d Ry2 = Rotation::Q_Ry(roll);
		Eigen::Matrix3d Rz2 = Rotation::Q_Rz(yaw);

		std::cout << "Rx1 - Rx2矩阵差值的范数（应接近0）：" << (Rx1 - Rx2).norm() << std::endl;
		std::cout << "Ry1 - Ry2矩阵差值的范数（应接近0）：" << (Ry1 - Ry2).norm() << std::endl;
		std::cout << "Rz1 - Rz2矩阵差值的范数（应接近0）：" << (Rz1 - Rz2).norm() << std::endl;


		{
			Eigen::Matrix3d Rxyz1 = Rx1 * Ry1 * Rz1;
			Eigen::Matrix3d Rxyz2 = Rotation::R_xyz(yaw, pitch, roll);
			Eigen::Matrix3d Rxyz3 = Rotation::Q_R_xyz(yaw, pitch, roll);

			Eigen::Vector3d vd = Rxyz2.eulerAngles(0, 1, 2);

			double pitch_n = vd[0];
			double roll_n = vd[1];
			double yaw_n = vd[2];
		
			std::cout << "yaw_n:" << yaw_n << "  pitch_n:" << pitch_n << "  roll_n:" << roll_n << std::endl;
			Eigen::Matrix3d Rxyz4 = Rotation::Q_R_xyz(yaw_n, pitch_n, roll_n);

			std::cout << "Rxyz1 - Rxyz2矩阵差值的范数（应接近0）：" << (Rxyz1 - Rxyz2).norm() << std::endl;
			std::cout << "Rxyz1 - Rxyz3矩阵差值的范数（应接近0）：" << (Rxyz1 - Rxyz3).norm() << std::endl;
			std::cout << "Rxyz2 - Rxyz3矩阵差值的范数（应接近0）：" << (Rxyz2 - Rxyz3).norm() << std::endl;
			std::cout << "Rxyz4 - Rxyz3矩阵差值的范数（应接近0）：" << (Rxyz4 - Rxyz3).norm() << std::endl;
		}

		{
			Eigen::Matrix3d Rxzy1 = Rx1 * Rz1 * Ry1;
			Eigen::Matrix3d Rxzy2 = Rotation::R_xzy(yaw, pitch, roll);
			Eigen::Matrix3d Rxzy3 = Rotation::Q_R_xzy(yaw, pitch, roll);

			Eigen::Vector3d vd = Rxzy2.eulerAngles(0, 2, 1);
			double pitch_n = vd[0];
			double roll_n = vd[2];
			double yaw_n = vd[1];
		

			std::cout << "yaw_n:" << yaw_n << "  pitch_n:" << pitch_n << "  roll_n:" << roll_n << std::endl;
			Eigen::Matrix3d Rxzy4 = Rotation::Q_R_xzy(yaw_n, pitch_n, roll_n);

			std::cout << "Rxzy1 - Rxzy2矩阵差值的范数（应接近0）：" << (Rxzy1 - Rxzy2).norm() << std::endl;
			std::cout << "Rxzy1 - Rxzy3矩阵差值的范数（应接近0）：" << (Rxzy1 - Rxzy3).norm() << std::endl;
			std::cout << "Rxzy2 - Rxzy3矩阵差值的范数（应接近0）：" << (Rxzy2 - Rxzy3).norm() << std::endl;
			std::cout << "Rxzy4 - Rxzy3矩阵差值的范数（应接近0）：" << (Rxzy4 - Rxzy3).norm() << std::endl;
		}

		{
			Eigen::Matrix3d Ryxz1 = Ry1* Rx1 * Rz1;
			Eigen::Matrix3d Ryxz2 = Rotation::R_yxz(yaw, pitch, roll);
			Eigen::Matrix3d Ryxz3 = Rotation::Q_R_yxz(yaw, pitch, roll);

			Eigen::Vector3d vd = Ryxz2.eulerAngles(1, 0, 2);
			double pitch_n = vd[1];
			double roll_n = vd[0];
			double yaw_n = vd[2];


			std::cout << "yaw_n:" << yaw_n << "  pitch_n:" << pitch_n << "  roll_n:" << roll_n << std::endl;
			Eigen::Matrix3d Ryxz4 = Rotation::Q_R_yxz(yaw_n, pitch_n, roll_n);

			std::cout << "Ryxz1 - Ryxz2矩阵差值的范数（应接近0）：" << (Ryxz1 - Ryxz2).norm() << std::endl;
			std::cout << "Ryxz1 - Ryxz3矩阵差值的范数（应接近0）：" << (Ryxz1 - Ryxz3).norm() << std::endl;
			std::cout << "Ryxz2 - Ryxz3矩阵差值的范数（应接近0）：" << (Ryxz2 - Ryxz3).norm() << std::endl;
			std::cout << "Ryxz4 - Ryxz3矩阵差值的范数（应接近0）：" << (Ryxz4 - Ryxz3).norm() << std::endl;
		}

		{
			Eigen::Matrix3d Ryzx1 = Ry1 * Rz1 * Rx1;
			Eigen::Matrix3d Ryzx2 = Rotation::R_yzx(yaw, pitch, roll);
			Eigen::Matrix3d Ryzx3 = Rotation::Q_R_yzx(yaw, pitch, roll);

			Eigen::Vector3d vd = Ryzx2.eulerAngles(1, 2, 0);
			double pitch_n = vd[2];
			double roll_n = vd[0];
			double yaw_n = vd[1];


			std::cout << "yaw_n:" << yaw_n << "  pitch_n:" << pitch_n << "  roll_n:" << roll_n << std::endl;
			Eigen::Matrix3d Ryzx4 = Rotation::Q_R_yzx(yaw_n, pitch_n, roll_n);

			std::cout << "Ryzx1 - Ryzx2矩阵差值的范数（应接近0）：" << (Ryzx1 - Ryzx2).norm() << std::endl;
			std::cout << "Ryzx1 - Ryzx3矩阵差值的范数（应接近0）：" << (Ryzx1 - Ryzx3).norm() << std::endl;
			std::cout << "Ryzx2 - Ryzx3矩阵差值的范数（应接近0）：" << (Ryzx2 - Ryzx3).norm() << std::endl;
			std::cout << "Ryzx4 - Ryzx3矩阵差值的范数（应接近0）：" << (Ryzx4 - Ryzx3).norm() << std::endl;
		}

		{
			Eigen::Matrix3d Rzyx1 = Rz1 * Ry1 * Rx1;
			Eigen::Matrix3d Rzyx2 = Rotation::R_zyx(yaw, pitch, roll);
			Eigen::Matrix3d Rzyx3 = Rotation::Q_R_zyx(yaw, pitch, roll);

			Eigen::Vector3d vd = Rzyx2.eulerAngles(2, 1, 0);
			double pitch_n = vd[2];
			double roll_n = vd[1];
			double yaw_n = vd[0];


			std::cout << "yaw_n:" << yaw_n << "  pitch_n:" << pitch_n << "  roll_n:" << roll_n << std::endl;
			Eigen::Matrix3d Rzyx4 = Rotation::Q_R_zyx(yaw_n, pitch_n, roll_n);

			std::cout << "Rzyx1 - Rzyx2矩阵差值的范数（应接近0）：" << (Rzyx1 - Rzyx2).norm() << std::endl;
			std::cout << "Rzyx1 - Rzyx3矩阵差值的范数（应接近0）：" << (Rzyx1 - Rzyx3).norm() << std::endl;
			std::cout << "Rzyx2 - Rzyx3矩阵差值的范数（应接近0）：" << (Rzyx2 - Rzyx3).norm() << std::endl;
			std::cout << "Rzyx4 - Rzyx3矩阵差值的范数（应接近0）：" << (Rzyx4 - Rzyx3).norm() << std::endl;
		}

		{
			Eigen::Matrix3d Rzxy1 = Rz1 * Rx1 * Ry1;
			Eigen::Matrix3d Rzxy2 = Rotation::R_zxy(yaw, pitch, roll);
			Eigen::Matrix3d Rzxy3 = Rotation::Q_R_zxy(yaw, pitch, roll);

			Eigen::Vector3d vd = Rzxy2.eulerAngles(2, 0, 1);
			double pitch_n = vd[1];
			double roll_n = vd[2];
			double yaw_n = vd[0];


			std::cout << "yaw_n:" << yaw_n << "  pitch_n:" << pitch_n << "  roll_n:" << roll_n << std::endl;
			Eigen::Matrix3d Rzxy4 = Rotation::Q_R_zxy(yaw_n, pitch_n, roll_n);

			std::cout << "Rzxy1 - Rzxy2矩阵差值的范数（应接近0）：" << (Rzxy1 - Rzxy2).norm() << std::endl;
			std::cout << "Rzxy1 - Rzxy3矩阵差值的范数（应接近0）：" << (Rzxy1 - Rzxy3).norm() << std::endl;
			std::cout << "Rzxy2 - Rzxy3矩阵差值的范数（应接近0）：" << (Rzxy2 - Rzxy3).norm() << std::endl;
			std::cout << "Rzxy4 - Rzxy3矩阵差值的范数（应接近0）：" << (Rzxy4 - Rzxy3).norm() << std::endl;
		}
	}
	
	std::cout << "\n\n";
	Eigen::Matrix3d Ra = Rotation::rotationAroundAxis(Vector3d(1.5, 2.0, 0.3), 1.265);
	Eigen::Matrix3d Ra2 = Rotation::rotationAroundAxis2(Vector3d(1.5, 2.0, 0.3), 1.265);
	Eigen::Matrix3d Ra3 = Rotation::rotationAroundAxis3(Vector3d(1.5, 2.0, 0.3), 1.265);
	std::cout << "Ra2 - Ra矩阵差值的范数（应接近0）：" << (Ra2 - Ra).norm() << std::endl;
	std::cout << "Ra3 - Ra矩阵差值的范数（应接近0）：" << (Ra3 - Ra).norm() << std::endl;
	std::cout << "\n";
}

void Test::print()
{
	std::cout << std::endl;
	std::cout << std::endl;
	std::cout << "*************************************************************" << std::endl;
	std::cout << "*                 下面是测试DEMO的输出成果                  *" << std::endl;
	std::cout << "*************************************************************" << std::endl;

	doTest();

	std::cout << std::endl;
	std::cout << std::endl;
	std::cout << "********************  测试DEMO运行结束  ********************" << std::endl;
	std::cout << std::endl;
}

运行输出如下：

css 复制代码 
*************************************************************
*                 下面是测试DEMO的输出成果                  *
*************************************************************
****** Rx*Ry*Rz: ******
Matrix3d R;
R << cr*cy,-cr*sy,sr,sp*sr*cy+cp*sy,-sp*sr*sy+cp*cy,-sp*cr,-cp*sr*cy+sp*sy,cp*sr*sy+sp*cy,cp*cr;

R =
[cr*cy,-cr*sy,sr]
[sp*sr*cy+cp*sy,-sp*sr*sy+cp*cy,-sp*cr]
[-cp*sr*cy+sp*sy,cp*sr*sy+sp*cy,cp*cr]

****** Rx*Rz*Ry: ******
Matrix3d R;
R << cy*cr,-sy,cy*sr,cp*sy*cr+sp*sr,cp*cy,cp*sy*sr-sp*cr,sp*sy*cr-cp*sr,sp*cy,sp*sy*sr+cp*cr;

R =
[cy*cr,-sy,cy*sr]
[cp*sy*cr+sp*sr,cp*cy,cp*sy*sr-sp*cr]
[sp*sy*cr-cp*sr,sp*cy,sp*sy*sr+cp*cr]

****** Ry*Rx*Rz: ******
Matrix3d R;
R << cr*cy+sr*sp*sy,-cr*sy+sr*sp*cy,sr*cp,cp*sy,cp*cy,-sp,-sr*cy+cr*sp*sy,sr*sy+cr*sp*cy,cr*cp;

R =
[cr*cy+sr*sp*sy,-cr*sy+sr*sp*cy,sr*cp]
[cp*sy,cp*cy,-sp]
[-sr*cy+cr*sp*sy,sr*sy+cr*sp*cy,cr*cp]

****** Ry*Rz*Rx: ******
Matrix3d R;
R << cr*cy,-cr*sy*cp+sr*sp,cr*sy*sp+sr*cp,sy,cy*cp,-cy*sp,-sr*cy,sr*sy*cp+cr*sp,-sr*sy*sp+cr*cp;

R =
[cr*cy,-cr*sy*cp+sr*sp,cr*sy*sp+sr*cp]
[sy,cy*cp,-cy*sp]
[-sr*cy,sr*sy*cp+cr*sp,-sr*sy*sp+cr*cp]

****** Rz*Rx*Ry: ******
Matrix3d R;
R << cy*cr-sy*sp*sr,-sy*cp,cy*sr+sy*sp*cr,sy*cr+cy*sp*sr,cy*cp,sy*sr-cy*sp*cr,-cp*sr,sp,cp*cr;

R =
[cy*cr-sy*sp*sr,-sy*cp,cy*sr+sy*sp*cr]
[sy*cr+cy*sp*sr,cy*cp,sy*sr-cy*sp*cr]
[-cp*sr,sp,cp*cr]

****** Rz*Ry*Rx: ******
Matrix3d R;
R << cy*cr,-sy*cp+cy*sr*sp,sy*sp+cy*sr*cp,sy*cr,cy*cp+sy*sr*sp,-cy*sp+sy*sr*cp,-sr,cr*sp,cr*cp;

R =
[cy*cr,-sy*cp+cy*sr*sp,sy*sp+cy*sr*cp]
[sy*cr,cy*cp+sy*sr*sp,-cy*sp+sy*sr*cp]
[-sr,cr*sp,cr*cp]


@begin --> i = 0
_yaw:220.847  _pitch:-2.8  _roll:1.521
yaw:3.85451  pitch:-0.0488692  roll:0.0265465
Rx1 - Rx2矩阵差值的范数（应接近0）：0
Ry1 - Ry2矩阵差值的范数（应接近0）：0
Rz1 - Rz2矩阵差值的范数（应接近0）：0
yaw_n:0.712915  pitch_n:3.09272  roll_n:3.11505
Rxyz1 - Rxyz2矩阵差值的范数（应接近0）：0
Rxyz1 - Rxyz3矩阵差值的范数（应接近0）：7.61455e-16
Rxyz2 - Rxyz3矩阵差值的范数（应接近0）：7.61455e-16
Rxyz4 - Rxyz3矩阵差值的范数（应接近0）：2.61478e-16
yaw_n:-0.712915  pitch_n:3.09272  roll_n:-3.11505
Rxzy1 - Rxzy2矩阵差值的范数（应接近0）：0
Rxzy1 - Rxzy3矩阵差值的范数（应接近0）：7.85155e-16
Rxzy2 - Rxzy3矩阵差值的范数（应接近0）：7.85155e-16
Rxzy4 - Rxzy3矩阵差值的范数（应接近0）：2.87342e-16
yaw_n:-2.42868  pitch_n:-0.0488692  roll_n:0.0265465
Ryxz1 - Ryxz2矩阵差值的范数（应接近0）：0
Ryxz1 - Ryxz3矩阵差值的范数（应接近0）：7.61273e-16
Ryxz2 - Ryxz3矩阵差值的范数（应接近0）：7.61273e-16
Ryxz4 - Ryxz3矩阵差值的范数（应接近0）：3.14325e-16
yaw_n:-2.42868  pitch_n:-0.0488692  roll_n:0.0265465
Ryzx1 - Ryzx2矩阵差值的范数（应接近0）：0
Ryzx1 - Ryzx3矩阵差值的范数（应接近0）：7.36504e-16
Ryzx2 - Ryzx3矩阵差值的范数（应接近0）：7.36504e-16
Ryzx4 - Ryzx3矩阵差值的范数（应接近0）：3.8503e-16
yaw_n:0.712915  pitch_n:3.09272  roll_n:3.11505
Rzyx1 - Rzyx2矩阵差值的范数（应接近0）：0
Rzyx1 - Rzyx3矩阵差值的范数（应接近0）：7.61196e-16
Rzyx2 - Rzyx3矩阵差值的范数（应接近0）：7.61196e-16
Rzyx4 - Rzyx3矩阵差值的范数（应接近0）：4.36189e-16
yaw_n:0.712915  pitch_n:-3.09272  roll_n:-3.11505
Rzxy1 - Rzxy2矩阵差值的范数（应接近0）：0
Rzxy1 - Rzxy3矩阵差值的范数（应接近0）：7.61283e-16
Rzxy2 - Rzxy3矩阵差值的范数（应接近0）：7.61283e-16
Rzxy4 - Rzxy3矩阵差值的范数（应接近0）：7.51448e-16

@begin --> i = 1
_yaw:207.376  _pitch:-2.566  _roll:0.506
yaw:3.61939  pitch:-0.0447851  roll:0.00883137
Rx1 - Rx2矩阵差值的范数（应接近0）：0
Ry1 - Ry2矩阵差值的范数（应接近0）：0
Rz1 - Rz2矩阵差值的范数（应接近0）：0
yaw_n:0.477801  pitch_n:3.09681  roll_n:3.13276
Rxyz1 - Rxyz2矩阵差值的范数（应接近0）：0
Rxyz1 - Rxyz3矩阵差值的范数（应接近0）：4.87133e-16
Rxyz2 - Rxyz3矩阵差值的范数（应接近0）：4.87133e-16
Rxyz4 - Rxyz3矩阵差值的范数（应接近0）：3.82033e-16
yaw_n:-0.477801  pitch_n:3.09681  roll_n:-3.13276
Rxzy1 - Rxzy2矩阵差值的范数（应接近0）：0
Rxzy1 - Rxzy3矩阵差值的范数（应接近0）：2.98982e-16
Rxzy2 - Rxzy3矩阵差值的范数（应接近0）：2.98982e-16
Rxzy4 - Rxzy3矩阵差值的范数（应接近0）：2.93214e-16
yaw_n:-2.66379  pitch_n:-0.0447851  roll_n:0.00883137
Ryxz1 - Ryxz2矩阵差值的范数（应接近0）：0
Ryxz1 - Ryxz3矩阵差值的范数（应接近0）：3.55601e-16
Ryxz2 - Ryxz3矩阵差值的范数（应接近0）：3.55601e-16
Ryxz4 - Ryxz3矩阵差值的范数（应接近0）：3.1421e-16
yaw_n:-2.66379  pitch_n:-0.0447851  roll_n:0.00883137
Ryzx1 - Ryzx2矩阵差值的范数（应接近0）：0
Ryzx1 - Ryzx3矩阵差值的范数（应接近0）：4.71143e-16
Ryzx2 - Ryzx3矩阵差值的范数（应接近0）：4.71143e-16
Ryzx4 - Ryzx3矩阵差值的范数（应接近0）：3.29127e-16
yaw_n:0.477801  pitch_n:3.09681  roll_n:3.13276
Rzyx1 - Rzyx2矩阵差值的范数（应接近0）：0
Rzyx1 - Rzyx3矩阵差值的范数（应接近0）：4.99712e-16
Rzyx2 - Rzyx3矩阵差值的范数（应接近0）：4.99712e-16
Rzyx4 - Rzyx3矩阵差值的范数（应接近0）：8.05738e-16
yaw_n:0.477801  pitch_n:-3.09681  roll_n:-3.13276
Rzxy1 - Rzxy2矩阵差值的范数（应接近0）：0
Rzxy1 - Rzxy3矩阵差值的范数（应接近0）：3.55466e-16
Rzxy2 - Rzxy3矩阵差值的范数（应接近0）：3.55466e-16
Rzxy4 - Rzxy3矩阵差值的范数（应接近0）：8.10147e-16

@begin --> i = 2
_yaw:221.472  _pitch:-2.884  _roll:0.693
yaw:3.86542  pitch:-0.0503353  roll:0.0120951
Rx1 - Rx2矩阵差值的范数（应接近0）：0
Ry1 - Ry2矩阵差值的范数（应接近0）：0
Rz1 - Rz2矩阵差值的范数（应接近0）：1.11022e-16
yaw_n:0.723823  pitch_n:3.09126  roll_n:3.1295
Rxyz1 - Rxyz2矩阵差值的范数（应接近0）：0
Rxyz1 - Rxyz3矩阵差值的范数（应接近0）：3.51169e-16
Rxyz2 - Rxyz3矩阵差值的范数（应接近0）：3.51169e-16
Rxyz4 - Rxyz3矩阵差值的范数（应接近0）：2.34599e-16
yaw_n:-0.723823  pitch_n:3.09126  roll_n:-3.1295
Rxzy1 - Rxzy2矩阵差值的范数（应接近0）：0
Rxzy1 - Rxzy3矩阵差值的范数（应接近0）：2.93845e-16
Rxzy2 - Rxzy3矩阵差值的范数（应接近0）：2.93845e-16
Rxzy4 - Rxzy3矩阵差值的范数（应接近0）：1.88479e-16
yaw_n:-2.41777  pitch_n:-0.0503353  roll_n:0.0120951
Ryxz1 - Ryxz2矩阵差值的范数（应接近0）：0
Ryxz1 - Ryxz3矩阵差值的范数（应接近0）：3.51152e-16
Ryxz2 - Ryxz3矩阵差值的范数（应接近0）：3.51152e-16
Ryxz4 - Ryxz3矩阵差值的范数（应接近0）：3.14421e-16
yaw_n:-2.41777  pitch_n:-0.0503353  roll_n:0.0120951
Ryzx1 - Ryzx2矩阵差值的范数（应接近0）：0
Ryzx1 - Ryzx3矩阵差值的范数（应接近0）：2.4835e-16
Ryzx2 - Ryzx3矩阵差值的范数（应接近0）：2.4835e-16
Ryzx4 - Ryzx3矩阵差值的范数（应接近0）：3.9802e-16
yaw_n:0.723823  pitch_n:3.09126  roll_n:3.1295
Rzyx1 - Rzyx2矩阵差值的范数（应接近0）：0
Rzyx1 - Rzyx3矩阵差值的范数（应接近0）：2.7197e-16
Rzyx2 - Rzyx3矩阵差值的范数（应接近0）：2.7197e-16
Rzyx4 - Rzyx3矩阵差值的范数（应接近0）：5.01092e-16
yaw_n:0.723823  pitch_n:-3.09126  roll_n:-3.1295
Rzxy1 - Rzxy2矩阵差值的范数（应接近0）：0
Rzxy1 - Rzxy3矩阵差值的范数（应接近0）：3.51563e-16
Rzxy2 - Rzxy3矩阵差值的范数（应接近0）：3.51563e-16
Rzxy4 - Rzxy3矩阵差值的范数（应接近0）：5.04387e-16


Ra2 - Ra矩阵差值的范数（应接近0）：0
Ra3 - Ra矩阵差值的范数（应接近0）：1.57009e-16



********************  测试DEMO运行结束  ********************
相关推荐
草莓熊Lotso8 小时前
C++11 核心进阶：引用折叠、完美转发与可变参数模板实战
开发语言·c++·人工智能·经验分享·后端·visualstudio·gitee
乌萨奇也要立志学C++8 小时前
【洛谷】贪心专题之哈夫曼编码 从原理到模板题解析
c++·算法
落羽的落羽8 小时前
【C++】并查集的原理与使用
linux·服务器·c++·人工智能·深度学习·随机森林·机器学习
kk哥889918 小时前
C++ 对象 核心介绍
java·jvm·c++
helloworddm18 小时前
WinUI3 主线程不要执行耗时操作的原因
c++
无能者狂怒18 小时前
YOLO C++ Onnx Opencv项目配置指南
c++·opencv·yolo
集智飞行19 小时前
c++函数传参的几种推荐方式
开发语言·c++
点云SLAM21 小时前
C++ Template（模板）解读和模板报错如何“逆向阅读”定位
c++·c++20·c++模版·c++高级应用·c++模版报错定位
明洞日记21 小时前
【数据结构手册008】STL容器完全参考指南
开发语言·数据结构·c++
热门推荐
01GitHub 镜像站点02UV安装并设置国内源03Linux下V2Ray安装配置指南04在VSCode配置Java开发环境的保姆级教程（适配各类AI编程IDE）05Neo4j（一） - Neo4j安装教程（Windows）06【AutoGLM部署】本地私有化部署AI手机Agent07Cursor 又偷偷更新，这个功能太实用：Visual Editor for Cursor Browser08Open-AutoGLM Windows 安装部署教程09安娜的档案(Anna’s Archive) 镜像网站/国内最新可访问入口（持续更新）10BongoCat - 跨平台键盘猫动画工具