《基于多层次概率框架的联合语义建图算法设计与实现》本科毕设每日进展总结

之前的一些总结

任务书

标题《基于多层次概率框架的联合语义建图算法设计与实现》

题目内容：

传统的三维地图仅重构环境的几何维度信息，并未融合物体语义标签和类别等高层次信息。随着深度学习的快速发展，语义建图的相关研究引起了大量关注。本课题将研究基于多层次概率框架的机器人联合语义建图算法，首先要求检索多机器人协同语义感知及建图的国内外研究现状，在实现单机器人构建三维语义地图的基础上，设计基于多层次概率框架的全局语义地图更新和融合方法，在现有数据集进行测试和验证。

任务要求：

1.通过对传感器特性分析，设计基于多层次概率框架的机器人联合语义建图算法，实现对环境的三维语义重构。
2.通过公开数据集，开展协同语义建图算法的实际验证。
3.开展定量实验数据处理，分析协同语义建图的精度和算法复杂度。
4.进行实车搭载验证实验或者半实物搭载验证实验。

主要参考文献（4-5篇）：

1.Y. Yue and D. Wang, “Collaborative Perception, Localization and Mapping for Autonomous Systems”. Springer, 2020.
2. Yue Y, Zhao C, Wu Z, Yang C, Wang Y, Wang D. “Collaborative Semantic Understanding and Mapping Framework for Autonomous Systems”. IEEE/ASME Transactions on Mechatronics. 2020.
3. J.Jeong,T.S.Yoon,andJ.B.Park, “Multimodal sensor-based semantic 3d mapping for a large-scale environment,” Expert Systems with Applications, vol. 105, pp. 1 – 10, 2018.
4. J. S. Berrio, W. Zhou, J. Ward, S. Worrall, and E. Nebot, “Octree map based on sparse point cloud and heuristic probability distribution for labeled images,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3174–3181, 2018.
5. S. Yang, Y. Huang, and S. Scherer, “Semantic 3d occupancy mapping through efficient high order crfs,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 590–597, 2017.

ICRA视频的笔记

题目：未删减版：自主系统的协同语义理解和建图框架
ICRA删减之后的题目：协同概率语义建图的层次框架

Introduction：

多机器人协同：希望机器人相序交互，并协作执行任务。

基本需求：协作系统需要执行协同语义理解并共享信息。

应用领域：
1、可控性分析的形成控制
2、一组执行高级任务的机器人
3、自动驾驶车辆共享语义地图并更新城市的全景地图。

动机：
·当前的工作仅专注于几何信息融合，例如平面，线和点
·尚未研究协作语义建图方法的综合分析，建模和实现。

贡献：
·层次协同概率语义建图框架被提出
·提出了期望最大化算法（EM）来估计隐藏的数据关联
·开发协作系统来执行场景理解和全局语义建图

Method：

总体流程：
目标：实现协同机器人语义建图
子模块：
1、多模态语义信息融合：点云和标签图像融合得到语义点云，再经过占用更新和语义更新
2、单机器人语义建图：得到一个机器人的语义地图
3、协同语义地图融合：协同机器人交流，协作式语义地图融合

协同语义建图问题表述：
单机器人级别：目标是根据其相机观测值和3D激光观测值和机器人轨迹来建立它的局部语义地图
协作机器人级别：目标是使用来自相邻这机器人的局部地图，在完全分布式网络中构建全局语义地图

多模态语义信息融合：
1、我们的语义3D建图使用了一个3D激光雷达和一个相机作为它的主要传感器
2、使用Deeplab模型（一种语义分割模型）处理图像以进行分割
3、生成语义点云

Experiment：

多模态感知、几何建图、语义建图、协同融合、分布式交互

Conclusions：

·提出了一种新颖的分层概率语义图融合框架，以解决低层次和高层次语义映射中的问题。
·提出了一种期望最大化（EM）方法来估计局部地图中体素之间的隐藏数据关联
·开发了协作机器人系统以执行场景理解和全局语义映射

目前的创新想法

加入3D CRF优化地图

目前已做工作

实现了源代码和CRF优化代码各自的运行。除此之外还运行了LOAM的代码，和husky的代码，但都局限于对源代码的运行，没有任何改动。
阅读了很多有关语义建图的文献。

请教岳老师的一些思路问题

1、联合语义建图框架属于我的创新点吗？我能不能说我的工作是在两个大山之间搭建了桥梁？
答：单机离线处理的，不涉及通信，联合语义建图和地图优化是两个创新。
这部分介绍一下，作为自己已有的知识。

2、如果只是地图优化的话，是不是就是文章的拼凑呀，能算是创新吗？论文查重怎么办？
答：不是拼凑，而是把一个方法应用过来。

3、如果专业一点，我是不是需要找到语义地图融合这个融合方法上的创新点？
答：不一定，当然也可以，比如之后可以找连续域内的。可以多参考几何建图的方法，加一个维度。

4、我可不可以优化为：语义地图去除移动的车和人，然后CRF优化，得到静态地图？
这里用了Multimodal sensor-based semantic 3D mapping for a large-scale environment ，这篇文章3D细化就是xy有一个标签，起初车辆聚类分析，然后提出长度或体素太多的。
答：可以的

5、构造的地图是八叉图吗，是栅格图吗？LiDAR构建出来的点都在离散坐标下吗？是不是只有投影到了图像上的点才会展示出来，并且占用概率大于0.5的点才展示出来？
Robust and Efficient Multirobot 3-D Mapping Merging With Octree-Based Occupancy Grids
答：是的，是的，是的，是的

6、感觉如果只是单纯的语义地图的融合只是增加了判断纬度上的信息（语义概率），难点在哪里？
答：发现问题是很难的问题

7、两个机器人之间最开始的相对位姿是不是给定的？是不是可以根据匹配的点求他们两个最开始的相对位姿？
看到了您的一篇IROS：Collaborative Semantic Perception and Relative Localization Based on Map Matching，是期望最大化的方法求解变换矩阵，感觉和庆祥学长的很像，如果做起来是不是有点难了，还是直接使用学长的。我可不可以用6个相匹配点得到相对位姿，可行吗，属于又一个创新吗，还是算法里面用的就是做好了的？不对不是用的5D坐标吗？
答：这以后你们可以一起做，可以往前做，也可以往后做

8、优化的时候，肯定要消耗大量资源，什么时候优化呢？有必要一直传输局部地图吗，最后整合起来不就可以了吗？或者用户需要的时候整合起来。
答：

*9、现在这个分布式的多机建图框架是每个机器人都接收其他机器人的地图吗？那岂不是每个机器人都有一个全局地图了，是不是资源浪费了？可不可以建一个总站，所有机器人分别建图，都存在自己的局部地图，但不发送出来，当最开始两个机器人相遇时，把这两个地图融合起来发送给总站，然后以其中一个机器人的基准坐标，这两个机器人如果和其他机器人相遇（或经过相同场景，ICP？），就把其它机器人的地图也融合进来。被融合进来的机器人需要每隔一段时间发送一下自己的地图，没有被融合的机器人不需要发送。换个思路，是不是不需要每次发送整个局部地图，只需要发送新建立的地图，相当于每次都地图融合一下。——之后的研究
答：每个机器人都要有全局地图，方便以后决策。

octovais

请教学长的一些代码问题

1、代码中有很多注释掉的部分，有哪些文件或者代码可以删掉
2、有没有南洋理工大学的数据呀，需要用到
3、怎么把一个bag拆分为两个bag呀
4、公开数据集在哪里找的

并行化了map的publish
mappointsaver定义Octomap

添加函数：用了模板，Octomap 通用Cloud Octree 否则写清楚
子、父、委托，在base
map_tool map_fusion
发出来的是点云，提取成点云。地图有时有bug。
如果用传感器数据，两个机器人定位算法不用同名，tf树

2021/2/1

收获

一、借助Husky，主要学习怎么在gazebo中搭建一个带有velodyne16线传感器的小车，但是还有一些问题，总是报错。了解了一些urdf的知识。

husky gazebo

二、roscd可以用来看看目前有哪些包。

三、 发现gazebo中发布的消息，可以在rviz中展示，但主要要修改上面的map。

感想

希望明天可以完成3D激光雷达的搭载，和主题的发布，并使用程序完成LOAM。

2021/2/2

收获

一、VLP16成功了！
搞了这么久VLP16，发现直接用官方的包就可以了。只需要给出父节点，和子节点，和topic。其他参数默认的就行。
只需要引入.urdf.xacro文件，然后相应位置加上：

       <VLP-16 parent="top_plate_link" name="velodyne" topic="/velodyne_points" hz="10" samples="440">
         <origin xyz="0.2206 0.0 0.00635" rpy="0 0 0" />
       </VLP-16>

但是还有一个问题就是太卡了，不知道改hz和samples会有什么影响。 可以先在这边录制一个包，然后去那边播放。
LOAM是可以用了，但是由于一些原因（特征太少，可能还需要有交通道路的那种才好），里程信息并不准确。

二、launch文件之间可以相互引用。
如我在husky_playpen.launch里面引用了键盘控制：

<include file="$(find teleop_twist_keyboard)/keyboard.launch" /> 

感想

希望明天可以找到合适的world，使得LOAM正常运作

2021/2/3

收获

一、腾讯会议同济大学符长虹教授：
《无人机+视觉》

https://blog.csdn.net/weixin_43807148/article/details/113619719

二、腾讯会议北京航空航天余翔教授讲：
《怎么准备科研论文》

https://blog.csdn.net/weixin_43807148/article/details/113619548

感想

哎，今天开了三个腾讯会议，没干什么特别的事情。
原来论文《SemanticFusion: Dense 3D Semantic Mapping with Convolutional Neural Networks》中已经有了CRF优化3D，但是他是surfel的，而且是二阶的。看来那个国人写的《Large-Scale 3D Semantic Mapping Using Monocular Vision》真的是模仿这个呀，前两阶一样。
我本科毕设创新点可以提一些新的CRF的吉布斯能量公式
明天需要建立好world，然后解决时延和位姿问题。

2021/2/4

收获

VLP16中samples确实是采样点,1801代表1800个点的意思，加大的话确实有助于位姿估计，然而只要一开某个接收器（LOAM程序或rviz），husky_playpen.launch的程序就会变得比较卡。现在用的1、2Hz
而且即使直线时里程计走的挺好的，但一旦转弯，里程计就崩溃了，不知道是不是小车转弯不够稳造成的。

感想

现在只运行仿真和LOAM就这频率了，到时候再运行语义分割岂不是要废了。
今天上午+中午搞了党课的心得体会，下午搞了LOAM+gazebo，晚上又在搞开题报告的修改，哎，还有文献翻译也得搞，什么时候才能休息一下呢？
今天小年，晚上吃了饺子，可惜还是一个人吃饭。

2021/2/5

收获

一、太奇怪了！
影响里程计判断的确实是samples，hz=“3” samples="2500"就可以。
以为LOAM影响，所以打算用包录制。可是rviz中明明看到的没有畸变，在LOAM里面每一帧都发生了畸变。也不是播放速度的原因，0.5或者1或者2倍速播放包都是这样。看来确实是录制的时候，有畸变。说明LOAM和husky并不冲突，而是husky本身有问题。或许还是因为采样点太多了，而不知道它采样一次要多久。
又发现畸变和HZ好像有关，可以找个15Hz
最后用的hz=“15” samples=“2000”
二、动态环境SLAM的方法
处理动态因素三种方法：基于信息融合，和惯导结合；基于深度信息，可以找到可靠区域；纯使用视觉，多摄像机，成本低，但是导致动态因素监测的模糊性。

感想

仿真软件就是卡，实际硬件上应该没事。明天把PPT做出来，然后看看gazebo仿真还能不能拯救。
也该看看CRF的代码了。

2021/2/6

收获

制作明天的PPT，疯狂演习

感想

又是碌碌无为的一天，哎，主要是对自己英语水平的不放心

2021/2/7

收获

一、上午组会，下午花了三个小时写了博客

在Ubuntu18.04中使用gazebo配合LOAM算法仿真

二、几次测试发现，不是gazebo的问题，gazebo发出的点云是正常的，LOAM中显示的velodyne_point_registered不正常

感想

既然发现了问题所在，明天尝试一下新的LOAM算法，看看能不能成功

2021/2/8

收获

一、学习了一些深度学习的知识

• 编码：就是将输入的序列编码成一个固定长度的向量；
• 解码：就是将之前生成的固定向量再解码成输出序列；
• 输入序列和输出序列：正是机器翻译的结果和输出；
• 分心机制：显然，在未考虑注意力机制的 Encoder-Decoder 模型中，这种不同输入的重要程度并没有体现处理；
• Attention 机制：语义编码 C 就不在是输入序列 X 的直接编码了，而是各个元素按其重要程度加权求和得到的；
• Transformer：第一个完全依靠自我注意来计算其输入和输出的表示而不使用序列对齐的RNN或卷积的转导模型；

ResNet被全面超越了的论文的作者的报告：

T2T-ViT

二、Lego-LOAM有毒
搞了好久，中途桌面卡死的三次，不知道什么原因。
成功了一次，但不知道为什么标红，明明有那个库，他却找不到，而且rviz中显示的没有整个构建完成的地图，有点难受

感想

明天一定要把lego-loam和gazebo配合起来，然后阅读论文。
如果实在还不行，可以试着用A-LOAM。

2021/2/9

收获

一、果然LOAM有问题，现在换成了LEGO-LOAM，和A-LOAM都没事，而且LEGO-LOAM好像更准确一点。
二、程序找动态库默认位置为/usr/lib

感想

明天大年二十九，搞一搞相机和雷达的标定吧。然后把文献翻译搞了。
既然LEGO-LOAM中用到了平滑度s，那我可以把这个作为CRF优化的一个二阶能量！
突然又想到，LEGO-LOAM中用到了聚类之后标签，

2021/2/10

收获

一、摄像机和雷达之间的标定，主要注重RT的获得，在gazebo仿真中可以通过TF得到准确的RT。其实不太理解这句话，当时为啥不多问！
二、完成了大部分的文献翻译。

感想

还以为文献翻译很简单，发现我错了。哎，打公式可太难了。
明天就是大年三十了，我却感受不到一点年味，可能是因为远离家乡，也可能因为确实年味淡了。
明天给自己放个假吧，把剩下的文献翻译搞完就行了。

2021/2/11

收获

完成了文献翻译

感想

今天是大年三十，看春晚，嗑瓜子，自己一个人也要搞年味！！！
明天大年初一，给自己放个假！！！

2021/2/13

收获

一、看了一些CRF代码，感觉好乱呀，来回调用，不过大致思想是了解的。
二、尝试着把Lidar和camera结合起来，不过困难重重呀。不知道这么做到底有没有意义。

感想

之前岳老师说这个代码里面没有概率更新，说我主要做优化。可是这怎么优化啊啊啊，原始标签得到优化后的标签。关键CRF是在图像上直接CRF的，我要在点云里面CRF，也就CRF需要每个点的特征向量（位置xyz，颜色RGB，光滑度s）了好奇怪呀。

2021/2/14

收获

一、安装了octovis。发现安装这些东西时一定要加上ros-melodic-xxx。使用octovis可以改变叶节点的大小，也可以看各种类型（Occpanied, free，Octree结构），可以看各种渲染（语义的，高度渲染的），可以改变叶节点大小。octovis octomap.bt
二、学习了一些PCL的知识。pcl_viewer xxx.pcd

感想

今天又没干出什么东西，学习pcl也是为了给点云加色彩，希望明天点云结合有进展。
今天看PCL，他提到了features这个库，可以找到点的特征，那CRF特征可以从这里找

2021/2/15

收获

一、对PCL有了大致的了解
二、阅读了LEGO-LOAM的源码，学习了一些PCL知识，读起来轻松了不少

感想

明天阅读完LOAM的代码，找到自己可以创新的位置。继续尝试标定（发现拖了快一周了）
有点迷茫不知道自己从哪里下手了。

2021/2/16

收获

一、尝试着把VLP16改成只看前面180度的，但是LEGO-LOAM就用不了了(程序里面参数已经改了)，不知道为什么呢！
二、搭建了一个新的world，但是运行起来是真慢，花了40分钟昨晚一圈，不过建图效果真不错。
三、在gazebo中如果没有订阅器，小车走起来没问题，有订阅器，则会走的一卡一卡，就好像定时停下来发送一样。卡的频率和Velodyne的频率有关，每卡的时间与采样点有关。到时候测试的时候可以拿museum那个图，毕设用自己建的这个world。
四、匆匆忙忙地看完了LEGO-LOAM的代码注释，只有浅薄的理解。

感想

如果只用一部分点云对里程计估计的话，会更不准确。不如改成图片看到的部分（多少列到多少列）用语义，看不到的用聚类。
可以在LEGO-LOAM程序里面修改，加一个包，语义分割+语义融合。让它发送语义点云和tf。然后取semantic_ws里面构建地图。
代码都这么长，之后在哪修改都是个问题，更别提加入自己的创新点了，哎，毕不了业了。

2021/2/17

收获

学习了PCL的一些基本操作，写了两个程序，一个是发布动态的彩色点云，另一个是接收点云消息，换一个颜色重新发布。

感想

pcl学习的差不多了，明天试试语义分割

2021/2/18

收获

一、学习了怎么把没有颜色的点云消息，变为自己定义的点云消息，增加标签和颜色属性
二、deeplab v3+ 好麻烦，配置环境一堆，还要自己训练？

感想

哎，又学了不少pcl，这次pcl真的应该学习够了。
有时间需要在gazebo里面录制一个包，不能每次都走这么慢呀。

2021/2/19

收获

一、使用rosbag录制了一个包，包含相机图像和点云，以后可以用这个包。但是在LEGO-LOAM中测试时候，有问题，就是TF没找准，总是会偏，每次运行结果都不一样，但是ALOAM却是正常的，难道是电脑性能不行。应该不是LEGO-loam的原因吧，因为当时没有录包是正常的，奇怪啊。
二、学习了读取图像的消息，转化为Mat。但是在从Mat读取颜色的时候，出现了问题，还不知道为什么。

感想

哇，吓死了，今天为了录包，用了apt-get autoremove 这个指令，运行了一会发现不对劲，赶紧ctrl z了。以后一定不要用，据说是新手收割机，会删掉，一些依赖的自动下载的文件，太可怕了，全下回来！
明天要让点云中每个点为相机观察到的颜色。

2021/2/20

收获

一、去掉了昨天的全局变量Mat，改成了一个颜色cv::Scalar，虽然成功了，但这个可能因为全局变量很小才成功了。不知道之后如果需要整个图像全局变量该怎么办，毕竟一个图像大小太大了，反应速度不够。肯定不能把Mat类型做全局，应该要自己写个矩阵了。
二、重新搭了一个小一点的world，录了一个包，和LEGO-LOAM运行了一下，没有什么畸变。
三、python文件都要在权限中改成可执行文件，并且在第一行加上#! /usr/bin/env python。PCL的python依赖和c++还不一样，需要另外安装。
四、把python默认从2.7改到了3.6，导致pip也得安装很新的，然后好多pip需要重新下载。

感想

明天又是组会了，明天下午看论文，写周报、周志，时间真快呀。
如果有机会可以把投影运行出来。

2021/2/21

收获

一、开了组会。
二、放弃了Python-pcl，环境太难了

感想

今天没干什么实际的事情，明天加油搞pcl

2021/2/22

收获

一、ros中很多包使用的python2.7,所以我把默认Python又换成了2.7
二、把点云的range和inetensity投影在了2D平面上，成功使用灰度图可视化了。
三、gazebo中录制的点云有intensity通道，但没有intensity信息，毕竟是仿真

感想

搞深度和距离可视化又搞了两天，真麻烦呀。现在最主要的问题就是不知道怎么把点云投影在图像上了。如果投影成功该多好啊。
对了今天还想了一下，既然图像语义分割只是为了得到点云的语义信息，那为什么不直接对点云进行语义分割呢，不行，16线不够？如果是因为点云太稀疏的化，那可不可以使用多个相机，没必要的，这样就不会造成点云的浪费了。

2021/2/23

收获

一、gazebo录制的包，点云有缺失，一是因为小车顶面和相机支架挡住了，二是看到了高处的天空，所以重新调整了world和传感器位置，又录制了一个包。
二、gazebo激光雷达和相机融合不需要标定，但参数不知道求的对不对，哎。

感想

今天岳老师否了我的第二个创新点，难过。岳老师说创新点不能随便提，要有的放矢。
明天把彩色点云搞出来，看一遍岳老师发的那个文章。

2021/2/24

收获

还在搞相机和点云的融合，但是依然存在问题，没有重叠。

感想

哎，搞了一天还是没有把融合搞出来，难道真的要去标定吗，也太麻烦了。还是说因为是RGBD的原因，所以需要标定？啊，不知道，明天加油搞出来！

2021/2/25

收获

一、有了图案，但是偏离。
二、语义标签的概率是不确定性，可以表示为到类边界的距离？（一篇论文是这样的）

感想

不搞数据融合了，到时候还是标定来的实在。明天看CRF代码如何融入到原本毕设中。

2021/2/26

收获

一、对相机内参进行了标定，得到的结果和理论差不多
二、掌握了外参标定的方法，得到了参数，但还不会用

感想

明天制作组会PPT，有空的话写个周志周报，能不能把投影搞出来！

2021/2/27

收获

一、完成了组会PPT的制作

感想

晚上搞了一下标定，发现很容易检测不到板子，不知道为啥。而且图片会被成为灰度图，若是以还是的有后面的板子。可以尝试把小车垫高试试？

2021/2/28

收获

一、岳老师说可以GPmap做另一个创新点，GPmap加语义
二、终于成功融合成为了彩色点云，而且还比较精确

感想

开组会的时候，岳老师提到了我的第二个创新点，他说先Gpmap把点云的洞补上，这是高斯核对占用概率的更新，然后在进行语义的CRF优化。听起来真不错。老师还说春阳学长可以给我数据集和网络，真不错。
明天看看Gp这篇论文，然后把月报写了。

2021/3/1

收获

一、阅读Gpmap这篇文章，太难以理解了，而且感觉不是正式的高斯过程。高斯过程还需要自己学习一下。
二、完成了月报

感想

没有简单的事情，万事开头难啊。这个月还是主要把CRF加到代码里面吧，这样中期还可以展示展示。一个月过去了，进度要加快了

2021/3/2

收获

一、运行了多机联合语义建图。
二、安装了anacanda3和pytorch，运行了一个deeplab的小代码。

感想

学长从屎山代码里面把我需要的部分抠了出来，真好。现在的输入应该为两个包，每个包里面需要tf、clock和语义点云。

2021/3/3

收获

一、今天又看了一下所谓的高阶CRF，代码里面都是针对图像的，位置也是使用的2D的，感觉被骗了。所以我想我是不是可以针对每一帧点云CRF优化一下。
二、CRF的求解怎么都是用的神经网络呀，不会整了。

感想

看了一下贝叶斯核的论文，太难懂了，哭哭。

2021/3/4

收获

一、复现了CRFasRNN的代码，不知道还是不知道怎么解决CRF问题。
二、实现了正确姿态的多级联合语义建图。

感想

最近怎么效率变低了，不行啊！！