exampleEDT3D.cpp
Go to the documentation of this file.
00001 
00009 /*
00010  * Copyright (c) 2011-2012, C. Sprunk, B. Lau, W. Burgard, University of Freiburg
00011  * All rights reserved.
00012  *
00013  * Redistribution and use in source and binary forms, with or without
00014  * modification, are permitted provided that the following conditions are met:
00015  *
00016  *     * Redistributions of source code must retain the above copyright
00017  *       notice, this list of conditions and the following disclaimer.
00018  *     * Redistributions in binary form must reproduce the above copyright
00019  *       notice, this list of conditions and the following disclaimer in the
00020  *       documentation and/or other materials provided with the distribution.
00021  *     * Neither the name of the University of Freiburg nor the names of its
00022  *       contributors may be used to endorse or promote products derived from
00023  *       this software without specific prior written permission.
00024  *
00025  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00028  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00029  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00030  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00031  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00032  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00033  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00034  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  * POSSIBILITY OF SUCH DAMAGE.
00036  */
00037 
00038 #include <dynamicEDT3D/dynamicEDT3D.h>
00039 
00040 #include <iostream>
00041 #include <stdlib.h>
00042 
00043 int main( int , char** ) {
00044 
00045         //we build a sample map
00046   int sizeX, sizeY, sizeZ;
00047   sizeX=100;
00048   sizeY=100;
00049   sizeZ=100;
00050 
00051   bool*** map;
00052   map = new bool**[sizeX];
00053   for(int x=0; x<sizeX; x++){
00054         map[x] = new bool*[sizeY];
00055         for(int y=0; y<sizeY; y++){
00056                 map[x][y] = new bool[sizeZ];
00057                 for(int z=0; z<sizeZ; z++){
00058                                 if(x<2 || x > sizeX-3 || y < 2 || y > sizeY-3 || z<2 || z > sizeZ-3)
00059                                         map[x][y][z] = 1;
00060           else
00061                 map[x][y][z] = 0;
00062                 }
00063         }
00064   }
00065 
00066   map[51][45][67] = 1;
00067   map[50][50][68] = 1;
00068 
00069   // create the EDT object and initialize it with the map
00070   int maxDistInCells = 20;
00071   DynamicEDT3D distmap(maxDistInCells*maxDistInCells);
00072   distmap.initializeMap(sizeX, sizeY, sizeZ, map);
00073 
00074   //compute the distance map
00075   distmap.update();
00076 
00077   // now perform some updates with random obstacles
00078   int numPoints = 20;
00079   for (int frame=1; frame<=10; frame++) {
00080         std::cout<<"\n\nthis is frame #"<<frame<<std::endl;
00081     std::vector<IntPoint3D> newObstacles;
00082     for (int i=0; i<numPoints; i++) {
00083       double x = 2+rand()/(double)RAND_MAX*(sizeX-4);
00084       double y = 2+rand()/(double)RAND_MAX*(sizeY-4);
00085       double z = 2+rand()/(double)RAND_MAX*(sizeZ-4);
00086       newObstacles.push_back(IntPoint3D(x,y,z));
00087     }
00088 
00089     // register the new obstacles (old ones will be removed)
00090     distmap.exchangeObstacles(newObstacles);
00091 
00092     //update the distance map
00093     distmap.update();
00094 
00095     //retrieve distance at a point
00096     float dist = distmap.getDistance(30,67,33);
00097     int distSquared = distmap.getSQCellDistance(30,67,33);
00098     std::cout<<"distance at  30,67,33: "<< dist << " squared: "<< distSquared << std::endl;
00099     if(distSquared == maxDistInCells*maxDistInCells)
00100         std::cout<<"we hit a cell with d = dmax, distance value is clamped."<<std::endl;
00101 
00102     //retrieve closest occupied cell at a point
00103     IntPoint3D closest = distmap.getClosestObstacle(30,67,33);
00104     if(closest.x == DynamicEDT3D::invalidObstData)
00105         std::cout<<"we hit a cell with d = dmax, no information about closest occupied cell."<<std::endl;
00106     else
00107     std::cout<<"closest occupied cell to 30,67,33: "<< closest.x<<","<<closest.y<<","<<closest.z<<std::endl;
00108 
00109   }
00110 
00111 
00112   std::cout<<"\n\nthis is the last frame"<<std::endl;
00113 
00114   // now remove all random obstacles again.
00115   std::vector<IntPoint3D> empty;
00116   distmap.exchangeObstacles(empty);
00117   distmap.update();
00118 
00119 
00120   //retrieve distance at a point
00121   float dist = distmap.getDistance(30,67,33);
00122   int distSquared = distmap.getSQCellDistance(30,67,33);
00123   std::cout<<"distance at  30,67,33: "<< dist << " squared: "<< distSquared << std::endl;
00124   if(distSquared == maxDistInCells*maxDistInCells)
00125         std::cout<<"we hit a cell with d = dmax, distance value is clamped."<<std::endl;
00126 
00127   //retrieve closest occupied cell at a point
00128   IntPoint3D closest = distmap.getClosestObstacle(30,67,33);
00129   if(closest.x == DynamicEDT3D::invalidObstData)
00130         std::cout<<"we hit a cell with d = dmax, no information about closest occupied cell."<<std::endl;
00131   else
00132         std::cout<<"closest occupied cell to 30,67,33: "<< closest.x<<","<<closest.y<<","<<closest.z<<std::endl;
00133 
00134   return 0;
00135 }


dynamicEDT3D
Author(s): Christoph Sprunk
autogenerated on Thu Feb 11 2016 23:51:18