#include "ros/ros.h"
#include<Eigen/Core>
#include<Eigen/Geometry>
// using namespace std; https://www.cnblogs.com/lovebay/p/11215028.html https://blog.csdn.net/u011092188/article/details/77430988/
// using namespace Eigen;
double m_PI = 3.14159265358979323;
double x,y,z,w;
void test01() // 基本类型 定义
{
Eigen::Matrix<float,2,3> matrix_23; // Eigen 中所有向量和矩阵 都是 Eigen::Matrix 这是一个模板类 前三个参数 为 数据类型 行 列
Eigen::Vector3d v_3d ; // 同时， Eigen 通过 typedf 提供了许多内置类型 底层依然是 Eigen::Matrix 列向量
Eigen::Matrix<double,3,1> vd_3d; // v_3d yu vd_3d 等价 Eigen::Vector3d v_3d 是double 类型
std::cout << "Vector3d " << std::endl << Eigen::Vector3d::UnitX() << std::endl; // (1,0,0) 列向量
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Identity(); // 代表double 类型的 3*3 的矩阵 单位矩阵
Eigen::MatrixXd matrix_xx; // 代表double 类型的 不确定维度的矩阵
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_dynamic;
std::cout << "Rotation Matrix" << std::endl << matrix_33 << std::endl;
Eigen::Matrix3cd matrix_r; //旋转矩阵
Eigen::AngleAxisd vector_rotation; // 旋转向量
Eigen::Vector3d translation; // 平移向量
Eigen::Vector3d Eulerangler; // 欧拉角
Eigen::Quaterniond q1; // 四元数
Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); //变换矩阵 定义与初始化
std::cout << "Transform Matrix" << std::endl << T.matrix() << std::endl; // 4*4 单位矩阵
}
void test02() // 旋转向量
{
// 旋转向量 旋转向量，是一个三维向量，其方向与旋转轴一致，长度等于旋转角； 任意的旋转都可以用一个旋转轴和绕轴的旋转角来描述，简称“轴角”（Axis-Angle）；
// https://zhuanlan.zhihu.com/p/93563218
// https://blog.csdn.net/xuewend/article/details/84645213
Eigen::AngleAxisd t_V(M_PI / 3, Eigen::Vector3d(0, 0, 1)); // 旋转向量
Eigen::Matrix3d t_R = t_V.matrix(); // 旋转矩阵
Eigen::Quaterniond t_Q(t_V); // 四元数
Eigen::Vector3d eulerAngle(0, M_PI/6, M_PI); // 欧拉角 yaw pitch roll z y x
//对旋转向量（轴角）赋值的三大种方法
// 1 使用旋 转角度 和 旋转轴向量（此向量为单位向量） 来初始化角轴
Eigen::AngleAxisd V1(M_PI/4, Eigen::Vector3d(0,0,1)); // 以(0,0,1) 为旋转轴 旋转45度
std::cout << "Rotation_vector1 " << std::endl << V1.matrix() << std::endl;
//2.使用旋转矩阵转旋转向量的方式
Eigen::AngleAxisd V2(t_R); //2.3 使用旋转矩阵来对旋转向量进行初始化
V2.fromRotationMatrix(t_R); //2.1 使用旋转向量的fromRotationMatrix()函数来对旋转向量赋值（注意此方法为旋转向量独有,四元数没有）
V2 = t_R; //2.2 直接使用旋转矩阵来对旋转向量赋值
std::cout << "Rotation_vector2 " << std::endl << V2.matrix() << std::endl;
//3. 使用四元数来对旋转向量进行赋值
Eigen::AngleAxisd V3(t_Q); //3.2 使用四元数来对旋转向量进行初始化
V3 = t_Q; //3.1 直接使用四元数来对旋转向量赋值
std::cout << "Rotation_vector3 " << std::endl << V3.matrix() << std::endl;
//4. 欧拉角给旋转向量赋值
Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(2),Eigen::Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1),Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(0),Eigen::Vector3d::UnitZ()));
Eigen::AngleAxisd V4;
V4 =yawAngle*pitchAngle*rollAngle;
std::cout << "Rotation_vector4 " << std::endl << V4.matrix() << std::endl;
}
void test03() // 四元数 的 赋值 及元素访问
{
Eigen::AngleAxisd t_V(M_PI / 3, Eigen::Vector3d(0, 0, 1)); // 旋转向量
Eigen::Matrix3d t_R = t_V.matrix(); // 旋转矩阵
Eigen::Quaterniond t_Q(t_V); // 四元数
Eigen::Vector3d eulerAngle(0, M_PI/6, M_PI); // 欧拉角 yaw pitch roll z y x
// 对四元数赋值
//1.使用旋转的角度A和旋转轴向量n（此向量为单位向量）来初始化四元数,即使用q=[cos(A/2),n_x*sin(A/2),n_y*sin(A/2),n_z*sin(A/2)]
double A = M_PI/4;
Eigen::Quaterniond q1(cos(A/2),0*sin(A/2),0*sin(A/2),1*sin(A/2)); // Eigen::Quaterniond q1(w,x,y,z); // 四元数的初始化
std::cout << "Rotation_q1 " << std::endl << q1.coeffs() << std::endl; // x y z w
std::cout << "Rotation_q1 " << std::endl << q1.x() << ", " << q1.y() << ", " << q1.z() << ", " << q1.w() << std::endl;
//2. 使用旋转矩阵转四元數的方式
Eigen::Quaterniond q2(t_R); // 2.2 使用旋转矩阵来对四元數进行初始化
q2 = t_R; //2.1 直接使用旋转矩阵来对旋转向量赋值
std::cout << "Rotation_q2 " << std::endl << q2.coeffs() << std::endl;
//3. 使用旋转向量对四元数来进行赋值
Eigen::Quaterniond q3(t_V); // 2.2 使用旋转向量来对四元數进行初始化
q3 = t_V; //2.1 直接使用旋转向量来对旋转向量赋值
std::cout << "Rotation_q3 " << std::endl << q3.coeffs() << std::endl;
std::cout << "Rotation_q3 Inverse" << std::endl << q3.inverse().coeffs() << std::endl;
// 4. 欧拉角转四元数 借用 旋转向量
Eigen::AngleAxisd rollangle(eulerAngle(2), Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitchangle(eulerAngle(1), Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawangle(eulerAngle(0), Eigen::Vector3d::UnitZ());
Eigen::Quaterniond q4 = yawangle*pitchangle*rollangle;
q4.normalize();
std::cout << "Rotation_q4 " << std::endl << q4.coeffs() << std::endl;
// Eigen::Quaterniond q1(w,x,y,z); // 四元数的初始化
// Eigen::Quaterniond q2(Eigen::Vector4d(w,x,y,z)); // 向量
// Eigen::Quaterniond q3(Eigen::Matrix3d(t_R));
// Eigen::Quaterniond q3(Eigen::AngleAxisd(t_R));
// q1.toRotationMatrix(); // 转换成 旋转矩阵
}
void test04() // 旋转矩阵 的 赋值
{
Eigen::AngleAxisd t_V(M_PI / 4, Eigen::Vector3d(0, 0, 1)); // 旋转向量
Eigen::Matrix3d t_R = t_V.matrix(); // 旋转矩阵
Eigen::Quaterniond t_Q(t_V); // 四元数
Eigen::Vector3d eulerAngle(0,M_PI/6,0); //欧拉角 yaw pitch roll z y x 飞机的头部是x轴正方向
//1.使用旋转矩阵的函数来初始化旋转矩阵
Eigen::Matrix3d R1 = Eigen::Matrix3d::Identity();
std::cout << "Rotation_matrix1" << std::endl << R1 << std::endl; // 3*3 单位矩阵
R1<<1,2,3,4,5,6,7,8,9; // 由左至右 按行输入
std::cout << "Rotation_matrix1" << std::endl << R1 << std::endl; // 3*3 单位矩阵
//2. 使用旋转向量 转 旋转矩阵来对旋转矩阵赋值
Eigen::Matrix3d R2;
R2 =t_V.matrix(); // 使用旋转向量的成员函数matrix()来对旋转矩阵赋值
R2 = t_V.toRotationMatrix(); // 使用旋转向量的成员函数toRotationMatrix()来对旋转矩阵赋值
R2 = t_V;
std::cout << "Rotation_matrix2" << std::endl << R2 << std::endl; //
//3. 使用四元数转旋转矩阵来对旋转矩阵赋值
Eigen::Matrix3d R3;
R3 = t_Q.matrix(); // 使用四元数的成员函数matrix()来对旋转矩阵赋值
R3 = t_Q.toRotationMatrix(); // 使用四元数的成员函数toRotationMatrix()来对旋转矩阵赋值
R3 = t_Q;
std::cout << "Rotation_matrix3" << std::endl << R3 << std::endl; //
std::cout << "Rotation_matrix3 Inverse" << std::endl << R3.inverse() << std::endl; // 求旋转矩阵的逆变换
std::cout << "R3(0,0) " << R3(0,0)<< std::endl ;
//4. 使用 欧拉角 来为旋转矩阵赋值
Eigen::Matrix3d R4;
Eigen::AngleAxisd rollAngle(eulerAngle(2),Eigen::Vector3d(1,0,0));
Eigen::AngleAxisd pitchAngle(eulerAngle(1),Eigen::Vector3d(0,1,0));
Eigen::AngleAxisd yawAngle(eulerAngle(0),Eigen::Vector3d(0,0,1));
R4 = yawAngle*pitchAngle*rollAngle;
std::cout << "Rotation_matrix4" << std::endl << R4 << std::endl; //
}
void test05() // 欧拉角
{
Eigen::AngleAxisd t_V(M_PI / 4, Eigen::Vector3d(0, 0, 1)); // 旋转向量
Eigen::Matrix3d t_R = t_V.matrix(); // 旋转矩阵
Eigen::Quaterniond t_Q(t_V); // 四元数
Eigen::Vector3d eulerAngle(0,M_PI/6,0); //欧拉角 yaw pitch roll z y x 飞机的头部是x轴正方向
// 旋转矩阵 转欧拉角
Eigen::Vector3d eulerAngle1 = t_R.eulerAngles(2,1,0);
std::cout << "Rotation_angle1 " << std::endl << eulerAngle1(0) << " " << eulerAngle1(1) << " " << eulerAngle1(2) << " " << std::endl;
// 旋转向量 转欧拉角
Eigen::Vector3d eulerAngle2 = t_V.matrix().eulerAngles(2,1,0);
std::cout << "Rotation_angle2 " << std::endl << eulerAngle2(0) << " " << eulerAngle2(1) << " " << eulerAngle2(2) << " " << std::endl;
// 四元数 转 欧拉角
Eigen::Vector3d eulerAngle3 = t_Q.matrix().eulerAngles(2,1,0);
std::cout << "Rotation_angle3 " << std::endl << eulerAngle3(0) << " " << eulerAngle3(1) << " " << eulerAngle3(2) << " " << std::endl;
}
void test06() // 变换矩阵
{
Eigen::AngleAxisd t_V(M_PI / 4, Eigen::Vector3d(0, 0, 1)); // 旋转向量
Eigen::Matrix3d t_R = t_V.matrix(); // 旋转矩阵
Eigen::Quaterniond t_Q(t_V); // 四元数
Eigen::Vector3d eulerAngle(0,M_PI/6,0); //欧拉角 yaw pitch roll z y x 飞机的头部是x轴正方向
Eigen::Vector3d trans_p(0,2,2);
Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity(); //变换矩阵 定义与初始化 虽然称为3d，实质上是4x4的矩阵(旋转R+平移t)
std::cout << "Transform Matrix1" << std::endl << T1.matrix() << std::endl; // 4*4 单位矩阵
//T1.rotate(t_V); // 通过旋转向量 为 变换矩阵的 旋转部分赋值 // 所有的旋转和平移都是以原坐标系为准
//T1.rotate(t_R); // 通过旋转矩阵 为 变换矩阵的 旋转部分赋值
T1.rotate(t_Q); // 通过四元数 为 变换矩阵的 旋转部分赋值
T1.pretranslate(trans_p); // 通过三维向量 为 变换矩阵的 平移部分赋值 // 顺序可以颠倒
std::cout << "Transform Matrix1" << std::endl << T1.matrix() << std::endl; //
std::cout << "Transform Matrix1 Inverse" << std::endl << T1.inverse().matrix() << std::endl; //求变换矩阵的逆，返回一个变换矩阵 对原矩阵无影响
Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
//T2.linear() = t_R;//通过旋转矩阵 为 变换矩阵的 旋转部分赋值
T2.linear() << 1,0,0,0,1,0,0,0,1;
T2.translation() = trans_p;
std::cout << "Transform Matrix2" << std::endl << T2.matrix() << std::endl; //
std::cout << "Transform Matrix2 Rotation" << std::endl << T2.linear().matrix() << std::endl; //
std::cout << "Transform Matrix2 translation" << std::endl << T2.translation().matrix() << std::endl; //
std::cout << "T*(1,2,3) " << std::endl << (T2*Eigen::Vector3cd(1,2,3)) << std::endl; // //相当于R21*v+t21,隐含齐次坐
}
void test07() // 已知 两个坐标系在全局坐标系的位姿 求其中一个在另一个坐标系下的位姿 求t1 在 t2 下的位姿
{
Eigen::Vector3d eulerAngle(0,0,0);
Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(2),Eigen::Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1),Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(0),Eigen::Vector3d::UnitZ()));
Eigen::Quaterniond q_1;
q_1=yawAngle*pitchAngle*rollAngle;
q_1.normalize();
Eigen::Vector3d t_1 = Eigen::Vector3d(1,1,0);
Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
T1.rotate(q_1.toRotationMatrix());
T1.pretranslate(t_1);
//std::cout << "T1 " << std::endl << T1.matrix() << std::endl;
Eigen::Vector3d eulerAngle2(M_PI,0,0); // 第一个是z轴
Eigen::AngleAxisd rollAngle2(Eigen::AngleAxisd(eulerAngle2(2),Eigen::Vector3d::UnitX()));
Eigen::AngleAxisd pitchAngle2(Eigen::AngleAxisd(eulerAngle2(1),Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle2(Eigen::AngleAxisd(eulerAngle2(0),Eigen::Vector3d::UnitZ()));
Eigen::Quaterniond q_2;
q_2=yawAngle2*pitchAngle2*rollAngle2;
q_2.normalize();
Eigen::Vector3d t_2 = Eigen::Vector3d(0,0, 0);
Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
T2.rotate(q_2.toRotationMatrix());
T2.pretranslate(t_2);
//std::cout << "T " << T.matrix() << std::endl;
Eigen::Isometry3d T_inv = T2.inverse();
//std::cout << "T_inv " << std::endl << T_inv.matrix() << std::endl;
Eigen::Isometry3d T =T_inv*T1;
Eigen::Matrix3d t_Relate;
t_Relate << T(0,0),T(0,1),T(0,2),
T(1,0),T(1,1),T(1,2),
T(2,0),T(2,1),T(2,2);
std::cout << "T " << std::endl << T.matrix() << std::endl;
std::cout << "t_Relate " << std::endl << t_Relate << std::endl;
}
void test08() // 对 一个四元数 作 一个欧拉角的旋转变换
{
Eigen::Vector3d eulerAngle(0,m_PI,0);// 创建 一个 欧拉角 zyx
Eigen::AngleAxisd rollAngle(Eigen::AngleAxisd(eulerAngle(2),Eigen::Vector3d::UnitX())); // 将欧拉角 转换成 四元数
Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(eulerAngle(1),Eigen::Vector3d::UnitY()));
Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(eulerAngle(0),Eigen::Vector3d::UnitZ()));
Eigen::Quaterniond q_fix;
q_fix=yawAngle*pitchAngle*rollAngle;
q_fix.normalize();
Eigen::Vector3d eulerAngle_tep = q_fix.matrix().eulerAngles(2,1,0);
std::cout << eulerAngle_tep(2)*180/m_PI << " " << eulerAngle_tep(1)*180/m_PI << " " << eulerAngle_tep(0)*180/m_PI << " " << std::endl;
Eigen::Quaterniond q(1,0,0,0); // 旋转角度为 0 的四元数
Eigen::Quaterniond q_new = q_fix*q; // q_fix*
q_new.normalize();
Eigen::Vector3d eulerAngle2 = q_new.matrix().eulerAngles(2,1,0);
std::cout<< "@@@@@@@@@ " << eulerAngle2(2)*180/m_PI << " " << eulerAngle2(1)*180/m_PI << " " << eulerAngle2(0)*180/m_PI << " @@@" << std::endl;
}
void test09()// 常用运算
{
// 旋转
Eigen::Vector3d v1(1,1,0); // 将 三维向量 绕z轴逆时针旋转 45度
Eigen::AngleAxisd Angle_axis1(M_PI/4,Eigen::Vector3d(0,0,1));
Eigen::Vector3d rotated_v1 = Angle_axis1.matrix().inverse()*v1; // 3x3 * 3x1
std::cout << "rotate_matrix1 " << std::endl << Angle_axis1.matrix() << std::endl; //
std::cout << "rotated_v1 " << std::endl << rotated_v1 << std::endl; //
Eigen::Vector3d v2(0,0,0); // 将 三维向量 绕y轴逆时针旋转 45度
v2.x() = 2;
v2[2] = 2;
Eigen::AngleAxisd Angle_axis2(M_PI/4,Eigen::Vector3d(0,1,0));
Eigen::Vector3d rotated_v2 = Angle_axis2.matrix().inverse()*v2; // 3x3 * 3x1
std::cout << "rotate_matrix2 " << std::endl << Angle_axis2.matrix() << std::endl; //
std::cout << "rotated_v2 " << std::endl << rotated_v2 << std::endl; //
Eigen::Vector3d v3(2,2,0);
std::cout << "v3 模长 " << v3.norm() << std::endl; // 求模长
std::cout << "v3 单位向量 " << std::endl << v3.normalized() << std::endl; // 求单位向量
Eigen::Vector3d v4(1,0,0), v5(0,1,0);
std::cout << "(1,0,0) 点乘 (0,1,0)" << std::endl << v4.dot(v5) << std::endl; // 点乘 0
std::cout << "(1,0,0) * (0,1,0)" << std::endl << v4.transpose()*v5 << std::endl; // 乘 维数要对应 0
std::cout << "(1,0,0) 叉乘 (0,1,0)" << std::endl << v4.cross(v5) << std::endl; // 叉乘 (0,0,1)
Eigen::Matrix<double,4,4> T;
T.Identity();
Eigen::AngleAxisd angle_axis(M_PI/4,Eigen::Vector3d(0,0,1));
Eigen::Matrix3d R(angle_axis);
Eigen::Vector3d t;
t.setOnes(); // 所有分量为1
//t.setZero(); // 所有分量为0
T.block<3,3>(0,0) = R;
T.block<3,1>(0,3) = t; // <3,1> 是形状 (0,3)是左上角位置
std::cout << "T 的旋转 " << std::endl << T.topLeftCorner(3,3) << std::endl;
std::cout << "T 的平移 " << std::endl << T.topRightCorner(3,1) << std::endl;
std::cout << "T block " << std::endl << T.block<3,3>(0,1) << std::endl; // 可以输出 block
}
void test10() // 欧拉角的逆 借助四元数 验证四元数的逆
{
Eigen::Vector3d eulerAngle(0,M_PI/6,M_PI);
std::cout << "eulerAngle " << std::endl << eulerAngle(0) << " " << eulerAngle(1) << " " << eulerAngle(2) << " " << std::endl;
Eigen::AngleAxisd rollangle(eulerAngle(2), Eigen::Vector3d::UnitX()); // 欧拉角转旋转向量
Eigen::AngleAxisd pitchangle(eulerAngle(1), Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawangle(eulerAngle(0), Eigen::Vector3d::UnitZ());
Eigen::Quaterniond q = yawangle*pitchangle*rollangle; // 旋转向量相乘 再转四元数
q.normalize();
std::cout << "Rotation_q " << std::endl << q.coeffs() << std::endl;
Eigen::Quaterniond q_inversed = q.inverse(); // 四元数求逆
std::cout << "Rotation_q_inversed " << std::endl << q_inversed.coeffs() << std::endl;
Eigen::Vector3d eulerAngle_inversed = q_inversed.matrix().eulerAngles(2,1,0); // 四元数转 旋转矩阵 转 欧拉角
std::cout << "eulerAngle_inversed " << std::endl << eulerAngle_inversed(0) << " " << eulerAngle_inversed(1) << " " << eulerAngle_inversed(2) << " " << std::endl;
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"Eigen_test");
ros::NodeHandle nh;
ros::Rate rate(1);
while(ros::ok())
{
//calrotation();
//test01();
//test02();
//test03();
//test04();
//test05();
//test06();
//test09();
test10();
rate.sleep();
}
return 0;
}