tile_map_view.cpp

Go to the documentation of this file.

// *****************************************************************************
//
// Copyright (c) 2015, Southwest Research Institute® (SwRI®)
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of Southwest Research Institute® (SwRI®) nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
// *****************************************************************************

#include <tile_map/tile_map_view.h>

#include <cmath>

#include <boost/make_shared.hpp>

#include <GL/gl.h>

#include <ros/ros.h>

#include <swri_math_util/constants.h>
#include <swri_math_util/trig_util.h>
#include <swri_transform_util/earth_constants.h>

#include <tile_map/image_cache.h>

namespace tile_map
{
  TileMapView::TileMapView() :
    level_(-1),
    width_(100),
    height_(100)
  {
    ImageCachePtr image_cache = boost::make_shared<ImageCache>("/tmp/tile_map");
    tile_cache_ = boost::make_shared<TextureCache>(image_cache);
  }

  void TileMapView::ResetCache()
  {
    tile_cache_->Clear();
  }

  void TileMapView::SetTileSource(const boost::shared_ptr<TileSource>& tile_source)
  {
    tile_source_ = tile_source;
    level_ = -1;
  }

  void TileMapView::SetTransform(const swri_transform_util::Transform& transform)
  {
    if (transform.GetOrigin() == transform_.GetOrigin() &&
        transform.GetOrientation() == transform_.GetOrientation())
    {
      return;
    }
  
    transform_ = transform;
    
    for (size_t i = 0; i < tiles_.size(); i++)
    {
      for (size_t j = 0; j < tiles_[i].points_t.size(); j++)
      {
        tiles_[i].points_t[j] = transform_ * tiles_[i].points[j];
      }
    }
    
    for (size_t i = 0; i < precache_.size(); i++)
    {
      for (size_t j = 0; j < precache_[i].points_t.size(); j++)
      {
        precache_[i].points_t[j] = transform_ * precache_[i].points[j];
      }
    }
  }
  
  void TileMapView::SetView(
    double latitude, 
    double longitude, 
    double scale, 
    int32_t width,
    int32_t height)
  {
    latitude = std::max(-90.0, std::min(90.0, latitude));
    longitude = std::max(-180.0, std::min(180.0, longitude));
  
    double lat = swri_math_util::ToRadians(latitude);
  
    // Calculate the current zoom level:
    // 
    // According to http://wiki.openstreetmap.org/wiki/Zoom_levels:
    //   meters_per_pixel = earth_circumference * cos(lat) / 2^(level + 8)
    //
    // Therefore,
    //   level = log2(earth_circumference * cos(lat) / meters_per_pixel) - 8
    //
    double lat_circumference = 
      swri_transform_util::_earth_equator_circumference * std::cos(lat) / scale;
    int32_t level =  std::min(tile_source_->GetMaxZoom(),
                             std::max(tile_source_->GetMinZoom(), static_cast<int32_t>(std::ceil(std::log(lat_circumference) / std::log(2) - 8))));
    int64_t max_size = std::pow(2, level);
    
    int64_t center_x = std::min(max_size - 1, static_cast<int64_t>(
      std::floor(((longitude + 180.0) / 360.0) * std::pow(2.0, level))));
    int64_t center_y = std::min(max_size - 1, static_cast<int64_t>(
      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)))); 
        
    width_ = width;
    height_ = height;
    
    double max_dimension = std::max(width, height);
    
    double meters_per_pixel = swri_transform_util::_earth_equator_circumference * std::cos(lat) / std::pow(2, level + 8);    
    double tile_size = 256.0 * (meters_per_pixel / scale);
    
    int64_t size = std::max(static_cast<int64_t>(1L), std::min(max_size, static_cast<int64_t>(
      std::ceil(0.5 * max_dimension / tile_size) * 2 + 1)));
    
    if (size > 50)
    {
      ROS_ERROR("Invalid map size: %ld", size);
      return;
    }
    
    if (size_ != size || level_ != level || center_x_ != center_x || center_y_ != center_y)
    {
      size_ = size;
      level_ = level;
      center_x_ = center_x;
      center_y_ = center_y;
      
      int64_t top = std::max(static_cast<int64_t>(0L), center_y_ - size_ / 2);
      int64_t left = std::max(static_cast<int64_t>(0L), center_x_ - size_ / 2);
      
      int64_t right = std::min(max_size, left + size_);
      int64_t bottom = std::min(max_size, top + size_);

      for (size_t i = 0; i < tiles_.size(); i++)
      {
        tile_cache_->AddTexture(tiles_[i].texture);
      }
      tiles_.clear();
      
      for (int64_t i = top; i < bottom; i++)
      {
        for (int64_t j = left; j < right; j++)
        {
          Tile tile;
          InitializeTile(level_, j, i, tile);
          tiles_.push_back(tile);
        }
      }
      
      for (size_t i = 0; i < precache_.size(); i++)
      {
        tile_cache_->AddTexture(precache_[i].texture);
      }
      precache_.clear();

      if (level_ > 0)
      {
        int64_t precache_x = std::floor(((longitude + 180.0) / 360.0) * std::pow(2.0, level - 1));
        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)); 
        
        int64_t precache_max_size = std::pow(2, level - 1);
        
        int64_t precache_top = std::max(static_cast<int64_t>(0L), precache_y - (size_ - 1) / 2);
        int64_t precache_left = std::max(static_cast<int64_t>(0L), precache_x - (size_ - 1) / 2);
      
        int64_t precache_right = std::min(precache_max_size, precache_left + size_);
        int64_t precache_bottom = std::min(precache_max_size, precache_top + size_);
        
        for (int64_t i = precache_top; i < precache_bottom; i++)
        {
          for (int64_t j = precache_left; j < precache_right; j++)
          {
            Tile tile;
            InitializeTile(level_ - 1, j, i, tile);
            precache_.push_back(tile);
          }
        }
      }
    }
  }
  
  void TileMapView::Draw()
  {
    if (!tile_source_)
    {
      return;
    }
    
    glEnable(GL_TEXTURE_2D);
    
    for (size_t i = 0; i < precache_.size(); i++)
    {
      TexturePtr texture = precache_[i].texture;
      
      if (!texture)
      {
        bool failed;
        texture = tile_cache_->GetTexture(precache_[i].url_hash, precache_[i].url, failed);
        if (failed)
        {
          tile_source_->SetMaxZoom(std::min(tile_source_->GetMaxZoom(), precache_[i].level - 1));
          ROS_WARN("===== SETTING MAX LEVEL TO %d =====", tile_source_->GetMaxZoom());
        }
      }
    
      if (texture)
      {
        glBindTexture(GL_TEXTURE_2D, texture->id);

        glBegin(GL_TRIANGLES);
                
        glColor4f(1.0f, 1.0f, 1.0f, 1.0f);

        for (int32_t row = 0; row < precache_[i].subdiv_count; row++)
        {
          for (int32_t col = 0; col < precache_[i].subdiv_count; col++)
          {
            double u_0 = col * precache_[i].subwidth;
            double v_0 = 1.0 - row * precache_[i].subwidth;
            double u_1 = (col + 1.0) * precache_[i].subwidth;
            double v_1 = 1.0 - (row + 1.0) * precache_[i].subwidth;
            
            const tf::Vector3& tl = precache_[i].points_t[row * (precache_[i].subdiv_count + 1) + col];
            const tf::Vector3& tr = precache_[i].points_t[row * (precache_[i].subdiv_count + 1) + col + 1];
            const tf::Vector3& br = precache_[i].points_t[(row + 1) * (precache_[i].subdiv_count + 1) + col + 1];
            const tf::Vector3& bl = precache_[i].points_t[(row + 1) * (precache_[i].subdiv_count + 1) + col];
            
            // Triangle 1
            glTexCoord2f(u_0, v_0); glVertex2d(tl.x(), tl.y());
            glTexCoord2f(u_1, v_0); glVertex2d(tr.x(), tr.y());
            glTexCoord2f(u_1, v_1); glVertex2d(br.x(), br.y());
              
            // Triangle 2
            glTexCoord2f(u_0, v_0); glVertex2d(tl.x(), tl.y());
            glTexCoord2f(u_1, v_1); glVertex2d(br.x(), br.y());
            glTexCoord2f(u_0, v_1); glVertex2d(bl.x(), bl.y());
          }
        }
      
        glEnd();
      
        glBindTexture(GL_TEXTURE_2D, 0);
      }
    }
    
    for (size_t i = 0; i < tiles_.size(); i++)
    {
      TexturePtr texture = tiles_[i].texture;
      
      if (!texture)
      {
        bool failed;
        texture = tile_cache_->GetTexture(tiles_[i].url_hash, tiles_[i].url, failed);
        if (failed)
        {
          tile_source_->SetMaxZoom(std::min(tile_source_->GetMaxZoom(), tiles_[i].level - 1));
          ROS_WARN("===== SETTING MAX LEVEL TO %d =====", tile_source_->GetMaxZoom());
        }
      }
    
      if (texture)
      {
        glBindTexture(GL_TEXTURE_2D, texture->id);

        glBegin(GL_TRIANGLES);
                
        glColor4f(1.0f, 1.0f, 1.0f, 1.0f);      
      
        for (int32_t row = 0; row < tiles_[i].subdiv_count; row++)
        {
          for (int32_t col = 0; col < tiles_[i].subdiv_count; col++)
          {
            double u_0 = col * tiles_[i].subwidth;
            double v_0 = 1.0 - row * tiles_[i].subwidth;
            double u_1 = (col + 1.0) * tiles_[i].subwidth;
            double v_1 = 1.0 - (row + 1.0) * tiles_[i].subwidth;
            
            const tf::Vector3& tl = tiles_[i].points_t[row * (tiles_[i].subdiv_count + 1) + col];
            const tf::Vector3& tr = tiles_[i].points_t[row * (tiles_[i].subdiv_count + 1) + col + 1];
            const tf::Vector3& br = tiles_[i].points_t[(row + 1) * (tiles_[i].subdiv_count + 1) + col + 1];
            const tf::Vector3& bl = tiles_[i].points_t[(row + 1) * (tiles_[i].subdiv_count + 1) + col];
            
            // Triangle 1
            glTexCoord2f(u_0, v_0); glVertex2d(tl.x(), tl.y());
            glTexCoord2f(u_1, v_0); glVertex2d(tr.x(), tr.y());
            glTexCoord2f(u_1, v_1); glVertex2d(br.x(), br.y());
              
            // Triangle 2
            glTexCoord2f(u_0, v_0); glVertex2d(tl.x(), tl.y());
            glTexCoord2f(u_1, v_1); glVertex2d(br.x(), br.y());
            glTexCoord2f(u_0, v_1); glVertex2d(bl.x(), bl.y());
          }
        }
      
        glEnd();
      
        glBindTexture(GL_TEXTURE_2D, 0);
      }
    }
      
    glDisable(GL_TEXTURE_2D);
  }
  
  void TileMapView::ToLatLon(int32_t level, double x, double y, double& latitude, double& longitude)
  {
    double n = std::pow(2, level);
    longitude = x / n * 360.0 - 180.0;
    
    double r = swri_math_util::_pi - swri_math_util::_2pi * y / n;
    latitude = swri_math_util::_rad_2_deg * std::atan(0.5 * (std::exp(r) - std::exp(-r)));
  }
  
  void TileMapView::InitializeTile(int32_t level, int64_t x, int64_t y, Tile& tile)
  {
    tile.url = tile_source_->GenerateTileUrl(level, x, y);
  
    tile.url_hash = tile_source_->GenerateTileHash(level, x, y);

    tile.level = level;
  
    bool failed;
    tile.texture = tile_cache_->GetTexture(tile.url_hash, tile.url, failed);
    if (failed)
    {
      tile_source_->SetMaxZoom(std::min(tile_source_->GetMaxZoom(), level - 1));
      ROS_WARN("===== SETTING MAX LEVEL TO %d =====", tile_source_->GetMaxZoom());
    }
  
    int32_t subdivs = std::max(0, 4 - level);
    tile.subwidth = 1.0 / (subdivs + 1.0);
    tile.subdiv_count = std::pow(2, subdivs);
    for (int32_t row = 0; row <= tile.subdiv_count; row++)
    {
      for (int32_t col = 0; col <= tile.subdiv_count; col++)
      {
        double t_lat, t_lon;
        ToLatLon(level, x + col * tile.subwidth, y + row * tile.subwidth, t_lat, t_lon);
        tile.points.push_back(tf::Vector3(t_lon, t_lat, 0));
      }
    }
    
    tile.points_t = tile.points;
    for (size_t i = 0; i < tile.points_t.size(); i++)
    {
      tile.points_t[i] = transform_ * tile.points_t[i];
    }
  }
}