Go to the documentation of this file.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 #ifndef DF_PROPAGATION_DISTANCE_FIELD_H_
00038 #define DF_PROPAGATION_DISTANCE_FIELD_H_
00039
00040 #include <distance_field/voxel_grid.h>
00041 #include <distance_field/distance_field.h>
00042 #include <tf/LinearMath/Vector3.h>
00043 #include <vector>
00044 #include <list>
00045 #include <ros/ros.h>
00046 #include <eigen3/Eigen/Core>
00047 #include <set>
00048
00049 namespace distance_field
00050 {
00051
00052
00054 typedef Eigen::Vector3i int3;
00055
00056
00057 struct compareInt3
00058 {
00059 bool operator()(int3 loc_1, int3 loc_2) const
00060 {
00061 if( loc_1.z() != loc_2.z() )
00062 return ( loc_1.z() < loc_2.z() );
00063 else if( loc_1.y() != loc_2.y() )
00064 return ( loc_1.y() < loc_2.y() );
00065 else if( loc_1.x() != loc_2.x() )
00066 return ( loc_1.x() < loc_2.x() );
00067 return false;
00068 }
00069 };
00070
00071
00075 struct PropDistanceFieldVoxel
00076 {
00077 PropDistanceFieldVoxel();
00078 PropDistanceFieldVoxel(int distance_sq);
00079
00080 int distance_square_;
00081 int3 location_;
00082 int3 closest_point_;
00083 int update_direction_;
00085 static const int UNINITIALIZED=-1;
00086 };
00087
00088 struct SignedPropDistanceFieldVoxel : public PropDistanceFieldVoxel
00089 {
00090 SignedPropDistanceFieldVoxel();
00091 SignedPropDistanceFieldVoxel(int distance_sq_positive, int distance_sq_negative);
00092 int positive_distance_square_;
00093 int negative_distance_square_;
00094 int3 closest_positive_point_;
00095 int3 closest_negative_point_;
00096
00097 static const int UNINITIALIZED=-999;
00098 };
00099
00108 class PropagationDistanceField: public DistanceField<PropDistanceFieldVoxel>
00109 {
00110 public:
00111
00112
00116 PropagationDistanceField(double size_x, double size_y, double size_z, double resolution,
00117 double origin_x, double origin_y, double origin_z, double max_distance);
00118
00119 virtual ~PropagationDistanceField();
00120
00126 virtual void updatePointsInField(const std::vector<tf::Vector3>& points, const bool iterative=true);
00127
00131 virtual void addPointsToField(const std::vector<tf::Vector3>& points);
00132
00136 virtual void reset();
00137
00142 void getOccupiedVoxelMarkers(const std::string & frame_id,
00143 const ros::Time stamp,
00144 const tf::Transform& cur,
00145 visualization_msgs::Marker& marker);
00146
00147 private:
00149 typedef std::set<int3, compareInt3> VoxelSet;
00150 VoxelSet object_voxel_locations_;
00151
00153 std::vector<std::vector<PropDistanceFieldVoxel*> > bucket_queue_;
00154 double max_distance_;
00155 int max_distance_sq_;
00156
00157 std::vector<double> sqrt_table_;
00158
00159
00160
00161
00162
00163
00164 std::vector<std::vector<std::vector<int3 > > > neighborhoods_;
00165
00166 std::vector<int3 > direction_number_to_direction_;
00167
00168 void addNewObstacleVoxels(const VoxelSet& points);
00169 void removeObstacleVoxels(const VoxelSet& points);
00170
00171 void propogate();
00172 virtual double getDistance(const PropDistanceFieldVoxel& object) const;
00173 int getDirectionNumber(int dx, int dy, int dz) const;
00174 int3 getLocationDifference(int directionNumber) const;
00175 void initNeighborhoods();
00176 static int eucDistSq(int3 point1, int3 point2);
00177
00178 };
00179
00181
00182 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel(int distance_sq):
00183 distance_square_(distance_sq)
00184 {
00185 closest_point_.x() = PropDistanceFieldVoxel::UNINITIALIZED;
00186 closest_point_.y() = PropDistanceFieldVoxel::UNINITIALIZED;
00187 closest_point_.z() = PropDistanceFieldVoxel::UNINITIALIZED;
00188 }
00189
00190 inline PropDistanceFieldVoxel::PropDistanceFieldVoxel()
00191 {
00192 }
00193
00194 inline double PropagationDistanceField::getDistance(const PropDistanceFieldVoxel& object) const
00195 {
00196 return sqrt_table_[object.distance_square_];
00197 }
00198
00199
00200 class SignedPropagationDistanceField : public DistanceField<SignedPropDistanceFieldVoxel>
00201 {
00202 public:
00203
00204 SignedPropagationDistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x,
00205 double origin_y, double origin_z, double max_distance);
00206 virtual ~SignedPropagationDistanceField();
00207
00208 virtual void addPointsToField(const std::vector<tf::Vector3> &points);
00209
00210 virtual void reset();
00211
00212 private:
00213 std::vector<std::vector<SignedPropDistanceFieldVoxel*> > positive_bucket_queue_;
00214 std::vector<std::vector<SignedPropDistanceFieldVoxel*> > negative_bucket_queue_;
00215 double max_distance_;
00216 int max_distance_sq_;
00217
00218 std::vector<double> sqrt_table_;
00219
00220
00221
00222
00223
00224 std::vector<std::vector<std::vector<int3 > > > neighborhoods_;
00225
00226 std::vector<int3 > direction_number_to_direction_;
00227
00228 virtual double getDistance(const SignedPropDistanceFieldVoxel& object) const;
00229 int getDirectionNumber(int dx, int dy, int dz) const;
00230 void initNeighborhoods();
00231 static int eucDistSq(int3 point1, int3 point2);
00232 };
00233
00234
00235
00236 inline SignedPropDistanceFieldVoxel::SignedPropDistanceFieldVoxel(int distance_sq_positive, int distance_sq_negative):
00237 positive_distance_square_(distance_sq_positive),
00238 negative_distance_square_(distance_sq_negative),
00239 closest_positive_point_(SignedPropDistanceFieldVoxel::UNINITIALIZED),
00240 closest_negative_point_(SignedPropDistanceFieldVoxel::UNINITIALIZED)
00241 {
00242 }
00243
00244 inline SignedPropDistanceFieldVoxel::SignedPropDistanceFieldVoxel()
00245 {
00246 }
00247
00248 inline double SignedPropagationDistanceField::getDistance(const SignedPropDistanceFieldVoxel& object) const
00249 {
00250 if(object.negative_distance_square_ != 0)
00251 {
00252
00253 }
00254 return sqrt_table_[object.positive_distance_square_] - sqrt_table_[object.negative_distance_square_];
00255 }
00256
00257 }
00258
00259 #endif