PCL-以坐标轴形式渲染小车

前置研究

因为已经自学了四元数，打算自己绘制三条直线表示坐标轴，但仔细研究了pcl的相关方法以后，发现存在现有的渲染方法。
pcl中有坐标轴的相关方法如下：

// 增加一个坐标系，包含初始的姿态（仿射变换）
void pcl::visualization::PCLVisualizer::addCoordinateSystem	(	double 	scale,
                                                                const Eigen::Affine3f & 	t,
                                                                const std::string & 	id = "reference",
                                                                int 	viewport = 0 
                                                            )	

// 更新指定id的坐标轴的姿态（仿射变换）
bool pcl::visualization::PCLVisualizer::updateCoordinateSystemPose	(	const std::string & 	id,
                                                                        const Eigen::Affine3f & 	pose 
                                                                    )	

通过这两个函数，我就可以将小车的位姿以坐标轴的形式实时渲染在点云上，达成rviz的显示效果。

仿射变换

定义：是指在几何中，一个向量空间进行一次线性变换并接上一个平移，变换为另一个向量空间。

简单的讲，就是线性变换和平移（平移比较简单就不讲了）。
一个比较通俗的讲解
线性变换

定义：线性变换(linear transformation)是线性空间V到其自身的线性映射。

旋转、投影、伸缩、升降维等操作均属于线性变换的范畴。
旋转例子：
前提：坐标轴原点和其他点构成的向量表示该点的向量。
通过旋转矩阵与一个向量左乘，就可以得到旋转后的向量。

因此，仿射变换就可以通过
$$\vec{y}=A\vec{x}+\vec{b}$$
其中，A为旋转矩阵，y为线性变换后向量，x为线性变换前向量，b为平移向量(将线性变换后的向量从原点平移到b)

实现

问题的核心在于如何将IMU的数据转换为仿射变换的形式。
IMU提供的数据有3维坐标以及四元数，四元数对应旋转矩阵，3维坐标对应平移向量。

这里需要用到Eigen来进行矩阵相关的计算和存储。
Eigen::Quaternionf A 存放四元数
Eigen::Translation3f b 存放平移向量，也就是坐标
Eigen::Affine3f y 仿射变换矩阵
计算仿射变换矩阵
y = b * A.toRotationMatrix()
再调用方法将计算出的仿射变换矩阵传送给坐标轴重新渲染即可。
viewer -> updateCoordinateSystemPose(“car”,y);

经过测试，平移向量使用小车坐标就可以，其平移是对原点进行平移，且平移效果不会叠加。
效果如下：

由于点云颜色和坐标轴颜色相似，导致显示效果不是很好。但可以看到坐标轴的位置和姿态能够精确表现出小车的位置和姿态，达到了用坐标轴渲染小车的目的。