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 #ifndef __HectorMapTools_h_
00030 #define __HectorMapTools_h_
00031
00032 #include <nav_msgs/OccupancyGrid.h>
00033 #include <nav_msgs/MapMetaData.h>
00034
00035 #include<Eigen/Core>
00036
00037 class HectorMapTools{
00038 public:
00039
00040 template<typename ConcreteScalar>
00041 class CoordinateTransformer{
00042 public:
00043
00044 CoordinateTransformer()
00045 {
00046
00047 }
00048
00049 CoordinateTransformer(const nav_msgs::OccupancyGridConstPtr map)
00050 {
00051 this->setTransforms(*map);
00052 }
00053
00054
00055 void setTransforms(const nav_msgs::OccupancyGrid& map)
00056 {
00057 this->setTransforms(map.info);
00058 }
00059
00060 void setTransforms(const nav_msgs::MapMetaData& meta)
00061 {
00062 origo_ = (Eigen::Matrix<ConcreteScalar, 2, 1>(static_cast<ConcreteScalar>(meta.origin.position.x),static_cast<ConcreteScalar>(meta.origin.position.y)));
00063 scale_ = (static_cast<ConcreteScalar>(meta.resolution));
00064 inv_scale_ = (static_cast<ConcreteScalar>(1.0f / meta.resolution));
00065 }
00066
00067 void setTransformsBetweenCoordSystems(const Eigen::Matrix<ConcreteScalar, 2, 1>& origoCS1, const Eigen::Matrix<ConcreteScalar, 2, 1>& endCS1,
00068 const Eigen::Matrix<ConcreteScalar, 2, 1>& origoCS2, const Eigen::Matrix<ConcreteScalar, 2, 1>& endCS2)
00069 {
00070 Eigen::Matrix<ConcreteScalar, 2, 1> map_t_geotiff_x_factors = getLinearTransform(Eigen::Vector2f(origoCS1[0], endCS1[0]), Eigen::Vector2f(origoCS2[0], endCS2[0]));
00071 Eigen::Matrix<ConcreteScalar, 2, 1> map_t_geotiff_y_factors = getLinearTransform(Eigen::Vector2f(origoCS1[1], endCS1[1]), Eigen::Vector2f(origoCS2[1], endCS2[1]));
00072
00073
00074
00075 origo_.x() = map_t_geotiff_x_factors[1];
00076 origo_.y() = map_t_geotiff_y_factors[1];
00077
00078 scale_ = map_t_geotiff_x_factors[0];
00079 inv_scale_ = 1.0 / scale_;
00080
00081
00082 }
00083
00084 Eigen::Matrix<ConcreteScalar, 2, 1> getC1Coords(const Eigen::Matrix<ConcreteScalar, 2, 1>& mapCoords) const
00085 {
00086 return origo_ + (mapCoords * scale_);
00087 }
00088
00089 Eigen::Matrix<ConcreteScalar, 2, 1> getC2Coords(const Eigen::Matrix<ConcreteScalar, 2, 1>& worldCoords) const
00090 {
00091 return ((worldCoords - origo_) * inv_scale_);
00092 }
00093
00094 ConcreteScalar getC1Scale(float c2_scale) const
00095 {
00096 return scale_ * c2_scale;
00097 }
00098
00099 ConcreteScalar getC2Scale(float c1_scale) const
00100 {
00101 return inv_scale_ * c1_scale;
00102 }
00103
00104 protected:
00105
00106 Eigen::Matrix<ConcreteScalar, 2, 1> getLinearTransform(const Eigen::Matrix<ConcreteScalar, 2, 1>& coordSystem1, const Eigen::Matrix<ConcreteScalar, 2, 1>& coordSystem2)
00107 {
00108 ConcreteScalar scaling = (coordSystem2[0] - coordSystem2[1]) / (coordSystem1[0] - coordSystem1[1]);
00109 ConcreteScalar translation = coordSystem2[0] - coordSystem1[0] * scaling;
00110 return Eigen::Vector2f (scaling, translation);
00111 }
00112
00113 Eigen::Matrix<ConcreteScalar, 2, 1> origo_;
00114 ConcreteScalar scale_;
00115 ConcreteScalar inv_scale_;
00116 };
00117
00118 class DistanceMeasurementProvider
00119 {
00120 public:
00121 DistanceMeasurementProvider()
00122 {
00123
00124 }
00125
00126 void setMap(const nav_msgs::OccupancyGridConstPtr map)
00127 {
00128 map_ptr_ = map;
00129
00130 world_map_transformer_.setTransforms(*map_ptr_);
00131 }
00132
00133 float getDist(const Eigen::Vector2f& begin_world, const Eigen::Vector2f& end_world, Eigen::Vector2f* hitCoords = 0)
00134 {
00135 Eigen::Vector2i end_point_map;
00136
00137 Eigen::Vector2i begin_map (world_map_transformer_.getC2Coords(begin_world).cast<int>());
00138 Eigen::Vector2i end_map (world_map_transformer_.getC2Coords(end_world).cast<int>());
00139 float dist = checkOccupancyBresenhami(begin_map, end_map, &end_point_map);
00140
00141 if (hitCoords != 0){
00142 *hitCoords = world_map_transformer_.getC1Coords(end_point_map.cast<float>());
00143 }
00144
00145 return world_map_transformer_.getC1Scale(dist);
00146 }
00147
00148 inline float checkOccupancyBresenhami( const Eigen::Vector2i& beginMap, const Eigen::Vector2i& endMap, Eigen::Vector2i* hitCoords = 0, unsigned int max_length = UINT_MAX){
00149
00150 int x0 = beginMap[0];
00151 int y0 = beginMap[1];
00152
00153 int sizeX = map_ptr_->info.width;
00154 int sizeY = map_ptr_->info.height;
00155
00156
00157 if ((x0 < 0) || (x0 >= sizeX) || (y0 < 0) || (y0 >= sizeY)) {
00158 return -1.0f;
00159 }
00160
00161 int x1 = endMap[0];
00162 int y1 = endMap[1];
00163
00164
00165
00166
00167 if ((x1 < 0) || (x1 >= sizeX) || (y1 < 0) || (y1 >= sizeY)) {
00168 return -1.0f;
00169 }
00170
00171 int dx = x1 - x0;
00172 int dy = y1 - y0;
00173
00174 unsigned int abs_dx = abs(dx);
00175 unsigned int abs_dy = abs(dy);
00176
00177 int offset_dx = dx > 0 ? 1 : -1;
00178 int offset_dy = (dy > 0 ? 1 : -1) * sizeX;
00179
00180 unsigned int startOffset = beginMap.y() * sizeX + beginMap.x();
00181
00182 int end_offset;
00183
00184
00185 if(abs_dx >= abs_dy){
00186 int error_y = abs_dx / 2;
00187 end_offset = bresenham2D(abs_dx, abs_dy, error_y, offset_dx, offset_dy, startOffset,5000);
00188 }else{
00189
00190 int error_x = abs_dy / 2;
00191 end_offset = bresenham2D(abs_dy, abs_dx, error_x, offset_dy, offset_dx, startOffset,5000);
00192 }
00193
00194 if (end_offset != -1){
00195 Eigen::Vector2i end_coords(end_offset % sizeX, end_offset / sizeX);
00196
00197 int distMap = ((beginMap - end_coords).cast<float>()).norm();
00198
00199 if (hitCoords != 0){
00200 *hitCoords = end_coords;
00201 }
00202
00203 return distMap;
00204 }
00205
00206 return -1.0f;
00207
00208
00209
00210 }
00211
00212 inline int bresenham2D(unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b,
00213 unsigned int offset, unsigned int max_length){
00214 unsigned int end = std::min(max_length, abs_da);
00215
00216 const std::vector<int8_t>& data = map_ptr_->data;
00217
00218 for(unsigned int i = 0; i < end; ++i){
00219 if (data[offset] == 100){
00220 return static_cast<int>(offset);
00221 }
00222
00223 offset += offset_a;
00224 error_b += abs_db;
00225 if((unsigned int)error_b >= abs_da){
00226 offset += offset_b;
00227 error_b -= abs_da;
00228 }
00229 }
00230 return -1;
00231
00232 }
00233
00234 protected:
00235 CoordinateTransformer<float> world_map_transformer_;
00236 nav_msgs::OccupancyGridConstPtr map_ptr_;
00237
00238
00239 };
00240
00241 static bool getMapExtends(const nav_msgs::OccupancyGrid& map, Eigen::Vector2i& topLeft, Eigen::Vector2i& bottomRight)
00242 {
00243 int lowerStart = -1;
00244 int upperStart = 10000000;
00245
00246 int xMaxTemp = lowerStart;
00247 int yMaxTemp = lowerStart;
00248 int xMinTemp = upperStart;
00249 int yMinTemp = upperStart;
00250
00251 int width = map.info.width;
00252 int height = map.info.height;
00253
00254 for (int y = 0; y < height; ++y){
00255 for (int x = 0; x < width; ++x){
00256
00257 if ( map.data[x+y*width] != -1){
00258
00259 if (x > xMaxTemp) {
00260 xMaxTemp = x;
00261 }
00262
00263 if (x < xMinTemp) {
00264 xMinTemp = x;
00265 }
00266
00267 if (y > yMaxTemp) {
00268 yMaxTemp = y;
00269 }
00270
00271 if (y < yMinTemp) {
00272 yMinTemp = y;
00273 }
00274 }
00275 }
00276 }
00277
00278 if ((xMaxTemp != lowerStart) &&
00279 (yMaxTemp != lowerStart) &&
00280 (xMinTemp != upperStart) &&
00281 (yMinTemp != upperStart)) {
00282
00283 topLeft = Eigen::Vector2i(xMinTemp,yMinTemp);
00284 bottomRight = Eigen::Vector2i(xMaxTemp+1, yMaxTemp+1);
00285
00286 return true;
00287 } else {
00288 return false;
00289 }
00290 };
00291 };
00292
00293 #endif