intersection_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 int main(int argc, char** argv) {
00044 
00045   cout << "generating example map" << endl;
00046 
00047   OcTree tree (0.1);  // create empty tree with resolution 0.1
00048 
00049   // insert some measurements of free cells
00050 
00051   for (float x = -2; x <= 0; x += 0.02) {
00052     for (float y = -2; y <= 0; y += 0.02) {
00053       for (float z = -2; z <= 0; z += 0.02) {
00054         point3d endpoint(x, y, z);
00055         tree.updateNode(endpoint, false); // integrate 'free' measurement
00056       }
00057     }
00058   }
00059 
00060   // insert some measurements of occupied cells (twice as much)
00061   for (float x = -1; x <= 0; x += 0.01) {
00062     for (float y = -1; y <= 0; y += 0.01) {
00063       for (float z = -1; z <= 0; z += 0.01) {
00064         point3d endpoint(x, y, z);
00065         tree.updateNode(endpoint, true); // integrate 'occupied' measurement
00066       }
00067     }
00068   }
00069 
00070   point3d origin(-1.5, -1.5, -0.5);
00071   point3d direction;
00072   point3d ray_end;
00073 
00074   
00075   for(float z = 0; z <= 0.25; z += 0.125){
00076     direction = point3d(1, 1, z);
00077     cout << endl;
00078     cout << "casting ray from " << origin  << " in the " << direction << " direction"<< endl;
00079     bool success = tree.castRay(origin, direction, ray_end);
00080 
00081     if(success){
00082       cout << "ray hit cell with center " << ray_end << endl;
00083       
00084       point3d intersection;
00085       success = tree.getRayIntersection(origin, direction, ray_end, intersection);
00086       if(success)
00087         cout << "entrance point is " << intersection << endl;
00088     }
00089   }
00090         
00091   return 0;
00092 }


octomap
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Feb 11 2016 23:50:59