octomap 简单自定义 OcTree

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

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

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

实际上我添加的数据是occupancy map中voxel的frontier标志,就是一个bool类型变量,表示当前voxel是否一个frontier。实现方法是派生octomap的OcTreeNode类和OccupancyOcTreeBase<NT>。下面代码分为FrontierMap.hppFrontierMap.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)
    {}

    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;
展开阅读全文

150讲轻松搞定Python网络爬虫

05-16
【为什么学爬虫?】        1、爬虫入手容易,但是深入较难,如何写出高效率的爬虫,如何写出灵活性高可扩展的爬虫都是一项技术活。另外在爬虫过程中,经常容易遇到被反爬虫,比如字体反爬、IP识别、验证码等,如何层层攻克难点拿到想要的数据,这门课程,你都能学到!        2、如果是作为一个其他行业的开发者,比如app开发,web开发,学习爬虫能让你加强对技术的认知,能够开发出更加安全的软件和网站 【课程设计】 一个完整的爬虫程序,无论大小,总体来说可以分成三个步骤,分别是: 网络请求:模拟浏览器的行为从网上抓取数据。 数据解析:将请求下来的数据进行过滤,提取我们想要的数据。 数据存储:将提取到的数据存储到硬盘或者内存中。比如用mysql数据库或者redis等。 那么本课程也是按照这几个步骤循序渐进的进行讲解,带领学生完整的掌握每个步骤的技术。另外,因为爬虫的多样性,在爬取的过程中可能会发生被反爬、效率低下等。因此我们又增加了两个章节用来提高爬虫程序的灵活性,分别是: 爬虫进阶:包括IP代理,多线程爬虫,图形验证码识别、JS加密解密、动态网页爬虫、字体反爬识别等。 Scrapy和分布式爬虫:Scrapy框架、Scrapy-redis组件、分布式爬虫等。 通过爬虫进阶的知识点我们能应付大量的反爬网站,而Scrapy框架作为一个专业的爬虫框架,使用他可以快速提高我们编写爬虫程序的效率和速度。另外如果一台机器不能满足你的需求,我们可以用分布式爬虫让多台机器帮助你快速爬取数据。   从基础爬虫到商业化应用爬虫,本套课程满足您的所有需求! 【课程服务】 专属付费社群+每周三讨论会+1v1答疑
©️2020 CSDN 皮肤主题: 大白 设计师: CSDN官方博客 返回首页
实付0元
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值