octomap 简单自定义 OcTree

之前的一篇博客记录了我安装调试octomap的过程。这里记录一下实现一个非常简单的自定义OcTree。

首先octomap 内部有定义好OcTree类，使用OcTreeNode作为node类型。实际上OcTree即是从OccupancyOcTreeBase<NT>派生出来的，NT为OcTreeNode。

我现在的需求特别简单，就是需要在保持OcTree功能的基础上，增加OcTree上node存储的数据内容。这些新加进来的数据不用参与occupancy map的生成，仅作为跟随max depth node的数据。根据octomap的设计，这个功能实现起来非常直观。octomap给我们提供了类似的例子，ColorOcTree和OcTreeStamped都是非常好的参考。这两个类的头文件和源文件是ColorOcTree.h、ColorOcTree.cpp、OcTreeStamped.h 和 OcTreeStamped.cpp。

实际上我添加的数据是occupancy map中voxel的frontier标志，就是一个bool类型变量，表示当前voxel是否一个frontier。实现方法是派生octomap的OcTreeNode类和OccupancyOcTreeBase<NT>。下面代码分为FrontierMap.hpp和FrontierMap.cpp两个文件。

作为一个node类型，需要实现的接口包括

• Default constructor.
• Copy constructor.
• operator == ().
• void copyData().
• std::istream& readData().
• std::istream& writeData().

从OccupancyOcTreeBase<NT>派生，需要实现的内容包括

• 接受一个double类型参数的构造函数，这个参数是resolution。将resolution传递给基类（OccupancyOcTreeBase<NT>）构造函数。
• create() 函数，new一个当前派生类的对象并返回其指针。
• std::string getTreeType()，返回以字符串形式表示的当前派生类的名字。
• 嵌套类StaticMemberInitializer和一个static成员变量StaticMemberInitializer frontierOcTreeMemberInit。
• 在构造函数中调用frontierOcTreeMemberInit.ensureLinking()完成类注册。

代码如下。

FrontierMap.hpp

#ifndef FRONTIERMAP_HPP
#define FRONTIERMAP_HPP

#include <iostream>
#include <memory>
#include <sstream>
#include <string>
#include <vector>

#include <octomap/octomap.h>
#include <octomap/OcTree.h>

namespace f_map
{
   

typedef bool Frontier_t;
static const Frontier_t FRONTIER     = true;
static const Frontier_t NON_FRONTIER = false;

using namespace octomap;

class FrontierOcTreeNode : public OcTreeNode {
   
public:
    FrontierOcTreeNode()
        : OcTreeNode(),
          fv(NON_FRONTIER)
    {