前言

在上一篇博客（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();
1
2

这个就是更新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;
}
1
2
3
4
5
6

其中

• 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> >
1
2

• **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())
1
2
3

举个最简单的例子

    int array[9];
    for (int i = 0; i < 9; ++i)
        array[i] = i;
    std::cout << Eigen::Map<Eigen::Matrix3i>(array) << std::endl;
1
2
3
4

输出为：
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
1
2
3
4
5

输出为：
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;
1
2

在后面后端会把 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);
1
2
3
4
5
6
7
8
9
10
11
12

可以在rviz中添加一个odometry的选项，选择topic为/aft_mapped_to_init来显示。