normals_example.cpp
Go to the documentation of this file.
00001 /*
00002  * OctoMap - An Efficient Probabilistic 3D Mapping Framework Based on Octrees
00003  * http://octomap.github.com/
00004  *
00005  * Copyright (c) 2009-2013, K.M. Wurm and A. Hornung, University of Freiburg
00006  * All rights reserved.
00007  * License: New BSD
00008  *
00009  * Redistribution and use in source and binary forms, with or without
00010  * modification, are permitted provided that the following conditions are met:
00011  *
00012  *     * Redistributions of source code must retain the above copyright
00013  *       notice, this list of conditions and the following disclaimer.
00014  *     * Redistributions in binary form must reproduce the above copyright
00015  *       notice, this list of conditions and the following disclaimer in the
00016  *       documentation and/or other materials provided with the distribution.
00017  *     * Neither the name of the University of Freiburg nor the names of its
00018  *       contributors may be used to endorse or promote products derived from
00019  *       this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
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);  // create empty tree with resolution 0.1
00058 
00059 
00060   // insert some measurements of occupied cells
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); // integrate 'occupied' measurement
00067       }
00068     }
00069   }
00070 
00071   // insert some measurements of free cells
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);  // integrate 'free' measurement
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 }


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:14