[C++]octomap安装后测试

测试环境：

vs2019

octomap==1.9.6

release x64

代码：

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

using namespace std;
using namespace octomap;
void print_query_info(point3d query, OcTreeNode* node) {
    if (node != NULL) {
        cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << endl;
    }
    else
        cout << "occupancy probability at " << query << ":\t is unknown" << endl;
}
int main(int argc, char** argv) {

    cout << endl;
    cout << "generating example map" << endl;
    OcTree tree(0.1);  // create empty tree with resolution 0.1
    // insert some measurements of occupied cells
    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); // integrate 'occupied' measurement
            }
        }
    }
    // insert some measurements of free cells
    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.0f, (float)y * 0.02f - 1.0f, (float)z * 0.02f - 1.0f);
                tree.updateNode(endpoint, false);  // integrate 'free' measurement
            }
        }
    }
    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;
}
 

结果：