00001 #ifndef NDT_COSTMAP_HH
00002 #define NDT_COSTMAP_HH
00003
00004 #include <ndt_map/ndt_map.h>
00005 #include <ndt_map/ndt_cell.h>
00006 #include <ndt_map/lazy_grid.h>
00007
00008 #include <opencv2/core/core.hpp>
00009 #include <opencv2/highgui/highgui.hpp>
00010 #include <nav_msgs/OccupancyGrid.h>
00011
00012
00013 namespace lslgeneric {
00014 struct LevelSet {
00015 std::vector<double> clear_levels;
00016 std::vector<int> map_ids;
00017 };
00018 class NDTCostmap {
00019 private:
00020 LevelSet **levels;
00021 NDTMap *map_;
00022 LazyGrid *grid_;
00023 int sx, sy, sz;
00024 bool alloc;
00025 public:
00026 NDTCostmap(NDTMap *map) {
00027
00028 map_ = map;
00029 if(map_ != NULL) {
00030 grid_ = dynamic_cast<LazyGrid*>(map_->getMyIndex());
00031 if(grid_ != NULL) {
00032 grid_->getGridSize(sx,sy,sz);
00033
00034 levels = new LevelSet*[sx];
00035 for(int i=0; i<sx; ++i) {
00036 levels[i] = new LevelSet[sy];
00037 }
00038 alloc = true;
00039 } else {
00040 alloc = false;
00041 }
00042 } else {
00043 alloc = false;
00044 }
00045 };
00046 ~NDTCostmap() {
00047 if(alloc) {
00048 for(int i=0; i<sx; ++i) {
00049 delete[] levels[i];
00050 }
00051 delete[] levels;
00052 }
00053 }
00054 void processMap(double robot_height, double occ);
00055
00056 void saveCostMap(std::string filename, double max_h);
00057 void saveCostMapIncr(std::string filename, double max_h);
00058 };
00059
00060 void NDTCostmap::processMap(double robot_height, double occ) {
00061 if(!alloc) return;
00062
00063 double cx,cy,cz;
00064 grid_->getCellSize(cx,cy,cz);
00065 int nheight_cells = ceil(robot_height/cz);
00066
00067 for(int ix=0; ix<sx; ++ix) {
00068 for(int iy=0; iy<sy; ++iy) {
00069 for(int iz=0; iz<sz-nheight_cells; ++iz) {
00070 NDTCell *ndcell;
00071 grid_->getCellAt(ix,iy,iz,ndcell);
00072 if(ndcell == NULL) { continue; }
00073
00074 if(ndcell->getOccupancy() > occ && ndcell->hasGaussian_) {
00075 if(ndcell->getClass() == NDTCell::HORIZONTAL ||ndcell->getClass() == NDTCell::INCLINED ) {
00076 bool space = true;
00077
00078 for(int j=1; j<nheight_cells; ++j) {
00079 NDTCell* ndcell_up;
00080 grid_->getCellAt(ix,iy,iz+j,ndcell_up);
00081 if(ndcell_up == NULL) {
00082 space = false;
00083 break;
00084 } else {
00085 space = !(ndcell_up->getOccupancy() > occ || ndcell_up->hasGaussian_ );
00086 if(!space) break;
00087 }
00088 }
00089 if(space) {
00090
00091 Eigen::Vector3d mean = ndcell->getMean();
00092 levels[ix][iy].clear_levels.push_back(mean(2));
00093 }
00094 }else {
00095
00096 }
00097 }
00098 else {
00099
00100 }
00101 }
00102
00103 }
00104
00105
00106 }
00107 }
00108
00109 struct MapPair {
00110 double mean_height;
00111 int n_samples;
00112 cv::Mat image;
00113 };
00114
00115 struct MapPairMsg {
00116 double mean_height;
00117 int n_samples;
00118 cv::Mat image;
00119 };
00120 class PairComp {
00121 public:
00122 bool operator() (const std::pair<int,int> &lhs, const std::pair<int,int> &rhs) {
00123 return (lhs.first > rhs.first || ( lhs.first == rhs.first && lhs.second > rhs.second)) ;
00124 }
00125 };
00126
00127 void NDTCostmap::saveCostMapIncr(std::string filename, double max_h) {
00128 if(!alloc) return;
00129
00130 std::vector<MapPair> maps, maps_final;
00131 std::set<std::pair<int,int>,PairComp >merge_maps;
00132
00133 for(int ix=0; ix<sx; ++ix) {
00134 for(int iy=0; iy<sy; ++iy) {
00135 for(int j=0; j<levels[ix][iy].clear_levels.size(); ++j) {
00136
00137 double my_h = levels[ix][iy].clear_levels[j];
00138 std::vector<int> map_ids_neighbours;
00139 std::vector<int> level_index_neighbours;
00140 std::vector<std::pair<int,int> > xy_ids_neighbours;
00141
00142
00143 if(ix > 0 && iy > 0) {
00144
00145 for(int p1=0; p1<levels[ix-1][iy-1].clear_levels.size(); ++p1) {
00146 if(fabsf(my_h-levels[ix-1][iy-1].clear_levels[p1]) < max_h) {
00147 xy_ids_neighbours.push_back(std::pair<int,int> (ix-1,iy-1));
00148 map_ids_neighbours.push_back(levels[ix-1][iy-1].map_ids[p1]);
00149 level_index_neighbours.push_back(p1);
00150 break;
00151 }
00152 }
00153 }
00154 if(ix > 0) {
00155
00156 for(int p1=0; p1<levels[ix-1][iy].clear_levels.size(); ++p1) {
00157 if(fabsf(my_h-levels[ix-1][iy].clear_levels[p1]) < max_h) {
00158 xy_ids_neighbours.push_back(std::pair<int,int> (ix-1,iy));
00159 map_ids_neighbours.push_back(levels[ix-1][iy].map_ids[p1]);
00160 level_index_neighbours.push_back(p1);
00161 break;
00162 }
00163 }
00164 }
00165 if( iy >0 ) {
00166
00167 for(int p1=0; p1<levels[ix][iy-1].clear_levels.size(); ++p1) {
00168 if(fabsf(my_h-levels[ix][iy-1].clear_levels[p1]) < max_h) {
00169 xy_ids_neighbours.push_back(std::pair<int,int> (ix,iy-1));
00170 map_ids_neighbours.push_back(levels[ix][iy-1].map_ids[p1]);
00171 level_index_neighbours.push_back(p1);
00172 break;
00173 }
00174 }
00175 }
00176
00177
00178 if(level_index_neighbours.size() > 0) {
00179
00180 levels[ix][iy].map_ids.push_back(levels[xy_ids_neighbours[0].first][xy_ids_neighbours[0].second].map_ids[level_index_neighbours[0]]);
00181 maps[map_ids_neighbours[0]].image.at<float>(ix,iy) = 1;
00182 maps[map_ids_neighbours[0]].mean_height = (maps[map_ids_neighbours[0]].mean_height*maps[map_ids_neighbours[0]].n_samples + my_h) /
00183 (maps[map_ids_neighbours[0]].n_samples + 1);
00184 maps[map_ids_neighbours[0]].n_samples ++;
00185
00186
00187 if(level_index_neighbours.size() == 2) {
00188
00189 if(map_ids_neighbours[0] > map_ids_neighbours[1]) {
00190 merge_maps.insert(std::pair<int,int> (map_ids_neighbours[0],map_ids_neighbours[1]));
00191 }
00192 if(map_ids_neighbours[0] < map_ids_neighbours[1]) {
00193 merge_maps.insert(std::pair<int,int> (map_ids_neighbours[1],map_ids_neighbours[0]));
00194 }
00195 }
00196 if(level_index_neighbours.size() == 3) {
00197 if(map_ids_neighbours[0] > map_ids_neighbours[1]) {
00198 merge_maps.insert(std::pair<int,int> (map_ids_neighbours[0],map_ids_neighbours[1]));
00199 }
00200 if(map_ids_neighbours[1] > map_ids_neighbours[2]) {
00201 merge_maps.insert(std::pair<int,int> (map_ids_neighbours[1],map_ids_neighbours[2]));
00202 }
00203 if(map_ids_neighbours[0] > map_ids_neighbours[2]) {
00204 merge_maps.insert(std::pair<int,int> (map_ids_neighbours[0],map_ids_neighbours[2]));
00205 }
00206 if(map_ids_neighbours[0] < map_ids_neighbours[1]) {
00207 merge_maps.insert(std::pair<int,int> (map_ids_neighbours[1],map_ids_neighbours[0]));
00208 }
00209 if(map_ids_neighbours[1] < map_ids_neighbours[2]) {
00210 merge_maps.insert(std::pair<int,int> (map_ids_neighbours[2],map_ids_neighbours[1]));
00211 }
00212 if(map_ids_neighbours[0] < map_ids_neighbours[2]) {
00213 merge_maps.insert(std::pair<int,int> (map_ids_neighbours[2],map_ids_neighbours[0]));
00214 }
00215 }
00216 } else {
00217
00218 MapPair mp;
00219 mp.mean_height = levels[ix][iy].clear_levels[j];
00220 mp.n_samples = 1;
00221
00222 mp.image = cv::Mat::zeros(sx,sy,CV_32F);
00223
00224 mp.image.at<float>(ix,iy) = 1;
00225 levels[ix][iy].map_ids.push_back(maps.size());
00226 maps.push_back(mp);
00227 }
00228 }
00229 }
00230
00231 }
00232
00233 std::set<std::pair<int,int>,PairComp >::reverse_iterator itr;
00234 std::vector<std::vector<int> > mapindex;
00235 std::vector<std::vector<int> >::iterator jtr, jtr_tmp, prev_insert, tmp;
00236 for(int q=0; q<maps.size(); ++q) {
00237 std::vector<int> tmp;
00238 tmp.push_back(q);
00239 mapindex.push_back(tmp);
00240 }
00241
00242 jtr = mapindex.begin();
00243 jtr_tmp = jtr;
00244
00245 for(itr=merge_maps.rbegin(); itr!=merge_maps.rend(); itr++) {
00246 bool did_insert = false;
00247
00248
00249
00250 while((*jtr)[0] < itr->first) jtr++;
00251
00252 for(jtr_tmp = mapindex.begin(); jtr_tmp!=jtr; jtr_tmp++) {
00253 for(int i=0; i<jtr_tmp->size(); i++) {
00254 if((*jtr_tmp)[i] == itr->second) {
00255 if(!did_insert) {
00256 jtr_tmp->push_back(itr->first);
00257 prev_insert = jtr_tmp;
00258 did_insert = true;
00259 } else {
00260
00261 prev_insert->insert(prev_insert->end(), jtr_tmp->begin(), jtr_tmp->end());
00262
00263 tmp = jtr_tmp;
00264 jtr_tmp --;
00265 mapindex.erase(tmp);
00266 }
00267 break;
00268 }
00269 if(i > 0 && (*jtr_tmp)[i] == itr->first) {
00270 if(!did_insert) {
00271 jtr_tmp->push_back(itr->second);
00272 prev_insert = jtr_tmp;
00273 did_insert = true;
00274 } else {
00275
00276 prev_insert->insert(prev_insert->end(), jtr_tmp->begin(), jtr_tmp->end());
00277
00278 tmp = jtr_tmp;
00279 jtr_tmp --;
00280 mapindex.erase(tmp);
00281 }
00282 break;
00283 }
00284 }
00285 }
00286 jtr_tmp = jtr;
00287 jtr--;
00288
00289 mapindex.erase(jtr_tmp);
00290 }
00291
00292 for(jtr = mapindex.begin(); jtr!=mapindex.end(); jtr++) {
00293 if(jtr->size() == 1) {
00294
00295 if(maps[(*jtr)[0]].n_samples > 20) {
00296 maps_final.push_back(maps[(*jtr)[0]]);
00297
00298 }
00299 } else {
00300 MapPair mp;
00301 mp.image = cv::Mat::zeros(sx,sy,CV_32F);
00302 mp.mean_height = 0;
00303 mp.n_samples = 0;
00304 for(int i=0; i<jtr->size(); ++i) {
00305
00306 mp.mean_height += maps[(*jtr)[i]].mean_height;
00307 mp.image = mp.image | maps[(*jtr)[i]].image;
00308 mp.n_samples += maps[(*jtr)[i]].n_samples;
00309 }
00310 if(mp.n_samples > 20) {
00311 mp.mean_height /= mp.n_samples;
00312 maps_final.push_back(mp);
00313 }
00314
00315 }
00316 }
00317
00318 maps = maps_final;
00319
00320
00321 for(int q=0; q<maps.size(); ++q) {
00322 char win_name[300];
00323 snprintf(win_name,299,"Map Level %lf",maps[q].mean_height);
00324 cv::namedWindow(win_name);
00325 maps[q].image *= 255;
00326 cv::imshow(win_name, maps[q].image);
00327 snprintf(win_name,299,"MapLevel%lf.jpg",maps[q].mean_height);
00328 cv::imwrite(win_name,maps[q].image);
00329 }
00330
00331
00332 }
00333 void NDTCostmap::saveCostMap(std::string filename, double max_h) {
00334 if(!alloc) return;
00335
00336 std::vector<MapPair> maps;
00337 for(int ix=0; ix<sx; ++ix) {
00338 for(int iy=0; iy<sy; ++iy) {
00339 for(int j=0; j<levels[ix][iy].clear_levels.size(); ++j) {
00340 bool found = false;
00341 int idx = -1;
00342 for(int q=0; q<maps.size(); ++q) {
00343 if(fabsf(maps[q].mean_height - levels[ix][iy].clear_levels[j]) < max_h) {
00344 found = true;
00345 idx = q;
00346 break;
00347 }
00348 }
00349 if(found) {
00350 maps[idx].mean_height = maps[idx].mean_height*maps[idx].n_samples + levels[ix][iy].clear_levels[j];
00351 maps[idx].n_samples++;
00352 maps[idx].mean_height /= maps[idx].n_samples;
00353
00354 maps[idx].image.at<float>(ix,iy) = 1;
00355 } else {
00356
00357 MapPair mp;
00358 mp.mean_height = levels[ix][iy].clear_levels[j];
00359 mp.n_samples = 1;
00360
00361 mp.image = cv::Mat::zeros(sx,sy,CV_32F);
00362
00363 mp.image.at<float>(ix,iy) = 1;
00364 maps.push_back(mp);
00365 }
00366 }
00367
00368
00369
00370
00371
00372
00373 }
00374
00375 }
00376
00377 for(int q=0; q<maps.size(); ++q) {
00378 char win_name[300];
00379 snprintf(win_name,299,"Map Level %lf",maps[q].mean_height);
00380 cv::namedWindow(win_name);
00381 cv::imshow(win_name, maps[q].image);
00382 snprintf(win_name,299,"MapLevel%lf.png",maps[q].mean_height);
00383 cv::imwrite(win_name,maps[q].image);
00384 }
00385
00386
00387 }
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462 }
00463 #endif