机器人位姿与齐次矩阵之间的相互转换（基于Eigen/OpenCV实现）

原创 已于 2024-11-25 21:54:15 修改 · 627 阅读 · 10 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#机器人 #矩阵 #c++ #计算机视觉

于 2024-11-16 22:36:55 首次发布

源码

网上查找了挺久机器人位姿与齐次矩阵之间的变换算法，但一直没找到理想的，这里自己做了实现，做过简单验证，但还可能存在问题，欢迎大家评论区指正讨论~

跳过理论，直接放代码了：

 common.h

#ifndef COMMON_H
#define COMMON_H

#include <math.h>

#ifndef M_PI
#define M_PI std::acos(-1)
#endif // !M_PI

/// <summary>
/// 旋转角顺序
/// </summary>
enum class E_ROTATION_SEQUENCE
{
	E_SEQ_XYZ = 0,
	E_SEQ_ZYX = 1
};

/// <summary>
/// 旋转角角度单位类型（degree/radian）
/// </summary>
enum class E_ANGLE_TYPE
{
	E_TYPE_DEGREE = 0,
	E_TYPE_RADIAN = 1
};

/// <summary>
/// 位姿结构体
/// </summary>
struct S_POSE
{
public:
	explicit S_POSE(double x, double y, double z, double rx, double ry, double rz)
		:X(x), Y(y), Z(z), Rx(rx), Ry(ry), Rz(rz) {};
public:
	double X;	double Y;	double Z;
	double Rx;	double Ry;	double Rz;
};

#endif // !COMMON_H

transformation_eg.h

/*****************************************************************************
*  @file     transformation_eg.h                                             *
*  @brief    用Eigen实现三维空间中位姿与矩阵之间的变换                          *
*  Details.  具体包括旋转角、旋转矩阵、齐次矩阵之间的变换						 *
*			 在固定角坐标系下描述旋转角顺序									 *
*                                                                            *
*  @author   Chan_XM                                                         *
*  @email    chan_xm14@163.com			                                     *
*  @version  V1.0														     *
*  @date     2024/11/16													     *
*  @license  NON															 *
*****************************************************************************/

#ifndef TRANSFORMATION_EG_H
#define TRANSFORMATION_EG_H

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include "common.h"

class CTransformation_EG
{
public:
	/// <summary>
	/// 角度转弧度
	/// </summary>
	/// <param name="degree">角度（-180 180）</param>
	/// <returns>弧度（-3.14 3.14）</returns>
	static double degree2Radian(const double& degree);

	/// <summary>
	/// 弧度转角度
	/// </summary>
	/// <param name="radian">弧度（-3.14 3.14）</param>
	/// <returns>角度（-180 180）</returns>
	static double radian2Degree(const double& radian);

	/// <summary>
	/// 旋转角（degree）-> 旋转矩阵 默认按照X-Y-Z固定角坐标系（或Z-Y-X欧拉角坐标系 二者等价）转换
	/// </summary>
	/// <param name="rx">绕X轴旋转角</param>
	/// <param name="ry">绕Y轴旋转角</param>
	/// <param name="rz">绕Z轴旋转角</param>
	/// <param name="rotSeq">固定角坐标系描述下的旋转顺序（与欧拉角坐标系反向）</param>
	/// <returns>旋转矩阵（3x3）</returns>
	static Eigen::Matrix3d rotDegree2Matrix(double rx, double ry, double rz, const E_ROTATION_SEQUENCE& rotSeq = E_ROTATION_SEQUENCE::E_SEQ_XYZ);

	/// <summary>
	/// 旋转角（radian）-> 旋转矩阵 默认按照X-Y-Z固定角坐标系（或Z-Y-X欧拉角坐标系 二者等价）转换
	/// </summary>
	/// <param name="rx">绕X轴旋转角</param>
	/// <param name="ry">绕Y轴旋转角</param>
	/// <param name="rz">绕Z轴旋转角</param>
	/// <param name="rotSeq">固定角坐标系描述下的旋转顺序（与欧拉角坐标系反向）</param>
	/// <returns>旋转矩阵（3x3）</returns>
	static Eigen::Matrix3d rotRadian2Matrix(double rx, double ry, double rz, const E_ROTATION_SEQUENCE& rotSeq = E_ROTATION_SEQUENCE::E_SEQ_XYZ);

	/// <summary>
	/// 旋转矩阵 -> 旋转角（degree）默认按照X-Y-Z固定角坐标系（或Z-Y-X欧拉角坐标系 二者等价）转换
	/// </summary>
	/// <param name="matrix">旋转矩阵</param>
	/// <param name="rotSeq">固定角坐标系描述下的旋转顺序（与欧拉角坐标系反向）</param>
	/// <returns>旋转角（degree）</returns>
	static Eigen::Vector3d rotMatrix2RotDegree(const Eigen::Matrix3d& matrix, const E_ROTATION_SEQUENCE& rotSeq = E_ROTATION_SEQUENCE::E_SEQ_XYZ);

	/// <summary>
	/// 旋转矩阵 -> 旋转角（Radian）默认按照X-Y-Z固定角坐标系（或Z-Y-X欧拉角坐标系 二者等价）转换
	/// </summary>
	/// <param name="matrix">旋转矩阵</param>
	/// <param name="rotSeq">固定角坐标系描述下的旋转顺序（与欧拉角坐标系反向）</param>
	/// <returns>旋转角（Radian）</returns>
	static Eigen::Vector3d rotMatrix2RotRadian(const Eigen::Matrix3d& matrix, const E_ROTATION_SEQUENCE& rotSeq = E_ROTATION_SEQUENCE::E_SEQ_XYZ);

	/// <summary>
	/// 位姿 -> 齐次矩阵
	/// </summary>
	/// <param name="x">X</param>
	/// <param name="y">Y</param>
	/// <param name="z">Z</param>
	/// <param name="rx">Rx</param>
	/// <param name="ry">Ry</param>
	/// <param name="rz">Rz</param>
	/// <param name="rotSeq">旋转角顺序 默认按照X-Y-Z固定角坐标系（或Z-Y-X欧拉角坐标系 二者等价） </param>
	/// <param name="angleType">角度单位 默认为degree</param>
	/// <returns>齐次矩阵（4x4）</returns>
	static Eigen::Matrix4d pose2HmMatrix(double x, double y, double z, double rx, double ry, double rz,
		const E_ROTATION_SEQUENCE& rotSeq = E_ROTATION_SEQUENCE::E_SEQ_XYZ,
		const E_ANGLE_TYPE& angleType = E_ANGLE_TYPE::E_TYPE_DEGREE);
	
	/// <summary>
	/// 位姿转齐次矩阵
	/// </summary>
	/// <param name="pose">位姿（包含X Y Z Rx Ry Rz）</param>
	/// <param name="rotSeq">旋转角顺序 默认按照X-Y-Z固定角坐标系（或Z-Y-X欧拉角坐标系 二者等价）</param>
	/// <param name="angleType">角度单位 默认为degree</param>
	/// <returns>齐次矩阵（4x4）</returns>
	static Eigen::Matrix4d pose2HmMatrix(const S_POSE& pose,
		const E_ROTATION_SEQUENCE& rotSeq = E_ROTATION_SEQUENCE::E_SEQ_XYZ,
		const E_ANGLE_TYPE& angleType = E_ANGLE_TYPE::E_TYPE_DEGREE);

	/// <summary>
	/// 齐次矩阵转为位姿
	/// </summary>
	/// <param name="hmMatrix">齐次矩阵（4x4）</param>
	/// <param name="rotSeq">旋转角顺序 默认按照X-Y-Z固定角坐标系（或Z-Y-X欧拉角坐标系 二者等价）</param>
	/// <param name="angleType">角度单位 默认为degree</param>
	/// <returns>位姿（包含X Y Z Rx Ry Rz）</returns>
	static S_POSE hmMatrix2Pose(const Eigen::Matrix4d& hmMatrix, 
		const E_ROTATION_SEQUENCE& rotSeq = E_ROTATION_SEQUENCE::E_SEQ_XYZ,
		const E_ANGLE_TYPE& angleType = E_ANGLE_TYPE::E_TYPE_DEGREE);

};

#endif // !TRANSFORMATION_EG_H

transformation_eg.cpp

/*****************************************************************************
*  @file     transformation_eg.cpp                                           *
*  @brief    用Eigen实现三维空间中位姿与矩阵之间的变换                          *
*                                                                            *
*  @author   Chan_XM                                                         *
*  @email    chan_xm14@163.com			                                     *
*  @version  V1.0														     *
*  @date     2024/11/16													     *
*  @license  NON															 *
*****************************************************************************/

#include "transformation_eg.h"
#include <iostream>

double CTransformation_EG::degree2Radian(const double& degree)
{
	return degree * M_PI / 180;
}

double CTransformation_EG::radian2Degree(const double& radian)
{
	return radian * 180 / M_PI;
}

Eigen::Matrix3d CTransformation_EG::rotDegree2Matrix(double rx, double ry, double rz, const E_ROTATION_SEQUENCE& rotSeq)
{
	rx = degree2Radian(rx);
	ry = degree2Radian(ry);
	rz = degree2Radian(rz);

	return CTransformation_EG::rotRadian2Matrix(rx, ry, rz, rotSeq);
}

Eigen::Matrix3d CTransformation_EG::rotRadian2Matrix(double rx, double ry, double rz, const E_ROTATION_SEQUENCE& rotSeq)
{
	Eigen::Matrix3d rotationMatrix;

	Eigen::AngleAxisd rotX(Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()));
	Eigen::AngleAxisd rotY(Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()));
	Eigen::AngleAxisd rotZ(Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()));

	if (rotSeq == E_ROTATION_SEQUENCE::E_SEQ_XYZ)
	{
		rotationMatrix = rotZ * rotY * rotX;
	}
	else
	{
		rotationMatrix = rotX * rotY * rotZ;
	}

	return rotationMatrix;
}

Eigen::Vector3d CTransformation_EG::rotMatrix2RotDegree(const Eigen::Matrix3d& matrix, const E_ROTATION_SEQUENCE& rotSeq)
{
	return rotMatrix2RotRadian(matrix, rotSeq) * 180 / M_PI;
}

Eigen::Vector3d CTransformation_EG::rotMatrix2RotRadian(const Eigen::Matrix3d& matrix, const E_ROTATION_SEQUENCE& rotSeq)
{
	double R11, R12, R13, R21, R22, R23, R31, R32, R33;
	R11 = matrix(0, 0);		R12 = matrix(0, 1);		R13 = matrix(0, 2);
	R21 = matrix(1, 0);		R22 = matrix(1, 1);		R23 = matrix(1, 2);
	R31 = matrix(2, 0);		R32 = matrix(2, 1);		R33 = matrix(2, 2);

	double alfa = 0.0, beta = 0.0, gama = 0.0;

	if (rotSeq == E_ROTATION_SEQUENCE::E_SEQ_XYZ)
	{
		beta = std::atan2(-R31, std::sqrt(R11 * R11 + R21 * R21));

		if (std::abs(beta - M_PI / 2) < 1e-6)
		{
			beta = M_PI / 2;
			alfa = 0.0;
			gama = std::atan2(R12, R22);
		}
		else if (std::abs(beta + M_PI / 2) < 1e-6)
		{
			beta = -M_PI / 2;
			alfa = 0.0;
			gama = -std::atan2(R12, R22);
		}
		else
		{
			alfa = std::atan2(R21 / std::cos(beta), R11 / std::cos(beta));
			gama = std::atan2(R32 / std::cos(beta), R33 / std::cos(beta));
		}

		Eigen::Vector3d rotDegree = { gama, beta,alfa };
		return rotDegree;
	}
	else if (rotSeq == E_ROTATION_SEQUENCE::E_SEQ_ZYX)	// 未考虑到R11、R13为0时，解存在退化的问题
	{
		beta = std::atan2(R13, std::sqrt(R11 * R11 + R12 * R12));

		if (std::abs(beta - M_PI / 2) < 1e-6)
		{
			beta = M_PI / 2;
			alfa = 0.0;
			gama = std::atan2(R32, R22);
		}
		else if (std::abs(beta + M_PI / 2) < 1e-6)
		{
			beta = -M_PI / 2;
			alfa = 0.0;
			gama = -std::atan2(R32, R22);
		}
		else
		{
			alfa = std::atan2(-R23 / std::cos(beta), R33 / std::cos(beta));
			gama = std::atan2(-R12 / std::cos(beta), R11 / std::cos(beta));
		}

		Eigen::Vector3d rotDegree = { alfa, beta, gama };
		return rotDegree;
	}
	else
	{
		return Eigen::Vector3d();
	}
}

Eigen::Matrix4d CTransformation_EG::pose2HmMatrix(double x, double y, double z, double rx, double ry, double rz, const E_ROTATION_SEQUENCE& rotSeq, const E_ANGLE_TYPE& angleType)
{
	Eigen::Matrix4d hmMatrix = Eigen::Matrix4d::Identity();

	Eigen::Matrix3d rotMatrix;

	if (angleType == E_ANGLE_TYPE::E_TYPE_DEGREE)
	{
		rotMatrix = rotDegree2Matrix(rx, ry, rz, rotSeq);
	}
	else
	{
		rotMatrix = rotRadian2Matrix(rx, ry, rz, rotSeq);
	}

	hmMatrix.block<3, 3>(0, 0) = rotMatrix;

	hmMatrix(0, 3) = x;
	hmMatrix(1, 3) = y;
	hmMatrix(2, 3) = z;

	return hmMatrix;
}

Eigen::Matrix4d CTransformation_EG::pose2HmMatrix(const S_POSE& pose, const E_ROTATION_SEQUENCE& rotSeq, const E_ANGLE_TYPE& angleType)
{
	return pose2HmMatrix(pose.X, pose.Y, pose.Z, pose.Rx, pose.Ry, pose.Rz, rotSeq, angleType);
}

S_POSE CTransformation_EG::hmMatrix2Pose(const Eigen::Matrix4d& hmMatrix, const E_ROTATION_SEQUENCE& rotSeq, const E_ANGLE_TYPE& angleType)
{
	Eigen::Matrix3d rotMatrix = hmMatrix.block<3, 3>(0, 0);

	Eigen::Vector3d rotAnagles;

	if (angleType == E_ANGLE_TYPE::E_TYPE_DEGREE)
	{
		rotAnagles = rotMatrix2RotDegree(rotMatrix, rotSeq);
	}
	else
	{
		rotAnagles = rotMatrix2RotRadian(rotMatrix, rotSeq);
	}

	double x = hmMatrix(0, 3);
	double y = hmMatrix(1, 3);
	double z = hmMatrix(2, 3);
	double rx = rotAnagles[0];
	double ry = rotAnagles[1];
	double rz = rotAnagles[2];

	return S_POSE(x, y, z, rx, ry, rz);
}

也可以访问GitHub仓库内容：https://github.com/Chan-XM/Transformation.git

transformation_cv.h

更新OpenCV版本实现位姿转换内容：

/*****************************************************************************
*  @file     transformation_cv.h                                             *
*  @brief    用OpenCV实现三维空间中位姿与矩阵之间的变换                         *
*  Details.  具体包括旋转角、旋转矩阵、齐次矩阵之间的变换						 *
*			 在固定角坐标系下描述旋转角顺序									 *
*                                                                            *
*  @author   Chan_XM                                                         *
*  @email    chan_xm14@163.com			                                     *
*  @version  V1.0														     *
*  @date     2024/11/20													     *
*  @license  NON															 *
*****************************************************************************/

#ifndef TRANSFORMATION_CV_H
#define TRANSFORMATION_CV_H

#include "common.h"

#include <opencv2/opencv.hpp>

class CTransformation_CV
{
public:
	/// <summary>
	/// 角度转弧度
	/// </summary>
	/// <param name="degree">角度（-180 180）</param>
	/// <returns>弧度（-3.14 3.14）</returns>
	static double degree2Radian(const double& degree);

	/// <summary>
	/// 弧度转角度
	/// </summary>
	/// <param name="radian">弧度（-3.14 3.14）</param>
	/// <returns>角度（-180 180）</returns>
	static double radian2Degree(const double& radian);

	/// <summary>
	/// 旋转角（degree）-> 旋转矩阵 默认按照X-Y-Z固定角坐标系（或Z-Y-X欧拉角坐标系 二者等价）转换
	/// </summary>
	/// <param name="rx">绕X轴旋转角</param>
	/// <param name="ry">绕Y轴旋转角</param>
	/// <param name="rz">绕Z轴旋转角</param>
	/// <param name="rotSeq">固定角坐标系描述下的旋转顺序（与欧拉角坐标系反向）</param>
	/// <returns>旋转矩阵（3x3）</returns>
	static cv::Mat rotDegree2Matrix(double rx, double ry, double rz, const E_ROTATION_SEQUENCE& rotSeq = E_ROTATION_SEQUENCE::E_SEQ_XYZ);

	/// <summary>
	/// 旋转角（radian）-> 旋转矩阵 默认按照X-Y-Z固定角坐标系（或Z-Y-X欧拉角坐标系 二者等价）转换
	/// </summary>
	/// <param name="rx">绕X轴旋转角</param>
	/// <param name="ry">绕Y轴旋转角</param>
	/// <param name="rz">绕Z轴旋转角</param>
	/// <param name="rotSeq">固定角坐标系描述下的旋转顺序（与欧拉角坐标系反向）</param>
	/// <returns>旋转矩阵（3x3）</returns>
	static cv::Mat rotRadian2Matrix(double rx, double ry, double rz, const E_ROTATION_SEQUENCE& rotSeq = E_ROTATION_SEQUENCE::E_SEQ_XYZ);

	/// <summary>
	/// 旋转矩阵 -> 旋转角（degree）默认按照X-Y-Z固定角坐标系（或Z-Y-X欧拉角坐标系 二者等价）转换
	/// </summary>
	/// <param name="matrix">旋转矩阵</param>
	/// <param name="rotSeq">固定角坐标系描述下的旋转顺序（与欧拉角坐标系反向）</param>
	/// <returns>旋转角（degree）</returns>
	static cv::Vec3d rotMatrix2RotDegree(const cv::Mat& matrix, const E_ROTATION_SEQUENCE& rotSeq = E_ROTATION_SEQUENCE::E_SEQ_XYZ);

	/// <summary>
	/// 旋转矩阵 -> 旋转角（Radian）默认按照X-Y-Z固定角坐标系（或Z-Y-X欧拉角坐标系 二者等价）转换
	/// </summary>
	/// <param name="matrix">旋转矩阵</param>
	/// <param name="rotSeq">固定角坐标系描述下的旋转顺序（与欧拉角坐标系反向）</param>
	/// <returns>旋转角（Radian）</returns>
	static cv::Vec3d rotMatrix2RotRadian(const cv::Mat& matrix, const E_ROTATION_SEQUENCE& rotSeq = E_ROTATION_SEQUENCE::E_SEQ_XYZ);

	/// <summary>
	/// 位姿 -> 齐次矩阵
	/// </summary>
	/// <param name="x">X</param>
	/// <param name="y">Y</param>
	/// <param name="z">Z</param>
	/// <param name="rx">Rx</param>
	/// <param name="ry">Ry</param>
	/// <param name="rz">Rz</param>
	/// <param name="rotSeq">旋转角顺序 默认按照X-Y-Z固定角坐标系（或Z-Y-X欧拉角坐标系 二者等价） </param>
	/// <param name="angleType">角度单位 默认为degree</param>
	/// <returns>齐次矩阵（4x4）</returns>
	static cv::Mat pose2HmMatrix(double x, double y, double z, double rx, double ry, double rz,
		const E_ROTATION_SEQUENCE& rotSeq = E_ROTATION_SEQUENCE::E_SEQ_XYZ,
		const E_ANGLE_TYPE& angleType = E_ANGLE_TYPE::E_TYPE_DEGREE);

	/// <summary>
	/// 位姿转齐次矩阵
	/// </summary>
	/// <param name="pose">位姿（包含X Y Z Rx Ry Rz）</param>
	/// <param name="rotSeq">旋转角顺序 默认按照X-Y-Z固定角坐标系（或Z-Y-X欧拉角坐标系 二者等价）</param>
	/// <param name="angleType">角度单位 默认为degree</param>
	/// <returns>齐次矩阵（4x4）</returns>
	static cv::Mat pose2HmMatrix(const S_POSE& pose,
		const E_ROTATION_SEQUENCE& rotSeq = E_ROTATION_SEQUENCE::E_SEQ_XYZ,
		const E_ANGLE_TYPE& angleType = E_ANGLE_TYPE::E_TYPE_DEGREE);

	/// <summary>
	/// 齐次矩阵转为位姿
	/// </summary>
	/// <param name="hmMatrix">齐次矩阵（4x4）</param>
	/// <param name="rotSeq">旋转角顺序 默认按照X-Y-Z固定角坐标系（或Z-Y-X欧拉角坐标系 二者等价）</param>
	/// <param name="angleType">角度单位 默认为degree</param>
	/// <returns>位姿（包含X Y Z Rx Ry Rz）</returns>
	static S_POSE hmMatrix2Pose(const cv::Mat& hmMatrix,
		const E_ROTATION_SEQUENCE& rotSeq = E_ROTATION_SEQUENCE::E_SEQ_XYZ,
		const E_ANGLE_TYPE& angleType = E_ANGLE_TYPE::E_TYPE_DEGREE);
};

#endif // !TRANSFORMATION_CV_H

transformation_cv.cpp

/*****************************************************************************
*  @file     transformation_cv.cpp                                           *
*  @brief    用OpenCV实现三维空间中位姿与矩阵之间的变换                          *
*                                                                            *
*  @author   Chan_XM                                                         *
*  @email    chan_xm14@163.com			                                     *
*  @version  V1.0														     *
*  @date     2024/11/16													     *
*  @license  NON															 *
*****************************************************************************/
#include "transformation_cv.h"

double CTransformation_CV::degree2Radian(const double& degree)
{
	return degree * M_PI / 180.0;
}

double CTransformation_CV::radian2Degree(const double& radian)
{
	return radian * 180.0 / M_PI;
}

cv::Mat CTransformation_CV::rotDegree2Matrix(double rx, double ry, double rz, const E_ROTATION_SEQUENCE& rotSeq)
{
    rx = degree2Radian(rx);
    ry = degree2Radian(ry);
    rz = degree2Radian(rz);

    return rotRadian2Matrix(rx, ry, rz, rotSeq);
}

cv::Mat CTransformation_CV::rotRadian2Matrix(double rx, double ry, double rz, const E_ROTATION_SEQUENCE& rotSeq)
{
    cv::Mat rotX = (cv::Mat_<double>(3, 3) <<
        1, 0, 0,
        0, cos(rx), -sin(rx),
        0, sin(rx), cos(rx));

    cv::Mat rotY = (cv::Mat_<double>(3, 3) <<
        cos(ry), 0, sin(ry),
        0, 1, 0,
        -sin(ry), 0, cos(ry));

    cv::Mat rotZ = (cv::Mat_<double>(3, 3) <<
        cos(rz), -sin(rz), 0,
        sin(rz), cos(rz), 0,
        0, 0, 1);

    cv::Mat rotationMatrix;
    if (rotSeq == E_ROTATION_SEQUENCE::E_SEQ_XYZ) {
        rotationMatrix = rotZ * rotY * rotX;
    }
    else {
        rotationMatrix = rotX * rotY * rotZ;
    }

    return rotationMatrix;
}

cv::Vec3d CTransformation_CV::rotMatrix2RotDegree(const cv::Mat& matrix, const E_ROTATION_SEQUENCE& rotSeq)
{
    return rotMatrix2RotRadian(matrix, rotSeq) * 180 / M_PI;
}

cv::Vec3d CTransformation_CV::rotMatrix2RotRadian(const cv::Mat& matrix, const E_ROTATION_SEQUENCE& rotSeq)
{
    if (matrix.cols != 3 || matrix.rows != 3)
    {
        std::cerr << "rotMatrix2RotRadian() matrix ！= 3x3" << std::endl;
        return cv::Vec3d();
    }

    if (matrix.channels() != 1)
    {
        std::cerr << "rotMatrix2RotDegree() matrix channels != 1" << std::endl;
        return cv::Vec3d();
    }

    double R11, R12, R13, R21, R22, R23, R31, R32, R33;

    switch (CV_MAT_DEPTH(matrix.type()))
    {
    case CV_32F:
    {
        R11 = matrix.at<float>(0, 0);		R12 = matrix.at<float>(0, 1);		R13 = matrix.at<float>(0, 2);
        R21 = matrix.at<float>(1, 0);		R22 = matrix.at<float>(1, 1);		R23 = matrix.at<float>(1, 2);
        R31 = matrix.at<float>(2, 0);		R32 = matrix.at<float>(2, 1);		R33 = matrix.at<float>(2, 2);
        break;
    }
    case CV_64F:
    {
        R11 = matrix.at<double>(0, 0);		R12 = matrix.at<double>(0, 1);		R13 = matrix.at<double>(0, 2);
        R21 = matrix.at<double>(1, 0);		R22 = matrix.at<double>(1, 1);		R23 = matrix.at<double>(1, 2);
        R31 = matrix.at<double>(2, 0);		R32 = matrix.at<double>(2, 1);		R33 = matrix.at<double>(2, 2);
        break;
    }

    default:
    {
        std::cerr << "rotMatrix2RotDegree() matrix type != float/double" << std::endl;
        return cv::Vec3d();
    }
    }

    double alfa = 0.0, beta = 0.0, gama = 0.0;

    if (rotSeq == E_ROTATION_SEQUENCE::E_SEQ_XYZ)
    {
        beta = std::atan2(-R31, std::sqrt(R11 * R11 + R21 * R21));

        if (std::abs(beta - M_PI / 2) < 1e-6)
        {
            beta = M_PI / 2;
            alfa = 0.0;
            gama = std::atan2(R12, R22);
        }
        else if (std::abs(beta + M_PI / 2) < 1e-6)
        {
            beta = -M_PI / 2;
            alfa = 0.0;
            gama = -std::atan2(R12, R22);
        }
        else
        {
            alfa = std::atan2(R21 / std::cos(beta), R11 / std::cos(beta));
            gama = std::atan2(R32 / std::cos(beta), R33 / std::cos(beta));
        }

        cv::Vec3d rotRadian = { gama, beta,alfa };
        return rotRadian;
    }
    else if (rotSeq == E_ROTATION_SEQUENCE::E_SEQ_ZYX)
    {
        beta = std::atan2(R13, std::sqrt(R11 * R11 + R12 * R12));

        if (std::abs(beta - M_PI / 2) < 1e-6)
        {
            beta = M_PI / 2;
            alfa = 0.0;
            gama = std::atan2(R32, R22);
        }
        else if (std::abs(beta + M_PI / 2) < 1e-6)
        {
            beta = -M_PI / 2;
            alfa = 0.0;
            gama = -std::atan2(R32, R22);
        }
        else
        {
            alfa = std::atan2(-R23 / std::cos(beta), R33 / std::cos(beta));
            gama = std::atan2(-R12 / std::cos(beta), R11 / std::cos(beta));
        }

        cv::Vec3d rotRadian = { alfa, beta, gama };
        return rotRadian;
    }
    else
    {
        return cv::Vec3d();
    }
}

cv::Mat CTransformation_CV::pose2HmMatrix(double x, double y, double z, double rx, double ry, double rz, const E_ROTATION_SEQUENCE& rotSeq, const E_ANGLE_TYPE& angleType)
{
    cv::Mat hmMatrix = cv::Mat::eye(4, 4, CV_64F);

    cv::Mat rotMatrix;

    if (angleType == E_ANGLE_TYPE::E_TYPE_DEGREE)
    {
        rotMatrix = rotDegree2Matrix(rx, ry, rz, rotSeq);
    }
    else
    {
        rotMatrix = rotRadian2Matrix(rx, ry, rz, rotSeq);
    }

    rotMatrix.copyTo(hmMatrix(cv::Rect(0, 0, 3, 3)));

    hmMatrix.at<double>(0, 3) = x;
    hmMatrix.at<double>(1, 3) = y;
    hmMatrix.at<double>(2, 3) = z;

    return hmMatrix;
}

cv::Mat CTransformation_CV::pose2HmMatrix(const S_POSE& pose, const E_ROTATION_SEQUENCE& rotSeq, const E_ANGLE_TYPE& angleType)
{
    return pose2HmMatrix(pose.X, pose.Y, pose.Z, pose.Rx, pose.Ry, pose.Rz, rotSeq, angleType);
}

S_POSE CTransformation_CV::hmMatrix2Pose(const cv::Mat& hmMatrix, const E_ROTATION_SEQUENCE& rotSeq, const E_ANGLE_TYPE& angleType)
{
    double x = 0.0, y = 0.0, z = 0.0, rx = 0.0, ry = 0.0, rz = 0.0;

    if (hmMatrix.cols != 4 || hmMatrix.rows != 4)
    {
        std::cerr << "hmMatrix2Pose() hmMatrix != 4x4" << std::endl;
        return S_POSE(x, y, z, rx, ry, rz);
    }

    if (hmMatrix.channels() != 1)
    {
        std::cerr << "hmMatrix2Pose() hmMatrix channels != 1" << std::endl;
        return S_POSE(x, y, z, rx, ry, rz);
    }

    cv::Mat rotMatrix = hmMatrix(cv::Range(0, 3), cv::Range(0, 3));

    std::cout << "rotMatrix = " << rotMatrix << std::endl << std::endl;

    cv::Vec3d angles;

    if (angleType == E_ANGLE_TYPE::E_TYPE_DEGREE)
    {
        angles = rotMatrix2RotDegree(rotMatrix, rotSeq);
    }
    else
    {
        angles = rotMatrix2RotRadian(rotMatrix, rotSeq);
    }

    rx = angles[0];     ry = angles[1];     rz = angles[2];

    switch (CV_MAT_DEPTH(hmMatrix.type()))
    {
    case CV_32F:
    {
        x = hmMatrix.at<float>(0, 3);
        y = hmMatrix.at<float>(1, 3);
        z = hmMatrix.at<float>(2, 3);
        break;
    }
    case CV_64F:
    {
        x = hmMatrix.at<double>(0, 3);
        y = hmMatrix.at<double>(1, 3);
        z = hmMatrix.at<double>(2, 3);
        break;
    }
    default:
    {
        std::cerr << "hmMatrix2Pose() matrix type != float/double" << std::endl;
        return S_POSE(x, y, z, rx, ry, rz);
    }
    }

    return S_POSE(x, y, z, rx, ry, rz);
}

参考

放几个关于欧拉角/旋转矩阵的参考链接，有兴趣的可以移步学习：

  1. 欧拉角：https://en.wikipedia.org/wiki/Euler_angles
  2. 旋转矩阵：https://en.wikipedia.org/wiki/Rotation_matrix
  3. 演算工具（3D Rotation Converter）： 3D Rotation Converter
  4. 《机器人学导论》自行查找

后续

当前通过Eigen实现了在固定角坐标系下按照XYZ/ZYX两种方式，旋转角与旋转矩阵之间的互逆变换，后续有时间将会追加一下内容的实现：

  1. 用OpenCV的cv::Mat实现上述内容 - 已完成
  2. 补充其它旋转顺序（如ZYZ/XZX等）；
  3. 补充Eigen/OpenCV两个框架下Rodrigues变换操作。
【机器人学】-位姿描述和齐次变换
weixin_45829708的博客
05-04 6640
位姿描述和齐次变换 物体之间的关系是用齐次坐标变换来描述的。机器人学中采用齐次坐标变换来描述机械手各关节坐标之间、各物体之间以及各物体与机械手之间的关系。 首先介绍刚体位姿描述方法，然后引出齐次坐标和齐次变换这些变换基本上是由平移和旋转组成，因此可以用坐标系来描述各种物体和机械手的空间位置和姿态。最后介绍变换方程、欧拉角以及旋转变换通式等内容。 位姿描述和齐次变换一、刚体位姿描述1-1 点的位置描述1-2 方位（姿态）描述1-3 刚体的位置与姿态描述1-4 末端执行器坐标系二、坐标变换2-1 坐标平移2-
5. 机器人正运动学---齐次变换矩阵
hitgavin的博客
12-31 1万+
这篇文章主要介绍了齐次变换矩阵的由来以及它的逆。
OpenCV cv::Mat到 Eigen 的正确转换——cv2eigen
u014374826的博客
06-26 1949
OpenCV cv::Mat到 Eigen 的正确转换——cv2eigen
使用Eigen计算三维坐标变换
weixin_50431732的博客
08-20 9781
本文讲述物体在三维空间的位姿表示与变换，欧拉角、四元数、旋转矩阵都是表示两个坐标系相对旋转关系，欧拉角有歧义，旋转矩阵有冗余，四元数不易理解，介绍了这三者相互转换公式以及如何使用Eigen库计算。
使用eigen库进行空间变换
旧人赋荒年
09-11 3万+
使用eigen库进行空间变换 在三维空间中，常常需要变换当前机器人的位姿计算定义的绝对坐标系和当前机器人所处相对坐标系之间的关系。而主要的变换则是平移和旋转，有时候可能需要尺度变换，那么就可以描述为： Transform&lt;float,3,Affine&gt; T = Translation3f(p) * AngleAxisf(a,axis) * Scaling(s); 而Affin...
基于特征点匹配与PnP位姿估计的视觉SLAM系统实现_包含特征提取与匹配_位姿计算与优化_三维重建与地图构建_适用于机器人导航与增强现实应用_使用OpenCV_Eigen_Soph.zip
最新发布
09-04
一个基于特征点匹配与PnP位姿估计的视觉SLAM系统涉及到从图像中提取特征点，通过匹配建立图像间关系，计算相机位姿，进行位姿优化，最终重建三维环境地图。这些技术的结合使得机器人能够在未知环境中实现自主导航，...
将已知 平面法向量 转换为机器人位姿 RPY 角度
Adoranya的博客
04-25 621
输入某已知平面法向量（A,B,C），输出该平面所对应的机器人位姿RPY值。
基于OpenCV与Eigen的视觉SLAM C++实现
本项目“基于OpenCV+Eigen+Sophus库实现的简易视觉SLAM算法C++源码+项目说明”是一个面向计算机视觉与机器人感知领域的综合性实践项目，涵盖了从图像处理、特征提取、位姿估计到三维空间重建等多个关键技术模块。...
基于OpenCV与Eigen实现的视觉SLAM算法源码
该资源标题为“基于OpenCV+Eigen+Sophus库实现的简易视觉SLAM算法C++源码+项目说明.zip”，从标题和描述中可以看出，该项目是一个面向计算机视觉与机器人领域的基础性视觉SLAM（Simultaneous Localization and ...
三点or多点的变换矩阵求解opencv & eigen
工大机器人工作室
12-24 5587
《Estimating 3-D Rigid Body Transformations: A Comparison of Four Major Algorithms》，它使用SVD方法计算 T 和 t 。 只要算出变换矩阵，就可以算出A坐标系的一个点P在坐标系B里的对应点坐标，即 R为3x3的转换矩阵， t 为3x1的位移变换向量，这里点坐标均为3x1的列向量（非齐次形式，齐次形式下为4x1列向量，多出的一个元素值补1而已）。理论上只要给定至少3对点，就能计算出 R 和 t 。自然的，点对越多，计算出来的转换
Eigen一些矩阵转换
weixin_43389008的博客
11-09 1755
由旋转矩阵和平移得到齐次变换矩阵 1） Eigen::Matrix3d R; // Find your Rotation Matrix Eigen::Vector3d T; // Find your translation Vector Eigen::Matrix4d Trans; // Your Transformation Matrix Trans.setIdentity(); // Set to Identity to make bottom row of Matrix 0,0,0,1 Tra..
视觉SLAM——第三章　Eigen几何模块Geometry使用 四元素 欧式变换矩阵
chengde6896383的专栏
11-02 1431
https://blog.youkuaiyun.com/xiaoxiaowenqiang/article/details/78076865 视觉SLAM——第三章 Eigen几何模块Geometry使用 四元素 欧式变换矩阵 github链接 点击打开链接 * 本程序演示了 Eigen 几何模块的使用方法 * 旋转向量 Eigen::AngleAxisd 角度 轴 Eigen::AngleAxisd ...
基于Eigen库的3D变换矩阵计算工具
lovely_yoshino的博客
01-21 274
代码实现了一个基于Eigen库的3D变换矩阵计算工具，主要用于处理齐次变换矩阵的创建、逆矩阵计算、位姿提取以及相对位姿的计算。以下是代码的主要功能和结构总结：齐次变换矩阵的创建：根据给定的位置（x, y, z）和旋转角度（roll, pitch, yaw，单位为度），生成一个4x4的齐次变换矩阵。逆矩阵计算：高效计算齐次变换矩阵的逆矩阵。位姿提取：从齐次变换矩阵中提取位置和四元数表示的旋转。相对位姿计算：计算两个变换矩阵之间的相对位姿。用户输入验证：确保用户输入的是有效的数值。：将角度转换为弧度。：根据位置
OBS 进阶之opengl使用方式 3 OBS 中 使用的相关矩阵及使用解释
11-03 1181
目录 一矩阵相乘 1.1 矩阵左乘顶点，获得变换后的顶点 1.2矩阵与矩阵相乘 二 OPENGL 几种重要的矩阵 2.1 平移矩阵Translation matrices 2.2单位矩阵Identity matrix 2.3缩放矩阵Scaling matrices 2.4 矩阵变换顺序 三 OBS使用的矩阵 3.0 OBS中矩阵的含义 3.1 OBS中平移矩阵gs_matrix_translate 3.2 OBS中 缩放矩阵...
基于Eigen的位姿转换
整理ing
10-13 2078
位姿中姿态的表示形式有很多种，比如：旋转矩阵、四元数、欧拉角、旋转向量等等。这里基于Eigen实现四种数学形式的相互转换功能。
Eigen中几种表示三维位姿的方式以及相互转换
努力努力努力
08-14 2785
Eigen中几种表示三维位姿的方式以及相互转换
矩阵库eigen的用法(三)————求行列式值和三角分解求线性方程组的解
l93919861的博客
01-19 5701
在经过前面2篇对 eigen库的基础知识了解之后，下面就可以用eigen库进行一些实际的操作了。 1.计算矩阵行列式的值 在Eigen里你不能混合两种不同类型的矩阵，像这样是错的 v_3d &amp;amp;amp;amp;amp;amp;amp;amp;lt;&amp;amp;amp;amp;amp;amp;amp;amp;lt; 3, 2, 1; Eigen::Matrix&amp;amp;amp;amp;amp;amp;amp;amp;lt;double, 2, 1&a
Eigen（3）矩阵Matrix及其简单操作
热门推荐
优快云1HAO的博客
08-03 10万+
1. Matrix类     在Eigen，所有的矩阵和向量都是Matrix模板类的对象，Vector只是一种特殊的矩阵（一行或者一列）。     Matrix有6个模板参数，主要使用前三个参数，剩下的有默认值。 Matrix&lt;typename Scalar, int RowsAtCompileTime, int ColsAtCompileTime&gt; Scalar是表示元素的...
Eigen之矩阵基本操作
weixin_42024702的博客
03-18 3438
开篇介绍 在实际应用中，当要进行姿态解算时，必不可少的便是矩阵操作，这里我们引入Eigen这一矩阵操作库。Eigen是一个C++开源线性代数库，它提供了快速的有关矩阵的线性代数运算，还包括解方程等功能。本博客通过一些实例应用，演示Eigen下矩阵逆运算、伴随矩阵运算、行列式计算和特征值、特征向量的计算等等。 一、旋转矩阵 我们知道向量的内积可以描述向量之间的投影关系，而向量的外积，能表示向量的旋转。在欧氏变换中，首先来考虑旋转。我们设某个单位正交基(e1,e2,e3)(e_1,e_2,e_3)(e1​,e2
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值