Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include <octomap/octomap.h>
00035 #include <octomap/OcTree.h>
00036
00037 using namespace std;
00038 using namespace octomap;
00039
00040
00041 void print_query_info(point3d query, OcTreeNode* node) {
00042 if (node != NULL) {
00043 cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << endl;
00044 }
00045 else
00046 cout << "occupancy probability at " << query << ":\t is unknown" << endl;
00047 }
00048
00049 int main(int argc, char** argv) {
00050
00051 cout << endl;
00052 cout << "generating example map" << endl;
00053
00054 OcTree tree (0.1);
00055
00056
00057
00058
00059 for (int x=-20; x<20; x++) {
00060 for (int y=-20; y<20; y++) {
00061 for (int z=-20; z<20; z++) {
00062 point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
00063 tree.updateNode(endpoint, true);
00064 }
00065 }
00066 }
00067
00068
00069
00070 for (int x=-30; x<30; x++) {
00071 for (int y=-30; y<30; y++) {
00072 for (int z=-30; z<30; z++) {
00073 point3d endpoint ((float) x*0.02f-1.0f, (float) y*0.02f-1.0f, (float) z*0.02f-1.0f);
00074 tree.updateNode(endpoint, false);
00075 }
00076 }
00077 }
00078
00079 cout << endl;
00080 cout << "performing some queries:" << endl;
00081
00082 point3d query (0., 0., 0.);
00083 OcTreeNode* result = tree.search (query);
00084 print_query_info(query, result);
00085
00086 query = point3d(-1.,-1.,-1.);
00087 result = tree.search (query);
00088 print_query_info(query, result);
00089
00090 query = point3d(1.,1.,1.);
00091 result = tree.search (query);
00092 print_query_info(query, result);
00093
00094
00095 cout << endl;
00096 tree.writeBinary("simple_tree.bt");
00097 cout << "wrote example file simple_tree.bt" << endl << endl;
00098 cout << "now you can use octovis to visualize: octovis simple_tree.bt" << endl;
00099 cout << "Hint: hit 'F'-key in viewer to see the freespace" << endl << endl;
00100
00101 }