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/propagation_distance_field.h>
00038 #include <visualization_msgs/Marker.h>
00039
00040 namespace distance_field
00041 {
00042
00043 PropagationDistanceField::~PropagationDistanceField()
00044 {
00045 }
00046
00047 PropagationDistanceField::PropagationDistanceField(double size_x, double size_y, double size_z, double resolution,
00048 double origin_x, double origin_y, double origin_z, double max_distance):
00049 DistanceField<PropDistanceFieldVoxel>(size_x, size_y, size_z, resolution, origin_x, origin_y, origin_z, PropDistanceFieldVoxel(max_distance))
00050 {
00051 max_distance_ = max_distance;
00052 int max_dist_int = ceil(max_distance_/resolution);
00053 max_distance_sq_ = (max_dist_int*max_dist_int);
00054 initNeighborhoods();
00055
00056
00057 sqrt_table_.resize(max_distance_sq_+1);
00058 for (int i=0; i<=max_distance_sq_; ++i)
00059 sqrt_table_[i] = sqrt(double(i))*resolution;
00060 }
00061
00062 int PropagationDistanceField::eucDistSq(int* point1, int* point2)
00063 {
00064 int dx = point1[DIM_X] - point2[DIM_X];
00065 int dy = point1[DIM_Y] - point2[DIM_Y];
00066 int dz = point1[DIM_Z] - point2[DIM_Z];
00067 return dx*dx + dy*dy + dz*dz;
00068 }
00069
00070 void PropagationDistanceField::addPointsToField(const std::vector<btVector3> points)
00071 {
00072
00073 bucket_queue_.resize(max_distance_sq_+1);
00074
00075 bucket_queue_[0].reserve(points.size());
00076
00077 int x, y, z, nx, ny, nz;
00078 int loc[3];
00079 int initial_update_direction = getDirectionNumber(0,0,0);
00080 for (unsigned int i=0; i<points.size(); ++i)
00081 {
00082 bool valid = worldToGrid(points[i].x(), points[i].y(), points[i].z(), x, y, z);
00083 if (!valid)
00084 continue;
00085 PropDistanceFieldVoxel& voxel = getCell(x,y,z);
00086 voxel.distance_square_ = 0;
00087 voxel.closest_point_[DIM_X] = x;
00088 voxel.closest_point_[DIM_Y] = y;
00089 voxel.closest_point_[DIM_Z] = z;
00090 voxel.location_[DIM_X] = x;
00091 voxel.location_[DIM_Y] = y;
00092 voxel.location_[DIM_Z] = z;
00093 voxel.update_direction_ = initial_update_direction;
00094 bucket_queue_[0].push_back(&voxel);
00095 }
00096
00097
00098 for (unsigned int i=0; i<bucket_queue_.size(); ++i)
00099 {
00100 std::vector<PropDistanceFieldVoxel*>::iterator list_it = bucket_queue_[i].begin();
00101 while(list_it!=bucket_queue_[i].end())
00102 {
00103 PropDistanceFieldVoxel* vptr = *list_it;
00104
00105 x = vptr->location_[DIM_X];
00106 y = vptr->location_[DIM_Y];
00107 z = vptr->location_[DIM_Z];
00108
00109
00110 std::vector<std::vector<int> >* neighborhood;
00111 int D = i;
00112 if (D>1)
00113 D=1;
00114
00115 if (vptr->update_direction_<0 || vptr->update_direction_>26)
00116 {
00117
00118 ++list_it;
00119 continue;
00120 }
00121
00122 neighborhood = &neighborhoods_[D][vptr->update_direction_];
00123
00124 for (unsigned int n=0; n<neighborhood->size(); n++)
00125 {
00126 int dx = (*neighborhood)[n][DIM_X];
00127 int dy = (*neighborhood)[n][DIM_Y];
00128 int dz = (*neighborhood)[n][DIM_Z];
00129 nx = x + dx;
00130 ny = y + dy;
00131 nz = z + dz;
00132 if (!isCellValid(nx,ny,nz))
00133 continue;
00134
00135
00136
00137 PropDistanceFieldVoxel* neighbor = &getCell(nx, ny, nz);
00138 loc[DIM_X] = nx;
00139 loc[DIM_Y] = ny;
00140 loc[DIM_Z] = nz;
00141 int new_distance_sq = eucDistSq(vptr->closest_point_, loc);
00142 if (new_distance_sq > max_distance_sq_)
00143 continue;
00144 if (new_distance_sq < neighbor->distance_square_)
00145 {
00146
00147 neighbor->distance_square_ = new_distance_sq;
00148 neighbor->closest_point_[DIM_X] = vptr->closest_point_[DIM_X];
00149 neighbor->closest_point_[DIM_Y] = vptr->closest_point_[DIM_Y];
00150 neighbor->closest_point_[DIM_Z] = vptr->closest_point_[DIM_Z];
00151 neighbor->location_[DIM_X] = loc[DIM_X];
00152 neighbor->location_[DIM_Y] = loc[DIM_Y];
00153 neighbor->location_[DIM_Z] = loc[DIM_Z];
00154 neighbor->update_direction_ = getDirectionNumber(dx, dy, dz);
00155
00156
00157 bucket_queue_[new_distance_sq].push_back(neighbor);
00158 }
00159 }
00160
00161 ++list_it;
00162 }
00163 bucket_queue_[i].clear();
00164 }
00165
00166 }
00167
00168 void PropagationDistanceField::reset()
00169 {
00170 VoxelGrid<PropDistanceFieldVoxel>::reset(PropDistanceFieldVoxel(max_distance_sq_));
00171 }
00172
00173 void PropagationDistanceField::initNeighborhoods()
00174 {
00175
00176 direction_number_to_direction_.resize(27);
00177 for (int dx=-1; dx<=1; ++dx)
00178 {
00179 for (int dy=-1; dy<=1; ++dy)
00180 {
00181 for (int dz=-1; dz<=1; ++dz)
00182 {
00183 int direction_number = getDirectionNumber(dx, dy, dz);
00184 std::vector<int> n_point(3);
00185 n_point[DIM_X] = dx;
00186 n_point[DIM_Y] = dy;
00187 n_point[DIM_Z] = dz;
00188 direction_number_to_direction_[direction_number] = n_point;
00189 }
00190 }
00191 }
00192
00193 neighborhoods_.resize(2);
00194 for (int n=0; n<2; n++)
00195 {
00196 neighborhoods_[n].resize(27);
00197
00198 for (int dx=-1; dx<=1; ++dx)
00199 {
00200 for (int dy=-1; dy<=1; ++dy)
00201 {
00202 for (int dz=-1; dz<=1; ++dz)
00203 {
00204 int direction_number = getDirectionNumber(dx, dy, dz);
00205
00206 for (int tdx=-1; tdx<=1; ++tdx)
00207 {
00208 for (int tdy=-1; tdy<=1; ++tdy)
00209 {
00210 for (int tdz=-1; tdz<=1; ++tdz)
00211 {
00212 if (tdx==0 && tdy==0 && tdz==0)
00213 continue;
00214 if (n>=1)
00215 {
00216 if ((abs(tdx) + abs(tdy) + abs(tdz))!=1)
00217 continue;
00218 if (dx*tdx<0 || dy*tdy<0 || dz*tdz <0)
00219 continue;
00220 }
00221 std::vector<int> n_point(3);
00222 n_point[DIM_X] = tdx;
00223 n_point[DIM_Y] = tdy;
00224 n_point[DIM_Z] = tdz;
00225 neighborhoods_[n][direction_number].push_back(n_point);
00226 }
00227 }
00228 }
00229
00230 }
00231 }
00232 }
00233 }
00234
00235 }
00236
00237 int PropagationDistanceField::getDirectionNumber(int dx, int dy, int dz) const
00238 {
00239 return (dx+1)*9 + (dy+1)*3 + dz+1;
00240 }
00241
00242
00243 }