pf_distance_field.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Intel Labs Pittsburgh
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Intel Labs nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00037 #include <distance_field/pf_distance_field.h>
00038 #include <limits>
00039 
00040 namespace distance_field
00041 {
00042 
00043 PFDistanceField::PFDistanceField(double size_x, double size_y, double size_z, double resolution,
00044     double origin_x, double origin_y, double origin_z):
00045   DistanceField<float>(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, DT_INF),
00046   DT_INF(std::numeric_limits<float>::max())
00047 {
00048 
00049 }
00050 
00051 PFDistanceField::~PFDistanceField()
00052 {
00053 }
00054 
00055 void PFDistanceField::addPointsToField(const std::vector<tf::Vector3> points)
00056 {
00057   int x, y, z;
00058   float init = 0.0;
00059   for (unsigned int i=0; i<points.size(); ++i)
00060   {
00061     bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(), x, y, z);
00062     if (!valid)
00063       continue;
00064     setCell(x,y,z, init);
00065   }
00066   computeDT();
00067 }
00068 
00069 void PFDistanceField::computeDT()
00070 {
00071 
00072   size_t nx = num_cells_[DIM_X];
00073   size_t ny = num_cells_[DIM_Y];
00074   size_t nz = num_cells_[DIM_Z];
00075 
00076     size_t     maxdim = std::max( nx, std::max(ny, nz) );
00077     FloatArray f(maxdim), ft(maxdim), zz(maxdim+1);
00078     IntArray   v(maxdim);
00079 
00080     // along z
00081     for (size_t y=0; y<ny; ++y) {
00082         for (size_t x=0; x<nx; ++x) {
00083             for (size_t z=0; z<nz; ++z) {
00084                 f[z] = getCell(x,y,z);
00085             }
00086             dt(f, nz, ft, v, zz);
00087             for (size_t z=0; z<nz; ++z) {
00088                 setCell(x,y,z,ft[z]);
00089             }
00090         }
00091     }
00092 
00093     // along y
00094     for (size_t z=0; z<nz; ++z) {
00095         for (size_t x=0; x<nx; ++x) {
00096             for (size_t y=0; y<ny; ++y) {
00097                 f[y] = getCell(x,y,z);
00098             }
00099             dt(f, ny, ft, v, zz);
00100             for (size_t y=0; y<ny; ++y) {
00101                 setCell(x,y,z,ft[y]);
00102             }
00103         }
00104     }
00105 
00106     // along x
00107     for (size_t z=0; z<nz; ++z) {
00108         for (size_t y=0; y<ny; ++y) {
00109             for (size_t x=0; x<nx; ++x) {
00110                 f[x] = getCell(x,y,z);
00111             }
00112             dt(f, nx, ft, v, zz);
00113             for (size_t x=0; x<nx; ++x) {
00114                 setCell(x,y,z,ft[x]);
00115             }
00116         }
00117     }
00118 
00119 }
00120 
00121 
00122 void PFDistanceField::dt(const FloatArray& f,
00123         size_t nn,
00124         FloatArray& ft,
00125         IntArray& v,
00126         FloatArray& z) {
00127 
00128 /*    assert(f.size() >= nn);
00129     assert(ft.size() >= nn);
00130     assert(v.size() >= nn);
00131     assert(z.size() >= nn+1);
00132 */
00133     int n = nn;
00134 
00135     int k = 0;
00136 
00137     v[0] = 0;
00138 
00139     z[0] = -DT_INF;
00140     z[1] =  DT_INF;
00141 
00142     for (int q=1; q<n; ++q) {
00143         float s = ((f[q]+sqr(q))-(f[v[k]]+sqr(v[k])))/(2*q-2*v[k]);
00144         while (s <= z[k]) {
00145             --k;
00146             s = ((f[q]+sqr(q))-(f[v[k]]+sqr(v[k])))/(2*q-2*v[k]);
00147         }
00148         ++k;
00149         v[k] = q;
00150         z[k] = s;
00151         z[k+1] = DT_INF;
00152     }
00153 
00154     k = 0;
00155     for (int q=0; q<n; ++q) {
00156         while (z[k+1] < q) { ++k; }
00157         ft[q] = sqr(q-v[k]) + f[v[k]];
00158     }
00159 
00160 }
00161 
00162 
00163 void PFDistanceField::reset()
00164 {
00165   VoxelGrid<float>::reset(DT_INF);
00166 }
00167 
00168 }


distance_field
Author(s): Mrinal Kalakrishnan / mail@mrinal.net
autogenerated on Thu Dec 12 2013 11:08:36