本文详细介绍了ROS中坐标变换的相关知识，重点讲解了tf::Quaternion和tf::Matrix3x3两个类，涉及四元数和旋转矩阵的初始化、转换以及与欧拉角、旋转向量的相互转换。通过示例代码展示了具体操作过程，并指出了ROS中四元数转欧拉角的实现方式。
坐标变换学习笔记—代码篇ROS
坐标变换-ROS
在ROS中，与旋转有关的函数主要集中在Matrix3x3和Quaternion 2个类中，它们都在tf命名空间中。下面旋转量之间的转换与 坐标变换学习笔记—理论篇 是对应的。
transform_datatypes
在 transform_datatypes.h 中，定义了一些常用的不同类型数据的转换方法，注意，ROS中的四元数默认的顺序是: (x, y, z, w)：
double yaw;
tf::Quaternion q; // 注意，ROS中的四元数默认的顺序是: (x, y, z, w)
geometry_msgs::Quaternion msg_q; // 四元数消息类型，在geometry_msgs中，通过ROS命令 rosmsg show geometry_msgs/Quaternion 可查看其中内容为四个float类型的变量：x,y,z,w;
// 从欧拉角创建四元数:
q = tf::createIdentityQuaternion(); // 构造单位四元数：(0, 0, 0, 1)
q = tf::createQuaternionFromRPY(roll, pitch, yaw); // 根据欧拉角构造四元数；
q = tf::createQuaternionFromYaw(yaw); // 根据航偏角构造四元数；
// 上面这三个函数，内部都是调用的四元数内部的setRPY函数来实现的:
// q.setRPY(0, 0, 0); q.setRPY(roll, pitch, yaw); q.setRPY(0, 0, yaw); // 根据欧拉角参数设置四元数；
// return q; // 返回四元数；
// 四元数消息类型转换:
tf::quaternionTFToMsg(q, msg_q); // 将 tf::Quaternion 类型的四元数直接转换成ROS的四元数消息类型。内部实现方式如下：
// 若q的模不为1，利用 normalize() 方法将其归一化；
// 然后 msg_q.x = q.x(); msg_q.y = q.y(); msg_q.z = q.z(); msg_q.w = q.w(); 对四元数消息赋值；
tf::quaternionMsgToTF(msg_q, q); // 从ROS的四元数消息类型中得到 tf::Quaternion 类型的四元数。内部实现方式如下：
// q = Quaternion(msg_q.x, msg_q.y, msg_q.z, msg_q.w); // 构造四元数；
// 如果q模不为1，则 q.normalize(); （利用 normalize() 函数将其归一化）；
msg_q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); // 将欧拉角（roll, pitch, yaw）直接转换为ROS的四元数消息类型。内部实现方式如下：
// quaternionTFToMsg(createQuaternionFromRPY(roll, pitch, yaw), q_msg);
// return q_msg;
msg_q = tf::createQuaternionMsgFromYaw(yaw); // 将欧拉角（yaw）直接转换为ROS的四元数消息类型。内部实现方式如下：
// q.setRPY(0.0, 0.0, yaw);
// quaternionTFToMsg(q, q_msg);
// return q_msg;
// 从四元数中得到欧拉角：
yaw = tf::getYaw(msg_q); // 从ROS的四元数消息类型中得到航偏角yaw。内部实现方式如下：
// quaternionMsgToTF(msg_q, q);
// return getYaw(q);
yaw = tf::getYaw(q); // 通过 tf::Matrix3x3(bt_q).getRPY( useless_roll, useless_pitch,yaw); 得到欧拉角，然后返回yaw值。内部实现方式如下：
// tf::Matrix3x3(q).getRPY(useless_roll, useless_pitch, yaw); // 先将四元数转化为Matrix3x3的矩阵，然后再将矩阵转成欧拉角；
// return yaw;
tf::Quaternion类
在Quaternion类中，主要涉及到有各种不同方式的四元数初始化，欧拉角转四元数，利用四元数计算旋转向量，重载与四元数相关的运算符和数据类型（两个四元数相乘，点乘，求逆，加减乘除，…），四元数插值等操作 ：
四元数初始化及与其它旋转量的转换
#include <ros/ros.h>
#include <tf/tf.h>
#include <geometry_msgs/Quaternion.h>
#include <iostream>
inline void printQuaternion(const tf::Quaternion& q, const std::string& info)
{
// std::cout << "quaternion: " << q[0] << ", " << q[1] << ", " << q[2] << ", " << q[3] << std::endl; // x,y,z,w
std::cout << info << " quaternion: " << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << std::endl; // 两种元素访问方式均可
}
inline void printQuaternion(const geometry_msgs::Quaternion& msg_q, const std::string& info) // 通过 rosmsg show geometry_msgs/Quaternion 可查看四元数消息内容:
{
std::cout << info << " msg quaternion: " << msg_q.x << ", " << msg_q.y << ", " << msg_q.z << ", " << msg_q.w << std::endl; // x,y,z,w
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "transform_ros");
ros::NodeHandle nh;
// 1, 四元数初始化方法：
std::cout << "1: -------------------- quaternion initialize --------------------\n";
double angle = M_PI / 4.0;
tf::Vector3 axis(0, 0, 1);
double qx = 0, qy = 0, qz = 0, qw = 1;
double roll = M_PI / 2.0, pitch = M_PI / 3.0, yaw = M_PI / 4.0;
tf::Quaternion q1(qx, qy, qz, qw); // 直接初始化；
tf::Quaternion q2(axis, angle); // 通过旋转向量来初始化四元数 (见 https://blog.youkuaiyun.com/sunqin_csdn/article/details/107969585 式(3.2))；
tf::Quaternion q3;
q3.setRotation(axis, angle);
tf::Quaternion q4 = tf::Quaternion(axis, angle); // 旋转向量转四元数, 内部实现方式如下:
// tfScalar d = axis.length();
// tfScalar s = tfSin(angle * tfScalar(0.5)) / d;
// setValue(axis.x() * s, axis.y() * s, axis.z() * s, tfCos(angle * tfScalar(0.5)));
tf::Quaternion q5;
q5.setRPY(roll, pitch, yaw); // 根据欧拉角转四元数公式(5.1)，初始化四元数；
printQuaternion(q1, "q1");
printQuaternion(q2, "q2");
printQuaternion(q3, "q3");
printQuaternion(q4, "q4");
printQuaternion(q5, "q5");
// 2, 创建四元数(下面几种创建四元数的方法，内部均是通过setRPY的函数方法实现的):
std::cout << "\n2: -------------------- quaternion creation --------------------\n";
tf::Quaternion q6 = tf::createIdentityQuaternion(); // x,y,z,w: 0,0,0,1
tf::Quaternion q7 = tf::createQuaternionFromRPY(roll, pitch, yaw); // setRPY(roll, pitch, yaw)
tf::Quaternion q8 = tf::createQuaternionFromYaw(yaw); // setRPY(0, 0, yaw)
geometry_msgs::Quaternion msg_q1 = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); // 根据欧拉角创建四元数, 再将其转换为消息类型;
geometry_msgs::Quaternion msg_q2 = tf::createQuaternionMsgFromYaw(yaw);
printQuaternion(q6, "q6");
printQuaternion(q7, "q7");
printQuaternion(q8, "q8");
printQuaternion(msg_q1, "msg q1");
printQuaternion(msg_q2, "msg q2");
tf::quaternionTFToMsg(q1, msg_q1); // 将四元数消息类型转换为数据类型(q1 --> msg_q1) // 四元数数据类型(tf::Quaternion)和四元数消息类型(geometry_msgs::Quaternion)间的转换
tf::quaternionMsgToTF(msg_q2, q2); // 将四元数数据类型转换为消息类型(msg_q2 --> q2)
// 3, 四元数与其它旋转量间的转换:
std::cout << "\n3: -------------------- quaternion conversion --------------------\n";
q6.setRotation(axis, angle); // Axis-Angle to quaternion:
angle = q3.getAngle(); // quaternion to Axis-Angle:
axis = q3.getAxis();
std::cout << "Axis-Angle from q3(axis, angle): " << axis[0] << ", " << axis[1] << ", " << axis[2] << ", " << angle << std::endl;
yaw = tf::getYaw(q5); // quaternion to euler: (内部实现方式如下:)
// tf::Matrix3x3(q).getRPY(useless_roll, useless_pitch, yaw); // ROS中通过四元数获取欧拉角的方式都是, 先将四元数转化为Matrix3x3的矩阵，然后再将矩阵转成欧拉角；
// return yaw;
std::cout << "euler-yaw from q5: " << yaw << std::endl;
return 0;
}
示例代码输出结果如下：
1: -------------------- quaternion initialize --------------------
q1 quaternion: 0, 0, 0, 1
q2 quaternion: 0, 0, 0.382683, 0.92388
q3 quaternion: 0, 0, 0.382683, 0.92388
q4 quaternion: 0, 0, 0.382683, 0.92388
q5 quaternion: 0.430459, 0.560986, -0.092296, 0.701057
2: -------------------- quaternion creation --------------------
q6 quaternion: 0, 0, 0, 1
q7 quaternion: 0.430459, 0.560986, -0.092296, 0.701057
q8 quaternion: 0, 0, 0.382683, 0.92388
msg q1 msg quaternion: 0.430459, 0.560986, -0.092296, 0.701057
msg q2 msg quaternion: 0, 0, 0.382683, 0.92388
3: -------------------- quaternion conversion --------------------
Axis-Angle from q3(axis, angle): 0, 0, 1, 0.785398
euler-yaw from q5: 0.785398
上面 transform_datatypes 中所使用的方法中，本质上使用了四元数内部的setRPY来将欧拉角转换成四元数，getRPY来将四元数转换成欧拉角。这里来探究下setRPY和getRPY是如何实现四元数与欧拉角间的转换的。
欧拉角 → \to → 四元数：
void setRPY(const tfScalar& roll, const tfScalar& pitch, const tfScalar& yaw) // 利用式（5.1）将欧拉角转换成四元数，其ROS的实现过程如下：
{
tfScalar halfYaw = tfScalar(yaw) * tfScalar(0.5);
tfScalar halfPitch = tfScalar(pitch) * tfScalar(0.5);
tfScalar halfRoll = tfScalar(roll) * tfScalar(0.5);
tfScalar cosYaw = tfCos(halfYaw);
tfScalar sinYaw = tfSin(halfYaw);
tfScalar cosPitch = tfCos(halfPitch);
tfScalar sinPitch = tfSin(halfPitch);
tfScalar cosRoll = tfCos(halfRoll);
tfScalar sinRoll = tfSin(halfRoll);
setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
}
四元数 → \to → 欧拉角：
在ROS中，没有直接的四元数转欧拉角的函数方法，即它没有用式（5.2）所描述的方法去计算欧拉角，而是先将四元数转换成了旋转矩阵tf::Matrix3x3，然后再利用tf::Matrix3x3内部的getRPY方法来将旋转矩阵转换成欧拉角。即：tf::Matrix3x3(tf_q).getRPY( roll, pitch, yaw) 间接计算得到欧拉角，然后返回yaw值。
yaw = tf::getYaw(q); 内部实现方式如下：
// tf::Matrix3x3(q).getRPY(useless_roll, useless_pitch, yaw); // 先将四元数转化为Matrix3x3的矩阵，然后再将矩阵转成欧拉角；
// return yaw;
旋转向量 → \to → 四元数：
void setRotation(const Vector3& axis, const tfScalar& angle) // 根据式(3.2)将旋转轴转换为四元数；
{
tfScalar d = axis.length();
tfAssert(d != tfScalar(0.0));
tfScalar s = tfSin(angle * tfScalar(0.5)) / d;
setValue(axis.x() * s, axis.y() * s, axis.z() * s,
tfCos(angle * tfScalar(0.5)));
}
四元数 → \to → 旋转向量：
// 利用式(3.1)计算旋转角和旋转轴：
tfScalar getAngle() const
{
tfScalar s = tfScalar(2.) * tfAcos(m_floats[3]);
}
Vector3 getAxis() const
{
tfScalar s_squared = tfScalar(1.) - tfPow(m_floats[3], tfScalar(2.));
if (s_squared < tfScalar(10.) * TFSIMD_EPSILON) //Check for divide by zero
return Vector3(1.0, 0.0, 0.0); // Arbitrary
tfScalar s = tfSqrt(s_squared);
return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
}
四元数其它常用函数：
// 四元数模的平方：q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w
tfScalar length2() const
{
return dot(*this);
}
// 四元数求模：sqrt(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w)
tfScalar length() const
{
return tfSqrt(length2());
}
// 四元数归一化：
Quaternion& normalize()
{
return *this /= length();
}
Quaternion normalized() const
{
return *this / length();
}
// 四元数点乘：
tfScalar dot(const Quaternion& q) const
{
return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
}
// 四元数求逆：
Quaternion inverse() const
{
return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
}
// 四元数相乘（在附录关于式(5.1)证明处，有提到两个四元数相乘的公式）：
TFSIMD_FORCE_INLINE Quaternion operator*(const Quaternion& q1, const Quaternion& q2) {
return Quaternion(
q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
}
Quaternion& operator*=(const Quaternion& q)
{
setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
return *this;
}
// 四元数插值：
Quaternion slerp(const Quaternion& q, const tfScalar& t) const
// .....
tf::Matrix3x3类
在Matrix3x3类中，主要涉及到有各种不同方式的旋转矩阵初始化，欧拉角转旋转矩阵，旋转矩阵转欧拉角，四元数转旋转矩阵，旋转矩阵转四元数，以及一些常用的矩阵操作（矩阵相乘，求逆，转置，求行列式，…）等操作 ：
旋转矩阵初始化及与其它旋转量的转换
#include <ros/ros.h>
#include <tf/tf.h>
#include <geometry_msgs/Quaternion.h>
#include <iostream>
#include <iomanip> //对于保留几位小数,需要头文件#include <iomanip>
inline void printQuaternion(const tf::Quaternion& q, const std::string& info)
{
// std::cout << "quaternion: " << q[0] << ", " << q[1] << ", " << q[2] << ", " << q[3] << std::endl; // x,y,z,w
std::cout << info << " quaternion: " << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << std::endl; // 两种元素访问方式均可
}
inline void printMatrix(const tf::Matrix3x3& mat, const std::string info)
{
std::cout << info << "-------------------- " << std::endl;
std::cout << std::setprecision(4) << mat[0][0] << "\t" << mat[0][1] << "\t" << mat[0][2] << "\n"
<< mat[1][0] << "\t" << mat[1][1] << "\t" << mat[1][2] << "\n"
<< mat[2][0] << "\t" << mat[2][1] << "\t" << mat[2][2] << "\n\n";
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "transform_ros");
ros::NodeHandle nh;
// 1, 旋转矩阵初始化方法：
double theta = M_PI / 3.0;
double ct = cos(theta), st = sin(theta);
double roll = M_PI / 2.0, pitch = M_PI / 3.0, yaw = M_PI / 4.0;
tf::Quaternion q(0, 0, 0, 1); // x,y,z,w;
tf::Matrix3x3 mat1(q); // 从四元数初始化旋转矩阵, 内部通过 setRotation 实现;
tf::Matrix3x3 mat2(ct, -st, 0, st, ct, 0, 0, 0, 1); // 直接初始化;
tf::Matrix3x3 mat3(mat2);
tf::Matrix3x3 mat4 = mat3;
tf::Matrix3x3 mat5;
mat5.setRPY(roll, pitch, yaw); // 欧拉角到旋转矩阵的变换, 内部直接调用 setEulerYPR(yaw, pitch, roll) 函数来实现;
printMatrix(mat1, "mat1:");
printMatrix(mat2, "mat2:");
printMatrix(mat3, "mat3:");
printMatrix(mat4, "mat4:");
printMatrix(mat5, "mat5:");
// 2, 旋转矩阵与其它旋转量间的转换:
// rotation matrix --> euler:
mat2.getRPY(roll, pitch, yaw, 1); // 第四个参数 solution_number, 缺省时默认值为1；
// 函数内部是直接使用 getEulerYPR(yaw, pitch, roll, solution_number); 方法实现的;
std::cout << "rpy from mat2: " << roll << ", " << pitch << ", " << yaw << std::endl;
mat5.getEulerYPR(yaw, pitch, roll);
std::cout << "rpy from mat5(solution_number not set): " << roll << ", " << pitch << ", " << yaw << std::endl;
mat5.getEulerYPR(yaw, pitch, roll, 1);
std::cout << "rpy from mat5(solution_number = 1): " << roll << ", " << pitch << ", " << yaw << std::endl;
mat5.getEulerYPR(yaw, pitch, roll, 0);
std::cout << "rpy from mat5(solution_number = 0): " << roll << ", " << pitch << ", " << yaw << std::endl;
// 可以看到，solution_number 为0和1时得到的结果时不一样的，它设置为默认值1时的结果与原结果是一致的，所以强烈建议使用getRPY函数时，直接缺省第四个参数；
// 其中的差别主要在于solution_number = 0时，对计算得到的pitch有：pitch = M_PI - pitch; 然后利用该pitch计算roll和yaw，使得最后的结果不一样；
// rotation matrix --> quaternion:
mat2.getRotation(q);
printQuaternion(q, "quaternion from matrix mat2: ");
// quaternion --> rotation matrix:
mat1.setRotation(q);
printMatrix(mat1, "\nmat from q(from mat2): ");
// rotation matrix --> euler:
// mat.getRPY(roll, pitch, yaw);
// euler --> rotation matrix:
// mat.setRPY(roll, pitch, yaw);
// 3, 其它操作:
tf::Vector3 vec1, vec2, vec3;
vec1 = mat1[2];
vec2 = mat1.getColumn(1);
vec3 = mat2.getRow(0);
std::cout << "get column of matrix[2]: " << vec1[0] << ", " << vec1[1] << ", " << vec1[2] << std::endl;
std::cout << "get column of matrix[1]: " << vec2[0] << ", " << vec2[1] << ", " << vec2[2] << std::endl;
std::cout << "get row of matrix[0]: " << vec3[0] << ", " << vec3[1] << ", " << vec3[2] << std::endl;
return 0;
}
示例代码输出结果如下：
mat1:--------------------
1 0 0
0 1 0
0 0 1
mat2:--------------------
0.5 -0.866 0
0.866 0.5 0
0 0 1
mat3:--------------------
0.5 -0.866 0
0.866 0.5 0
0 0 1
mat4:--------------------
0.5 -0.866 0
0.866 0.5 0
0 0 1
mat5:--------------------
0.3536 0.6124 0.7071
0.3536 0.6124 -0.7071
-0.866 0.5 3.062e-17
rpy from mat2: 0, -0, 1.047
rpy from mat5(solution_number not set): 1.571, 1.047, 0.7854
rpy from mat5(solution_number = 1): 1.571, 1.047, 0.7854
rpy from mat5(solution_number = 0): -1.571, 2.094, -2.356
quaternion from matrix mat2: quaternion: 0, 0, 0.5, 0.866
mat from q(from mat2): --------------------
0.5 -0.866 0
0.866 0.5 0
0 0 1
get column of matrix[2]: 0, 0, 1
get column of matrix[1]: -0.866, 0.5, 0
get row of matrix[0]: 0.5, -0.866, 0
欧拉角 → \to → 旋转矩阵：
void setRPY(tfScalar roll, tfScalar pitch,tfScalar yaw) {
setEulerYPR(yaw, pitch, roll); // setRPY 函数内部会直接调用 setEulerYPR(yaw, pitch, roll)方法，但它们的传参顺序不一样，这里需要注意；
}
// setEulerYPR 函数，欧拉角转四元数的实现方式如下，见式(4.1)；
void setEulerYPR(tfScalar eulerZ, tfScalar eulerY,tfScalar eulerX) {
tfScalar ci ( tfCos(eulerX));
tfScalar cj ( tfCos(eulerY));
tfScalar ch ( tfCos(eulerZ));
tfScalar si ( tfSin(eulerX));
tfScalar sj ( tfSin(eulerY));
tfScalar sh ( tfSin(eulerZ));
tfScalar cc = ci * ch;
tfScalar cs = ci * sh;
tfScalar sc = si * ch;
tfScalar ss = si * sh;
setValue(cj * ch, sj * sc - cs, sj * cc + ss,
cj * sh, sj * ss + cc, sj * cs - sc,
-sj, cj * si, cj * ci);
}
旋转矩阵 → \to → 欧拉角：
void getRPY(tfScalar& roll, tfScalar& pitch, tfScalar& yaw, unsigned int solution_number = 1) const
{ // getRPY 内部会直接调用 getEulerYPR 方法，但它们的传参顺序不一样，这里需要注意。solution_number 缺省时的默认值为1；
getEulerYPR(yaw, pitch, roll, solution_number);
}
// getEulerYPR函数的内部实现方式如下，根据式(4.2)将旋转矩阵转换成欧拉角,其中pitch是通过arcsin的方式求的，而不是atan2的的方式。
void getEulerYPR(tfScalar& yaw, tfScalar& pitch, tfScalar& roll, unsigned int solution_number = 1) const
{
struct Euler
{
tfScalar yaw;
tfScalar pitch;
tfScalar roll;
};
Euler euler_out;
Euler euler_out2; //second solution
//get the pointer to the raw data
// Check that pitch is not at a singularity
// Check that pitch is not at a singularity
if (tfFabs(m_el[2].x()) >= 1)
{
euler_out.yaw = 0;
euler_out2.yaw = 0;
// From difference of angles formula
if (m_el[2].x() < 0) //gimbal locked down
{
tfScalar delta = tfAtan2(m_el[0].y(),m_el[0].z());
euler_out.pitch = TFSIMD_PI / tfScalar(2.0);
euler_out2.pitch = TFSIMD_PI / tfScalar(2.0);
euler_out.roll = delta;
euler_out2.roll = delta;
}
else // gimbal locked up
{
tfScalar delta = tfAtan2(-m_el[0].y(),-m_el[0].z());
euler_out.pitch = -TFSIMD_PI / tfScalar(2.0);
euler_out2.pitch = -TFSIMD_PI / tfScalar(2.0);
euler_out.roll = delta;
euler_out2.roll = delta;
}
}
else
{
euler_out.pitch = - tfAsin(m_el[2].x());
euler_out2.pitch = TFSIMD_PI - euler_out.pitch;
euler_out.roll = tfAtan2(m_el[2].y()/tfCos(euler_out.pitch),
m_el[2].z()/tfCos(euler_out.pitch));
euler_out2.roll = tfAtan2(m_el[2].y()/tfCos(euler_out2.pitch),
m_el[2].z()/tfCos(euler_out2.pitch));
euler_out.yaw = tfAtan2(m_el[1].x()/tfCos(euler_out.pitch),
m_el[0].x()/tfCos(euler_out.pitch));
euler_out2.yaw = tfAtan2(m_el[1].x()/tfCos(euler_out2.pitch),
m_el[0].x()/tfCos(euler_out2.pitch));
}
if (solution_number == 1)
{
yaw = euler_out.yaw;
pitch = euler_out.pitch;
roll = euler_out.roll;
}
else
{
yaw = euler_out2.yaw;
pitch = euler_out2.pitch;
roll = euler_out2.roll;
}
}
旋转矩阵 → \to → 四元数：
void getRotation(Quaternion& q) const // 根据式(2.2)将旋转矩阵转换成四元数；
{
tfScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
tfScalar temp[4];
if (trace > tfScalar(0.0))
{
tfScalar s = tfSqrt(trace + tfScalar(1.0));
temp[3]=(s * tfScalar(0.5));
s = tfScalar(0.5) / s;
temp[0]=((m_el[2].y() - m_el[1].z()) * s);
temp[1]=((m_el[0].z() - m_el[2].x()) * s);
temp[2]=((m_el[1].x() - m_el[0].y()) * s);
}
else
{
int i = m_el[0].x() < m_el[1].y() ?
(m_el[1].y() < m_el[2].z() ? 2 : 1) :
(m_el[0].x() < m_el[2].z() ? 2 : 0);
int j = (i + 1) % 3;
int k = (i + 2) % 3;
tfScalar s = tfSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + tfScalar(1.0));
temp[i] = s * tfScalar(0.5);
s = tfScalar(0.5) / s;
temp[3] = (m_el[k][j] - m_el[j][k]) * s;
temp[j] = (m_el[j][i] + m_el[i][j]) * s;
temp[k] = (m_el[k][i] + m_el[i][k]) * s;
}
q.setValue(temp[0],temp[1],temp[2],temp[3]);
}
四元数 → \to → 旋转矩阵：
void setRotation(const Quaternion& q) // 根据式(2.1.3)将四元数转换成旋转矩阵；
{
tfScalar d = q.length2();
tfFullAssert(d != tfScalar(0.0));
tfScalar s = tfScalar(2.0) / d;
tfScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
tfScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
tfScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
tfScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
setValue(tfScalar(1.0) - (yy + zz), xy - wz, xz + wy,
xy + wz, tfScalar(1.0) - (xx + zz), yz - wx,
xz - wy, yz + wx, tfScalar(1.0) - (xx + yy));
}
旋转矩阵其它常用函数：
void setIdentity() // 将矩阵变成单位矩阵；
Matrix3x3 absolute() const; // 对矩阵内各元素求绝对值；
Matrix3x3 transpose() const; // 转置；
Matrix3x3 inverse() const; // 求逆；
TFSIMD_FORCE_INLINE Vector3 getColumn(int i) const // 获取矩阵的某一列；
TFSIMD_FORCE_INLINE const Vector3& getRow(int i) const // 获取矩阵的某一行；
operator*(const Matrix3x3& m, const Vector3& v) // 与向量相乘的运算符重载；
operator*(const Vector3& v, const Matrix3x3& m)
operator*(const Matrix3x3& m1, const Matrix3x3& m2) // 与矩阵相乘的运算符重载；
TFSIMD_FORCE_INLINE bool operator==(const Matrix3x3& m1, const Matrix3x3& m2) // 判断两个矩阵是否全等（即内部所有元素对应相等）；
void setValue(const tfScalar& xx, const tfScalar& xy, const tfScalar& xz, const tfScalar& yx, const fScalar& yy, const tfScalar& yz, const tfScalar& zx, const tfScalar& zy, const tfScalar& zz)
{ // 直接对矩阵元素赋值；
m_el[0].setValue(xx,xy,xz);
m_el[1].setValue(yx,yy,yz);
m_el[2].setValue(zx,zy,zz);
}
相关：坐标变换学习笔记—理论篇
相关：坐标变换学习笔记—代码篇Eigen
相关：坐标变换学习笔记—代码篇Matlab
参考：
https://docs.ros.org/api/tf/html/c++/annotated.html
https://docs.ros.org/api/tf/html/c++/transform__datatypes_8h_source.html
https://docs.ros.org/api/tf/html/c++/Quaternion_8h_source.html
https://docs.ros.org/api/tf/html/c++/Matrix3x3_8h_source.html
坐标变换学习笔记—理论篇
扫一扫
1694
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
