建图学习四 octomap库

1.绘制颜色直方图
tree.writeColorHistogram()
anzhuang依赖

sudo apt-get install gnuplot

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

using namespace std;
using namespace octomap;


int main(int /*argc*/, char** argv) {

  std::string filename("/home/n1/notes/simple_color_tree.ot");

  std::ifstream infile(filename.c_str(), std::ios_base::in |std::ios_base::binary);
  if (!infile.is_open()) {
    cout << "file "<< filename << " could not be opened for reading.\n";
    return -1;
  }

  ColorOcTree tree (0.1);
  tree.readData(infile);
  infile.close();
  cout << "color tree read from "<< filename <<"\n"; 

  tree.writeColorHistogram("histogram.eps");//绘制颜色直方图

  return 0;
}

查看

xdg-open histogram.eps

效果

2. 获取改变节点的值

#include <stdio.h>
#include <octomap/octomap.h>
#include <octomap/math/Utils.h>
#include <iostream>
using namespace std;
using namespace octomap;
void printChanges(OcTree& tree){
    unsigned int changedOccupid=0;
    unsigned int changeFree=0;
    unsigned int actualOccupid=0;
    unsigned int actualFree=0;
    unsigned int missingChanged=0;

    tree.expand();//展开
    KeyBoolMap::const_iterator it;//KeyBoolMap 可以高效跟踪OcTreeKeys和bool标志
    //OcTreeKey 是一个内部key寻址容器类，key计算原点（体素）作为体素的离散地址。
    
    //遍历所有别改变的key，使用之前必须开启enableChangeDetection(Ture)
    //统计所有change点占据开率
    for(it=tree.changedKeysBegin();it!=tree.changedKeysEnd();it++){
        OcTreeNode* node=tree.search(it->first);
        if(node!=nullptr){
            if(tree.isNodeOccupied(node)){
                changedOccupid +=1;
            }
            else{                       
                changeFree +=1;
            }
        }
        else{
            missingChanged +=1;
        }
    }
    //统计所有树的占有率
    for(OcTree::tree_iterator itn=tree.begin_tree();itn!=tree.end_tree();itn++){
        if(itn.isLeaf()){
            if(tree.isNodeOccupied(*itn)){
                actualOccupid +=1;
            }
            else{
                actualFree +=1;
            }
            
        }
    }
    cout<<"改变的节点数量:"<<changedOccupid<<";occ:"<<changeFree<<"free;"<<missingChanged<<"missing"<<endl;
    cout<<"实际节点占用的数据:"<<actualOccupid<<";occ:"<<actualFree<<"free;"<<endl;
    tree.prune();//数据结构重构
}


int main(int argc, const char** argv) {
    OcTree tree(0.05);
    tree.enableChangeDetection(true);//在插入值时，选择忽略或者跟踪默认值

    point3d origin(0.01f,0.01f,0.02f);
    point3d point_on_surface(4.01f,0.01f,0.01f);
    tree.insertRay(origin,point_on_surface);//origin：起点,end：结束,maxrange:最大投影范围，lazy_eval：是否更新内部节点默认为false
                                            //返回是否成功 1表示成果
    printChanges(tree);
    tree.updateNode(point3d(2.01f,0.01f,0.01f),2.0f);
    printChanges(tree);
    tree.updateNode(point3d(2.01f, 0.01f, 0.01f), -2.0f);
    printChanges(tree);
    cout << "generating spherical scan at " << origin << " ..." << endl;

    for(int i=-100;i<101;i++){
        Pointcloud cloud;
        for(int j=-100;j<101;j++){
            
            point3d rotated=point_on_surface;
            cout<<"rotated:"<<j<<":"<<rotated<<endl;
            rotated.rotate_IP(0,DEG2RAD(i*0.5),DEG2RAD(j*0.5));//旋转RPY
            cloud.push_back(rotated);
            cout<<"rotated:"<<j<<":"<<rotated<<endl;
            
        }
        tree.insertPointCloud(cloud,origin,-1);
    }
    printChanges(tree);
    cout<<"done."<<endl;
    return 0;
}

3. 射线投影

#include <stdio.h>
#include <octomap/octomap.h>
#include <octomap/math/Utils.h>
#include "testing.h"
using namespace std;
using namespace octomap;

int main(int argc, const char** argv) {
    OcTree tree(0.05);
    point3d origin(0.01f,0.01f,0.02f);
    point3d point_on_surface(2.01f,0.01f,0.01f);

    unsigned sphere_beams=500;//1个圈取点的个数
    double angle=2.0*M_PI/double(sphere_beams);
    Pointcloud p;
    for(int i=0;i<sphere_beams;i++){
        for(int j=0;j<sphere_beams;j++){
            
            point_on_surface.rotate_IP(0,0,angle);//旋转
            p.push_back(origin+point_on_surface);
            // point_on_surface.rotate_IP(0,angle,0);//旋转
            // p.push_back(origin+point_on_surface);
            // point_on_surface.rotate_IP(angle,0,0);//旋转
            // p.push_back(origin+point_on_surface);
        }
        point_on_surface.rotate_IP(0,angle,0);//旋转
    }

    tree.insertPointCloud(p,origin);//插入点云
    cout<<"写入 sphere.bt..."<<endl;
    tree.writeBinary("sphere.bt");
    
    OcTree simpled_surface(0.05);
    point3d direction=point3d(1.0,0.0,0.0);
    point3d obstacle(0,0,0);
    
    unsigned int hit(0);
    unsigned int miss(0);
    unsigned int unknown(0);
    double mean_dist(0);
    
    for(int i=0;i<sphere_beams;i++){
        for(int j=0;j<sphere_beams;j++){
            if(!tree.castRay(origin,direction,obstacle,3.)){
                //castRay 3d空间内进行光线投影映射
                //原理：光线从origin出发，沿着direction方向返回遇到的第一个非free类型的单元作为obstacle终点(作为光线中心坐标系)返回
                //如果范围值为true，表示命中了一个occupied节点
                //如果返回值为flase，在返回obstacle，search obstacle并确定其不为空。
                //参数： origin:光束起点;
                //      dirction:方向
                //      end：返回值（光线结束的地方）
                //      ignoreUnknownCells 默认值defult 是否忽略默认光线
                //      maxrange：光线搜索的最大范围
                if(!tree.search(obstacle)){
                    unknown++;
                }
                else
                {
                    miss++;
                }
                
            }
            else
            {
                hit++;
                mean_dist+=(obstacle-origin).norm();
                simpled_surface.updateNode(obstacle,true);
            }
            direction.rotate_IP(0,0,angle);//Yaw旋转        
        }
        direction.rotate_IP(0,angle,0);//Pitch 旋转
    }
    cout << "Writing sampled_surface.bt" << endl;
    simpled_surface.writeBinary("sampled_surface.bt");
    mean_dist /= (double) hit;
    std::cout << " hits / misses / unknown: " << hit  << " / " << miss << " / " << unknown << std::endl;
    std::cout << " mean obstacle dist: " << mean_dist << std::endl;
    EXPECT_NEAR(mean_dist, 2., 0.05);
    cout<<"(sphere_beams*sphere_beams):"<<sphere_beams*sphere_beams<<"hit:"<<hit<<endl;
    cout<<"miss:"<<miss<<"0:"<<0<<endl;
    cout<<"unknown:"<<unknown<<"0:"<<0<<endl;

    return 0;
}

4. octomap 里面的图（graph）操作
   注意point6d

#include <octomap/octomap.h>
#include <stdio.h>
#include <octomap/math/Utils.h>
using namespace std;
using namespace octomap;

void printUsage(char *self){
    std::cerr<<"\n USAGE:"<<self<<"spherical_scan.graph\n\n"<<endl;
    exit(1);
}
int main(int argc, const char** argv) {
    point3d point_on_graph(4.01f,0.01f,0.01f);
    Pointcloud* cloud=new Pointcloud();

    for(int i=-50;i<51;i++){
        for(int j=-50;j<51;j++){
            point3d rotated=point_on_graph;
            rotated.rotate_IP(0,DEG2RAD(i*0.5),DEG2RAD(j*0.5));
            cloud->push_back(rotated);
        }

    }
    pose6d origin(1.0,0,-0.5,0,0,0);//x,y,z,起点，roll,yaw,pitch姿态
    ScanGraph graph;
    graph.addNode(cloud,origin);//添加节点
    cout<<"graph 大小："<<graph.size()<<endl;
    graph.writeBinary("/home/n1/notes/test_graph.graph");

    //io操作
    ScanGraph readgraph;
    readgraph.readBinary("/home/n1/notes/test_graph.graph");

    //查看
    //大小
    cout<<"graph.size:"<<graph.size()<<"readgraph.size:"<<readgraph.size()<<endl;
    
    //起始节点
    ScanNode *g1=*graph.begin();
    ScanNode *g2=*readgraph.begin();
    //ScanNode:Pointcloud 点坐标 pose6d：起始点+坐标 ,id
    cout<<"g1 id:"<<g1->id<<"g2 id:"<<g2->id<<endl;
    cout<<"g1 pose:"<<g1->pose<<"g2 pose:"<<g2->pose<<endl;
    cout<<"g1 scan->size:"<<g1->scan->size()<<"g2 scan->size:"<<g2->scan->size()<<endl;
    
    //插入到八叉树
    OcTree tree(0.05);
    tree.insertPointCloud(*cloud,origin.trans());
    tree.writeBinary("graph_write_octree.bt");
    return 0;
}