diff --git a/modules/realm_core/src/cv_grid_map.cpp b/modules/realm_core/src/cv_grid_map.cpp index 5f6457ca..95584066 100644 --- a/modules/realm_core/src/cv_grid_map.cpp +++ b/modules/realm_core/src/cv_grid_map.cpp @@ -121,7 +121,21 @@ void CvGridMap::add(const CvGridMap &submap, int flag_overlap_handle, bool do_ex } } - // Get the data in the overlapping area of both mat + // Hack, don't access larger area than we have + // This should be handled above, but something is a few pixels off + if (m_layers[idx_layer].data.cols < dst_roi.x + dst_roi.width) { + dst_roi.width = submap_layer.data.cols - dst_roi.x; + } + if (m_layers[idx_layer].data.rows < dst_roi.y + dst_roi.height) { + dst_roi.height = submap_layer.data.rows - dst_roi.y; + } + if (submap_layer.data.cols < src_roi.x + src_roi.width) { + src_roi.width = submap_layer.data.cols - src_roi.x; + } + if (submap_layer.data.rows < src_roi.y + src_roi.height) { + src_roi.height = submap_layer.data.rows - src_roi.y; + } + cv::Mat src_data_roi = submap_layer.data(src_roi); cv::Mat dst_data_roi = m_layers[idx_layer].data(dst_roi); @@ -480,10 +494,15 @@ void CvGridMap::mergeMatrices(const cv::Mat &from, cv::Mat &to, int flag_merge_h break; case REALM_OVERWRITE_ZERO: cv::Mat mask; - if (to.type() == CV_32F || to.type() == CV_64F) - mask = (to != to) & (from == from); - else + if (to.type() == CV_32F || to.type() == CV_64F) { + cv::Mat to_mask = to.clone(); + cv::Mat from_mask = from.clone(); + cv::patchNaNs(to_mask, 0); + cv::patchNaNs(from_mask, 0); + mask = (to_mask == 0) & (from_mask != 0); + } else { mask = (to == 0) & (from > 0); + } from.copyTo(to, mask); break; } diff --git a/modules/realm_io/include/realm_io/utilities.h b/modules/realm_io/include/realm_io/utilities.h index 13047d8c..0f15f871 100644 --- a/modules/realm_io/include/realm_io/utilities.h +++ b/modules/realm_io/include/realm_io/utilities.h @@ -83,7 +83,7 @@ std::vector split(const char *str, char c = ' '); * @param dir Directory to grab the filenames * @return Vector of all files with absolute path */ -std::vector getFileList(const std::string& dir, const std::string &suffix = ""); +std::vector getFileList(const std::string& dir, const std::string &suffix = "", const std::function& sort = std::less<>()); /*! TODO: Einbaurichtung implementieren? * @brief Function to compute a 3x3 rotation matrix based on heading data. It is assumed, that the camera is pointing diff --git a/modules/realm_io/src/utilities.cpp b/modules/realm_io/src/utilities.cpp index 7a8ad429..5c6ce063 100644 --- a/modules/realm_io/src/utilities.cpp +++ b/modules/realm_io/src/utilities.cpp @@ -97,7 +97,7 @@ std::vector io::split(const char *str, char c) return result; } -std::vector io::getFileList(const std::string& dir, const std::string &suffix) +std::vector io::getFileList(const std::string& dir, const std::string &suffix, const std::function& sort) { std::vector filenames; if (!dir.empty()) @@ -105,16 +105,19 @@ std::vector io::getFileList(const std::string& dir, const std::stri boost::filesystem::path apk_path(dir); boost::filesystem::recursive_directory_iterator end; - for (boost::filesystem::recursive_directory_iterator it(apk_path); it != end; ++it) - { + for (boost::filesystem::recursive_directory_iterator it(apk_path); it != end; ++it) { const boost::filesystem::path cp = (*it); - const std::string &filepath = cp.string(); - if (suffix.empty() || filepath.substr(filepath.size() - suffix.size(), filepath.size()) == suffix) - filenames.push_back(cp.string()); + auto canon_path = boost::filesystem::canonical(cp, "/"); + const std::string &filepath = canon_path.string(); + if (suffix.empty() || filepath.substr(filepath.size() - suffix.size(), suffix.size()) == suffix) { + filenames.push_back(filepath); + } } } - std::sort(filenames.begin(), filenames.end()); + + // By default, return in reverse sort order, useful for zoom level creating / deletion + std::sort(filenames.begin(), filenames.end(), sort); return filenames; } diff --git a/modules/realm_ortho/CMakeLists.txt b/modules/realm_ortho/CMakeLists.txt index a06ff752..6b078c67 100755 --- a/modules/realm_ortho/CMakeLists.txt +++ b/modules/realm_ortho/CMakeLists.txt @@ -54,7 +54,6 @@ set(HEADER_FILES ${root}/include/realm_ortho/nearest_neighbor.h ${root}/include/realm_ortho/rectification.h ${root}/include/realm_ortho/tile.h - ${root}/include/realm_ortho/tile_cache.h ) set(SOURCE_FILES @@ -63,7 +62,6 @@ set(SOURCE_FILES ${root}/src/map_tiler.cpp ${root}/src/rectification.cpp ${root}/src/tile.cpp - ${root}/src/tile_cache.cpp ) # delaunay relies on CGAL to work diff --git a/modules/realm_ortho/include/realm_ortho/map_tiler.h b/modules/realm_ortho/include/realm_ortho/map_tiler.h index 1efba85f..ebaebf37 100644 --- a/modules/realm_ortho/include/realm_ortho/map_tiler.h +++ b/modules/realm_ortho/include/realm_ortho/map_tiler.h @@ -8,7 +8,7 @@ #include #include -#include +#include #include #include @@ -38,8 +38,9 @@ class MapTiler /*! * @brief Besides member initialization the required lookup tables to map zoom level to image resolution are created. * @param verbosity Flag to set verbose output + * @param verbosity Flag to use TMS standard for tile, false to use Google/OSM */ - explicit MapTiler(bool verbosity); + explicit MapTiler(bool verbosity, bool use_tms); ~MapTiler() = default; MapTiler(const MapTiler &other) = default; @@ -61,6 +62,38 @@ class MapTiler double getResolution(int zoom_level); + /*! + * @brief Computes one lat-lon coordinate for a given tile. It represents the upper left corner of the tile. + * @param x Coordinate of tile in x-direction + * @param y Coordinate of tile in y-direction + * @param zoom_level Zoom level of the map + * @param tms Whether TMS or Google standard is used (inverts y axis) + * @return (longitude, latitude) of upper left tile corner + */ + static WGSPose computeLatLonForTile(int x, int y, int zoom_level, bool tms); + + /*! + * @brief Computes one lat-lon coordinate for a given tile. It represents the center of the tile. This can be useful + * when converting from tiles to CoG as it can prevent including edge tiles along the boundary. + * @param x Coordinate of tile in x-direction + * @param y Coordinate of tile in y-direction + * @param zoom_level Zoom level of the map + * @param tms Whether TMS or Google standard is used (inverts y axis) + * @return (longitude, latitude) of the center of the tile + */ + static WGSPose computeCenterLatLonForTile(int x, int y, int zoom_level, bool tms); + + /*! + * @brief Computes the slippy tile index for a given zoom level that contains the requested coordinate in WGS84. The + * specifications are documented: https://wiki.openstreetmap.org/wiki/Slippy_map_tilenames#Pseudo-code + * @param lat Latitude in WGS84 + * @param lon Longitude in WGS84 + * @param zoom_level Zoom level of the map + * @param tms Whether TMS or Google standard is used (inverts y axis) + * @return Tile coordinate according to slippy tile standard + */ + static cv::Point2i computeTileFromLatLon(double lat, double lon, int zoom_level, bool tms) ; + private: /// Flag to set verbose output @@ -75,6 +108,9 @@ class MapTiler /// Shift of the coordinate frame origin double m_origin_shift; + /// Whether to use TMS or Google/OSM standards for the y origin + bool m_use_tms; + /// Size of the tiles in [pix], usually this is 256 int m_tile_size; @@ -84,16 +120,6 @@ class MapTiler /// Lookup table to map zoom levels to an absolute number of tiles std::map m_lookup_nrof_tiles_from_zoom; - /*! - * @brief Computes the slippy tile index for a given zoom level that contains the requested coordinate in WGS84. The - * specifications are documented: https://wiki.openstreetmap.org/wiki/Slippy_map_tilenames#Pseudo-code - * @param lat Latitude in WGS84 - * @param lon Longitude in WGS84 - * @param zoom_level Zoom level 0 - 35 - * @return Tile coordinate according to slippy tile standard - */ - cv::Point2i computeTileFromLatLon(double lat, double lon, int zoom_level) const; - /*! * @brief Each tile is indexed with (tx, ty). Together with the corresponding tile size this coordinate can be * transformed into a global pixel coordinate with tx * tile size, ty * tile size. If the resolution is known in @@ -173,15 +199,6 @@ class MapTiler */ cv::Rect2d computeTileBoundsMeters(const cv::Rect2i &idx_roi, int zoom_level); - /*! - * @brief Computes one lat-lon coordinate for a given tile. It represents the upper left corner of the tile. - * @param x Coordinate of tile in x-direction - * @param y Coordinate of tile in y-direction - * @param zoom_level Zoon level of the map - * @return (longitude, latitude) of upper left tile corner - */ - WGSPose computeLatLonForTile(int x, int y, int zoom_level) const; - /*! * @brief Maximal scale down zoom of the pyramid closest to the pixelSize. * @param GSD Ground sampling distance in m/pix diff --git a/modules/realm_ortho/include/realm_ortho/tile.h b/modules/realm_ortho/include/realm_ortho/tile.h index 84ccd0a3..0846ad75 100644 --- a/modules/realm_ortho/include/realm_ortho/tile.h +++ b/modules/realm_ortho/include/realm_ortho/tile.h @@ -14,7 +14,7 @@ namespace realm /*! * @brief Tile is a container class that is defined by a coordinate (x,y) in a specific zoom level following the - * Tiled Map Service specification and a multi-layered grid map holding the data. + * Tiled Map Service or Google/OSM specification and a multi-layered grid map holding the data. */ class Tile { @@ -24,12 +24,13 @@ class Tile public: /*! * @brief Non-default constructor - * @param zoom_level Zoom level or z-coordinate according to TMS standard - * @param tx Tile index in x-direction according to TMS standard - * @param ty Tile index in y-direction according to TMS standard + * @param zoom_level Zoom level or z-coordinate according to TMS or Google/OSM specification + * @param tx Tile index in x-direction according to TMS or Google/OSM specification + * @param ty Tile index in y-direction according to TMS or Google/OSM specification * @param map Multi-layered grid map holding the data for the tile + * @param is_tms Indicates this is a TMS rather than google tile */ - Tile(int zoom_level, int tx, int ty, const CvGridMap &map); + Tile(int zoom_level, int tx, int ty, const CvGridMap &map, bool is_tms); /*! * @brief Locks the tile when being accessed or modified to prevent multi-threading problems. @@ -41,6 +42,12 @@ class Tile */ void unlock(); + /*! + * @brief Indicates if this is a TMS or Google/OSM tile + * @return True if tms + */ + bool is_tms() const; + /*! * @brief Getter for the zoom level * @return Zoom level of the data @@ -69,15 +76,18 @@ class Tile private: - /// Zoom level according to TMS standard + /// Zoom level according to TMS or Google/OSM specification int m_zoom_level; - /// Tile index according to TMS standard + /// Tile index according to TMS or Google/OSM specification cv::Point2i m_index; /// Multi-layered grid map container CvGridMap::Ptr m_data; + /// Indicates this is a tms tiles + bool m_tms; + /// Main mutex to prevent simultaneous access from different threads std::mutex m_mutex_data; }; diff --git a/modules/realm_ortho/include/realm_ortho/tile_cache.h b/modules/realm_ortho/include/realm_ortho/tile_cache.h deleted file mode 100644 index 69268257..00000000 --- a/modules/realm_ortho/include/realm_ortho/tile_cache.h +++ /dev/null @@ -1,98 +0,0 @@ - - -#ifndef GENERAL_TESTBED_TILE_CACHE_H -#define GENERAL_TESTBED_TILE_CACHE_H - -#include -#include -#include - -#include - -#include -#include -#include -#include - -namespace realm -{ - -class TileCache : public WorkerThreadBase -{ -public: - using Ptr = std::shared_ptr; - - struct LayerMetaData - { - std::string name; - int type; - int interpolation_flag; - }; - - struct CacheElement - { - using Ptr = std::shared_ptr; - long timestamp; - std::vector layer_meta; - Tile::Ptr tile; - bool was_written; - - mutable std::mutex mutex; - }; - - using CacheElementGrid = std::map>; - -public: - TileCache(const std::string &id, double sleep_time, const std::string &output_directory, bool verbose); - ~TileCache(); - - void add(int zoom_level, const std::vector &tiles, const cv::Rect2i &roi_idx); - - Tile::Ptr get(int tx, int ty, int zoom_level); - - void setOutputFolder(const std::string &dir); - - void flushAll(); - void loadAll(); - -private: - - bool m_has_init_directories; - - std::mutex m_mutex_settings; - std::string m_dir_toplevel; - - std::mutex m_mutex_cache; - std::map m_cache; - - std::mutex m_mutex_do_update; - bool m_do_update; - - std::mutex m_mutex_roi_prev_request; - std::map m_roi_prev_request; - - std::mutex m_mutex_roi_prediction; - std::map m_roi_prediction; - - bool process() override; - - void reset() override; - - void load(const CacheElement::Ptr &element) const; - void write(const CacheElement::Ptr &element) const; - - void flush(const CacheElement::Ptr &element) const; - - bool isCached(const CacheElement::Ptr &element) const; - - size_t estimateByteSize(const Tile::Ptr &tile) const; - - void updatePrediction(int zoom_level, const cv::Rect2i &roi_current); - - void createDirectories(const std::string &toplevel, const std::vector &layer_names, const std::string &tile_tree); - -}; - -} - -#endif //GENERAL_TESTBED_TILE_CACHE_H diff --git a/modules/realm_ortho/src/map_tiler.cpp b/modules/realm_ortho/src/map_tiler.cpp index d531ef65..590fc225 100644 --- a/modules/realm_ortho/src/map_tiler.cpp +++ b/modules/realm_ortho/src/map_tiler.cpp @@ -4,11 +4,12 @@ using namespace realm; -MapTiler::MapTiler(bool verbosity) +MapTiler::MapTiler(bool verbosity, bool use_tms) : m_verbosity(verbosity), m_zoom_level_min(11), m_zoom_level_max(35), - m_tile_size(256) + m_tile_size(256), + m_use_tms(use_tms) { m_origin_shift = 2 * M_PI * 6378137 / 2.0; @@ -49,6 +50,17 @@ std::map MapTiler::createTiles(const CvGridMap::Ptr &ma zoom_level_max = zoom_level_base; } + // There isn't really a reason to make the image MUCH larger than GSD, so correct that here. computeZoom should already + // perform minor upscaling for us. + if (zoom_level_max > zoom_level_base) + { + LOG_F(WARNING, "Maximum zoom level requested (%d) was greater than GSD based estimate (%d). Using max GSD.", zoom_level_max, zoom_level_base); + if (zoom_level_max == zoom_level_min) { + zoom_level_min = zoom_level_base; + } + zoom_level_max = zoom_level_base; + } + if (zoom_level_min > zoom_level_max) throw(std::invalid_argument("Error computing tiles: Minimum zoom level larger than maximum.")); @@ -75,12 +87,24 @@ std::map MapTiler::createTiles(const CvGridMap::Ptr &ma std::vector tiles; for (int x = 0; x < tile_bounds_idx.width; ++x) - // Note: Coordinate system of the tiles is up positive, while image is down positive. Therefore the inverse loop - for (int y = tile_bounds_idx.height; y > 0; --y) + + if (m_use_tms) { - cv::Rect2i data_roi(x*256, y*256, 256, 256); - Tile::Ptr tile_current = std::make_shared(zoom_level, tile_bounds_idx.x + x, tile_bounds_idx.y + tile_bounds_idx.height - y, map->getSubmap(map->getAllLayerNames(), data_roi)); - tiles.push_back(tile_current); + // Note: For TMS, Coordinate system of the tiles is up positive, while image is down positive. Therefore the inverse loop + for (int y = tile_bounds_idx.height; y > 0; --y) + { + cv::Rect2i data_roi(x*256, y*256, 256, 256); + Tile::Ptr tile_current = std::make_shared(zoom_level, tile_bounds_idx.x + x, tile_bounds_idx.y + tile_bounds_idx.height - y, map->getSubmap(map->getAllLayerNames(), data_roi), true); + tiles.push_back(tile_current); + } + } else { + // Note: For Google/OSM, Coordinate system of tiles down is positive, which matching image down positive, so forward loop + for (int y = 0; y < tile_bounds_idx.height; ++y) + { + cv::Rect2i data_roi(x*256, y*256, 256, 256); + Tile::Ptr tile_current = std::make_shared(zoom_level, tile_bounds_idx.x + x, tile_bounds_idx.y + y, map->getSubmap(map->getAllLayerNames(), data_roi), false); + tiles.push_back(tile_current); + } } tiles_from_zoom[zoom_level] = TiledMap{tile_bounds_idx, tiles}; @@ -97,23 +121,59 @@ void MapTiler::computeLookupResolutionFromZoom(double latitude) } } -cv::Point2i MapTiler::computeTileFromLatLon(double lat, double lon, int zoom_level) const +cv::Point2i MapTiler::computeTileFromLatLon(double lat, double lon, int zoom_level, bool tms) { double lat_rad = lat * M_PI / 180.0; - auto n = static_cast(m_lookup_nrof_tiles_from_zoom.at(zoom_level)); + int n = static_cast(std::pow(2, zoom_level)); cv::Point2i pos; pos.x = static_cast(std::floor((lon + 180.0) / 360.0 * n)); pos.y = static_cast(std::floor((1.0 - asinh(tan(lat_rad)) / M_PI) / 2.0 * n)); + + if (tms) pos.y = n - pos.y - 1; return pos; } +WGSPose MapTiler::computeLatLonForTile(int x, int y, int zoom_level, bool tms) +{ + double n = std::pow(2, zoom_level); + if (tms) y = static_cast((n - 1) - y); + double k = M_PI - 2.0 * M_PI * y / n; + + WGSPose wgs{}; + wgs.latitude = 180.0 / M_PI * atan(0.5 * (exp(k) - exp(-k))); + wgs.longitude = x / n * 360.0 - 180; + + return wgs; +} + +WGSPose MapTiler::computeCenterLatLonForTile(int x, int y, int zoom_level, bool tms) +{ + double n = std::pow(2, zoom_level); + if (tms) y = static_cast((n - 1) - y); + double k = M_PI - 2.0 * M_PI * (y + 0.5) / n; + + WGSPose wgs{}; + wgs.latitude = 180.0 / M_PI * atan(0.5 * (exp(k) - exp(-k))); + wgs.longitude = (x + 0.5) / n * 360.0 - 180; + + return wgs; +} + cv::Point2d MapTiler::computeMetersFromPixels(int px, int py, int zoom_level) { cv::Point2d meters; double resolution = m_lookup_resolution_from_zoom.at(zoom_level); meters.x = px * resolution - m_origin_shift; - meters.y = py * resolution - m_origin_shift; + + // TMS can directly map pixels with lower left origin to meters with an origin shift, since this coordinate system + // is symmetric, we can just invert the result to get non-TMS meters. + if (m_use_tms) { + meters.y = py * resolution - m_origin_shift; + } else { + meters.y = -(py * resolution - m_origin_shift); + } + return meters; } @@ -130,7 +190,12 @@ cv::Point2i MapTiler::computeTileFromPixels(int px, int py, int zoom_level) { cv::Point2i tile; tile.x = int(std::ceil(px / (double)(m_tile_size)) - 1); - tile.y = int(std::ceil(py / (double)(m_tile_size)) - 1); + + if (m_use_tms) { + tile.y = int(std::ceil(py / (double)(m_tile_size)) - 1); + } else { + tile.y = m_lookup_nrof_tiles_from_zoom.at(zoom_level) - int(std::ceil(py / (double)(m_tile_size)) - 1) - 1; + } return tile; } @@ -142,36 +207,42 @@ cv::Point2i MapTiler::computeTileFromMeters(double mx, double my, int zoom_level cv::Rect2i MapTiler::computeTileBounds(const cv::Rect2d &roi, int zoom_level) { - cv::Point2i tile_idx_low = computeTileFromMeters(roi.x, roi.y, zoom_level); - cv::Point2i tile_idx_high = computeTileFromMeters(roi.x + roi.width, roi.y + roi.height, zoom_level); + // TMS has geographically low tile with tile y at bottom, while non-tms is based on the top + int y_low, y_high; + if (m_use_tms) { + y_low = roi.y; + y_high = roi.y + roi.height; + } else { + y_low = roi.y + roi.height; + y_high = roi.y; + } + cv::Point2i tile_idx_low = computeTileFromMeters(roi.x, y_low, zoom_level); + cv::Point2i tile_idx_high = computeTileFromMeters(roi.x + roi.width, y_high, zoom_level); // Note: +1 in both directions because tile origin sits in the lower left corner of the tile return cv::Rect2i(tile_idx_low.x, tile_idx_low.y, tile_idx_high.x - tile_idx_low.x + 1, tile_idx_high.y - tile_idx_low.y + 1); } cv::Rect2d MapTiler::computeTileBoundsMeters(int tx, int ty, int zoom_level) { - cv::Point2d p_min = computeMetersFromPixels(tx * m_tile_size, ty * m_tile_size, zoom_level); - cv::Point2d p_max = computeMetersFromPixels((tx + 1) * m_tile_size, (ty + 1) * m_tile_size, zoom_level); - return cv::Rect2d(p_min.x, p_min.y, p_max.x - p_min.x, p_max.y - p_min.y); + cv::Point2d p_min = computeMetersFromPixels(tx * m_tile_size, ty * m_tile_size, zoom_level); + cv::Point2d p_max = computeMetersFromPixels((tx + 1) * m_tile_size, (ty + 1) * m_tile_size, zoom_level); + return cv::Rect2d(p_min.x, p_min.y, p_max.x - p_min.x, p_max.y - p_min.y); } cv::Rect2d MapTiler::computeTileBoundsMeters(const cv::Rect2i &idx_roi, int zoom_level) { - cv::Rect2d tile_bounds_low = computeTileBoundsMeters(idx_roi.x, idx_roi.y, zoom_level); - cv::Rect2d tile_bounds_high = computeTileBoundsMeters(idx_roi.x + idx_roi.width + 1, idx_roi.y + idx_roi.height + 1, zoom_level); - return cv::Rect2d(tile_bounds_low.x, tile_bounds_low.y, tile_bounds_high.x - tile_bounds_low.x, tile_bounds_high.y - tile_bounds_low.y); -} - -WGSPose MapTiler::computeLatLonForTile(int x, int y, int zoom_level) const -{ - auto n = static_cast(m_lookup_nrof_tiles_from_zoom.at(zoom_level)); - double k = M_PI - 2.0 * M_PI * y / n; - - WGSPose wgs{}; - wgs.latitude = 180.0 / M_PI * atan(0.5 * (exp(k) - exp(-k))); - wgs.longitude = x / n * 360.0 - 180; + int y_low, y_high; + if (m_use_tms) { + y_low = idx_roi.y; + y_high = idx_roi.y + idx_roi.height + 1; + } else { + y_low = idx_roi.y + idx_roi.height + 1; + y_high = idx_roi.y; + } - return wgs; + cv::Rect2d tile_bounds_low = computeTileBoundsMeters(idx_roi.x, y_low, zoom_level); + cv::Rect2d tile_bounds_high = computeTileBoundsMeters(idx_roi.x + idx_roi.width + 1, y_high, zoom_level); + return cv::Rect2d(tile_bounds_low.x, tile_bounds_low.y, tile_bounds_high.x - tile_bounds_low.x, tile_bounds_high.y - tile_bounds_low.y); } int MapTiler::computeZoomForPixelSize(double GSD, bool do_upscale) const diff --git a/modules/realm_ortho/src/tile.cpp b/modules/realm_ortho/src/tile.cpp index 8a642d68..9193f2b6 100644 --- a/modules/realm_ortho/src/tile.cpp +++ b/modules/realm_ortho/src/tile.cpp @@ -4,10 +4,11 @@ using namespace realm; -Tile::Tile(int zoom_level, int tx, int ty, const CvGridMap &map) +Tile::Tile(int zoom_level, int tx, int ty, const CvGridMap &map, bool is_tms) : m_zoom_level(zoom_level), m_index(tx, ty), - m_data(std::make_shared(map)) + m_data(std::make_shared(map)), + m_tms(is_tms) { } @@ -21,6 +22,11 @@ void Tile::unlock() m_mutex_data.unlock(); } +bool Tile::is_tms() const +{ + return m_tms; +} + int Tile::zoom_level() const { return m_zoom_level; diff --git a/modules/realm_ortho/src/tile_cache.cpp b/modules/realm_ortho/src/tile_cache.cpp deleted file mode 100644 index 8484dfb0..00000000 --- a/modules/realm_ortho/src/tile_cache.cpp +++ /dev/null @@ -1,404 +0,0 @@ - - -#include - -#include -#include -#include - -using namespace realm; - -TileCache::TileCache(const std::string &id, double sleep_time, const std::string &output_directory, bool verbose) - : WorkerThreadBase("tile_cache_" + id, sleep_time, verbose), - m_dir_toplevel(output_directory), - m_has_init_directories(false), - m_do_update(false) -{ - m_data_ready_functor = [=]{ return (m_do_update || isFinishRequested()); }; -} - -TileCache::~TileCache() -{ - flushAll(); -} - -void TileCache::setOutputFolder(const std::string &dir) -{ - std::lock_guard lock(m_mutex_settings); - m_dir_toplevel = dir; -} - -bool TileCache::process() -{ - bool has_processed = false; - - if (m_mutex_do_update.try_lock()) - { - long t; - - // Give update lock free as fast as possible, so we won't block other threads from adding data - bool do_update = m_do_update; - m_do_update = false; - m_mutex_do_update.unlock(); - - if (do_update) - { - int n_tiles_written = 0; - - t = getCurrentTimeMilliseconds(); - - for (auto &cached_elements_zoom : m_cache) - { - cv::Rect2i roi_prediction = m_roi_prediction.at(cached_elements_zoom.first); - for (auto &cached_elements_column : cached_elements_zoom.second) - { - for (auto &cached_elements : cached_elements_column.second) - { - std::lock_guard lock(cached_elements.second->mutex); - cached_elements.second->tile->lock(); - - if (!cached_elements.second->was_written) - { - n_tiles_written++; - write(cached_elements.second); - } - - if (isCached(cached_elements.second)) - { - int tx = cached_elements.second->tile->x(); - int ty = cached_elements.second->tile->y(); - if (tx < roi_prediction.x || tx > roi_prediction.x + roi_prediction.width - || ty < roi_prediction.y || ty > roi_prediction.y + roi_prediction.height) - { - flush(cached_elements.second); - } - } - cached_elements.second->tile->unlock(); - } - } - } - - LOG_IF_F(INFO, m_verbose, "Tiles written: %i", n_tiles_written); - LOG_IF_F(INFO, m_verbose, "Timing [Cache Flush]: %lu ms", getCurrentTimeMilliseconds() - t); - - has_processed = true; - } - } - return has_processed; -} - -void TileCache::reset() -{ - m_cache.clear(); -} - -void TileCache::add(int zoom_level, const std::vector &tiles, const cv::Rect2i &roi_idx) -{ - std::lock_guard lock(m_mutex_cache); - - // Assuming all tiles are based on the same data, therefore have the same number of layers and layer names - std::vector layer_names = tiles[0]->data()->getAllLayerNames(); - - std::vector layer_meta; - for (const auto &layer_name : layer_names) - { - // Saving the name and the type of the layer into the meta data - CvGridMap::Layer layer = tiles[0]->data()->getLayer(layer_name); - layer_meta.emplace_back(LayerMetaData{layer_name, layer.data.type(), layer.interpolation}); - } - - if (!m_has_init_directories) - { - createDirectories(m_dir_toplevel + "/", layer_names, ""); - m_has_init_directories = true; - } - - auto it_zoom = m_cache.find(zoom_level); - - long timestamp = getCurrentTimeMilliseconds(); - - long t = getCurrentTimeMilliseconds(); - - // Cache for this zoom level already exists - if (it_zoom != m_cache.end()) - { - for (const auto &t : tiles) - { - // Here we find a tile grid for a specific zoom level and add the new tiles to it. - // Important: Tiles that already exist will be overwritten! - t->lock(); - auto it_tile_x = it_zoom->second.find(t->x()); - if (it_tile_x == it_zoom->second.end()) - { - // Zoom level exists, but tile column is - createDirectories(m_dir_toplevel + "/", layer_names, "/" + std::to_string(zoom_level) + "/" + std::to_string(t->x())); - it_zoom->second[t->x()][t->y()].reset(new CacheElement{timestamp, layer_meta, t, false}); - } - else - { - auto it_tile_xy = it_tile_x->second.find(t->y()); - if (it_tile_xy == it_tile_x->second.end()) - { - // Zoom level and column was found, but tile did not yet exist - it_tile_x->second[t->y()].reset(new CacheElement{timestamp, layer_meta, t, false}); - } - else - { - // Existing tile was found inside zoom level and column - it_tile_xy->second->mutex.lock(); // note: mutex goes out of scope after this operation, no unlock needed. - it_tile_xy->second.reset(new CacheElement{timestamp, layer_meta, t, false}); - } - } - t->unlock(); - } - } - // Cache for this zoom level does not yet exist - else - { - createDirectories(m_dir_toplevel + "/", layer_names, "/" + std::to_string(zoom_level)); - - CacheElementGrid tile_grid; - for (const auto &t : tiles) - { - // By assigning a new grid of tiles to the zoom level we overwrite all existing data. But in this case there was - // no prior data found for the specific zoom level. - t->lock(); - auto it_tile_x = it_zoom->second.find(t->x()); - if (it_tile_x == it_zoom->second.end()) - createDirectories(m_dir_toplevel + "/", layer_names, "/" + std::to_string(zoom_level) + "/" + std::to_string(t->x())); - - tile_grid[t->x()][t->y()].reset(new CacheElement{timestamp, layer_meta, t, false}); - t->unlock(); - } - m_cache[zoom_level] = tile_grid; - } - - LOG_IF_F(INFO, m_verbose, "Timing [Cache Push]: %lu ms", getCurrentTimeMilliseconds() - t); - - updatePrediction(zoom_level, roi_idx); - - std::lock_guard lock1(m_mutex_do_update); - m_do_update = true; - notify(); -} - -Tile::Ptr TileCache::get(int tx, int ty, int zoom_level) -{ - auto it_zoom = m_cache.find(zoom_level); - if (it_zoom == m_cache.end()) - { - return nullptr; - } - - auto it_tile_x = it_zoom->second.find(tx); - if (it_tile_x == it_zoom->second.end()) - { - return nullptr; - } - - auto it_tile_xy = it_tile_x->second.find(ty); - if (it_tile_xy == it_tile_x->second.end()) - { - return nullptr; - } - - std::lock_guard lock(it_tile_xy->second->mutex); - - // Warning: We lock the tile now and return it to the calling thread locked. Therefore the responsibility to unlock - // it is on the calling thread! - it_tile_xy->second->tile->lock(); - if (!isCached(it_tile_xy->second)) - { - load(it_tile_xy->second); - } - - return it_tile_xy->second->tile; -} - -void TileCache::flushAll() -{ - int n_tiles_written = 0; - - LOG_IF_F(INFO, m_verbose, "Flushing all tiles..."); - - long t = getCurrentTimeMilliseconds(); - - for (auto &zoom_levels : m_cache) - for (auto &cache_column : zoom_levels.second) - for (auto &cache_element : cache_column.second) - { - std::lock_guard lock(cache_element.second->mutex); - cache_element.second->tile->lock(); - if (!cache_element.second->was_written) - { - write(cache_element.second); - n_tiles_written++; - } - - cache_element.second->tile->data() = nullptr; - cache_element.second->tile->unlock(); - } - - LOG_IF_F(INFO, m_verbose, "Tiles written: %i", n_tiles_written); - LOG_IF_F(INFO, m_verbose, "Timing [Flush All]: %lu ms", getCurrentTimeMilliseconds() - t); -} - -void TileCache::loadAll() -{ - for (auto &zoom_levels : m_cache) - for (auto &cache_column : zoom_levels.second) - for (auto &cache_element : cache_column.second) - { - std::lock_guard lock(cache_element.second->mutex); - cache_element.second->tile->lock(); - if (!isCached(cache_element.second)) - load(cache_element.second); - cache_element.second->tile->unlock(); - } -} - -void TileCache::load(const CacheElement::Ptr &element) const -{ - for (const auto &meta : element->layer_meta) - { - std::string filename = m_dir_toplevel + "/" - + meta.name + "/" - + std::to_string(element->tile->zoom_level()) + "/" - + std::to_string(element->tile->x()) + "/" - + std::to_string(element->tile->y()); - - int type = meta.type & CV_MAT_DEPTH_MASK; - - switch(type) - { - case CV_8U: - filename += ".png"; - break; - case CV_16U: - filename += ".bin"; - break; - case CV_32F: - filename += ".bin"; - break; - case CV_64F: - filename += ".bin"; - break; - default: - throw(std::invalid_argument("Error reading tile: data type unknown!")); - } - - if (io::fileExists(filename)) - { - cv::Mat data = io::loadImage(filename); - - element->tile->data()->add(meta.name, data, meta.interpolation_flag); - - LOG_IF_F(INFO, m_verbose, "Read tile from disk: %s", filename.c_str()); - } - else - { - LOG_IF_F(WARNING, m_verbose, "Failed reading tile from disk: %s", filename.c_str()); - throw(std::invalid_argument("Error loading tile.")); - } - } -} - -void TileCache::write(const CacheElement::Ptr &element) const -{ - for (const auto &meta : element->layer_meta) - { - cv::Mat data = element->tile->data()->get(meta.name); - - std::string filename = m_dir_toplevel + "/" - + meta.name + "/" - + std::to_string(element->tile->zoom_level()) + "/" - + std::to_string(element->tile->x()) + "/" - + std::to_string(element->tile->y()); - - int type = data.type() & CV_MAT_DEPTH_MASK; - - switch(type) - { - case CV_8U: - filename += ".png"; - break; - case CV_16U: - filename += ".bin"; - break; - case CV_32F: - filename += ".bin"; - break; - case CV_64F: - filename += ".bin"; - break; - default: - throw(std::invalid_argument("Error writing tile: data type unknown!")); - } - - io::saveImage(data, filename); - - element->was_written = true; - } -} - -void TileCache::flush(const CacheElement::Ptr &element) const -{ - if (!element->was_written) - write(element); - - for (const auto &meta : element->layer_meta) - { - element->tile->data()->remove(meta.name); - } - - LOG_IF_F(INFO, m_verbose, "Flushed tile (%i, %i, %i) [zoom, x, y]", element->tile->zoom_level(), element->tile->x(), element->tile->y()); -} - -bool TileCache::isCached(const CacheElement::Ptr &element) const -{ - return !(element->tile->data()->empty()); -} - -size_t TileCache::estimateByteSize(const Tile::Ptr &tile) const -{ - tile->lock(); - //size_t bytes = tile->data().total() * tile->data().elemSize(); - tile->unlock(); - - //return bytes; - return 0; -} - -void TileCache::updatePrediction(int zoom_level, const cv::Rect2i &roi_current) -{ - std::lock_guard lock(m_mutex_roi_prev_request); - std::lock_guard lock1(m_mutex_roi_prediction); - - auto it_roi_prev_request = m_roi_prev_request.find(zoom_level); - if (it_roi_prev_request == m_roi_prev_request.end()) - { - // There was no previous request, so there can be no prediction which region of tiles might be needed in the next - // processing step. Therefore set the current roi to be the prediction for the next request. - m_roi_prediction[zoom_level] = roi_current; - } - else - { - // We have a previous roi that was requested, therefore we can extrapolate what the next request might look like - // utilizing our current roi - auto it_roi_prediction = m_roi_prediction.find(zoom_level); - it_roi_prediction->second.x = roi_current.x + (roi_current.x - it_roi_prev_request->second.x); - it_roi_prediction->second.y = roi_current.y + (roi_current.y - it_roi_prev_request->second.y); - it_roi_prediction->second.width = roi_current.width + (roi_current.width - it_roi_prev_request->second.width); - it_roi_prediction->second.height = roi_current.height + (roi_current.height - it_roi_prev_request->second.height); - } - - it_roi_prev_request->second = roi_current; -} - -void TileCache::createDirectories(const std::string &toplevel, const std::vector &layer_names, const std::string &tile_tree) -{ - for (const auto &layer_name : layer_names) - { - io::createDir(toplevel + layer_name + tile_tree); - } -} \ No newline at end of file diff --git a/modules/realm_stages/CMakeLists.txt b/modules/realm_stages/CMakeLists.txt index 0fea7626..f02adbf5 100755 --- a/modules/realm_stages/CMakeLists.txt +++ b/modules/realm_stages/CMakeLists.txt @@ -62,12 +62,14 @@ if(WITH_ortho) include/realm_stages/ortho_rectification.h include/realm_stages/surface_generation.h include/realm_stages/mosaicing.h - include/realm_stages/tileing.h) + include/realm_stages/tileing.h + ) list(APPEND SOURCE_FILES src/ortho_rectification.cpp src/surface_generation.cpp src/mosaicing.cpp - src/tileing.cpp) + src/tileing.cpp + ) endif() if(WITH_vslam_base) diff --git a/modules/realm_stages/include/realm_stages/stage_base.h b/modules/realm_stages/include/realm_stages/stage_base.h index 25a148cc..3b4f8b53 100644 --- a/modules/realm_stages/include/realm_stages/stage_base.h +++ b/modules/realm_stages/include/realm_stages/stage_base.h @@ -9,6 +9,7 @@ #include #include #include +#include #include @@ -62,6 +63,7 @@ class StageBase : public WorkerThreadBase using ImageTransportFunc = std::function; using MeshTransportFunc = std::function &, const std::string &)>; using CvGridMapTransportFunc = std::function; + using TilingTransportFunc = std::function updated_tiles, const std::string &)>; public: /*! * @brief Basic constructor for stage class @@ -171,9 +173,19 @@ class StageBase : public WorkerThreadBase * triggered inside the derived stage to transport results. Therefore the callbacks MUST be set, otherwise no data * will leave the stage. * @param func This function consists of a CvGridMap type, a defined topic as description for the data (for example: - * "output/result_gridmap". Timestamp may or may not be set inside the stage fo */ + * "output/result_gridmap". Timestamp may or may not be set inside the stage */ void registerCvGridMapTransport(const CvGridMapTransportFunc &func); + /*! + * @brief Because REALM is independent from the communication infrastructure (e.g. ROS), a transport to the + * corresponding communication interface has to be defined. We chose to use callback functions, that can be + * triggered inside the derived stage to transport results. Therefore the callbacks MUST be set, otherwise no data + * will leave the stage. + * @param func This function consists of a TileCache type, a std::map with tile bounds and zoom levels, and + * a defined topic as description for the data (for example: + * "output/update/tile_cache". Timestamp may or may not be set inside the stage */ + void registerTilingTransport(const TilingTransportFunc &func); + protected: bool m_is_output_dir_initialized; @@ -256,6 +268,13 @@ class StageBase : public WorkerThreadBase */ CvGridMapTransportFunc m_transport_cvgridmap; + /*! + * @brief This function consists of a reference to a tile cache with the current tiles, and a std::map that contains + * the 2d tile boundaries in the current map for each zoom level in the map and a defined topic as a description + * for the data. + */ + TilingTransportFunc m_transport_tiling; + /*! * @brief Setting an async data ready functor allows the thread to wake up from sleep outside the sleep time. It * will only sleep as long as the data ready functor returns falls. It could therefore be provided with a function diff --git a/modules/realm_stages/include/realm_stages/stage_settings.h b/modules/realm_stages/include/realm_stages/stage_settings.h index e6b002ea..59cb9170 100644 --- a/modules/realm_stages/include/realm_stages/stage_settings.h +++ b/modules/realm_stages/include/realm_stages/stage_settings.h @@ -132,7 +132,11 @@ class TileingSettings : public StageSettings public: TileingSettings() { - + add("tms_tiles", Parameter_t{1, "Generate output tiles using TMS standard. If false, Google/OSM standard is used."}); + add("min_zoom", Parameter_t{11, "The minimum tile zoom to generate for the output tiles."}); + add("max_zoom", Parameter_t{20, "The maximum tile zoom to generate for the output tiles. (Set to -1 for maximum zoom based on GSD)"}); + add("delete_cache_on_init", Parameter_t{0, "If there are leftover cache items in the cache folder, delete them before starting the stage."}); + add("load_cache_on_init", Parameter_t{0, "If there are leftover cache items in the cache folder, load them into cache before starting the stage."}); } }; diff --git a/modules/realm_stages/include/realm_stages/tileing.h b/modules/realm_stages/include/realm_stages/tileing.h index 5b5167f4..82eddfce 100644 --- a/modules/realm_stages/include/realm_stages/tileing.h +++ b/modules/realm_stages/include/realm_stages/tileing.h @@ -23,6 +23,10 @@ #include #include +#include +#include +#include +#include #include #include @@ -34,17 +38,22 @@ #include #include #include +#include +#include namespace realm { namespace stages { +class TileCache; + class Tileing : public StageBase { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; + friend TileCache; struct SaveSettings { @@ -54,10 +63,16 @@ class Tileing : public StageBase public: explicit Tileing(const StageSettings::Ptr &stage_set, double rate); ~Tileing(); + + void initStagePath(std::string stage_path); + void initStagePath(std::string stage_path, std::string cache_path); void addFrame(const Frame::Ptr &frame) override; bool process() override; void saveAll(); + void deleteCache(); + void deleteCache(std::string layer); + private: std::deque m_buffer; std::mutex m_mutex_buffer; @@ -66,11 +81,32 @@ class Tileing : public StageBase UTMPose::Ptr m_utm_reference; + /// If true, uses the TMS standard for y (bottom-top) rather than the Google/OSM standard (top-bottom) + bool m_generate_tms_tiles; + + /// The minimum zoom level to generate + int m_min_tile_zoom; + + /// The maximum zoom to generate. May not be generated if GSD isn't sufficient + int m_max_tile_zoom; + + /// Indicates we should wipe the cache directory when starting or resetting the stage + bool m_delete_cache_on_init; + + /// If true, files from disk will be loaded into the tile cache before stitching begins + bool m_load_cache_on_init; + + /// Indicates we have published that initial tile cache update for our cache_on_init load + bool m_initial_cache_published; + + /// The directory to store the output map tiles in, defaults to log directory + std::string m_cache_path; + /// Warper to transform incoming grid maps from UTM coordinates to Web Mercator (EPSG:3857) gis::GdalWarper m_warper; MapTiler::Ptr m_map_tiler; - TileCache::Ptr m_tile_cache; + std::unique_ptr m_tile_cache; Tile::Ptr merge(const Tile::Ptr &t1, const Tile::Ptr &t2); Tile::Ptr blend(const Tile::Ptr &t1, const Tile::Ptr &t2); @@ -82,12 +118,103 @@ class Tileing : public StageBase void initStageCallback() override; uint32_t getQueueDepth() override; - void publish(const Frame::Ptr &frame, const CvGridMap::Ptr &global_map, const CvGridMap::Ptr &update, uint64_t timestamp); + void publish(const Frame::Ptr &frame, TileCache &cache, std::map updated_tiles, uint64_t timestamp); void saveIter(uint32_t id, const CvGridMap::Ptr &map_update); Frame::Ptr getNewFrame(); }; + class TileCache : public WorkerThreadBase + { + public: + using Ptr = std::shared_ptr; + + struct LayerMetaData + { + std::string name; + int type; + int interpolation_flag; + }; + + struct CacheElement + { + using Ptr = std::shared_ptr; + long timestamp; + std::vector layer_meta; + Tile::Ptr tile; + bool was_written; + + mutable std::mutex mutex; + }; + + using CacheElementGrid = std::map>; + using CacheElementItem = std::map; + + public: + TileCache(Tileing *tiling_stage, double sleep_time, std::string output_directory, bool verbose); + ~TileCache(); + + void add(int zoom_level, const std::vector &tiles, const cv::Rect2i &roi_idx); + + Tile::Ptr get(int tx, int ty, int zoom_level); + + std::map getBounds() const; + + void setOutputFolder(const std::string &dir); + std::string getCachePath(const std::string &layer); + + void publishWrittenTiles(std::map &update_region, int tiles_written); + + void flushAll(); + void loadAll(); + + void loadDiskCache(); + void deleteCache(); + void deleteCache(std::string layer); + + private: + + bool m_has_init_directories; + + std::mutex m_mutex_settings; + std::string m_dir_toplevel; + + std::mutex m_mutex_cache; + std::map m_cache; + + std::mutex m_mutex_do_update; + bool m_do_update; + + std::mutex m_mutex_roi_prev_request; + std::map m_roi_prev_request; + + std::mutex m_mutex_roi_prediction; + std::map m_roi_prediction; + + std::mutex m_mutex_roi_map_bounds; + std::map m_cache_bounds; + + Tileing *m_tiling_stage; + + bool process() override; + + void reset() override; + + void load(const CacheElement::Ptr &element); + void write(const CacheElement::Ptr &element); + + void flush(const CacheElement::Ptr &element); + + bool isCached(const CacheElement::Ptr &element) const; + + size_t estimateByteSize(const Tile::Ptr &tile) const; + + void updatePrediction(int zoom_level, const cv::Rect2i &roi_current); + + void createDirectories(const std::string &toplevel, const std::vector &layer_names, const std::string &tile_tree); + + }; + } // namespace stages } // namespace realm diff --git a/modules/realm_stages/src/mosaicing.cpp b/modules/realm_stages/src/mosaicing.cpp index ff623ce6..19ce0c9a 100644 --- a/modules/realm_stages/src/mosaicing.cpp +++ b/modules/realm_stages/src/mosaicing.cpp @@ -173,10 +173,7 @@ CvGridMap Mosaicing::blend(CvGridMap::Overlap *overlap) CvGridMap ref = *overlap->first; CvGridMap src = *overlap->second; - cv::Mat ref_not_elevated; - cv::bitwise_not(ref["elevated"], ref_not_elevated); - - // There are aparently a number of issues with NaN comparisons breaking in various ways. See: + // There are apparently a number of issues with NaN comparisons breaking in various ways. See: // https://github.com/opencv/opencv/issues/16465 // To avoid these, use patchNaNs before using boolean comparisons cv::patchNaNs(ref["elevation_angle"],0); diff --git a/modules/realm_stages/src/stage_base.cpp b/modules/realm_stages/src/stage_base.cpp index 7334145d..6a7f9193 100644 --- a/modules/realm_stages/src/stage_base.cpp +++ b/modules/realm_stages/src/stage_base.cpp @@ -92,6 +92,11 @@ void StageBase::registerCvGridMapTransport(const std::function updated_tiles, const std::string &)> &func) +{ + m_transport_tiling = func; +} + void StageBase::setStatisticsPeriod(uint32_t s) { std::unique_lock lock(m_mutex_statistics); diff --git a/modules/realm_stages/src/tileing.cpp b/modules/realm_stages/src/tileing.cpp index cd0c4e15..4b7b32eb 100644 --- a/modules/realm_stages/src/tileing.cpp +++ b/modules/realm_stages/src/tileing.cpp @@ -17,16 +17,26 @@ * You should have received a copy of the GNU General Public License * along with OpenREALM. If not, see . */ +#include +#include +#include #include - #include +#include +#include using namespace realm; using namespace stages; Tileing::Tileing(const StageSettings::Ptr &stage_set, double rate) : StageBase("tileing", (*stage_set)["path_output"].toString(), rate, (*stage_set)["queue_size"].toInt(), bool((*stage_set)["log_to_file"].toInt())), + m_generate_tms_tiles((*stage_set)["tms_tiles"].toInt() > 0), + m_min_tile_zoom((*stage_set)["min_zoom"].toInt()), + m_max_tile_zoom((*stage_set)["max_zoom"].toInt()), + m_delete_cache_on_init((*stage_set)["delete_cache_on_init"].toInt() > 0), + m_load_cache_on_init((*stage_set)["load_cache_on_init"].toInt() > 0), + m_initial_cache_published(false), m_utm_reference(nullptr), m_map_tiler(nullptr), m_tile_cache(nullptr), @@ -43,11 +53,6 @@ Tileing::Tileing(const StageSettings::Ptr &stage_set, double rate) Tileing::~Tileing() { - if (m_tile_cache) - { - m_tile_cache->requestFinish(); - m_tile_cache->join(); - } } void Tileing::addFrame(const Frame::Ptr &frame) @@ -75,15 +80,23 @@ void Tileing::addFrame(const Frame::Ptr &frame) bool Tileing::process() { + // Check if we have published our initial tile cache load if that option is set + // This is delayed to the first processing loop since on init, we may not have + // all listeners connected yet. this loop shoun after start() is called. + if (m_load_cache_on_init && !m_initial_cache_published) { + auto bounds = m_tile_cache->getBounds(); + LOG_F(INFO, "Initial publish of %d loaded cache tiles", bounds.size()); + m_tile_cache->publishWrittenTiles(bounds, bounds.size()); + m_initial_cache_published = true; + } + bool has_processed = false; if (!m_buffer.empty() && m_map_tiler && m_tile_cache) { // Prepare timing long t; - // Prepare output of incremental map update - CvGridMap::Ptr map_update; - + // Get the image to add to the tile cache Frame::Ptr frame = getNewFrame(); LOG_F(INFO, "Processing frame #%u...", frame->getFrameId()); @@ -135,7 +148,7 @@ bool Tileing::process() t = getCurrentTimeMilliseconds(); - std::map tiled_map_max_zoom = m_map_tiler->createTiles(map_3857); + std::map tiled_map_max_zoom = m_map_tiler->createTiles(map_3857, m_max_tile_zoom, m_max_tile_zoom); LOG_F(INFO, "Timing [Tileing]: %lu ms", getCurrentTimeMilliseconds()-t); @@ -190,7 +203,8 @@ bool Tileing::process() // They can be required for the blending for example though, which is why we computed them on maximum resolution map_3857->remove("elevated"); - std::map tiled_map_range = m_map_tiler->createTiles(map_3857, 11, zoom_level_max - 1); + std::map tiled_map_range = m_map_tiler->createTiles(map_3857, m_min_tile_zoom, zoom_level_max - 1); + std::map tiles_merged_roi; for (const auto& tiled_map : tiled_map_range) { @@ -204,7 +218,11 @@ bool Tileing::process() Tile::Ptr tile_merged; if (tile_cached) { - tile_merged = merge(tile, tile_cached); + // Currently merge appears to be having issues properly combining tiles, leading to + // odd artifacts. Merge is slightly more intensive, but works cleaner. I am not yet sure why merge + // is having issues. + //tile_merged = merge(tile, tile_cached); + tile_merged = blend(tile, tile_cached); tile_cached->unlock(); } else @@ -216,6 +234,7 @@ bool Tileing::process() } m_tile_cache->add(zoom_level, tiles_merged, tiled_map.second.roi); + tiles_merged_roi[zoom_level] = tiled_map.second.roi; } LOG_F(INFO, "Timing [Downscaling]: %lu ms", getCurrentTimeMilliseconds()-t); @@ -226,17 +245,15 @@ bool Tileing::process() // //=======================================// - // Publishings every iteration + // Publishings every iteration, most publishing is done by separate IO thread when map is updated LOG_F(INFO, "Publishing..."); - t = getCurrentTimeMilliseconds(); - //publish(frame, _global_map, map_update, frame->getTimestamp()); + publish(frame, *m_tile_cache.get(), tiles_merged_roi, frame->getTimestamp()); LOG_F(INFO, "Timing [Publish]: %lu ms", getCurrentTimeMilliseconds()-t); - // Savings every iteration t = getCurrentTimeMilliseconds(); - saveIter(frame->getFrameId(), map_update); + //saveIter(frame->getFrameId(), map_update); LOG_F(INFO, "Timing [Saving]: %lu ms", getCurrentTimeMilliseconds()-t); has_processed = true; @@ -244,6 +261,19 @@ bool Tileing::process() return has_processed; } +void Tileing::deleteCache() { + if (m_tile_cache) + { + m_tile_cache->deleteCache(); + } +} + +void Tileing::deleteCache(std::string layer) { + if (m_tile_cache) { + m_tile_cache->deleteCache(layer); + } +} + Tile::Ptr Tileing::merge(const Tile::Ptr &t1, const Tile::Ptr &t2) { if (t2->data()->empty()) @@ -259,86 +289,28 @@ Tile::Ptr Tileing::blend(const Tile::Ptr &t1, const Tile::Ptr &t2) CvGridMap::Ptr& src = t2->data(); CvGridMap::Ptr& dst = t1->data(); - // Cells in the elevation, which have no value are marked as NaN. Therefore v == v returns false for those. - cv::Mat src_mask = ((*src)["elevation"] == (*src)["elevation"]); - - // Find all the cells, that are better in the destination tile - // First get all elements in the destination tile, that are not elevated (have an elevation, but were not 3D reconstructed) - cv::Mat dst_not_elevated; - cv::bitwise_not((*dst)["elevated"], dst_not_elevated); - - // Now remove all cells from the source tile, that have a smaller elevation angle than the destination, except those - // that are elevated where the destination is not. - cv::Mat dst_mask = ((*src)["elevation_angle"] < (*dst)["elevation_angle"]) | ((*src)["elevated"] & dst_not_elevated); + // There are apparently a number of issues with NaN comparisons breaking in various ways. See: + // https://github.com/opencv/opencv/issues/16465 + // To avoid these, use patchNaNs before using boolean comparisons + cv::patchNaNs((*dst)["elevation_angle"],0); + cv::Mat dst_mask = ((*src)["elevation_angle"] > (*dst)["elevation_angle"]);// | ((*src)["elevated"] & dst_not_elevated)); - // Now remove them - src_mask.setTo(0, dst_mask); - - (*src)["color_rgb"].copyTo((*dst)["color_rgb"], src_mask); - (*src)["elevation"].copyTo((*dst)["elevation"], src_mask); - (*src)["elevation_angle"].copyTo((*dst)["elevation_angle"], src_mask); + (*src)["color_rgb"].copyTo((*dst)["color_rgb"], dst_mask); + (*src)["elevation"].copyTo((*dst)["elevation"], dst_mask); + (*src)["elevation_angle"].copyTo((*dst)["elevation_angle"], dst_mask); return t1; } void Tileing::saveIter(uint32_t id, const CvGridMap::Ptr &map_update) { - /*if (_settings_save.save_valid) - io::saveImage((*_global_map)["valid"], io::createFilename(_stage_path + "/valid/valid_", id, ".png")); - if (_settings_save.save_ortho_rgb_all) - io::saveImage((*_global_map)["color_rgb"], io::createFilename(_stage_path + "/ortho/ortho_", id, ".png")); - if (_settings_save.save_elevation_all) - io::saveImageColorMap((*_global_map)["elevation"], (*_global_map)["valid"], _stage_path + "/elevation/color_map", "elevation", id, io::ColormapType::ELEVATION); - if (_settings_save.save_elevation_var_all) - io::saveImageColorMap((*_global_map)["elevation_var"], (*_global_map)["valid"], _stage_path + "/variance", "variance", id,io::ColormapType::ELEVATION); - if (_settings_save.save_elevation_obs_angle_all) - io::saveImageColorMap((*_global_map)["elevation_angle"], (*_global_map)["valid"], _stage_path + "/obs_angle", "angle", id, io::ColormapType::ELEVATION); - if (_settings_save.save_num_obs_all) - io::saveImageColorMap((*_global_map)["num_observations"], (*_global_map)["valid"], _stage_path + "/nobs", "nobs", id, io::ColormapType::ELEVATION); - if (_settings_save.save_ortho_gtiff_all && _gdal_writer != nullptr) - _gdal_writer->requestSaveGeoTIFF(std::make_shared(_global_map->getSubmap({"color_rgb"})), _utm_reference->zone, _stage_path + "/ortho/ortho_iter.tif", true, _settings_save.split_gtiff_channels);*/ - - //io::saveGeoTIFF(*map_update, "color_rgb", _utm_reference->zone, io::createFilename(_stage_path + "/ortho/ortho_", id, ".tif")); + // Not really a good save iter, though we could save png representations of the tile cache? } void Tileing::saveAll() { - // 2D map output -// if (_settings_save.save_ortho_rgb_one) -// io::saveCvGridMapLayer(*_global_map, _utm_reference->zone, _utm_reference->band, "color_rgb", _stage_path + "/ortho/ortho.png"); -// if (_settings_save.save_elevation_one) -// io::saveImageColorMap((*_global_map)["elevation"], (*_global_map)["valid"], _stage_path + "/elevation/color_map", "elevation", io::ColormapType::ELEVATION); -// if (_settings_save.save_elevation_var_one) -// io::saveImageColorMap((*_global_map)["elevation_var"], (*_global_map)["valid"], _stage_path + "/variance", "variance", io::ColormapType::ELEVATION); -// if (_settings_save.save_elevation_obs_angle_one) -// io::saveImageColorMap((*_global_map)["elevation_angle"], (*_global_map)["valid"], _stage_path + "/obs_angle", "angle", io::ColormapType::ELEVATION); -// if (_settings_save.save_num_obs_one) -// io::saveImageColorMap((*_global_map)["num_observations"], (*_global_map)["valid"], _stage_path + "/nobs", "nobs", io::ColormapType::ELEVATION); -// if (_settings_save.save_num_obs_one) -// io::saveGeoTIFF(_global_map->getSubmap({"num_observations"}), _utm_reference->zone, _stage_path + "/nobs/nobs.tif"); -// if (_settings_save.save_ortho_gtiff_one) -// io::saveGeoTIFF(_global_map->getSubmap({"color_rgb"}), _utm_reference->zone, _stage_path + "/ortho/ortho.tif", true, _settings_save.split_gtiff_channels); -// if (_settings_save.save_elevation_one) -// io::saveGeoTIFF(_global_map->getSubmap({"elevation"}), _utm_reference->zone, _stage_path + "/elevation/gtiff/elevation.tif"); -// -// // 3D Point cloud output -// if (_settings_save.save_dense_ply) -// { -// if (_global_map->exists("elevation_normal")) -// io::saveElevationPointsToPLY(*_global_map, "elevation", "elevation_normal", "color_rgb", "valid", _stage_path + "/elevation/ply", "elevation"); -// else -// io::saveElevationPointsToPLY(*_global_map, "elevation", "", "color_rgb", "valid", _stage_path + "/elevation/ply", "elevation"); -// } -// -// // 3D Mesh output -// if (_settings_save.save_elevation_mesh_one) -// { -// std::vector vertex_ids = _mesher->buildMesh(*_global_map, "valid"); -// if (_global_map->exists("elevation_normal")) -// io::saveElevationMeshToPLY(*_global_map, vertex_ids, "elevation", "elevation_normal", "color_rgb", "valid", _stage_path + "/elevation/mesh", "elevation"); -// else -// io::saveElevationMeshToPLY(*_global_map, vertex_ids, "elevation", "", "color_rgb", "valid", _stage_path + "/elevation/mesh", "elevation"); -// } + // Possibly merge tiles with gdal CoG if save option is set? + // Easier CoG support would require GDAL 3.2.1, or custom calls } void Tileing::reset() @@ -348,9 +320,14 @@ void Tileing::reset() void Tileing::finishCallback() { + if (m_tile_cache) + { + m_tile_cache->requestFinish(); + m_tile_cache->join(); + } + // Trigger savings saveAll(); - } Frame::Ptr Tileing::getNewFrame() @@ -362,23 +339,45 @@ Frame::Ptr Tileing::getNewFrame() return (std::move(frame)); } +void Tileing::initStagePath(std::string stage_path) { + StageBase::initStagePath(stage_path); +} +void Tileing::initStagePath(std::string stage_path, std::string cache_path) { + // Set our cache path first before calling down to the stage initialization + // The stage initialization callback will create directories and start the cache up. + m_cache_path = cache_path; + initStagePath(stage_path); +} void Tileing::initStageCallback() { + if (m_cache_path.empty()) { + m_cache_path = m_stage_path + "/tiles"; + } + // Stage directory first if (!io::dirExists(m_stage_path)) io::createDir(m_stage_path); - // Then sub directories - if (!io::dirExists(m_stage_path + "/tiles")) - io::createDir(m_stage_path + "/tiles"); + // Initialize cache path + if (!io::dirExists(m_cache_path)) + io::createDir(m_cache_path); // We can only create the map tiler, when we have the final initialized stage path, which might be synchronized // across different devies. Consequently it is not created in the constructor but here. if (!m_map_tiler) { - m_map_tiler = std::make_shared(true); - m_tile_cache = std::make_shared("tile_cache", 500, m_stage_path + "/tiles", false); + m_map_tiler = std::make_shared(true, m_generate_tms_tiles); + m_tile_cache = std::make_unique(this, 500, m_cache_path, true); + + // If both delete and load are selected, delete first, which will override the load + if (m_delete_cache_on_init) { + deleteCache(); + } + if (m_load_cache_on_init) { + m_tile_cache->loadDiskCache(); + } + m_tile_cache->start(); } } @@ -389,32 +388,630 @@ void Tileing::printSettingsToLog() //LOG_F(INFO, "- publish_mesh_nth_iter: %i", _publish_mesh_nth_iter); } -void Tileing::publish(const Frame::Ptr &frame, const CvGridMap::Ptr &map, const CvGridMap::Ptr &update, uint64_t timestamp) -{ +void Tileing::publish(const Frame::Ptr &frame, TileCache &cache, std::map updated_tiles, uint64_t timestamp) { // First update statistics about outgoing frame rate updateStatisticsOutgoing(); -// _transport_img((*_global_map)["color_rgb"], "output/rgb"); -// _transport_img(analysis::convertToColorMapFromCVC1((*_global_map)["elevation"], -// (*_global_map)["valid"], -// cv::COLORMAP_JET), "output/elevation"); -// _transport_cvgridmap(update->getSubmap({"color_rgb"}), _utm_reference->zone, _utm_reference->band, -// "output/update/ortho"); -// //_transport_cvgridmap(update->getSubmap({"elevation", "valid"}), _utm_reference->zone, _utm_reference->band, "output/update/elevation"); -// -// if (_publish_mesh_every_nth_kf > 0 && _publish_mesh_every_nth_kf == _publish_mesh_nth_iter) -// { -// std::vector faces = createMeshFaces(map); -// std::thread t(_transport_mesh, faces, "output/mesh"); -// t.detach(); -// _publish_mesh_nth_iter = 0; -// } else if (_publish_mesh_every_nth_kf > 0) -// { -// _publish_mesh_nth_iter++; -// } + // Right now we only update when we write tiles to the drive. Optionally, we could publish changed tiles here, but since + // we don't have anything that consumes them, we skip this step } uint32_t Tileing::getQueueDepth() { return m_buffer.size(); +} + + + +TileCache::TileCache(Tileing *tiling_stage, double sleep_time, std::string output_directory, bool verbose) + : WorkerThreadBase("tile_cache_io", sleep_time, verbose), + m_dir_toplevel(std::move(output_directory)), + m_has_init_directories(false), + m_do_update(false), + m_tiling_stage(tiling_stage) +{ + m_data_ready_functor = [=]{ return (m_do_update || isFinishRequested()); }; +} + +TileCache::~TileCache() +{ + flushAll(); + for (auto it = m_cache_bounds.begin(); it != m_cache_bounds.end(); it++) { + LOG_F(INFO, "Final cache bounds for zoom %d : X %d - %d : Y %d - %d", it->first, + it->second.x, it->second.x + it->second.width - 1, + it->second.y, it->second.y + it->second.height - 1); + } +} + +void TileCache::setOutputFolder(const std::string &dir) +{ + std::lock_guard lock(m_mutex_settings); + m_dir_toplevel = dir; +} + +std::string TileCache::getCachePath(const std::string &layer) +{ + std::string filename = m_dir_toplevel + "/"+ layer + "/"; + return filename; +} + +bool TileCache::process() +{ + bool has_processed = false; + + if (m_mutex_do_update.try_lock()) + { + long t; + + // Give update lock free as fast as possible, so we won't block other threads from adding data + bool do_update = m_do_update; + m_do_update = false; + m_mutex_do_update.unlock(); + + // Calculate the region where tiles were updated + std::map write_region; + + if (do_update) + { + int n_tiles_written = 0; + + t = getCurrentTimeMilliseconds(); + + for (auto &cached_elements_zoom : m_cache) + { + // Find our prediction region, default to a zero area prediction if it doesn't exist + cv::Rect2i roi_prediction(0,0,0,0); + if (m_roi_prediction.find(cached_elements_zoom.first) != m_roi_prediction.end()) { + roi_prediction = m_roi_prediction.at(cached_elements_zoom.first); + } + + for (auto &cached_elements_column : cached_elements_zoom.second) + { + for (auto &cached_elements : cached_elements_column.second) + { + std::lock_guard lock(cached_elements.second->mutex); + cached_elements.second->tile->lock(); + + if (!cached_elements.second->was_written) + { + n_tiles_written++; + write(cached_elements.second); + + // Update our roi containing written tiles + auto write_roi = write_region.find(cached_elements_zoom.first); + if (write_roi != write_region.end()) { + write_roi->second |= cv::Rect2i(cached_elements.second->tile->x(), cached_elements.second->tile->y(), 1, 1); + } else { + write_region[cached_elements_zoom.first] = cv::Rect2i(cached_elements.second->tile->x(), cached_elements.second->tile->y(), 1, 1); + } + } + + if (isCached(cached_elements.second)) + { + int tx = cached_elements.second->tile->x(); + int ty = cached_elements.second->tile->y(); + if (tx < roi_prediction.x || tx > roi_prediction.x + roi_prediction.width + || ty < roi_prediction.y || ty > roi_prediction.y + roi_prediction.height) + { + flush(cached_elements.second); + } + } + cached_elements.second->tile->unlock(); + } + } + } + + if (n_tiles_written > 0) { + // TODO: Update cache here instead of main, so we write when file system updates have occurred + for (auto it = write_region.begin(); it != write_region.end(); it++) { + LOG_IF_F(INFO, m_verbose, "Cache File Update for zoom %d : X %d - %d : Y %d - %d", it->first, + it->second.x, it->second.x + it->second.width - 1, + it->second.y, it->second.y + it->second.height - 1); + } + + // Publish update files by zoom and region + LOG_F(INFO, "Publishing..."); + t = getCurrentTimeMilliseconds(); + publishWrittenTiles(write_region, n_tiles_written); + LOG_F(INFO, "Timing [Publish]: %lu ms", getCurrentTimeMilliseconds()-t); + + } + LOG_IF_F(INFO, m_verbose, "Tiles written: %i", n_tiles_written); + LOG_IF_F(INFO, m_verbose, "Timing [Cache Flush]: %lu ms", getCurrentTimeMilliseconds() - t); + + has_processed = true; + } + } + return has_processed; +} + +void TileCache::reset() +{ + m_cache.clear(); + m_cache_bounds.clear(); +} + +void TileCache::add(int zoom_level, const std::vector &tiles, const cv::Rect2i &roi_idx) +{ + std::lock_guard lock(m_mutex_cache); + + // Assuming all tiles are based on the same data, therefore have the same number of layers and layer names + std::vector layer_names = tiles[0]->data()->getAllLayerNames(); + + std::vector layer_meta; + for (const auto &layer_name : layer_names) + { + // Saving the name and the type of the layer into the meta data + CvGridMap::Layer layer = tiles[0]->data()->getLayer(layer_name); + layer_meta.emplace_back(LayerMetaData{layer_name, layer.data.type(), layer.interpolation}); + } + + if (!m_has_init_directories) + { + createDirectories(m_dir_toplevel + "/", layer_names, ""); + m_has_init_directories = true; + } + + auto it_zoom = m_cache.find(zoom_level); + + long timestamp = getCurrentTimeMilliseconds(); + + long t = getCurrentTimeMilliseconds(); + + // Cache for this zoom level already exists + if (it_zoom != m_cache.end()) + { + for (const auto &t : tiles) + { + // Here we find a tile grid for a specific zoom level and add the new tiles to it. + // Important: Tiles that already exist will be overwritten! + t->lock(); + auto it_tile_x = it_zoom->second.find(t->x()); + if (it_tile_x == it_zoom->second.end()) + { + // Zoom level exists, but tile column is + createDirectories(m_dir_toplevel + "/", layer_names, "/" + std::to_string(zoom_level) + "/" + std::to_string(t->x())); + it_zoom->second[t->x()][t->y()].reset(new CacheElement{timestamp, layer_meta, t, false}); + } + else + { + auto it_tile_xy = it_tile_x->second.find(t->y()); + if (it_tile_xy == it_tile_x->second.end()) + { + // Zoom level and column was found, but tile did not yet exist + it_tile_x->second[t->y()].reset(new CacheElement{timestamp, layer_meta, t, false}); + } + else + { + // Existing tile was found inside zoom level and column + it_tile_xy->second->mutex.lock(); // note: mutex goes out of scope after this operation, no unlock needed. + it_tile_xy->second.reset(new CacheElement{timestamp, layer_meta, t, false}); + } + } + t->unlock(); + } + } + // Cache for this zoom level does not yet exist + else + { + createDirectories(m_dir_toplevel + "/", layer_names, "/" + std::to_string(zoom_level)); + + CacheElementGrid tile_grid; + for (const auto &t : tiles) + { + // By assigning a new grid of tiles to the zoom level we overwrite all existing data. But in this case there was + // no prior data found for the specific zoom level. + t->lock(); + auto it_tile_x = it_zoom->second.find(t->x()); + if (it_tile_x == it_zoom->second.end()) + createDirectories(m_dir_toplevel + "/", layer_names, "/" + std::to_string(zoom_level) + "/" + std::to_string(t->x())); + + tile_grid[t->x()][t->y()].reset(new CacheElement{timestamp, layer_meta, t, false}); + t->unlock(); + } + m_cache[zoom_level] = tile_grid; + } + + LOG_IF_F(INFO, m_verbose, "Timing [Cache Push]: %lu ms", getCurrentTimeMilliseconds() - t); + + // Finally, update the bounds to take into account the newly added tile + auto bounds_iter = m_cache_bounds.find(zoom_level); + if(bounds_iter != m_cache_bounds.end()) { + bounds_iter->second |= roi_idx; + } else { + m_cache_bounds[zoom_level] = roi_idx; + } + + updatePrediction(zoom_level, roi_idx); + + std::lock_guard lock1(m_mutex_do_update); + m_do_update = true; + notify(); +} + +Tile::Ptr TileCache::get(int tx, int ty, int zoom_level) +{ + auto it_zoom = m_cache.find(zoom_level); + if (it_zoom == m_cache.end()) + { + return nullptr; + } + + auto it_tile_x = it_zoom->second.find(tx); + if (it_tile_x == it_zoom->second.end()) + { + return nullptr; + } + + auto it_tile_xy = it_tile_x->second.find(ty); + if (it_tile_xy == it_tile_x->second.end()) + { + return nullptr; + } + + std::lock_guard lock(it_tile_xy->second->mutex); + + // Warning: We lock the tile now and return it to the calling thread locked. Therefore the responsibility to unlock + // it is on the calling thread! + it_tile_xy->second->tile->lock(); + if (!isCached(it_tile_xy->second)) + { + load(it_tile_xy->second); + } + return it_tile_xy->second->tile; +} + +std::map TileCache::getBounds() const +{ + return m_cache_bounds; +} + +void TileCache::publishWrittenTiles(std::map &update_region, int num_tiles) { + LOG_IF_F(INFO, m_verbose, "Publishing %d tiles...", num_tiles); + m_tiling_stage->m_transport_tiling(getCachePath("rgb_color"), "png", update_region, "output/update/rgb_color"); + m_tiling_stage->m_transport_tiling(getCachePath("rgb_color"), "png", getBounds(), "output/full/rgb_color"); +} + +void TileCache::flushAll() +{ + int n_tiles_written = 0; + + // Calculate the region where tiles were updated + std::map write_region; + + LOG_IF_F(INFO, m_verbose, "Flushing all tiles..."); + + long t = getCurrentTimeMilliseconds(); + + for (auto &zoom_levels : m_cache) + for (auto &cache_column : zoom_levels.second) + for (auto &cache_element : cache_column.second) + { + std::lock_guard lock(cache_element.second->mutex); + cache_element.second->tile->lock(); + if (!cache_element.second->was_written) + { + write(cache_element.second); + n_tiles_written++; + + // Update our roi containing written tiles + auto write_roi = write_region.find(zoom_levels.first); + if (write_roi != write_region.end()) { + write_roi->second |= cv::Rect2i(cache_element.second->tile->x(), cache_element.second->tile->y(), 1, 1); + } else { + write_region[zoom_levels.first] = cv::Rect2i(cache_element.second->tile->x(), cache_element.second->tile->y(), 1, 1); + } + } + + auto layers = cache_element.second->tile->data()->getAllLayerNames(); + for (auto layer : layers) { + cache_element.second->tile->data()->remove(layer); + } + cache_element.second->tile->unlock(); + } + + if (n_tiles_written > 0) { + // TODO: Update cache here instead of main, so we write when file system updates have occurred + for (auto it = write_region.begin(); it != write_region.end(); it++) { + LOG_IF_F(INFO, m_verbose, "Flushall File Update for zoom %d : X %d - %d : Y %d - %d", it->first, + it->second.x, it->second.x + it->second.width - 1, + it->second.y, it->second.y + it->second.height - 1); + } + + // Publish update files by zoom and region + LOG_F(INFO, "Publishing..."); + t = getCurrentTimeMilliseconds(); + publishWrittenTiles(write_region, n_tiles_written); + LOG_F(INFO, "Timing [Publish]: %lu ms", getCurrentTimeMilliseconds()-t); + publishWrittenTiles(write_region, n_tiles_written); + } +} + +void TileCache::loadAll() +{ + for (auto &zoom_levels : m_cache) + for (auto &cache_column : zoom_levels.second) + for (auto &cache_element : cache_column.second) + { + std::lock_guard lock(cache_element.second->mutex); + cache_element.second->tile->lock(); + if (!isCached(cache_element.second)) + load(cache_element.second); + cache_element.second->tile->unlock(); + } +} + + +void TileCache::loadDiskCache() +{ + LOG_F(INFO, "Attempting to load pre-existing map from cache..."); + + // Read which folders are present. Ensure all folders required to load the cache are there + if (!(boost::filesystem::exists(m_dir_toplevel + "/color_rgb") && + boost::filesystem::exists(m_dir_toplevel + "/elevation_angle") && + boost::filesystem::exists(m_dir_toplevel + "/elevation") && + boost::filesystem::exists(m_dir_toplevel + "/elevated"))) { + LOG_F(WARNING, "One or more items required to load cache does NOT exist. Creating new cache..."); + return; + } + + int element_count = 0; + + // If all major folders are present, load all items that are on disk into, using RGB as a reference + if (boost::filesystem::is_directory(m_dir_toplevel + "/color_rgb")) { + + // Sort in reverse order, so we add higher zoom levels first + auto cache_files = io::getFileList(m_dir_toplevel + "/color_rgb", ".png", std::greater<>()); + + for (auto& file : cache_files) { + + // Parse zoom, x, any y data from the path + auto path = boost::filesystem::path(file); + int z = std::stoi(path.parent_path().parent_path().filename().string()); + int x = std::stoi(path.parent_path().filename().string()); + int y = std::stoi(path.filename().stem().string()); + + // Figure out the tile bounds to load + cv::Rect2i data_roi(0, 0, 255, 255); + double resolution = 1.0; + + // NOTE: + // Currently, we don't store any way to tell the interpolation method used. The code right now defaults to INTER_LINEAR, so we can assume that for now. + // Longer term, it may make sense to write these settings out to the base folder of each category and load it again. + std::vector layers; + + // At a minimum, we should have color_rgb, elevation_angle, elevation. Elevated is optional + bool has_required_layers = true; + + // RGB Layer Cache - Should always exist. To load the others + layers.push_back(LayerMetaData{"color_rgb", CV_8UC4, cv::InterpolationFlags::INTER_LINEAR}); + + // Elevation Angle Cache + if (boost::filesystem::exists(m_dir_toplevel + "/elevation_angle/" + std::to_string(z) + "/" + std::to_string(x) + "/" + std::to_string(y) + ".bin")) { + layers.push_back(LayerMetaData{"elevation_angle", CV_32FC1, cv::InterpolationFlags::INTER_LINEAR}); + } else { + LOG_F(WARNING, "Unable to find required elevation_angle layer for z/x/y of %d / %d / %d.", z, x, y); + has_required_layers = false; + } + + // Elevation Cache + if (boost::filesystem::exists(m_dir_toplevel + "/elevation/" + std::to_string(z) + "/" + std::to_string(x) + "/" + std::to_string(y) + ".bin")) { + layers.push_back(LayerMetaData{"elevation", CV_32FC1, cv::InterpolationFlags::INTER_LINEAR}); + } else { + LOG_F(WARNING, "Unable to find required elevation layer for z/x/y of %d / %d / %d.", z, x, y); + has_required_layers = false; + } + + // Elevated Cache (Only exists for highest zoom) + if (boost::filesystem::exists(m_dir_toplevel + "/elevated/" + std::to_string(z) + "/" + std::to_string(x) + "/" + std::to_string(y) + ".png")) { + layers.push_back(LayerMetaData{"elevated", CV_8UC1, cv::InterpolationFlags::INTER_LINEAR}); + } + + if (has_required_layers) { + // Check if zoom already exists + auto it_zoom = m_cache.find(z); + if (it_zoom != m_cache.end()) { + // Zoom exists + + // Check if x map already exists + auto it_x = it_zoom->second.find(x); + if (it_x != it_zoom->second.end()) { + // X exists + it_x->second[y].reset(new CacheElement{getCurrentTimeMilliseconds(), layers, Tile::Ptr(new Tile(z,x,y, CvGridMap(data_roi, 1.0), false)), true}); + } else { + // X doesn't exist + CacheElementItem x_entry; + x_entry[y].reset(new CacheElement{getCurrentTimeMilliseconds(), layers, Tile::Ptr(new Tile(z,x,y, CvGridMap(data_roi, 1.0), false)), true}); + m_cache[z][x] = x_entry; + } + } else { + // Zoom doesn't exist + // Add to cache, but don't load any of the tiles + CacheElementGrid tile_grid; + tile_grid[x][y].reset(new CacheElement{getCurrentTimeMilliseconds(), layers, Tile::Ptr(new Tile(z,x,y, CvGridMap(data_roi, 1.0), false)), true}); + m_cache[z] = tile_grid; + } + + // Update cache bounds + auto bounds_iter = m_cache_bounds.find(z); + if(bounds_iter != m_cache_bounds.end()) { + bounds_iter->second |= cv::Rect2i(x, y, 1, 1); + } else { + m_cache_bounds[z] = cv::Rect2i(x, y, 1, 1); + } + + element_count++; + } else { + LOG_F(WARNING, "Unable to find all layers for z/x/y of %d / %d / %d, skipping add to cache!", z, x, y); + } + } + } + + LOG_F(INFO, "Loaded %d existing tiles from cache.", element_count); +} + +void TileCache::deleteCache() +{ + // Remove all cache items + flushAll(); + m_has_init_directories = false; + auto files = io::getFileList(m_dir_toplevel, ""); + for (auto & file : files) { + if (!file.empty()) io::removeFileOrDirectory(file); + } +} + +void TileCache::deleteCache(std::string layer) +{ + // Attempt to remove the specific layer name + flushAll(); + m_has_init_directories = false; + io::removeFileOrDirectory(m_dir_toplevel + "/" + layer); +} + +void TileCache::load(const CacheElement::Ptr &element) +{ + for (const auto &meta : element->layer_meta) + { + std::string filename = m_dir_toplevel + "/" + + meta.name + "/" + + std::to_string(element->tile->zoom_level()) + "/" + + std::to_string(element->tile->x()) + "/" + + std::to_string(element->tile->y()); + + int type = meta.type & CV_MAT_DEPTH_MASK; + + switch(type) + { + case CV_8U: + filename += ".png"; + break; + case CV_16U: + filename += ".bin"; + break; + case CV_32F: + filename += ".bin"; + break; + case CV_64F: + filename += ".bin"; + break; + default: + throw(std::invalid_argument("Error reading tile: data type unknown!")); + } + + if (io::fileExists(filename)) + { + cv::Mat data = io::loadImage(filename); + + element->tile->data()->add(meta.name, data, meta.interpolation_flag); + + LOG_IF_F(INFO, m_verbose, "Read tile from disk: %s", filename.c_str()); + } + else + { + LOG_IF_F(WARNING, m_verbose, "Failed reading tile from disk: %s", filename.c_str()); + throw(std::invalid_argument("Error loading tile.")); + } + } +} + +void TileCache::write(const CacheElement::Ptr &element) +{ + for (const auto &meta : element->layer_meta) + { + cv::Mat data = element->tile->data()->get(meta.name); + + std::string filename = m_dir_toplevel + "/" + + meta.name + "/" + + std::to_string(element->tile->zoom_level()) + "/" + + std::to_string(element->tile->x()) + "/" + + std::to_string(element->tile->y()); + + int type = data.type() & CV_MAT_DEPTH_MASK; + + switch(type) + { + case CV_8U: + filename += ".png"; + break; + case CV_16U: + filename += ".bin"; + break; + case CV_32F: + filename += ".bin"; + break; + case CV_64F: + filename += ".bin"; + break; + default: + throw(std::invalid_argument("Error writing tile: data type unknown!")); + } + + io::saveImage(data, filename); + + element->was_written = true; + } +} + +void TileCache::flush(const CacheElement::Ptr &element) +{ + if (!element->was_written) + write(element); + + for (const auto &meta : element->layer_meta) + { + element->tile->data()->remove(meta.name); + } + + LOG_IF_F(INFO, m_verbose, "Flushed tile (%i, %i, %i) [zoom, x, y]", element->tile->zoom_level(), element->tile->x(), element->tile->y()); +} + +bool TileCache::isCached(const CacheElement::Ptr &element) const +{ + return !(element->tile->data()->empty()); +} + +size_t TileCache::estimateByteSize(const Tile::Ptr &tile) const +{ + tile->lock(); + //size_t bytes = tile->data().total() * tile->data().elemSize(); + tile->unlock(); + + //return bytes; + return 0; +} + +void TileCache::updatePrediction(int zoom_level, const cv::Rect2i &roi_current) +{ + std::lock_guard lock(m_mutex_roi_prev_request); + std::lock_guard lock1(m_mutex_roi_prediction); + + auto it_roi_prev_request = m_roi_prev_request.find(zoom_level); + if (it_roi_prev_request == m_roi_prev_request.end()) + { + // There was no previous request, so there can be no prediction which region of tiles might be needed in the next + // processing step. Therefore set the current roi to be the prediction for the next request. + m_roi_prediction[zoom_level] = roi_current; + } + else + { + // We have a previous roi that was requested, therefore we can extrapolate what the next request might look like + // utilizing our current roi + auto it_roi_prediction = m_roi_prediction.find(zoom_level); + it_roi_prediction->second.x = roi_current.x + (roi_current.x - it_roi_prev_request->second.x); + it_roi_prediction->second.y = roi_current.y + (roi_current.y - it_roi_prev_request->second.y); + it_roi_prediction->second.width = roi_current.width + (roi_current.width - it_roi_prev_request->second.width); + it_roi_prediction->second.height = roi_current.height + (roi_current.height - it_roi_prev_request->second.height); + } + + // Create or update the previous request for this zoom. This will overwrite the old entry. + m_roi_prev_request[zoom_level] = roi_current; +} + +void TileCache::createDirectories(const std::string &toplevel, const std::vector &layer_names, const std::string &tile_tree) +{ + for (const auto &layer_name : layer_names) + { + io::createDir(toplevel + layer_name + tile_tree); + } } \ No newline at end of file