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 #include <tile_map/tile_map_view.h>
00031
00032 #include <cmath>
00033
00034 #include <boost/make_shared.hpp>
00035
00036 #include <GL/glew.h>
00037 #include <GL/gl.h>
00038 #include <GL/glu.h>
00039
00040 #include <ros/ros.h>
00041
00042 #include <swri_math_util/constants.h>
00043 #include <swri_math_util/trig_util.h>
00044 #include <swri_transform_util/earth_constants.h>
00045
00046 #include <tile_map/image_cache.h>
00047
00048 namespace tile_map
00049 {
00050 TileMapView::TileMapView() :
00051 level_(-1),
00052 width_(100),
00053 height_(100)
00054 {
00055 ImageCachePtr image_cache = boost::make_shared<ImageCache>("/tmp/tile_map");
00056 tile_cache_ = boost::make_shared<TextureCache>(image_cache);
00057 }
00058
00059 void TileMapView::ResetCache()
00060 {
00061 tile_cache_->Clear();
00062 }
00063
00064 void TileMapView::SetTileSource(const boost::shared_ptr<TileSource>& tile_source)
00065 {
00066 tile_source_ = tile_source;
00067 level_ = -1;
00068 }
00069
00070 void TileMapView::SetTransform(const swri_transform_util::Transform& transform)
00071 {
00072 if (transform.GetOrigin() == transform_.GetOrigin() &&
00073 transform.GetOrientation() == transform_.GetOrientation())
00074 {
00075 return;
00076 }
00077
00078 transform_ = transform;
00079
00080 for (size_t i = 0; i < tiles_.size(); i++)
00081 {
00082 for (size_t j = 0; j < tiles_[i].points_t.size(); j++)
00083 {
00084 tiles_[i].points_t[j] = transform_ * tiles_[i].points[j];
00085 }
00086 }
00087
00088 for (size_t i = 0; i < precache_.size(); i++)
00089 {
00090 for (size_t j = 0; j < precache_[i].points_t.size(); j++)
00091 {
00092 precache_[i].points_t[j] = transform_ * precache_[i].points[j];
00093 }
00094 }
00095 }
00096
00097 void TileMapView::SetView(
00098 double latitude,
00099 double longitude,
00100 double scale,
00101 int32_t width,
00102 int32_t height)
00103 {
00104 latitude = std::max(-90.0, std::min(90.0, latitude));
00105 longitude = std::max(-180.0, std::min(180.0, longitude));
00106
00107 double lat = swri_math_util::ToRadians(latitude);
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117 double lat_circumference =
00118 swri_transform_util::_earth_equator_circumference * std::cos(lat) / scale;
00119 int32_t level = std::min(tile_source_->GetMaxZoom(),
00120 std::max(tile_source_->GetMinZoom(), static_cast<int32_t>(std::ceil(std::log(lat_circumference) / std::log(2) - 8))));
00121 int64_t max_size = std::pow(2, level);
00122
00123 int64_t center_x = std::min(max_size - 1, static_cast<int64_t>(
00124 std::floor(((longitude + 180.0) / 360.0) * std::pow(2.0, level))));
00125 int64_t center_y = std::min(max_size - 1, static_cast<int64_t>(
00126 std::floor((1.0 - std::log(std::tan(lat) + 1.0 / std::cos(lat)) / swri_math_util::_pi) / 2.0 * std::pow(2.0, level))));
00127
00128 width_ = width;
00129 height_ = height;
00130
00131 double max_dimension = std::max(width, height);
00132
00133 double meters_per_pixel = swri_transform_util::_earth_equator_circumference * std::cos(lat) / std::pow(2, level + 8);
00134 double tile_size = 256.0 * (meters_per_pixel / scale);
00135
00136 int64_t size = std::max(static_cast<int64_t>(1L), std::min(max_size, static_cast<int64_t>(
00137 std::ceil(0.5 * max_dimension / tile_size) * 2 + 1)));
00138
00139 if (size > 50)
00140 {
00141 ROS_ERROR("Invalid map size: %ld", size);
00142 return;
00143 }
00144
00145 if (size_ != size || level_ != level || center_x_ != center_x || center_y_ != center_y)
00146 {
00147 size_ = size;
00148 level_ = level;
00149 center_x_ = center_x;
00150 center_y_ = center_y;
00151
00152 int64_t top = std::max(static_cast<int64_t>(0L), center_y_ - size_ / 2);
00153 int64_t left = std::max(static_cast<int64_t>(0L), center_x_ - size_ / 2);
00154
00155 int64_t right = std::min(max_size, left + size_);
00156 int64_t bottom = std::min(max_size, top + size_);
00157
00158 for (size_t i = 0; i < tiles_.size(); i++)
00159 {
00160 tile_cache_->AddTexture(tiles_[i].texture);
00161 }
00162 tiles_.clear();
00163
00164 for (int64_t i = top; i < bottom; i++)
00165 {
00166 for (int64_t j = left; j < right; j++)
00167 {
00168 Tile tile;
00169 InitializeTile(level_, j, i, tile, 10000);
00170 tiles_.push_back(tile);
00171 }
00172 }
00173
00174 for (size_t i = 0; i < precache_.size(); i++)
00175 {
00176 tile_cache_->AddTexture(precache_[i].texture);
00177 }
00178 precache_.clear();
00179
00180 if (level_ > 0)
00181 {
00182 int64_t precache_x = std::floor(((longitude + 180.0) / 360.0) * std::pow(2.0, level - 1));
00183 int64_t precache_y = std::floor((1.0 - std::log(std::tan(lat) + 1.0 / std::cos(lat)) / swri_math_util::_pi) / 2.0 * std::pow(2.0, level - 1));
00184
00185 int64_t precache_max_size = std::pow(2, level - 1);
00186
00187 int64_t precache_top = std::max(static_cast<int64_t>(0L), precache_y - (size_ - 1) / 2);
00188 int64_t precache_left = std::max(static_cast<int64_t>(0L), precache_x - (size_ - 1) / 2);
00189
00190 int64_t precache_right = std::min(precache_max_size, precache_left + size_);
00191 int64_t precache_bottom = std::min(precache_max_size, precache_top + size_);
00192
00193 for (int64_t i = precache_top; i < precache_bottom; i++)
00194 {
00195 for (int64_t j = precache_left; j < precache_right; j++)
00196 {
00197 Tile tile;
00198 InitializeTile(level_ - 1, j, i, tile, 0);
00199 precache_.push_back(tile);
00200 }
00201 }
00202 }
00203 }
00204 }
00205
00206 void TileMapView::DrawTiles(std::vector<Tile>& tiles, int priority)
00207 {
00208 for (size_t i = 0; i < tiles.size(); i++)
00209 {
00210 TexturePtr& texture = tiles[i].texture;
00211
00212 if (!texture)
00213 {
00214 bool failed;
00215 texture = tile_cache_->GetTexture(tiles[i].url_hash, tiles[i].url, failed, priority);
00216 }
00217
00218 if (texture)
00219 {
00220 glBindTexture(GL_TEXTURE_2D, texture->id);
00221
00222 glBegin(GL_TRIANGLES);
00223
00224 glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
00225
00226 for (int32_t row = 0; row < tiles[i].subdiv_count; row++)
00227 {
00228 for (int32_t col = 0; col < tiles[i].subdiv_count; col++)
00229 {
00230 double u_0 = col * tiles[i].subwidth;
00231 double v_0 = 1.0 - row * tiles[i].subwidth;
00232 double u_1 = (col + 1.0) * tiles[i].subwidth;
00233 double v_1 = 1.0 - (row + 1.0) * tiles[i].subwidth;
00234
00235 const tf::Vector3& tl = tiles[i].points_t[row * (tiles[i].subdiv_count + 1) + col];
00236 const tf::Vector3& tr = tiles[i].points_t[row * (tiles[i].subdiv_count + 1) + col + 1];
00237 const tf::Vector3& br = tiles[i].points_t[(row + 1) * (tiles[i].subdiv_count + 1) + col + 1];
00238 const tf::Vector3& bl = tiles[i].points_t[(row + 1) * (tiles[i].subdiv_count + 1) + col];
00239
00240
00241 glTexCoord2f(u_0, v_0); glVertex2d(tl.x(), tl.y());
00242 glTexCoord2f(u_1, v_0); glVertex2d(tr.x(), tr.y());
00243 glTexCoord2f(u_1, v_1); glVertex2d(br.x(), br.y());
00244
00245
00246 glTexCoord2f(u_0, v_0); glVertex2d(tl.x(), tl.y());
00247 glTexCoord2f(u_1, v_1); glVertex2d(br.x(), br.y());
00248 glTexCoord2f(u_0, v_1); glVertex2d(bl.x(), bl.y());
00249 }
00250 }
00251
00252 glEnd();
00253
00254 glBindTexture(GL_TEXTURE_2D, 0);
00255 }
00256 }
00257 }
00258
00259 void TileMapView::Draw()
00260 {
00261 if (!tile_source_)
00262 {
00263 return;
00264 }
00265
00266 glEnable(GL_TEXTURE_2D);
00267
00268 DrawTiles( precache_, 0 );
00269 DrawTiles( tiles_, 10000 );
00270
00271 glDisable(GL_TEXTURE_2D);
00272 }
00273
00274 void TileMapView::ToLatLon(int32_t level, double x, double y, double& latitude, double& longitude)
00275 {
00276 double n = std::pow(2, level);
00277 longitude = x / n * 360.0 - 180.0;
00278
00279 double r = swri_math_util::_pi - swri_math_util::_2pi * y / n;
00280 latitude = swri_math_util::_rad_2_deg * std::atan(0.5 * (std::exp(r) - std::exp(-r)));
00281 }
00282
00283 void TileMapView::InitializeTile(int32_t level, int64_t x, int64_t y, Tile& tile, int priority)
00284 {
00285 tile.url = tile_source_->GenerateTileUrl(level, x, y);
00286
00287 tile.url_hash = tile_source_->GenerateTileHash(level, x, y);
00288
00289 tile.level = level;
00290
00291 bool failed;
00292 tile.texture = tile_cache_->GetTexture(tile.url_hash, tile.url, failed, priority);
00293
00294 int32_t subdivs = std::max(0, 4 - level);
00295 tile.subwidth = 1.0 / (subdivs + 1.0);
00296 tile.subdiv_count = std::pow(2, subdivs);
00297 for (int32_t row = 0; row <= tile.subdiv_count; row++)
00298 {
00299 for (int32_t col = 0; col <= tile.subdiv_count; col++)
00300 {
00301 double t_lat, t_lon;
00302 ToLatLon(level, x + col * tile.subwidth, y + row * tile.subwidth, t_lat, t_lon);
00303 tile.points.push_back(tf::Vector3(t_lon, t_lat, 0));
00304 }
00305 }
00306
00307 tile.points_t = tile.points;
00308 for (size_t i = 0; i < tile.points_t.size(); i++)
00309 {
00310 tile.points_t[i] = transform_ * tile.points_t[i];
00311 }
00312 }
00313 }