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 <vector>
00035 #include <string>
00036
00037 #include <octomap/octomap.h>
00038 #include <octomap/OcTree.h>
00039
00040 using namespace std;
00041 using namespace octomap;
00042
00043
00044 void print_query_info(point3d query, OcTreeNode* node) {
00045 if (node != NULL) {
00046 cout << "occupancy probability at " << query << ":\t " << node->getOccupancy() << endl;
00047 }
00048 else
00049 cout << "occupancy probability at " << query << ":\t is unknown" << endl;
00050 }
00051
00052 int main(int argc, char** argv) {
00053
00054 cout << endl;
00055 cout << "generating example map" << endl;
00056
00057 OcTree tree (0.1);
00058
00059
00060
00061
00062 for (int x=-20; x<20; x++) {
00063 for (int y=-20; y<20; y++) {
00064 for (int z=-20; z<20; z++) {
00065 point3d endpoint ((float) x*0.05f, (float) y*0.05f, (float) z*0.05f);
00066 tree.updateNode(endpoint, true);
00067 }
00068 }
00069 }
00070
00071
00072
00073 for (int x=-30; x<30; x++) {
00074 for (int y=-30; y<30; y++) {
00075 for (int z=-30; z<30; z++) {
00076 point3d endpoint ((float) x*0.02f-1.0f, (float) y*0.02f-1.0f, (float) z*0.02f-1.0f);
00077 tree.updateNode(endpoint, false);
00078 }
00079 }
00080 }
00081
00082 cout << endl;
00083 cout << "performing some queries around the desired voxel:" << endl;
00084
00085 point3d query;
00086 OcTreeNode* result = NULL;
00087
00088 for(float z = -0.6; z < -0.21; z += 0.1){
00089 for(float y = -0.6; y < -0.21; y += 0.1){
00090 for(float x = -0.6; x < -0.21; x += 0.1){
00091 query = point3d(x, y, z);
00092 result = tree.search(query);
00093 print_query_info(query, result);
00094 }
00095 }
00096 }
00097
00098 query = point3d(-0.5, -0.4, -0.4);
00099 result = tree.search(query);
00100
00101 vector<point3d> normals;
00102 if (tree.getNormals(query, normals)){
00103
00104 cout << endl;
00105 string s_norm = (normals.size() > 1) ? " normals " : " normal ";
00106 cout << "MC algorithm gives " << normals.size() << s_norm << "in voxel at " << query << endl;
00107 for(unsigned i = 0; i < normals.size(); ++i)
00108 cout << "\t" << normals[i].x() << "; " << normals[i].y() << "; " << normals[i].z() << endl;
00109 } else{
00110 cout << "query point unknown (no normals)\n";
00111 }
00112 }