Octomap使用记录_2

Octomap文件

内容

octomap文件内容如下：

文件头：第一行很重要，和文件后缀一样，是表明文件类型的。Octomap读取文件会首先对第一行进行判断

# Octomap OcTree file
# (feel free to add / change comments, but leave the first line as it is!)
#

id、size、res：tree的类型、大小、分辨率

data： 包含了八叉树结构及其节点的信息，以及每个节点的占据状态。

八叉树结构信息：包括节点的层级、坐标、索引等。八叉树将三维空间划分为立方体，每个立方体可以进一步细分为八个子立方体。这个结构以二进制的方式存储在文件中。

节点的占据信息：每个节点包含了有关相应立方体的占据信息。这表示节点是占据（occupied）、自由（free）还是未知（unknown）。占据信息用于表示八叉树中的对象是否存在，自由表示该立方体为空间的一部分，未知表示在当前分辨率下无法确定状态。

颜色信息：只有.ot文件可以储存颜色信息。

文件格式

octomap文件有两种格式，.bt和.ot

区别在于：

1. 文件头不同；

2. 对应tree的类型不同，.bt为Octree，.ot无直接读的接口，多数用于可视化；

3. bt文件内存更小

4. bt不能存放颜色，ot可以存放颜色

文件读写和格式转换

octomap中是有ot，bt转换的程序的，convert_octree，简单改了一下，并加了bt和pcd互转的功能，我比较懒，没有把他集成到一个函数里，写了四个。

pcd2bt：核心方法就是OcTree的updateNode()方法

bt2pcd：核心方法是用OcTree的leaf_iterator遍历叶子节点，再用PointCloud的push_back()方法加点

bt2ot：直接用write是写不了的，octomap的write是写binaryfile的，所以要用这种方式

ot2bt：注意，这是ot文件我目前发现的唯一的打开方式，AbstractOctree和ColorOctree都不能直接打开ot文件，对比起来Octree就可以直接load一个bt文件，如果不需要颜色，bt很明显更适合开发

#include <iostream>
#include <fstream>
#include <pcl/io/pcd_io.h>
#include <octomap/OcTree.h>
#include <octomap/OcTreeBase.h>

//load pcd, ot, bt
//convert pcd, ot, bt

using namespace std;
using namespace octomap;

void convertPcdToBt(const string& pcd_filename, const string& octomap_filename) {
    // Load PCD file
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>(pcd_filename, *cloud) == -1) {
        cerr << "Error loading PCD file: " << pcd_filename << endl;
        return;
    }

    // Create OctoMap
    octomap::OcTree tree(0.05);

    // Insert points into OctoMap
    for (const auto& point : *cloud) {
        tree.updateNode(octomap::point3d(point.x, point.y, point.z), true);
    }

    // Save OctoMap to file
    tree.writeBinary(octomap_filename);
}

void convertBtToPcd(const string& octomap_filename, const string& pcd_filename) {
    // Load OctoMap
    octomap::OcTree tree(octomap_filename);

    // Extract points from OctoMap
    pcl::PointCloud<pcl::PointXYZ> cloud;
    for (octomap::OcTree::leaf_iterator it = tree.begin_leafs(), end = tree.end_leafs(); it != end; ++it) {
        if (tree.isNodeOccupied(*it)) {
            cloud.push_back(pcl::PointXYZ(it.getX(), it.getY(), it.getZ()));
        }
    }

    // Save PCD file
    pcl::io::savePCDFileBinary(pcd_filename, cloud);
}


void convertOtToBt(const string& inputFilename, const string& outputFilename) {
    std::ifstream file(inputFilename.c_str(), std::ios_base::in | std::ios_base::binary);

    if (!file.is_open()) {
        cerr << "Filestream to " << inputFilename << " not open, nothing read." << endl;
        exit(-1);
    }

    AbstractOcTree* tree = AbstractOcTree::read(file);
    file.close();

    if (!tree) {
        cerr << "Could not detect OcTree in file." << endl;
        exit(-1);
    }

    // Write binary (BonsaiTree) file
    if (outputFilename.length() > 3 && (outputFilename.compare(outputFilename.length()-3, 3, ".bt") == 0)) {
        cerr << "Writing binary (BonsaiTree) file" << endl;
        AbstractOccupancyOcTree* octree = dynamic_cast<AbstractOccupancyOcTree*>(tree);
        if (octree) {
            if (!octree->writeBinary(outputFilename)) {
                cerr << "Error writing to " << outputFilename << endl;
                exit(-2);
            }
        } else {
            cerr << "Error: Writing to .bt is not supported for this tree type: " << tree->getTreeType() << endl;
            exit(-2);
        }
    } else {
        cerr << "Output file must have a .bt extension for conversion." << endl;
        exit(-1);
    }

    cout << "Finished writing to " << outputFilename << endl;
}

// Function to convert .bt file to .ot file
void convertBtToOt(const string& inputFilename, const string& outputFilename) {
    std::ifstream file(inputFilename.c_str(), std::ios_base::in | std::ios_base::binary);

    if (!file.is_open()) {
        cerr << "Filestream to " << inputFilename << " not open, nothing read." << endl;
        exit(-1);
    }

    OcTree* binaryTree = new OcTree(0.1);

    if (binaryTree->readBinary(file) && binaryTree->size() > 1) {
        // Write general OcTree file
        cerr << "Writing general OcTree file" << endl;
        if (!binaryTree->write(outputFilename)) {
            cerr << "Error writing to " << outputFilename << endl;
            exit(-2);
        }
    } else {
        cerr << "Could not detect binary OcTree format in file." << endl;
        exit(-1);
    }

    file.close();
    delete binaryTree;

    cout << "Finished writing to " << outputFilename << endl;
}



int main() {

    convertPcdToBt("60.pcd", "61.bt");
    //convertBtToPcd("input_octomap.bt", "output_cloud.pcd");  
    //convertOtToBt("input.ot", "output.bt");
    //convertBtToOt("input.bt", "output.ot");

    return 0;
}