Eigen：：局部坐标系和ba坐标系转换"越有故事的⼈越沉静简单，越肤浅单薄的⼈越浮躁不安！"
⽆论是做SLAM还是传感器标定，离不开的是坐标之间的转换关系
Eigen ⼀个开源的C++矩阵运算库
perfect~
⾔归正传：
使⽤Eigen库进⾏空间坐标的转换。
１.ba坐标系－>局部坐标系
已知p1 和p2求 t12
假设传感器在ba坐标系下p1处的位姿为（x，y，z，θ）=（1，1，0，0）
在ba坐标系下p2处的位姿为（x，y，z，θ）=（2，2，0，45）
养肝护肝食疗求p1 和p2之间的转换关系 t12 简单表⽰为(1,1,0,45)。
/*
* 2019年01⽉04⽇
*/
#include <Eigen/Core>
#include <Eigen/Den>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include<iostream>
using namespace std;
int main()
{
//1.p1 world position
double p1a=0;
double p1x=1;
double p1y=1;
Eigen::AngleAxisd rotzp1(p1a*M_PI/180, Eigen::Vector3d::UnitZ());
Eigen::Matrix3d rotp1 = RotationMatrix();
Eigen::Isometry3d p1;学生在校表现评语
p1 = rotp1;
//2.p2 world positon
double p2a=45;
double p2x=2;
double p2y=2;
Eigen::AngleAxisd rotzp2(p2a*M_PI/180, Eigen::Vector3d::UnitZ());
Eigen::Matrix3d rotp2 = RotationMatrix();
Eigen::Isometry3d p2;
p2 = rotp2;
李世民有几个皇后p2.translation() = Eigen::Vector3d(p2x,p2y, 0);
//3.calculate t12
Eigen::Isometry3d t12=p1.inver() *p2;
Eigen::Matrix3d ation();
水仙花公主Eigen::Vector3d anslation();
Eigen::Vector3d eulerAngle=r12m.eulerAngles(0,1,2);
cout<<"eulerAngle roll pitch yaw\n"<<180*eulerAngle/M_PI<<endl;
cout<<"t12="<<endl<<vt12<<endl;
}
２.局部坐标系－>ba坐标系
已知p1和t12求p2
假设传感器在ba坐标系下p1处的位姿为（x，y，z，θ）=（1，1，0，0），
动物名称p1 和p2之间的转换（局部坐标系）为t12表⽰为(1, 1, 0, 45)， 求p2处世界坐标系下的位姿（x，y，z，θ）？
结果为（2，2，0，45）
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include<iostream>
using namespace std;
int main()
{
//1.p1 world position
double p1a=0;
double p1x=1;
double p1y=1;
Eigen::AngleAxisd rotzp1(p1a*M_PI/180, Eigen::Vector3d::UnitZ()); Eigen::Isometry3d p1;
p1 = RotationMatrix();
//2. t12 value
double t12a=45;
double t12x=1;
double t12y=1;
Eigen::AngleAxisd rott12(t12a*M_PI/180, Eigen::Vector3d::UnitZ()); Eigen::Isometry3d t12;
t12= RotationMatrix();
//3.calculate p2 world positon
Eigen::Isometry3d p2=p1*t12;
Eigen::Matrix3d ation();
Eigen::Vector3d anslation();
Eigen::Vector3d eulerAngle=p2rm.eulerAngles(0,1,2);
cout<<"eulerAngle roll pitch yaw\n"<<180*eulerAngle/M_PI<<endl; cout<<"t12="<<endl<<p2t<<endl;
}
3.附⼀个Lidar标定转换的⼩程序，可以参考
using namespace std;
using namespace Eigen;
int main(int argc, char **argv) {
Eigen::Vector3f e,p;
p.x() = 4.6;
p.y() = 1.25;
p.z() = 0;
e.x() = 41.9 * M_PI / 180; //yaw 偏航⾓
e.y() = -1 * M_PI / 180; //pitch 俯仰⾓
家庭火灾e.z() = -0.6 * M_PI / 180; //roll ⽔平⾓
Eigen::Matrix3f rot;
rot = Eigen::AngleAxisf(e(0), Eigen::Vector3d::UnitZ())//使⽤旋转的⾓度和旋转轴向量（此向量为单位向量）来初始化⾓轴:AngleAxisd V1(M_PI / 4, Vector3d(0, 0 * Eigen::AngleAxisf(e(1), Eigen::Vector3d::UnitY())
* Eigen::AngleAxisf(e(2), Eigen::Vector3d::UnitX());
Eigen::Matrix4f m; //1.使⽤旋转矩阵的函数来初始化旋转矩阵:Matrix3d R1=Matrix3d::Identity();
m.tIdentity();
m.block<3, 3>(0, 0) = rot;
m.block<3, 1>(0, 3) = p;
Eigen::Affine3f vlp;
vlp.matrix() = m;
PointCloud pointcloud;
pcl::fromROSMsg(*msg, pointcloud);河南轻工业学校
PointCloudPtr ba(new PointCloud);
pcl::transformPointCloud(pointcloud, *ba, vlp);
四级作文万能模板}
最后，附（特别好）
1.⼀个旋转矩阵（R），旋转向量（V）和四元数（Q）在Eigen中转换关系的总结:
