图优化以及回环demo

        参考cartographer的图优化设计思路，完成了自己的后端优化模块，目前由于前面设计的数据结构导致了保存读取子图以及节点数据比较麻烦，只加入了子图与节点间的约束，编码器、imu、landmark都没有添加进去，还算得到了一个不错的效果。

        简单说下设计思路：

               子图：每个子图长度为80帧，前后20帧是跟相邻子图重合的部分，按照cartographer思想，用于添加intra-submap约束，当然这个可以用自己的说法，用于约束相邻子图。

                节点：保存了电晕你数据，经过了体素滤波后的数据，localpose、globalpose、submapid。

                 spa-optimization:由于我是先在windows上验证框架是否合理，suitesparse一直没配好，用的是eigensparse,导致优化计算申请内存不能大于16位，目前还不知道什么问题，有经验的麻烦提示一下，所以只选用了每张子图上的一部分节点数据。

demo效果：

                测试图优化效果（非回环），给初始的submap的globalpose的角度随机高斯噪声，然后用子图相交节点作为约束优化submap的全局位姿，原理同cartographer的图优化部分。

 优化前的子图数据

优化后的数据

没有经过ceres_match ,因为还不够稳定，优化后的地图虽然位置基本都是对的，但比较粗糙，还要继续改进。

                测试回环效果，用粒子滤波去扫了一圈办公室环境，如下图，可以看到地图无法闭合，经过图优化之后地图正常闭合了。

上位机的地图数据，地图闭合失败

优化前障碍物数据

 优化后的障碍物数据，地图成功回环。

        中间也是调试了很久，对图优化的理解更清晰了点，整体框架验证没问题，可以开始完成细节部分了，马上要做自认为最头疼的多线程设计了，c++菜鸡哭泣。