SLAM学习之路---绘制相机轨迹

一、概述

相机轨迹指相机在运动过程中每一帧的相机位姿由第一帧的相机位姿通过刚体变换（旋转矩阵和平移向量组成一个4x4的变换矩阵）而得到的，而这一系列的变换矩阵则组成了该相机的运动轨迹。本篇文章通过学习《视觉SLAM十四讲》第二版第三讲来记录。

二、轨迹绘制思路

1. 以第一帧对应的相机位姿为参考，可将第一帧位姿的原点设置为世界坐标系原点，第一帧的x轴为(1, 0, 0)，y轴为(0, 1, 0)，z轴为(0, 0, 1)。

2. 从相机轨迹文件中得到每一帧的变换矩阵，从第二帧开始，每一帧的变换矩阵乘上第一帧的相机位姿，即可得到每一帧所对应的相机位姿，然后绘制出每一帧的坐标系原点和三维坐标轴即可。

3. 绘制坐标轴时，坐标轴是由向量表示的，所以绘制的时候，利用每一帧相机位姿的原点加上该位姿的三个坐标轴向量即可绘制出三个坐标轴。将(x, y, z)三个坐标轴分别用（红，绿，蓝）色表示，每帧位姿的原点间利用白色线段连接起来，即可表示相机的轨迹。

三、绘制结果展示

不同视角：

四、代码

本代码根据书中代码改编，使用PCL的可视化进行轨迹显示

#include <iostream>
#include <Eigen/Eigen>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>

void DrawTrajector(const std::vector <Eigen::Isometry3d> &poses)
{
	pcl::visualization::PCLVisualizer visulizer;
	pcl::PointXYZ last_O_pos;
	for (int i = 0; i < poses.size(); i++)
	{
		pcl::PointXYZ O_pose;
		//每帧位姿的原点坐标只由变换矩阵中的平移向量得到
		O_pose.x = poses[i].translation()[0];
		O_pose.y = poses[i].translation()[1];
		O_pose.z = poses[i].translation()[2];
		if (i > 0)
			visulizer.addLine(last_O_pos, O_pose, 255, 255, 255, "trac_" + std::to_string(i));

		pcl::PointXYZ X;
		Eigen::Vector3d Xw = poses[i] * (0.1 * Eigen::Vector3d(1, 0, 0));
		X.x = Xw[0];
		X.y = Xw[1];
		X.z = Xw[2];
		visulizer.addLine(O_pose, X, 255, 0, 0, "X_" + std::to_string(i));
		
		pcl::PointXYZ Y;
		Eigen::Vector3d Yw = poses[i] * (0.1 * Eigen::Vector3d(0, 1, 0));
		Y.x = Yw[0];
		Y.y = Yw[1];
		Y.z = Yw[2];
		visulizer.addLine(O_pose, Y, 0, 255, 0, "Y_" + std::to_string(i));

		pcl::PointXYZ Z;
		Eigen::Vector3d Zw = poses[i] * (0.1 * Eigen::Vector3d(0, 0, 1));
		Z.x = Zw[0];
		Z.y = Zw[1];
		Z.z = Zw[2];
		visulizer.addLine(O_pose, Z, 0, 0, 255, "Z_" + std::to_string(i));

		last_O_pos = O_pose;
	}
	visulizer.spin();
}

int main()
{
	std::vector<Eigen::Isometry3d> poses;
	ifstream fin("./trajectory.txt");
	while (!fin.eof())
	{
		double time, tx, ty, tz, qx, qy, qz, qw;
		fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
		Eigen::Isometry3d Twr(Eigen::Quaterniond(qw, qx, qy, qz)); //将四元数转成旋转矩阵存储到变换矩阵的对应位置
		Twr.pretranslate(Eigen::Vector3d(tx, ty, tz));  //将变换矩阵的平移向量部分初始化
		poses.emplace_back(Twr);
	}
	DrawTrajector(poses);

	return 0;
}