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 #include <ros/console.h>
00030 #include <hector_geotiff/geotiff_writer.h>
00031
00032 #include <QtGui/QPainter>
00033 #include <QtGui/QImageWriter>
00034 #include <QtGui/QApplication>
00035 #include <QtCore/QFile>
00036
00037 #include <QtCore/QTime>
00038 #include <QtCore/QTextStream>
00039
00040 namespace hector_geotiff{
00041
00042
00043 GeotiffWriter::GeotiffWriter(bool useCheckerboardCacheIn)
00044 : useCheckerboardCache(useCheckerboardCacheIn)
00045 , use_utc_time_suffix_(true)
00046 {
00047 cached_map_meta_data_.height = -1;
00048 cached_map_meta_data_.width = -1;
00049 cached_map_meta_data_.resolution = -1.0f;
00050
00051 fake_argc_ = 0;
00052
00053
00054 app = new QApplication(fake_argc_, fake_argv_, false);
00055
00056 map_file_name_ = "";
00057 map_file_path_ = "";
00058 }
00059
00060 GeotiffWriter::~GeotiffWriter()
00061 {
00062 delete app;
00063 }
00064
00065 void GeotiffWriter::setMapFileName(const std::string& mapFileName)
00066 {
00067 map_file_name_ = mapFileName;
00068
00069 if (use_utc_time_suffix_){
00070
00071
00072 QTime now (QTime::currentTime());
00073 std::string current_time_string = now.toString(Qt::ISODate).toStdString();
00074
00075 map_file_name_ += "_" + current_time_string;
00076 }
00077 }
00078
00079 void GeotiffWriter::setMapFilePath(const std::string& mapFilePath)
00080 {
00081 map_file_path_ = mapFilePath;
00082 }
00083
00084 void GeotiffWriter::setUseUtcTimeSuffix(bool useSuffix)
00085 {
00086 use_utc_time_suffix_ = useSuffix;
00087 }
00088
00089
00090 bool GeotiffWriter::setupTransforms(const nav_msgs::OccupancyGrid& map)
00091 {
00092 resolution = static_cast<float>(map.info.resolution);
00093 origin = Eigen::Vector2f(map.info.origin.position.x, map.info.origin.position.y);
00094
00095 resolutionFactor = 3;
00096 resolutionFactorf = static_cast<float>(resolutionFactor);
00097
00098 pixelsPerMapMeter = 1.0f / map.info.resolution;
00099 pixelsPerGeoTiffMeter = pixelsPerMapMeter * static_cast<float>(resolutionFactor);
00100
00101 minCoordsMap = Eigen::Vector2i::Zero();
00102 maxCoordsMap = Eigen::Vector2i(map.info.width, map.info.height);
00103
00104 if(!HectorMapTools::getMapExtends(map, minCoordsMap, maxCoordsMap)){
00105 ROS_INFO("Cannot determine map extends!");
00106 return false;
00107 }
00108
00109 sizeMap = Eigen::Vector2i(maxCoordsMap - minCoordsMap);
00110 sizeMapf = ((maxCoordsMap - minCoordsMap).cast<float>());
00111
00112
00113 rightBottomMarginMeters = Eigen::Vector2f(1.0f, 1.0f);
00114 rightBottomMarginPixelsf = Eigen::Vector2f( rightBottomMarginMeters.array() * pixelsPerGeoTiffMeter);
00115 rightBottomMarginPixels = ((rightBottomMarginPixelsf.array() +0.5f).cast<int>());
00116
00117 leftTopMarginMeters= Eigen::Vector2f(3.0f, 3.0f);
00118
00119 totalMeters = (rightBottomMarginMeters + sizeMapf* map.info.resolution + leftTopMarginMeters);
00120
00121
00122 totalMeters.x() = ceil(totalMeters.x());
00123 totalMeters.y() = ceil(totalMeters.y());
00124
00125
00126 geoTiffSizePixels = ( (totalMeters.array() * pixelsPerGeoTiffMeter).cast<int>());
00127
00128
00129 mapOrigInGeotiff = (rightBottomMarginPixelsf);
00130 mapEndInGeotiff = (rightBottomMarginPixelsf + sizeMapf * resolutionFactorf);
00131
00132
00133
00134 world_map_transformer_.setTransforms(map);
00135
00136 map_geo_transformer_.setTransformsBetweenCoordSystems(mapOrigInGeotiff,mapEndInGeotiff, minCoordsMap.cast<float>(),maxCoordsMap.cast<float>());
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148 Eigen::Vector2f p1_w (Eigen::Vector2f::Zero());
00149 Eigen::Vector2f p2_w (Eigen::Vector2f(100.0f, 100.0f));
00150
00151 Eigen::Vector2f p1_m (world_map_transformer_.getC2Coords(p1_w));
00152 Eigen::Vector2f p2_m (world_map_transformer_.getC2Coords(p2_w));
00153
00154 Eigen::Vector2f p1_g (map_geo_transformer_.getC2Coords(p1_m));
00155 Eigen::Vector2f p2_g (map_geo_transformer_.getC2Coords(p2_m));
00156
00157 world_geo_transformer_.setTransformsBetweenCoordSystems(p1_g, p2_g, p1_w, p2_w);
00158
00159 map_draw_font_ = QFont();
00160 map_draw_font_.setPixelSize(6*resolutionFactor);
00161
00162 if (useCheckerboardCache){
00163
00164 if ((cached_map_meta_data_.height != map.info.height) ||
00165 (cached_map_meta_data_.width != map.info.width) ||
00166 (cached_map_meta_data_.resolution = map.info.resolution)){
00167
00168 cached_map_meta_data_ = map.info;
00169
00170 Eigen::Vector2f img_size (Eigen::Vector2f(map.info.width,map.info.height)* resolutionFactorf + (rightBottomMarginMeters + leftTopMarginMeters)*pixelsPerGeoTiffMeter );
00171 checkerboard_cache = QImage(img_size.y(),img_size.x(), QImage::Format_RGB32);
00172
00173 QPainter qPainter(&image);
00174 transformPainterToImgCoords(qPainter);
00175
00176 QBrush c1 = QBrush(QColor(226, 226, 227));
00177 QBrush c2 = QBrush(QColor(237, 237, 238));
00178 QRectF background_grid_tile(0.0f, 0.0f, pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter);
00179
00180
00181 int xMaxGeo = geoTiffSizePixels[0];
00182 int yMaxGeo = geoTiffSizePixels[1];
00183
00184 for (int y = 0; y < yMaxGeo; ++y){
00185 for (int x = 0; x < xMaxGeo; ++x){
00186
00187
00188 if ((x + y) % 2 == 0) {
00189
00190 qPainter.fillRect(static_cast<float>(x)*pixelsPerGeoTiffMeter,static_cast<float>(y)*pixelsPerGeoTiffMeter,pixelsPerGeoTiffMeter,pixelsPerGeoTiffMeter, c1);
00191
00192 } else {
00193
00194 qPainter.fillRect(static_cast<float>(x)*pixelsPerGeoTiffMeter,static_cast<float>(y)*pixelsPerGeoTiffMeter,pixelsPerGeoTiffMeter,pixelsPerGeoTiffMeter, c2);
00195 }
00196
00197 }
00198 }
00199
00200 }
00201
00202 }
00203
00204 return true;
00205 }
00206
00207 void GeotiffWriter::setupImageSize()
00208 {
00209 bool painter_rotate = true;
00210
00211 int xMaxGeo = geoTiffSizePixels[0];
00212 int yMaxGeo = geoTiffSizePixels[1];
00213
00214 if (!useCheckerboardCache){
00215 if (painter_rotate){
00216 image = QImage(yMaxGeo, xMaxGeo, QImage::Format_RGB32);
00217 }else{
00218 image = QImage(xMaxGeo, yMaxGeo, QImage::Format_RGB32);
00219 }
00220
00221 QPainter qPainter(&image);
00222
00223 QBrush grey = QBrush(QColor(128, 128, 128));
00224
00225 qPainter.fillRect(image.rect(), grey);
00226
00227 }
00228 }
00229
00230 void GeotiffWriter::drawBackgroundCheckerboard()
00231 {
00232 int xMaxGeo = geoTiffSizePixels[0];
00233 int yMaxGeo = geoTiffSizePixels[1];
00234
00235 bool painter_rotate = true;
00236
00237 if (!useCheckerboardCache){
00238
00239 QPainter qPainter(&image);
00240
00241 if (painter_rotate){
00242 transformPainterToImgCoords(qPainter);
00243 }
00244
00245
00246 QBrush c1 = QBrush(QColor(226, 226, 227));
00247 QBrush c2 = QBrush(QColor(237, 237, 238));
00248 QRectF background_grid_tile(0.0f, 0.0f, pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter);
00249
00250
00251
00252 for (int y = 0; y < yMaxGeo; ++y){
00253 for (int x = 0; x < xMaxGeo; ++x){
00254
00255
00256 if ((x + y) % 2 == 0) {
00257
00258 qPainter.fillRect(static_cast<float>(x)*pixelsPerGeoTiffMeter,static_cast<float>(y)*pixelsPerGeoTiffMeter,pixelsPerGeoTiffMeter,pixelsPerGeoTiffMeter, c1);
00259
00260 } else {
00261
00262 qPainter.fillRect(static_cast<float>(x)*pixelsPerGeoTiffMeter,static_cast<float>(y)*pixelsPerGeoTiffMeter,pixelsPerGeoTiffMeter,pixelsPerGeoTiffMeter, c2);
00263 }
00264
00265 }
00266 }
00267 }else{
00268 image = checkerboard_cache.copy(0,0,geoTiffSizePixels[0],geoTiffSizePixels[1]);
00269 }
00270 }
00271
00272 void GeotiffWriter::drawMap(const nav_msgs::OccupancyGrid& map, bool draw_explored_space_grid)
00273 {
00274 QPainter qPainter(&image);
00275
00276 transformPainterToImgCoords(qPainter);
00277
00278
00279
00280 QRectF map_cell_grid_tile(0.0f, 0.0f, resolutionFactor, resolutionFactor);
00281
00282 QBrush occupied_brush(QColor(0, 40, 120));
00283 QBrush free_brush(QColor(255, 255, 255));
00284 QBrush explored_space_grid_brush(QColor(190,190,191));
00285
00286 int width = map.info.width;
00287
00288 float explored_space_grid_resolution_pixels = pixelsPerGeoTiffMeter * 0.5f;
00289
00290 float yGeo = 0.0f;
00291 float currYLimit = 0.0f;
00292
00293 bool drawY = false;
00294
00295 for (int y = minCoordsMap[1] ; y < maxCoordsMap[1]; ++y){
00296
00297 float xGeo = 0.0f;
00298
00299 if(yGeo >= currYLimit ){
00300 drawY = true;
00301 }
00302
00303 float currXLimit = 0.0f;
00304 bool drawX = false;
00305
00306 for (int x = minCoordsMap[0] ; x < maxCoordsMap[0]; ++x){
00307
00308 unsigned int i = y*width + x;
00309
00310 int8_t data = map.data[i];
00311
00312 if (xGeo >= currXLimit){
00313 drawX = true;
00314 }
00315
00316 if (data == 0){
00317
00318 Eigen::Vector2f coords(mapOrigInGeotiff + (Eigen::Vector2f(xGeo,yGeo)));
00319 qPainter.fillRect(coords[0],coords[1],resolutionFactorf, resolutionFactorf, free_brush);
00320
00321
00322 if (draw_explored_space_grid){
00323 if (drawY){
00324 qPainter.fillRect(coords[0],mapOrigInGeotiff.y() + currYLimit, resolutionFactorf, 1.0f, explored_space_grid_brush);
00325 }
00326
00327 if (drawX){
00328 qPainter.fillRect(mapOrigInGeotiff.x() + currXLimit, coords[1], 1.0f, resolutionFactorf, explored_space_grid_brush);
00329 }
00330 }
00331
00332 }else if(data == 100){
00333 qPainter.fillRect(mapOrigInGeotiff.x()+xGeo, mapOrigInGeotiff.y()+yGeo,resolutionFactorf, resolutionFactorf, occupied_brush);
00334 }
00335
00336 if(drawX){
00337 currXLimit += explored_space_grid_resolution_pixels;
00338 drawX=false;
00339 }
00340
00341 xGeo+=resolutionFactorf;
00342 }
00343
00344 if(drawY){
00345 drawY=false;
00346 currYLimit += explored_space_grid_resolution_pixels;
00347 }
00348
00349 yGeo+= resolutionFactorf;
00350 }
00351 }
00352
00353 void GeotiffWriter::drawObjectOfInterest(const Eigen::Vector2f& coords, const std::string& txt, const Color& color)
00354 {
00355 QPainter qPainter(&image);
00356
00357 transformPainterToImgCoords(qPainter);
00358
00359
00360
00361
00362 Eigen::Vector2f map_coords (world_map_transformer_.getC2Coords(coords) );
00363
00364 Eigen::Vector2f coords_g (world_geo_transformer_.getC2Coords(coords));
00365
00366 qPainter.translate(coords_g[0],coords_g[1]);
00367
00368 qPainter.rotate(90);
00369
00370 qPainter.setRenderHint(QPainter::Antialiasing, true);
00371
00372 float radius = pixelsPerGeoTiffMeter * 0.175f;
00373
00374 QRectF ellipse_shape( - radius, - radius, radius*2.0f, radius*2.0f);
00375 qPainter.save();
00376
00377
00378 QBrush tmpBrush(QColor(color.r,color.g,color.b));
00379 QPen tmpPen(Qt::NoPen);
00380 qPainter.setBrush(tmpBrush);
00381 qPainter.setPen(tmpPen);
00382
00383 qPainter.drawEllipse(ellipse_shape);
00384 qPainter.restore();
00385
00386
00387 QString tmp (txt.c_str());
00388
00389
00390 if (tmp.length() < 2){
00391 qPainter.setFont(map_draw_font_);
00392 }else{
00393 QFont tmp_font;
00394 tmp_font.setPixelSize(3*resolutionFactor);
00395 qPainter.setFont(tmp_font);
00396 }
00397
00398
00399
00400 qPainter.setPen(Qt::white);
00401 qPainter.scale(-1.0,1.0);
00402
00403 qPainter.drawText(ellipse_shape,Qt::AlignCenter , tmp);
00404 }
00405
00406 void GeotiffWriter::drawPath(const Eigen::Vector3f& start, const std::vector<Eigen::Vector2f>& points,int color_r, int color_g, int color_b)
00407 {
00408 QPainter qPainter(&image);
00409
00410 transformPainterToImgCoords(qPainter);
00411
00412 Eigen::Vector2f start_geo (world_geo_transformer_.getC2Coords(start.head<2>()));
00413
00414
00415
00416 size_t size = points.size();
00417
00418 QPolygonF polygon;
00419 polygon.reserve(size);
00420
00421 polygon.push_back(QPointF(start_geo.x(), start_geo.y()));
00422
00423 for (size_t i = 0; i < size; ++i){
00424 const Eigen::Vector2f vec (world_geo_transformer_.getC2Coords(points[i]));
00425 polygon.push_back(QPointF(vec.x(), vec.y()));
00426 }
00427
00428 QPen pen(qPainter.pen());
00429 pen.setColor(QColor(color_r, color_g, color_b));
00430 pen.setWidth(3);
00431
00432 qPainter.setPen(pen);
00433
00434
00435
00436
00437 qPainter.drawPolyline(polygon);
00438
00439 qPainter.save();
00440 qPainter.translate(start_geo.x(), start_geo.y());
00441 qPainter.rotate(start.z());
00442 qPainter.setRenderHint(QPainter::Antialiasing, true);
00443 drawArrow(qPainter);
00444
00445 qPainter.restore();
00446 }
00447
00448 std::string GeotiffWriter::getBasePathAndFileName() const
00449 {
00450 return std::string (map_file_path_ +"/" + map_file_name_);
00451 }
00452
00453 void GeotiffWriter::writeGeotiffImage()
00454 {
00455
00456
00457
00458
00459
00460 std::string complete_file_string ( map_file_path_ +"/" + map_file_name_ +".tif");
00461 QImageWriter imageWriter(QString::fromStdString(complete_file_string));
00462 imageWriter.setCompression(1);
00463
00464 bool success = imageWriter.write(image);
00465
00466 std::string tfw_file_name (map_file_path_ +"/" + map_file_name_ + ".tfw");
00467 QFile tfwFile(QString::fromStdString(tfw_file_name));
00468
00469 tfwFile.open(QIODevice::WriteOnly);
00470
00471 QTextStream out(&tfwFile);
00472
00473 float resolution_geo = resolution / resolutionFactorf;
00474
00475 QString resolution_string;
00476 resolution_string.setNum(resolution_geo,'f',10);
00477
00478
00479 out << resolution_string << "\n";
00480
00481 QString zero_string;
00482 zero_string.setNum(0.0f, 'f', 10);
00483
00484
00485 out << zero_string << "\n" << zero_string << "\n";
00486
00487
00488 out << "-" << resolution_string << "\n";
00489
00490 QString top_left_string_x;
00491 QString top_left_string_y;
00492
00493
00494 Eigen::Vector2f zero_geo_w (world_geo_transformer_.getC1Coords((geoTiffSizePixels.array()+1).cast<float>()));
00495
00496
00497 top_left_string_x.setNum(-zero_geo_w.y(),'f',10);
00498 top_left_string_y.setNum(zero_geo_w.x(),'f',10);
00499
00500 out << top_left_string_x << "\n" << top_left_string_y << "\n";
00501
00502 tfwFile.close();
00503
00504 if(!success){
00505 ROS_INFO("Writing image with file %s failed with error %s", complete_file_string.c_str(), imageWriter.errorString().toStdString().c_str());
00506 }else{
00507 ROS_INFO("Successfully wrote geotiff to %s", complete_file_string.c_str());
00508 }
00509 }
00510
00511 void GeotiffWriter::transformPainterToImgCoords(QPainter& painter)
00512 {
00513 painter.rotate(-90);
00514 painter.translate(-geoTiffSizePixels.x(),geoTiffSizePixels.y());
00515 painter.scale(1.0,-1.0);
00516 }
00517
00518 void GeotiffWriter::drawCoords()
00519 {
00520 QPainter qPainter(&image);
00521 qPainter.setFont(map_draw_font_);
00522
00523 float arrowOffset = pixelsPerGeoTiffMeter * 0.15f;
00524
00525
00526 qPainter.setPen(QColor(0, 50, 140));
00527 qPainter.drawLine(pixelsPerGeoTiffMeter / 2, pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter / 2, 2.0f * pixelsPerGeoTiffMeter);
00528 qPainter.drawLine(pixelsPerGeoTiffMeter * 2 / 5, pixelsPerGeoTiffMeter - 1, pixelsPerGeoTiffMeter * 3 / 5, pixelsPerGeoTiffMeter - 1);
00529 qPainter.drawLine(pixelsPerGeoTiffMeter * 2 / 5, 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter * 3 / 5, 2 * pixelsPerGeoTiffMeter);
00530
00531
00532
00533 qPainter.drawLine(pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter);
00534 qPainter.drawLine(pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter + arrowOffset, 2 * pixelsPerGeoTiffMeter - arrowOffset);
00535 qPainter.drawLine(pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter + arrowOffset, 2 * pixelsPerGeoTiffMeter + arrowOffset);
00536
00537 qPainter.drawLine(2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter);
00538 qPainter.drawLine(2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter + arrowOffset, pixelsPerGeoTiffMeter + arrowOffset);
00539 qPainter.drawLine(2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter - arrowOffset, pixelsPerGeoTiffMeter + arrowOffset);
00540
00541 qPainter.drawText(0.6 * pixelsPerGeoTiffMeter, 1.6 * pixelsPerGeoTiffMeter, QString("1m"));
00542
00543 qPainter.drawText(2.2 * pixelsPerGeoTiffMeter, 1.1 * pixelsPerGeoTiffMeter, QString("x"));
00544 qPainter.drawText(1.2 * pixelsPerGeoTiffMeter, 1.8 * pixelsPerGeoTiffMeter, QString("y"));
00545
00546 qPainter.drawText (0.5f*pixelsPerGeoTiffMeter,0.75f*pixelsPerGeoTiffMeter, QString((map_file_name_ + ".tif").c_str()));
00547 }
00548
00549 void GeotiffWriter::drawCross(QPainter& painter, const Eigen::Vector2f& coords)
00550 {
00551 painter.drawLine(QPointF(coords[0]-1.0f, coords[1]), QPointF(coords[0]+1.0f, coords[1]));
00552 painter.drawLine(QPointF(coords[0] , coords[1]-1.0f), QPointF(coords[0], coords[1]+1.0f));
00553 }
00554
00555 void GeotiffWriter::drawArrow(QPainter& painter)
00556 {
00557 float tip_distance = pixelsPerGeoTiffMeter * 0.3f;
00558
00559 QPolygonF polygon;
00560
00561 polygon << QPointF(tip_distance, 0.0f) << QPointF(-tip_distance*0.5f, -tip_distance*0.5f) << QPointF(0.0f, 0.0f) << QPointF(-tip_distance*0.5f, tip_distance*0.5f);
00562
00563 painter.save();
00564
00565 QBrush tmpBrush(QColor(255,200,0));
00566 QPen tmpPen(Qt::NoPen);
00567 painter.setBrush(tmpBrush);
00568 painter.setPen(tmpPen);
00569
00570 painter.drawPolygon(polygon);
00571
00572 painter.restore();
00573 }
00574
00575 void GeotiffWriter::drawCoordSystem(QPainter& painter)
00576 {
00577 painter.save();
00578 QPointF zero_point (0.0f, 0.0f);
00579 QPointF x_point (pixelsPerGeoTiffMeter, 0.0f);
00580 QPointF y_point (0.0f, pixelsPerGeoTiffMeter);
00581
00582 QPen tmp = painter.pen();
00583 tmp.setWidth(5);
00584 tmp.setColor(QColor(255.0,0.0,0.0));
00585
00586 painter.setPen(tmp);
00587 painter.drawLine(zero_point,x_point);
00588
00589 tmp.setColor(QColor(0,255,0));
00590 painter.setPen(tmp);
00591 painter.drawLine(zero_point,y_point);
00592
00593 painter.restore();
00594 }
00595
00596 }