cartographer输出地理坐标系路线

cartographer输出全局坐标

目的： 确定一种方法，使得cartographer算法实时输出WGS 84坐标系统下的坐标。

cartographer算法独立坐标建立过程：

设备初始位姿为（0，0，0，$\varphi$，$\omega$，0）

其中$\varphi$，$\omega$为利用惯导原始数据计算得到的相对于水平面（惯导测得）的横滚和俯仰角，航向为0意味着设备面向方向为基准方向.

在此基础上进行坐标计算和更新，最后得到独立坐标系。

注意：惯导单体测得的角加速度和线加速度是相对于当地水平坐标系的

输出全局坐标路线

• POS对准后，记录POS数据对准后初始化时的位姿的同时启动算法；启动算法命令如下：

roslaunch cartographer_ros offline_backpack_3d.launch bag_filenames:=${HOME}/Downloads/b3-2016-04-05-13-54-42.bag

• 等待算法完成，后输出状态文件.Pbstream；输出Pbstream命令如下：

// Finish the first trajectory. No further data will be accepted on it.
rosservice call /finish_trajectory 0`
// Ask Cartographer to serialize its current state.
// (press tab to quickly expand the parameter syntax)  
rosservice call /write_state "{filename: '${HOME}/Downloads/b3-2016-04-05-14-14-00.bag.pbstream'}"

• 参照ply_writing_points_processor.cc，编写las_writing_points_processor.cc对独立坐标系整体进行坐标转换;采用下述命令进行输出。

roslaunch cartographer_ros assets_writer_backpack_3d.launch \
   bag_filenames:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag \
   pose_graph_filename:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag.pbstream

路线细节

转换到地理坐标系依次涉及四个坐标转换:

• 惯导单体坐标系（硬件厂家定义）转换到载体坐标系（X轴向前，Y轴朝左，Z轴朝上），下面称转换一；
• 激光扫描仪坐标（硬件厂家定义）转换到载体坐标系，下面称转换二；
• 载体坐标系转换到当地水平坐标系（东北天坐标系），下面称转换三；
• 当地水平坐标系转换到WGS84空间直角坐标系，下面称转换四。

转换一&转换二

转换一和转换二cartographer算法中已经进行了实现：转换在编译文件install_isolated/share/cartographer_ros/urdf中体现。

文件位置：

转换内容：

转换三

思路： 经过转换一和转换二，cartographer得到的是基于初始位姿（0，0，0，$\varphi$，$\omega$，0）载体坐标系下的点云数据，那么如果可以找到初始位置的姿态角（相对于当地水平坐标（惯导输出的姿态角就是相对于当地水平坐标的）），那么就可以根据转换公式（1）将点云从载体坐标系转换到当地水平坐标系：

惯导测姿得到的三个姿态角—横滚角、俯仰角、航向角用符号表示为$\varphi$，$\omega$，$\kappa$ ;

设激光脚点在当地水平坐标系下的坐标为$(X,Y,Z{)}_{Local}$ ，惯导坐标系导当地水平坐标系只涉及导三个坐标轴的转换，转换顺序为：

①先绕y轴旋转$\varphi$；
②再绕x轴旋转$\omega$；
③最后绕z轴旋转$\kappa$。

转换公式为：

$${\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)}_{Local}^T={\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)}_{Body}^T{R}_y{R}_x{R}_Z={\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)}_{Body}^T{R}_2\text{\hspace{1em}(1)}$$

其中：

$${\mathbb{R}}_2=R_y{R}_X{R}_Z=\left(\begin{array}{ccc}cos\varphi & 0& -sin\varphi \\ 0& 1& 0\\ sin\varphi & 0& cos\varphi \end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& cos\omega & sin\omega \\ 0& -sin\omega & cos\omega \end{array}\right)\left(\begin{array}{ccc}cos\kappa & -sin\kappa & 0\\ sin\kappa & cos\kappa & 0\\ 0& 0& 1\end{array}\right)=\left(\begin{array}{ccc}cos\varphi cos\kappa +sin\varphi sin\omega sin\kappa & -cos\varphi sin\kappa +sin\varphi sin\omega cos\kappa & -sin\varphi cos\omega \\ cos\omega sin\kappa & cos\omega cos\kappa & sin\omega \\ sin\varphi cos\kappa -cos\varphi sin\omega sin\kappa & -sin\varphi sin\kappa -cos\varphi sin\omega cos\kappa & cos\varphi cos\omega \end{array}\right)$$

载体坐标系到当地水平坐标系代码

void ParaFinetune::ComputeIMUToLocalCoord(double* IMUCoord, double r, double p, double y)
{
r = angleToARC*r;//角度换弧度
p = angleToARC*p;
y = angleToARC*y;

double cr=cos(r);
double cy=cos(y);
double tempCoord[3];
double sr=sin(r);
double sp=sin(p);
double sy=sin(y);
double cp=cos(p);
tempCoord[0] = (cr * cy + sr * sp * sy) * IMUCoord[0] + cp * sy * IMUCoord[1] + (sr * cy - cr * sp * sy) * IMUCoord[2];
tempCoord[1] = (-cr * sy + sr * sp * cy) * IMUCoord[0] + cp * cy * IMUCoord[1] + (-sr * sy - cr * sp * cy) * IMUCoord[2];
tempCoord[2] = -sr * cp * IMUCoord[0] + sp *IMUCoord[1] + cr * cp * IMUCoord[2];

IMUCoord[0] = tempCoord[0];
IMUCoord[1] = tempCoord[1];
IMUCoord[2] = tempCoord[2];
}

转换四

在获得初始位姿处的大地坐标$（BLH）$的基础上，根据计算公式将载体坐标系转换到当地水平坐标系：
设当地水平坐标系原点O在WGS84坐标系下的空间直角坐标为$(X,Y,Z{)}_O$ ，大地坐标为$（B,L,H）$ ，激光脚点在WGS84坐标系下的坐标为为$(X,Y,Z{)}_{WGS84}$。当地水平坐标系转换为WGS84坐标系步骤为：

①先绕坐标轴x旋转$B$；
②再绕坐标轴z旋转$L$；
③坐标系原点平移到WGS84坐标系原点。

转换公式为，

$${\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)}_{WGS84}^T={\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)}_{Local}^T{R}_x{R}_z+{\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)}_o={\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)}_{Local}^T{R}_3+{\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)}_o\text{\hspace{1em}(2)}$$

其中：

$${\mathbb{R}}_3=R_X{R}_Z=\left(\begin{array}{ccc}1& 0& 0\\ 0& cos(90^o-B)& sin90^o-B\\ 0& -sin90^o-B& cos90^O-B\end{array}\right)\left(\begin{array}{ccc}cos90^o+L& sin90^o+L& 0\\ -sin90^o+L& cos90^o+L& 0\\ 0& 0& 1\end{array}\right)=\left(\begin{array}{ccc}-sinL& cosL& 0\\ -sinBcosL& -sinBsinL& cosB\\ cosBcosL& cosBsinL& sinB\end{array}\right)$$

当地水平坐标系到WGS84坐标系代码

void ParaFinetune::ComputeLocalToWGS84Coord(double* LocalCoord, double B, double L, double H, double xWGS84, double yWGS84, double zWGS84)
{
B = angleToARC*B;//角度换弧度
L = angleToARC*L;
double sr=sin(L);
double sb=sin(B);
double cl=cos(L);
double cb=cos(B);

double tempCoord[3];
tempCoord[0] = -sr * LocalCoord[0] - sb * cl * LocalCoord[1] + cb * cl *LocalCoord[2] + xWGS84;
tempCoord[1] = cl * LocalCoord[0] - sb * sr * LocalCoord[1] + cb * sr * LocalCoord[2] + yWGS84;
tempCoord[2] = cb * LocalCoord[1] + sb * LocalCoord[2] + zWGS84;

LocalCoord[0] = tempCoord[0];
LocalCoord[1] = tempCoord[1];
LocalCoord[2] = tempCoord[2];
}