octomap 简单自定义 OcTree

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

首先octomap 内部有定义好OcTree类，使用OcTreeNode作为node类型。实际上OcTree即是从OccupancyOcTreeBase派生出来的，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。下面代码分为FrontierMap.hpp和FrontierMap.cpp两个文件。

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

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

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

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

代码如下。

FrontierMap.hpp

#ifndef FRONTIERMAP_HPP
#define FRONTIERMAP_HPP

#include 
#include 
#include 
#include 
#include 

#include 
#include 

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)
    {}

    FrontierOcTreeNode(Frontier_t f)
    : OcTreeNode(),
      fv(f)
    {}

    FrontierOcTreeNode( const FrontierOcTreeNode &other )
    : OcTreeNode(other),
      fv(other.fv)
      {}

    bool operator == ( const FrontierOcTreeNode& other ) const {
        return ( other.value == value && other.fv == fv );
    }

    void copyData( const FrontierOcTreeNode &other ) {
        OcTreeNode::copyData(other);
        this->fv = other.is_frontier() ? FRONTIER : NON_FRONTIER;
    }

    bool is_frontier() const { return fv; }
    void set_frontier() { fv = FRONTIER; }
    void clear_frontier() { fv = NON_FRONTIER; }

    std::istream& readData( std::istream &ins ) {
        ins.read((char*) &value, sizeof(value)); // Occupancy.
        ins.read((char*) &fv, sizeof(Frontier_t));  // Frontier.

        return ins;
    }

    std::ostream& writeData( std::ostream &out ) const {
        out.write((const char*) &value, sizeof(value)); // Occupancy.
        out.write((const char*) &fv, sizeof(Frontier_t));  // Frontier.

        return out;
    }

public:
    Frontier_t fv;
};

class FrontierOcTree : public OccupancyOcTreeBase<FrontierOcTreeNode> {
public:
    FrontierOcTree( double in_resolution );
    FrontierOcTree* create() const { return new FrontierOcTree(resolution); }

    std::string getTreeType() const { return "FrontierOcTree"; }

    FrontierOcTreeNode* set_node_frontier( const OcTreeKey &key ) {
        FrontierOcTreeNode *node = search(key);
        if ( node ) {
            node->set_frontier();
        }
        return node;
    }

    FrontierOcTreeNode* set_node_frontier( float x, float y, float z ) {
        OcTreeKey key;
        if( !this->coordToKeyChecked( point3d(x,y,z), key ) ) {
            return nullptr;
        }

        return set_node_frontier(key);
    }

    FrontierOcTreeNode* clear_node_frontier( const OcTreeKey &key ) {
        FrontierOcTreeNode *node = search(key);
        if ( node ) {
            node->clear_frontier();
        }
        return node;
    }

    FrontierOcTreeNode* clear_node_frontier( float x, float y, float z ) {
        OcTreeKey key;
        if( !this->coordToKeyChecked( point3d(x,y,z), key ) ) {
            return nullptr;
        }

        return clear_node_frontier(key);
    }

protected:
    class StaticMemberInitializer{
    public:
        StaticMemberInitializer() {
            FrontierOcTree *tree = new FrontierOcTree(0.1);
            tree->clearKeyRays();
            AbstractOcTree::registerTreeType(tree);
        }

        void ensureLinking() {}
    };

    static StaticMemberInitializer frontierOcTreeMemberInit;
};

} // namespace f_map

#endif // FRONTIERMAP_HPP

FrontierMap.cpp

#include "OccupancyMap/FrontierMap.hpp"

using namespace f_map;

FrontierOcTree::FrontierOcTree( double in_resolution )
        : OccupancyOcTreeBase<FrontierOcTreeNode>(in_resolution) {
    frontierOcTreeMemberInit.ensureLinking();
}

FrontierOcTree::StaticMemberInitializer FrontierOcTree::frontierOcTreeMemberInit;