建图学习三八叉树地图和octomap库

最新推荐文章于 2021-11-05 15:54:03 发布

磊磊哈哈

最新推荐文章于 2021-11-05 15:54:03 发布

阅读量1.9k

点赞数

原文链接：https://editor.csdn.net/md?not_checkout=1&articleId=112239814

版权

SLAM十四讲专栏收录该内容

37 篇文章 8 订阅

订阅专栏

slam14讲中的八叉树：

点云地图存在如下缺点：

点云地图规模大，一副640×480像素的图像，会产生30万个空间点，提供很多不必要的细节，需要对其压缩。
点云地图无法处理运动物体。只有添加点的方法，没用当点消失时移除其方法，实际中运动物体普遍存在，点云地图不使用。

八叉树：

把三维空间建模为许多小方块，每个面均匀切成两块，那么可以得到8个同样大的小块
<>八叉树的节点表示塔是否被占据的信息，0：表示空白 1：表示占据，更加精细的用 $x\in[0,1]$ 表示某节点被占据的可能。x一开始取0.5,如果观测到不断观测到他被占据，这个值不断增加，反之如果不断观测到他是控被这个值不断减小。
<>如果x不断增加或者减小可能会超过 $[0, 1]$ ,所以不能直接用概率表示，需要用概率的对数值(Log-adds)表示，设 $y\in R,x\in[0,1]$ ，它们之间的logit变换为：
$y=logit(x)=log(\frac{x}{1-x})\\ 反变换:logit^{-1}(y)=\frac{e^y}{e^y+1}$
<>实际中使用y，如果观测到y被占用那么y加1，否则减小，查询时在用logit函数得到x,在 $t + 1$ 时刻为：
$L(n|z_t):t时刻内概率对数的变化$
$L(n|z_{1:t+1}):开始到t+1时刻某节点的概率对数值$
$L(n|z_{1:t}):开始到t时刻某节点的概率对数值$
$L(n|z_{1:t+1})=L(n|z_{1:t-1})+L(n|z_t)$
用概率表示为:
$P(n|z_{1:T})=[1+\frac{1-P(n|z_T)}{P(n|z_T)}\frac{1-P(n|z_{1:T-1})}{P(n|z_{1:T-1})}\frac{P(n)}{1-P(n)}]^{-1}$

octomap库

下载安装

git clone https://github.com/OctoMap/octomap
cd octomap
make build
cd build
make 
sudo make install

配置

find_package(octomap REQUIRED)
include_directories(${OCTOMAP_INCLUDE_DIRS})
target_link_libraries(octmap_exe ${OCTOMAP_LIBRARIES}}

octomap::OcTree

来自官网

绘制简单的正方形源码下:src/simple_example.cpp

#include <iostream>
#include <octomap/octomap.h>
#include <octomap/OcTree.h>
using namespace std;
using namespace octomap;
//查询
void print_query_info(point3d query,OcTreeNode* node){
//OcTreeNode 使用OcTreeDataNode类（一些基础操作：深拷贝、浅拷贝、拷贝成员函数、删除成员函数）实现
//OcTreeNode 存储的为概率的对数值
//内置方法
//getOccupancy() 获取观测为占据的概率
//getLogOdds() 获取观测为占据概率的对数
//getMeanChildLogOdds()     
//getMaxChildLogOdds() 
//addValue(p)    占据概率的对数加p
    if(node !=NULL){
        cout<<"点："<<query<<"可能在：\t"<<node->getOccupancy()<<endl;
    }
    else
    {
        cout<<"点："<<query<<"所在区域未知"<<":\t is unknown"<<endl;
    }
    
}
int main(int argc, const char** argv) {
    cout<<"生成一个检点的地图"<<endl;
    OcTree tree(0.1);
    //添加一占有网格1
    for(int x=-20;x<20;x++){
        for(int y=-20;y<20;y++){
            for(int z=-20;z<20;z++){
                point3d endpoint((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
                tree.updateNode(endpoint,true);//将此区域设为被占据
            }
        }
    }
    //设置空白网格
    for(int x=-30;x<30;x++){
        for(int y=-30;y<30;y++){
            for(int z=-30;z<30;z++){
                point3d endpoint((float) x*0.02f-1.0, (float) y*0.02f-1.0, (float) z*0.02f-1.0);
                tree.updateNode(endpoint,false);//将此区域设为未知点（空白）
            }
        }
    }
      cout << endl;
  cout << "performing some queries:" << endl;
  
  point3d query (0., 0., 0.);
  OcTreeNode* result = tree.search (query);
  print_query_info(query, result);

  query = point3d(-1.,-1.,-1.);
  result = tree.search (query);
  print_query_info(query, result);

  query = point3d(1.,1.,1.);
  result = tree.search (query);
  print_query_info(query, result);


  cout << endl;
  tree.writeBinary("simple_tree.bt");
  cout << "wrote example file simple_tree.bt" << endl << endl;
  cout << "now you can use octovis to visualize: octovis simple_tree.bt"  << endl;
  cout << "Hint: hit 'F'-key in viewer to see the freespace" << endl  << endl;
    return 0;
}

结果:缺一角
在这里插入图片描述

有颜色的图形:
testing.h

#include <math.h>
#include <stdlib.h>

// this is mimicing gtest expressions
//这个宏 如若args无效，输出当前无效行和源文件
//__FILE__ 当前文件源
//__LINE__ 当前代码行
#define EXPECT_TRUE(args) {                                             \
    if (!(args)) { fprintf(stderr, "test failed (EXPECT_TRUE) in %s, line %d\n", __FILE__, __LINE__); \
      exit(1);                                                         \
    } }
//与EXPECT_TRUE 相反
#define EXPECT_FALSE(args) {                                             \
    if (args) { fprintf(stderr, "test failed (EXPECT_FALSE) in %s, line %d\n", __FILE__, __LINE__); \
      exit(1);                                                         \
    } }
//比较宏 不相等输出信息
#define EXPECT_EQ(a,b) {                                                \
    if (!(a == b)) { std::cerr << "test failed: " <<a<<"!="<<b<< " in " \
                      << __FILE__ << ", line " <<__LINE__ << std::endl; \
      exit(1);                                                          \
    } }
//如果abs(a-b)>0.00001 报错
#define EXPECT_FLOAT_EQ(a,b) {                                          \
    if (!(fabs(a-b) <= 1e-5)) { fprintf(stderr, "test failed: %f != %f in %s, line %d\n", a, b, __FILE__, __LINE__); \
      exit(1);                                                         \
    } }
//如果abs(a-b)>prec 报错
#define EXPECT_NEAR(a,b,prec) {                                         \
    if (!(fabs(a-b) <= prec)) { fprintf(stderr, "test failed: |%f - %f| > %f in %s, line %d\n", a, b, prec, __FILE__, __LINE__); \
      exit(1);                                                         \
    } }

#include <octomap/octomap.h>
#include <octomap/ColorOcTree.h>
#include "testing.h"
using namespace std;
using namespace octomap;

void print_query_info(point3d query,ColorOcTreeNode* node){
    if(node !=NULL){
      cout<<"被占据的概率为:"<<query<<";"<<node->getOccupancy()<<endl;
      cout<<"颜色为："<<node->getColor()<<endl;
    }
    else
    {
      cout<<"无法确定点："<<query<<"状态"<<endl;
    }
    
}

int main(int argc, const char** argv) {
    
  ColorOcTree tree(0.05);
  for(int x=-20;x<20;x++){
    for(int y=-20;y<20;y++){
      for(int z=-20;z<20;z++){
        point3d endpoint ((float) x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f);
        ColorOcTreeNode* n=tree.updateNode(endpoint,true);//更新节点
        n->setColor(z*5+100,x*5+100,y*5+100);//设置颜色
      }
    }
  }
  for(int x=-30;x<30;x++){
    for(int y=-30;y<30;y++){
      for(int z=-30;z<30;z++){
        point3d endpoint ((float) x*0.02f+2.0f, (float) y*0.02f+2.0f, (float) z*0.02f+2.0f);
        ColorOcTreeNode* n=tree.updateNode(endpoint,false);
        n->setColor(255,255,0);//黄色
      }
    }
  }
  tree.updateInnerOccupancy();//更新颜色
  
  EXPECT_EQ(tree.size(),tree.calcNumNodes());//查看是否想等
  cout<<"树大小："<<tree.size()<<endl;
  cout<<"节点个数"<<tree.calcNumNodes()<<endl;
  const size_t initialSize=tree.size();
  EXPECT_EQ(initialSize,1034);
  tree.prune(); //八叉树的无损压缩，如果一个节点的八个值全部相同，那么删除。不必在每次更新之后调用，updateNode
                //会修改相应影响的节点
  EXPECT_EQ(tree.size(), tree.calcNumNodes()); 
  EXPECT_EQ(initialSize, tree.size());
  cout<<"压缩后树大小："<<tree.size()<<endl;
  cout<<"压缩后节点个数"<<tree.calcNumNodes()<<endl;

  EXPECT_TRUE(tree.write("test_color_tree.ot"))
  return 0;
}

效果
在这里插入图片描述

其他常用简单操作

  //一些简单操作
  //读取文件
  AbstractOcTree *read_tree=AbstractOcTree::read("test_color_tree.ot");

  //方法
  read_tree->getTreeType();             //返回类型
  read_tree->getTreeType().compare(tree.getTreeType());//比较类型
  read_tree.size();//大小 面   
  point3d query (0., 0., 0.);
  ColorOcTreeNode* result = read_tree.search (query);  //查询

  result->getLogOdds();                           //获取当前占据概率的对数
  read_tree.expand();                                  //将整个树展开
  result->getColor();                             //获取此节点颜色
  result->setColor();                             //设置此节点颜色
  cout<<"展开后树大小："<<read_tree.size()<<endl;
  cout<<"展开后节点个数"<<read_tree.calcNumNodes()<<endl;

  //加入节点
  point3d newCoord(-2.0, -2.0, -2.0);
  ColorOcTreeNode* newNode = read_tree.updateNode(newCoord, true);
  newNode->setColor(255,0,0);
  //查询节点
  ColorOcTreeNode* parentNode = read_tree.search(newCoord, read_tree.getTreeDepth() -1);//查询父节点
  
  read_tree.expandNode(parentNode);//展开单个节点
  
  read_tree.nodeHasChildren(parentNode);//查询是否存在parentNode
  for (size_t i = 1; i < 8; ++i){//遍历查询所属i节点是否为parentNode
      EXPECT_FALSE(tree.nodeChildExists(parentNode, i));
    }
  
  read_tree.deleteNodeChild(parentNode, 0);//删除parentNode的第0个节点
  
  tree.write("1.ot");//直接保存
  
  tree.writeBinary("2.bt");//保存2进制