octomap库的一点总结
前些天突然想起高博《视觉SLAM十四讲》提到的octomap八叉树地图，在网上百度又只有高博写的那么一片博文，于是就想自己看看这个库，了解其几个基本的类和接口，以下是我这几天来的总结，如果有错误请及时指出，或者联系我，我也会及时改正。（[email protected]）
一，官方的 Introduction部分翻译(http://octomap.github.io/octomap/doc/index.html)
Octomap库实现了一种填充3D栅格的构图方式，并提供了相应的数据结构和构图算法。整个地图利用一个Octree的类来实现。
直接查看本库主要的类octomap::OcTree，以及其示例程序src/octomap/simple_example.cpp。如果你想要将单组测量数据整合到3D地图中，请查看OcTree::insertRay(…)函数；如果你想要将整个3D扫描数据（点云）整合到地图中，请查看OcTree::insertPointCloud(…)函数。同时可以通过OcTree::search(...) 和OcTree::castRay(...)函数来进行地图空间占用的查询。推荐使用迭代器的方式对Octree的节点等进行查询，例如leaf_iterator，tree_iterator和leaf_bbx_iterator。
二，示例代码分析(Github源码地址：https://github.com/OctoMap/octomap)
在使用该库时，主要使用了基本的OcTree类和带有颜色信息的ColorOcTree类，所以只简单分析了这两个类的示例代码。
1，octomap/octomap/src/simple_example.cpp
1. #include <octomap/octomap.h>
2. #include <octomap/OcTree.h>
3. using namespace std;
4. using namespace octomap;
5.
6. //查询信息输出函数，输入为octomap命名空间下的3D点位置和用OcTree::search(...)得到的返回值
7. void print_query_info(point3d query, OcTreeNode* node)
8. {
9. if (node != NULL)
10. {
11. cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << endl;
12. }
13. else
14. cout << "occupancy probability at " << query << ":\t is unknown" << endl;
15. }
16. // 主函数入口
17. int main(int argc, char** argv)
18. {
19. cout << endl;
20. cout << "generating example map" << endl;
21. // 建立一个空的OcTree对象地图，分辨率为0.1
22. OcTree tree (0.1); // create empty tree with resolution 0.1
23.
24. // insert some measurements of occupied cells
25. // 向地图中插入一些测量数据，这些数据点在地图中表现为已占用状态
26. for (int x=-20; x<20; x++)
27. {
28. for (int y=-20; y<20; y++)
29. {
30. for (int z=-20; z<20; z++)
31. {
32. //建立空间点对象，并向地图中更新节点
33. point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
34. tree.updateNode(endpoint, true); // integrate 'occupied' measurement
35. }
36. }
37. }
38.
39. // insert some measurements of free cells
40. // 向地图中插入一些测量数据，这些数据点在地图中表现为已未占用状态
41. for (int x=-30; x<30; x++)
42. {
43. for (int y=-30; y<30; y++)
44. {
45. for (int z=-30; z<30; z++)
46. {
47. //建立空间点对象，并向地图中更新节点
48. point3d endpoint ((float) x*0.02f-1.0f, (float) y*0.02f-1.0f, (float) z*0.02f-1.0f);
49. tree.updateNode(endpoint, false); // integrate 'free' measurement
50. }
51. }
52. }
53. //上面已经完成了对地图的创建，之后的程序是对地图节点数据的访问
54. cout << endl;
55. cout << "performing some queries:" << endl;
56. // 节点（0,0,0）
57. point3d query (0., 0., 0.);
58. OcTreeNode* result = tree.search (query);
59. print_query_info(query, result);
60. // 节点（-1,-1,-1）
61. query = point3d(-1.,-1.,-1.);
62. result = tree.search (query);
63. print_query_info(query, result);
64. // 节点（1,1,1）
65. query = point3d(1.,1.,1.);
66. result = tree.search (query);
67. print_query_info(query, result);
68. cout << endl;
69. // 将建好的地图保存
70. tree.writeBinary("simple_tree.bt");
71. cout << "wrote example file simple_tree.bt" << endl << endl;
72. cout << "now you can use octovis to visualize: octovis simple_tree.bt" << endl;
73. cout << "Hint: hit 'F'-key in viewer to see the freespace" << endl << endl;
74. }
这是执行该代码后的运行结果：
图表 1
这是用octovis查看所生成的地图simple_tree.bt：
图表 2
下面针对一些关键语句进行分析：
(1)OcTree tree(0.1);建立OcTree地图对象。这个函数的声明是这样的：
resolution译为“分辨率”，即访问地图的最小单位；
(2) point3d endpoint( (float)x*0.02f-1.0f, (float)y*0.02f-1.0f, (float)z*0.02f-1.0f);
tree.updateNode( endpoint, false );
建立一个三维空间点，并将其添加到地图当中；point3d构造函数的三个参数分别是三维点的x/y/z坐标；使用.updateNode()函数将空间点信息更新到地图当中，第二个参数表示该空间点对应的节点未被占用(false);
(3) OcTreeNode* result = tree.search (query);节点信息查询函数；函数声明是这样的
传入参数为一个三维点对象，如果在这个地图tree中，这个三维点对应的位置有节点（不管占用与否），那么将返回该位置的节点指针，否则将返回一个空指针；
(4) cout << "occupancy probability at " << query<< ":\t " << node->getOccupancy() << endl；如果查询到有该位置上节点的信息，则使用getOccupancy()函数输出该节点占用情况，那为什么这个Occupancy是个小数呢？这是因为Octomap在描述一个栅格是否被占用时，并不是单一的只描述为占用和被占用，而是用一个概率（Occupancy probability）来描述它，即这个栅格被占用的概率是多少，通过这个概率来确定这个栅格被占用的可能性。
2，Occupancy probability
关于这个占用概率值，高博在一篇博客中做出了和通俗易懂的解释，我就不再复制粘贴了，下面附上连接(https://www.cnblogs.com/gaoxiang12/p/5041142.html)。
我要说的是，对于一个最小单位的栅格，如何判断updateNode()函数对其做出了贡献或者说更新。就比如说
1. point3d endpoint( 4.05f, 4.05f, 4.05f );
2. tree.updateNode( endpoint, true );这两行代码，由于分辨率是0.1，并不能精确到0.01，所以要把这个（4.05,4.05,4.05）归到（4.0,4.0,4.0）这个节点呢，还是（4.1,4.1,4.1）这个节点或者其他节点呢？经过我的多次测试，应当归到（4.0,4.0,4.0）节点当中，也就是说，对于节点（4.0,4.0,4.0）来说，凡是同时满足x=[4.0,4.1)，y=[4.0,4.1)，z=[4.0,4.1)的point，如果使用updateNode()函数将这个point更新到地图当中，那么必然会影响到节点（4.0,4.0,4.0）的Occupancy probability。
总结就是，对于一个点point(x,y,z)，使用updateNode()函数将其更新到地图当中，那么Occupancy probability受到影响的节点将是
即小于point坐标值的最大节点。同时当我们用search ()函数对point进行查询时，返回的信息也将是小于point坐标值的最大节点信息。
另外，在对某个节点进行不同次数的更新之后，发现Occupancy probability最大值为0.971，最小值为0.1192，这也验证了高博那篇博文中提到的最大值和最小值限制。
3，octomap/src/testing/test_color_tree.cpp
带有色彩的八叉树地图ColorOcTree与基本的OcTree类似，需要知道如何向节点当中添加颜色信息就好了。下面是test_color_tree.cpp的部分代码：
1. int main(int argc, char** argv)
2. {
3. //分辨率
4. double res = 0.05; // create empty tree with resolution 0.05(different from default 0.1 for test)
5. //建立彩色地图对象
6. ColorOcTree tree (res);
7. // insert some measurements of occupied cells
8. for (int x=-20; x<20; x++)
9. {
10. for (int y=-20; y<20; y++)
11. {
12. for (int z=-20; z<20; z++)
13. {
14. point3d endpoint ((float)x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f);
15. ColorOcTreeNode* n =tree.updateNode(endpoint, true);
16. //设置节点颜色信息的函数，每个地图节点差五个像素大小，渐变
17. n->setColor(z*5+100,x*5+100,y*5+100);
18. }
19. }
20. }
21.
22. // insert some measurements of free cells
23. for (int x=-30; x<30; x++)
24. {
25. for (int y=-30; y<30; y++)
26. {
27. for (int z=-30; z<30; z++)
28. {
29. point3d endpoint ((float) x*0.02f+2.0f,(float) y*0.02f+2.0f, (float) z*0.02f+2.0f);
30. ColorOcTreeNode* n =tree.updateNode(endpoint, false);
31. //不被占用的节点设置为黄色
32. n->setColor(255,255,0); // set colorto yellow
33. }
34. }
35. }
36. // set inner node colors
37. tree.updateInnerOccupancy();
38. }这是执行该代码后的运行结果：
这是用octovis查看所生成的地图simple_color_tree.ot：
三，安装和编译octomap库时的一个小问题
第一次编译安装octomap库时，直接按照网上的教程进行的操作：
1. git clone https://github.com/OctoMap/octomap
2. cd octomap
3. mkdir build
4. cd build
5. cmake ..
6. make
7. sudo make install可是在执行make指令时却出现了undefined reference to的错误
在网上百度了好久，找到了关于这个问题的帖子（https://github.com/OctoMap/octomap/issues/171）
1， 我首先用的方法是 sudo make，确实，编译通过，也安装成功了，但是我在编译自己写的octomap_test程序时，仍然会出现类似的undefined reference to的错误，于是在编译时还是需要sudo；
2， 后来我觉得有一个对方法1的评价特别对：
于是我尝试了第二种做法：
打开查询了这个/opt/ros/indigo/share/octpmap/octomap-config-version文件，我的版本是1.6.9，在重新下载了octomap源码之后，校对了版本，具体指令如下：
1. git clone https://github.com/OctoMap/octomap
2. cd octomap
3. git tag //列出所有版本，查看是否有自己的版本
4. git checkout v1.6.9 //校验为自己的版本
5. mkdir build
6. cd build
7. cmake ..
8. make
9. sudo make instal感觉第二种做法才是真正的解决方案。