前言
在上一篇博客（ALOAM：后端lasermapping通过Ceres进行帧到地图的位姿优化）中，通过Ceres优化得到了 当前帧到地图的最优位姿
下面要做的是更新地图模块中维护的一个位姿，这个位姿就是odom到map之间的位姿变换。
为什么要更新这个位姿呢？
因为在前面这篇博客中（ALOAM:后端laserMapping代码结构与数据处理分析），在收到前端里程计数据后，会以前端里程计的频率，向外发布一个高频率当前帧到地图坐标系下的位姿。
这这里用到了T_map_odom,就是在后端通过Ceres得到当前帧到map的位姿后，再计算odom到map的位姿，所以要更新这个位姿，为下一帧做准备。
并且在进行栅格地图位置更新处理的时候，也通过上一帧维护的T_map_odom，得到当前帧的一个初值估计。所以得到当前帧到map的位姿后，需要更新odom到map的位姿，为下一帧的处理做准备。相关内容在这篇博客（ALOAM:后端lasermapping地图栅格化处理与提取）的这个地方:
odom到map之间的位姿更新原理
通过Ceres的优化 得到了，当前帧到map的位姿变换
前端里程计向后端发布的是，当前帧到odom的位姿变换
那么计算odom到map的位姿变换的公式为：
写成旋转+平移的齐次矩阵的形式，上面的公式则变为：
那么 odom到map之间旋转和平移则为
下面来看ALOAM中代码是如何实现的
代码解析
在前面通过ceres的优化得到了 当前帧到地图的位姿变换。该位姿变换存在 parameters 变量中。
这个parameters是一个double 的7维数组
前4位是旋转四元数，后三位是平移向量
然后程序则调用了
//更新odom到map之间的位姿变换
transformUpdate();
这个就是更新odom到map之间的位姿变换的函数。
里面的内容是：
//更新odom到map之间的位姿变换
void transformUpdate()
{
q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
}
其中
- q_wmap_wodom 就是 odom到map 的旋转 四元数
- t_wmap_wodom 就是 odom到map 的平移
q_wodom_curr 是前端里程计发来的 curr到odom的 旋转四元数
t_wodom_curr 是前端里程计发来的 curr到odom的 平移
- q_w_curr 是Ceres优化的 curr到map的旋转四元数
- t_w_curr 是Ceres优化的 curr到map的平移
在整个程序中并没有发现q_w_curr 和 t_w_curr 被赋值的地方，尤其是ceres计算完后并没给这两个变量赋值。
那么这两个变量是如何被赋值的呢？
在它俩声明的地方：
是通过eigen的map声明的
当ceres 更新 parameters之后 会通过 eigen的map 实时更新q_w_curr和t_w_curr 不需要自己再去转换
这里简单介绍下 Eigen 的Map 类：
Eigen中定义了一系列的vector和matrix，相比copy数据，更一般的方式是复用数据的内存，将它们转变为Eigen类型。Map类很好地实现了这个功能。
简单理解Map就是将原始“连续内存存储”的数据，以矩阵形式重新组织。在使用Map时就需要原始数据，Map后的数据的维度形式，Map时使用的Stride设定。所谓Stride，既指矩阵中沿着矩阵列或行方向移动一个位置，内存中需要移动的位置数。这个需要移动的内存位置数与矩阵采用的存储方式有关（列或行主导）。
Eigen::Map原型
template<typename PlainObjectType, int MapOptions, typename StrideType>
class Map: public MapBase<Map<PlainObjectType, MapOptions, StrideType> >
- PlainObjectType ：映射数据的等价矩阵类型
- MapOptions：指定指针对齐方式，默认是未对齐的
- StrideType:指定步长
这个类的作用就是让非Eigen数据结构变成Eigenj矩阵或者向量时，减少在复制过程中的开销，Eigen官网描述这样使用没有开销。
构造函数有以下
Map (PointerArgType dataPtr, const StrideType &stride=StrideType())
Map (PointerArgType dataPtr, Index size, const StrideType &stride=StrideType())
Map (PointerArgType dataPtr, Index rows, Index cols, const StrideType &stride=StrideType())
举个最简单的例子
int array[9];
for (int i = 0; i < 9; ++i)
array[i] = i;
std::cout << Eigen::Map<Eigen::Matrix3i>(array) << std::endl;
输出为：
0 3 6
1 4 7
2 5 8
再举一个和ALOAM使用情况一样的例子，将一个数组元素转换成Eigen数据结构
int data[] = {
1,2,3,4,5,6,7,8,9};
Map<RowVectorXi> v(data,4);
cout << "The mapped vector v is: " << v << "\n";
new (&v) Map<RowVectorXi>(data+4,5);
cout << "Now v is: " << v << "\n
输出为：
The mapped vector v is: 1 2 3 4
Now v is: 5 6 7 8 9
看了这几个例子，就明白，在Ceres优化完成后，这两个变量（q_w_curr ，t_w_curr ）就被更新值了。
- q_w_curr 是Ceres优化的 curr到map的旋转四元数
- t_w_curr 是Ceres优化的 curr到map的平移
在transformUpdate() 函数中计算的公式就是上面推导的：
q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
在后面后端会把 q_w_curr，t_w_curr做为里程计更新发布出来
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
odomAftMapped.pose.pose.position.x = t_w_curr.x();
odomAftMapped.pose.pose.position.y = t_w_curr.y();
odomAftMapped.pose.pose.position.z = t_w_curr.z();
pubOdomAftMapped.publish(odomAftMapped);
可以在rviz中添加一个odometry的选项，选择topic为/aft_mapped_to_init来显示。
