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
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
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
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
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
00129
00130
00131
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 }