画SLAM的轨迹方式

先说机器人位姿，正常情况下，变换矩阵T包含：旋转矩阵【3*3】与平移矩阵【3*1】

但是，旋转矩阵9维只表示了3个自由度，具有冗余性，于是一种更紧凑的描述方式：旋转向量出现了。

1.旋转向量3*1：该向量方向与旋转轴一致，向量长度等于旋转角。通过罗德里格斯公式将旋转向量3*1转化为旋转矩阵3*3

缺点：有奇异性。

2.四元数4*1：复平面上，乘上复数i相当于逆时针把一个复向量旋转90度。一个实数，三个虚部。

q=q1i+q2i+q3i+q0

之所以讲述上述是由于在输出数据集中，会以上述方式表示旋转矩阵R。

---

Eigen几何模块：

旋转矩阵:3*3 Matrix3d

旋转向量: 3*1 AngleAxisd

四元数:4*1 Quaterniond

欧拉角: 3*1 Vector3d

欧式变换矩阵： 4*4  Isometry3d：R|t

仿射变换： 4*4 Affine3d：A|t，A可逆即可，无须正交矩阵R。

射影变换： 4*4 Projective3d
 

//旋转矩阵R（3*3）
Matrix3d rotation_matrix=Matrix3d::Identity()
 
//旋转向量，并沿Z轴转45度（3*1）
AngleAxisd rotation_vector(M_PI/4,Vector3d(0,0,1))
 
//欧拉角(ZYX顺序，yaw,pitch,roll)（3*1）
Vector3d eular_angles=rotataion_matrix.eularAngles(2,1,0);
 
//变换矩阵T（4*4）
Isometry3d T=Isometry3d::Identity();
T.rotate(rotation_vector);
T.pretranslate(Vector3d(1,3,4));
cout<

举例来说：

        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );//四元数4*1
        Eigen::Isometry3d T(q);//四元数转为变换矩阵4*4
        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));//平移向量

data中数据集是以平移向量+四元数的方式保存的。

 Mat rvec = Mat::zeros(1, 3, CV_32F);//旋转向量

    rvec.at(0,0) = 0.f;
    rvec.at(0,1) += CV_PI*0.01f;
    rvec.at(0,2) = 0.f;

    Mat rmat;
    Rodrigues(rvec, rmat);//罗德里格斯公式将旋转向量转为旋转矩阵

    Affine3f pose(rmat, Vec3f(0,0,0));

 数据集以平移向量+旋转向量的方式保存。

---

---

读取pose.txt数据

 

ifstream fin("./pose.txt");

vector> pose;

	for (int i = 0; i<5; i++)
	{
        
        //读取位姿数据
		double data[7] = { 0 };
		for (auto& d : data)
			fin >> d;

		//四元数
		Eigen::Quaterniond q(data[6], data[3], data[4], data[5]);
		//变换矩阵T初始化旋转部分
		Eigen::Isometry3d T(q);
		//T初始化平移向量部分
		T.pretranslate(Eigen::Vector3d(data[0], data[1], data[2]));
		//pose的对象类型就是Isometry3d，变换矩阵T
		poses.push_back(T);
	}

double data[7]={0} //初始化一个有7个数的数组data，初始化为0；

for (auto& d : data) // >>遇到空格就停止，把data中所有原数据0通过d遍历更改为fin中数据。
       fin >> d;

最终：data中为这一行中pose.txt的数据。

fin也会随着赋值给data,自动向下一行转过去吗？

---

画轨迹

        vector lines; //lines是vector，对象类型为WLine
        point_end = Point3d( // 更新当前帧的终点位置
            T.translation()(0, 0),
            T.translation()(1, 0),
            T.translation()(2, 0)
            );
 
        viz::WLine line(point_begin, point_end, cv::viz::Color::green());//起点到终点画直线
        lines.push_back(line); //不断收集当前Line
        // 迭代器进行循环，迭代lines
        for (vector::iterator iter = lines.begin(); iter != lines.end(); iter++)
        {
            string id = to_string(i);
            vis.showWidget(id, *iter);//不断展示线条Line
            i++;
        }
        point_begin = point_end; // 更新起始点