From 70c0d4ad145566b8cc5ab30cc8e09e23bf1e8f84 Mon Sep 17 00:00:00 2001 From: Zach Thorson Date: Tue, 3 Aug 2021 12:35:57 -0500 Subject: [PATCH 01/14] Handle case where roi of tiles doesn't fit in the new merged matrix --- modules/realm_core/src/cv_grid_map.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/modules/realm_core/src/cv_grid_map.cpp b/modules/realm_core/src/cv_grid_map.cpp index 5f6457ca..d0770b5c 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); From 6d627fa667890122e0cb75b09400e6f9e174473f Mon Sep 17 00:00:00 2001 From: Zach Thorson Date: Tue, 3 Aug 2021 12:37:07 -0500 Subject: [PATCH 02/14] Fix NaN comparisons that may not be valid w/ OpenCV - Comparisons using OpenCV and NaN numbers can have undetermined results due to some shortcuts OpenCV takes in performing comparison operations - This can results in odd behavior where only some of the NaNs are included --- modules/realm_core/src/cv_grid_map.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/modules/realm_core/src/cv_grid_map.cpp b/modules/realm_core/src/cv_grid_map.cpp index d0770b5c..95584066 100644 --- a/modules/realm_core/src/cv_grid_map.cpp +++ b/modules/realm_core/src/cv_grid_map.cpp @@ -494,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; } From 76c5bfd324525025708a373ce8dd3d3fcaa65cb4 Mon Sep 17 00:00:00 2001 From: Zach Thorson Date: Tue, 3 Aug 2021 12:45:05 -0500 Subject: [PATCH 03/14] Update blend in tiling to match mosaicing method --- modules/realm_stages/src/mosaicing.cpp | 5 +---- modules/realm_stages/src/tileing.cpp | 27 +++++++++----------------- 2 files changed, 10 insertions(+), 22 deletions(-) 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/tileing.cpp b/modules/realm_stages/src/tileing.cpp index cd0c4e15..a46b16c8 100644 --- a/modules/realm_stages/src/tileing.cpp +++ b/modules/realm_stages/src/tileing.cpp @@ -259,24 +259,15 @@ 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); - - // 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); + // 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)); + + (*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; } From 98293869b36207bd39ed9d5f4d2fdc6917a6a8b6 Mon Sep 17 00:00:00 2001 From: Zach Thorson Date: Tue, 3 Aug 2021 12:46:41 -0500 Subject: [PATCH 04/14] Switch from merge to blend for high zoom tiles - Merge, even with the previous patches, appears to improperly detect which tiles to overwrite when creating zoomed out tiles. This results in empty mosaic/elevation data overwriting good data in some tiles. - The real fix would be to figure out what is going on with merge, but blend() is a good workaround --- modules/realm_stages/src/tileing.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/modules/realm_stages/src/tileing.cpp b/modules/realm_stages/src/tileing.cpp index a46b16c8..615880d9 100644 --- a/modules/realm_stages/src/tileing.cpp +++ b/modules/realm_stages/src/tileing.cpp @@ -204,7 +204,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 From 161620e8fce7ad41f06f81d1d9013206c0ada8c6 Mon Sep 17 00:00:00 2001 From: Zach Thorson Date: Fri, 6 Aug 2021 10:59:18 -0500 Subject: [PATCH 05/14] Allow selection of TMS or Google/OSM Tiles - TMS is a standard that isn't used by a lot of mapping software, so allow usage of non-tms tiles (Google/OSM) as well with a config flag. --- .../include/realm_ortho/map_tiler.h | 6 +- .../realm_ortho/include/realm_ortho/tile.h | 24 ++++-- modules/realm_ortho/src/map_tiler.cpp | 79 +++++++++++++++---- modules/realm_ortho/src/tile.cpp | 10 ++- .../include/realm_stages/stage_settings.h | 2 +- .../include/realm_stages/tileing.h | 3 + modules/realm_stages/src/tileing.cpp | 3 +- 7 files changed, 99 insertions(+), 28 deletions(-) diff --git a/modules/realm_ortho/include/realm_ortho/map_tiler.h b/modules/realm_ortho/include/realm_ortho/map_tiler.h index 1efba85f..7dba88be 100644 --- a/modules/realm_ortho/include/realm_ortho/map_tiler.h +++ b/modules/realm_ortho/include/realm_ortho/map_tiler.h @@ -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; @@ -75,6 +76,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; 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/src/map_tiler.cpp b/modules/realm_ortho/src/map_tiler.cpp index d531ef65..dfbc5a47 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; @@ -66,8 +67,11 @@ std::map MapTiler::createTiles(const CvGridMap::Ptr &ma // Therefore first identify how many tiles we have to split our map into by computing the tile indices cv::Rect2i tile_bounds_idx = computeTileBounds(roi, zoom_level); + LOG_F(INFO, "SENTERA: Tile bounds idx: %d, %d", tile_bounds_idx.x, tile_bounds_idx.y); + // With the tile indices we can compute the exact region of interest in the geographic frame in meters cv::Rect2d tile_bounds_meters = computeTileBoundsMeters(tile_bounds_idx, zoom_level); + LOG_F(INFO, "SENTERA: Tile bounds meters: %f, %f %f x %f", tile_bounds_meters.x, tile_bounds_meters.y, tile_bounds_meters.width, tile_bounds_meters.height); // Because our map is not yet guaranteed to have exactly the size of the tile region, we have to perform padding to // to fit exactly the tile map boundaries @@ -75,12 +79,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}; @@ -113,7 +129,15 @@ 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 +154,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,23 +171,41 @@ 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); + 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; + } + + 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); } 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_stages/include/realm_stages/stage_settings.h b/modules/realm_stages/include/realm_stages/stage_settings.h index e6b002ea..13782564 100644 --- a/modules/realm_stages/include/realm_stages/stage_settings.h +++ b/modules/realm_stages/include/realm_stages/stage_settings.h @@ -132,7 +132,7 @@ 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."}); } }; diff --git a/modules/realm_stages/include/realm_stages/tileing.h b/modules/realm_stages/include/realm_stages/tileing.h index 5b5167f4..ecedbfef 100644 --- a/modules/realm_stages/include/realm_stages/tileing.h +++ b/modules/realm_stages/include/realm_stages/tileing.h @@ -66,6 +66,9 @@ 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; + /// Warper to transform incoming grid maps from UTM coordinates to Web Mercator (EPSG:3857) gis::GdalWarper m_warper; diff --git a/modules/realm_stages/src/tileing.cpp b/modules/realm_stages/src/tileing.cpp index 615880d9..8d4911c3 100644 --- a/modules/realm_stages/src/tileing.cpp +++ b/modules/realm_stages/src/tileing.cpp @@ -27,6 +27,7 @@ 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_utm_reference(nullptr), m_map_tiler(nullptr), m_tile_cache(nullptr), @@ -372,7 +373,7 @@ void Tileing::initStageCallback() // 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_map_tiler = std::make_shared(true, m_generate_tms_tiles); m_tile_cache = std::make_shared("tile_cache", 500, m_stage_path + "/tiles", false); m_tile_cache->start(); } From 6caf6415c719400e2292b2c4e96209e372f88a7e Mon Sep 17 00:00:00 2001 From: Zach Thorson Date: Wed, 11 Aug 2021 14:02:39 -0500 Subject: [PATCH 06/14] Add settings for min/max zoom levels * -1 can be used in max zoom level to select zoom based on GSD * Max zoom can be overidden if it is significantly higher than the available GSD to avoid oversampling an image. --- modules/realm_ortho/src/map_tiler.cpp | 11 +++++++++++ .../include/realm_stages/stage_settings.h | 2 ++ modules/realm_stages/include/realm_stages/tileing.h | 6 ++++++ modules/realm_stages/src/tileing.cpp | 6 ++++-- 4 files changed, 23 insertions(+), 2 deletions(-) diff --git a/modules/realm_ortho/src/map_tiler.cpp b/modules/realm_ortho/src/map_tiler.cpp index dfbc5a47..f9247972 100644 --- a/modules/realm_ortho/src/map_tiler.cpp +++ b/modules/realm_ortho/src/map_tiler.cpp @@ -50,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.")); diff --git a/modules/realm_stages/include/realm_stages/stage_settings.h b/modules/realm_stages/include/realm_stages/stage_settings.h index 13782564..4c282b08 100644 --- a/modules/realm_stages/include/realm_stages/stage_settings.h +++ b/modules/realm_stages/include/realm_stages/stage_settings.h @@ -133,6 +133,8 @@ class TileingSettings : public StageSettings 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)"}); } }; diff --git a/modules/realm_stages/include/realm_stages/tileing.h b/modules/realm_stages/include/realm_stages/tileing.h index ecedbfef..6c79fe3f 100644 --- a/modules/realm_stages/include/realm_stages/tileing.h +++ b/modules/realm_stages/include/realm_stages/tileing.h @@ -69,6 +69,12 @@ class Tileing : public StageBase /// 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; + /// Warper to transform incoming grid maps from UTM coordinates to Web Mercator (EPSG:3857) gis::GdalWarper m_warper; diff --git a/modules/realm_stages/src/tileing.cpp b/modules/realm_stages/src/tileing.cpp index 8d4911c3..83dee124 100644 --- a/modules/realm_stages/src/tileing.cpp +++ b/modules/realm_stages/src/tileing.cpp @@ -28,6 +28,8 @@ 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_utm_reference(nullptr), m_map_tiler(nullptr), m_tile_cache(nullptr), @@ -136,7 +138,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); @@ -191,7 +193,7 @@ 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); for (const auto& tiled_map : tiled_map_range) { From 08b7fb241c9b414a6ad3bd55c377f37a385ae76e Mon Sep 17 00:00:00 2001 From: Zach Thorson Date: Wed, 11 Aug 2021 17:04:03 -0500 Subject: [PATCH 07/14] Allow specifying the tile cache location directly - To help save/load cache files more easily, the stage can be initialized with a specific path for all cache items, rather than the default stage path. - If a path is not specified, operation will be unchanged. --- .../include/realm_stages/tileing.h | 8 ++++++- modules/realm_stages/src/tileing.cpp | 21 +++++++++++++++---- 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/modules/realm_stages/include/realm_stages/tileing.h b/modules/realm_stages/include/realm_stages/tileing.h index 6c79fe3f..7fa4d0c9 100644 --- a/modules/realm_stages/include/realm_stages/tileing.h +++ b/modules/realm_stages/include/realm_stages/tileing.h @@ -54,6 +54,9 @@ 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(); @@ -72,9 +75,12 @@ class Tileing : public StageBase /// 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 + /// The maximum zoom to generate. May not be generated if GSD isn't sufficient int m_max_tile_zoom; + /// 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; diff --git a/modules/realm_stages/src/tileing.cpp b/modules/realm_stages/src/tileing.cpp index 83dee124..dace4f9b 100644 --- a/modules/realm_stages/src/tileing.cpp +++ b/modules/realm_stages/src/tileing.cpp @@ -360,23 +360,36 @@ 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_generate_tms_tiles); - m_tile_cache = std::make_shared("tile_cache", 500, m_stage_path + "/tiles", false); + m_tile_cache = std::make_shared("tile_cache", 500, m_cache_path, false); m_tile_cache->start(); } } From cf6a492d25a2a4e5c05c85917d9ccc4e35af0354 Mon Sep 17 00:00:00 2001 From: Zach Thorson Date: Mon, 16 Aug 2021 13:42:42 -0500 Subject: [PATCH 08/14] Move thread join to finish callback for shutdown - Shutdown is cleaner if we join the cache thread when the other threads shut down. Otherwise, the cache thread may keep running up until the program exits, causing odd behavior. --- modules/realm_stages/src/tileing.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/modules/realm_stages/src/tileing.cpp b/modules/realm_stages/src/tileing.cpp index dace4f9b..c860c7d6 100644 --- a/modules/realm_stages/src/tileing.cpp +++ b/modules/realm_stages/src/tileing.cpp @@ -46,11 +46,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) @@ -346,9 +341,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() From 00d50fc8917d4cab1b545903c6c247c3d26c4f97 Mon Sep 17 00:00:00 2001 From: Zach Thorson Date: Mon, 16 Aug 2021 15:40:11 -0500 Subject: [PATCH 09/14] Add ability to delete cache files from tiler - To allow the system to reset after subsequent runs, it can be useful to have the system automatically delete cache tiles when starting a new run. - This is added as a command line option to allow enabling/disabling of the behavior - This is preparation for allowing a job to resume (sans old keypoints) after a crash --- .../include/realm_ortho/tile_cache.h | 10 +++++-- modules/realm_ortho/src/map_tiler.cpp | 3 -- modules/realm_ortho/src/tile_cache.cpp | 29 +++++++++++++++++-- .../include/realm_stages/stage_settings.h | 1 + .../include/realm_stages/tileing.h | 6 ++++ modules/realm_stages/src/tileing.cpp | 17 +++++++++++ 6 files changed, 57 insertions(+), 9 deletions(-) diff --git a/modules/realm_ortho/include/realm_ortho/tile_cache.h b/modules/realm_ortho/include/realm_ortho/tile_cache.h index 69268257..49542aaf 100644 --- a/modules/realm_ortho/include/realm_ortho/tile_cache.h +++ b/modules/realm_ortho/include/realm_ortho/tile_cache.h @@ -55,6 +55,9 @@ class TileCache : public WorkerThreadBase void flushAll(); void loadAll(); + void deleteCache(); + void deleteCache(std::string layer); + private: bool m_has_init_directories; @@ -64,6 +67,7 @@ class TileCache : public WorkerThreadBase std::mutex m_mutex_cache; std::map m_cache; + std::mutex m_mutex_file_write; std::mutex m_mutex_do_update; bool m_do_update; @@ -78,10 +82,10 @@ class TileCache : public WorkerThreadBase void reset() override; - void load(const CacheElement::Ptr &element) const; - void write(const CacheElement::Ptr &element) const; + void load(const CacheElement::Ptr &element); + void write(const CacheElement::Ptr &element); - void flush(const CacheElement::Ptr &element) const; + void flush(const CacheElement::Ptr &element); bool isCached(const CacheElement::Ptr &element) const; diff --git a/modules/realm_ortho/src/map_tiler.cpp b/modules/realm_ortho/src/map_tiler.cpp index f9247972..e873eb3c 100644 --- a/modules/realm_ortho/src/map_tiler.cpp +++ b/modules/realm_ortho/src/map_tiler.cpp @@ -78,11 +78,8 @@ std::map MapTiler::createTiles(const CvGridMap::Ptr &ma // Therefore first identify how many tiles we have to split our map into by computing the tile indices cv::Rect2i tile_bounds_idx = computeTileBounds(roi, zoom_level); - LOG_F(INFO, "SENTERA: Tile bounds idx: %d, %d", tile_bounds_idx.x, tile_bounds_idx.y); - // With the tile indices we can compute the exact region of interest in the geographic frame in meters cv::Rect2d tile_bounds_meters = computeTileBoundsMeters(tile_bounds_idx, zoom_level); - LOG_F(INFO, "SENTERA: Tile bounds meters: %f, %f %f x %f", tile_bounds_meters.x, tile_bounds_meters.y, tile_bounds_meters.width, tile_bounds_meters.height); // Because our map is not yet guaranteed to have exactly the size of the tile region, we have to perform padding to // to fit exactly the tile map boundaries diff --git a/modules/realm_ortho/src/tile_cache.cpp b/modules/realm_ortho/src/tile_cache.cpp index 8484dfb0..bc52fcbf 100644 --- a/modules/realm_ortho/src/tile_cache.cpp +++ b/modules/realm_ortho/src/tile_cache.cpp @@ -257,8 +257,30 @@ void TileCache::loadAll() } } -void TileCache::load(const CacheElement::Ptr &element) const +void TileCache::deleteCache() { + std::lock_guard lock(m_mutex_file_write); + // 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) +{ + std::lock_guard lock(m_mutex_file_write); + // 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) +{ + std::lock_guard lock(m_mutex_file_write); for (const auto &meta : element->layer_meta) { std::string filename = m_dir_toplevel + "/" @@ -303,8 +325,9 @@ void TileCache::load(const CacheElement::Ptr &element) const } } -void TileCache::write(const CacheElement::Ptr &element) const +void TileCache::write(const CacheElement::Ptr &element) { + std::lock_guard lock(m_mutex_file_write); for (const auto &meta : element->layer_meta) { cv::Mat data = element->tile->data()->get(meta.name); @@ -341,7 +364,7 @@ void TileCache::write(const CacheElement::Ptr &element) const } } -void TileCache::flush(const CacheElement::Ptr &element) const +void TileCache::flush(const CacheElement::Ptr &element) { if (!element->was_written) write(element); diff --git a/modules/realm_stages/include/realm_stages/stage_settings.h b/modules/realm_stages/include/realm_stages/stage_settings.h index 4c282b08..bb9506f9 100644 --- a/modules/realm_stages/include/realm_stages/stage_settings.h +++ b/modules/realm_stages/include/realm_stages/stage_settings.h @@ -135,6 +135,7 @@ class TileingSettings : public StageSettings 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."}); } }; diff --git a/modules/realm_stages/include/realm_stages/tileing.h b/modules/realm_stages/include/realm_stages/tileing.h index 7fa4d0c9..f669b504 100644 --- a/modules/realm_stages/include/realm_stages/tileing.h +++ b/modules/realm_stages/include/realm_stages/tileing.h @@ -61,6 +61,9 @@ class Tileing : public StageBase bool process() override; void saveAll(); + void deleteCache(); + void deleteCache(std::string layer); + private: std::deque m_buffer; std::mutex m_mutex_buffer; @@ -78,6 +81,9 @@ class Tileing : public StageBase /// 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; + /// The directory to store the output map tiles in, defaults to log directory std::string m_cache_path; diff --git a/modules/realm_stages/src/tileing.cpp b/modules/realm_stages/src/tileing.cpp index c860c7d6..4d6398ed 100644 --- a/modules/realm_stages/src/tileing.cpp +++ b/modules/realm_stages/src/tileing.cpp @@ -30,6 +30,7 @@ Tileing::Tileing(const StageSettings::Ptr &stage_set, double rate) 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_utm_reference(nullptr), m_map_tiler(nullptr), m_tile_cache(nullptr), @@ -246,6 +247,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()) @@ -390,6 +404,9 @@ void Tileing::initStageCallback() { m_map_tiler = std::make_shared(true, m_generate_tms_tiles); m_tile_cache = std::make_shared("tile_cache", 500, m_cache_path, false); + if (m_delete_cache_on_init) { + deleteCache(); + } m_tile_cache->start(); } } From a79df178e364c6d4ef6af1db2cb87c8d02eaafb4 Mon Sep 17 00:00:00 2001 From: Zach Thorson Date: Tue, 24 Aug 2021 16:08:56 -0500 Subject: [PATCH 10/14] Remove uneeded mutex on file i/o - This was causing crashes on iOS for a currently unknown reason. It was removed since the file I/O isn't expected to be called while the application is running anyway. --- modules/realm_ortho/include/realm_ortho/tile_cache.h | 1 - modules/realm_ortho/src/tile_cache.cpp | 4 ---- 2 files changed, 5 deletions(-) diff --git a/modules/realm_ortho/include/realm_ortho/tile_cache.h b/modules/realm_ortho/include/realm_ortho/tile_cache.h index 49542aaf..972cac10 100644 --- a/modules/realm_ortho/include/realm_ortho/tile_cache.h +++ b/modules/realm_ortho/include/realm_ortho/tile_cache.h @@ -67,7 +67,6 @@ class TileCache : public WorkerThreadBase std::mutex m_mutex_cache; std::map m_cache; - std::mutex m_mutex_file_write; std::mutex m_mutex_do_update; bool m_do_update; diff --git a/modules/realm_ortho/src/tile_cache.cpp b/modules/realm_ortho/src/tile_cache.cpp index bc52fcbf..3d73c465 100644 --- a/modules/realm_ortho/src/tile_cache.cpp +++ b/modules/realm_ortho/src/tile_cache.cpp @@ -259,7 +259,6 @@ void TileCache::loadAll() void TileCache::deleteCache() { - std::lock_guard lock(m_mutex_file_write); // Remove all cache items flushAll(); m_has_init_directories = false; @@ -271,7 +270,6 @@ void TileCache::deleteCache() void TileCache::deleteCache(std::string layer) { - std::lock_guard lock(m_mutex_file_write); // Attempt to remove the specific layer name flushAll(); m_has_init_directories = false; @@ -280,7 +278,6 @@ void TileCache::deleteCache(std::string layer) void TileCache::load(const CacheElement::Ptr &element) { - std::lock_guard lock(m_mutex_file_write); for (const auto &meta : element->layer_meta) { std::string filename = m_dir_toplevel + "/" @@ -327,7 +324,6 @@ void TileCache::load(const CacheElement::Ptr &element) void TileCache::write(const CacheElement::Ptr &element) { - std::lock_guard lock(m_mutex_file_write); for (const auto &meta : element->layer_meta) { cv::Mat data = element->tile->data()->get(meta.name); From f7ab0e00fc582bae94c557e59e330de3712c1349 Mon Sep 17 00:00:00 2001 From: Zach Thorson Date: Fri, 27 Aug 2021 11:21:12 -0500 Subject: [PATCH 11/14] Add message for when tiling stage writes files - To better integrate with external systems, a message that occurs when cache files are written to disk to be consumed by other applications is added. - Contains: - Path to the tile directory w/ x/y/z tiles - The type of tile (png) - The region of tiles updated at each zoom level - The topic - Partial updates and full updates are both written out when new tiles are written to the drive - Required moving tile_cache into the tiling stage for tighter integration with messages [ES-217] --- modules/realm_ortho/CMakeLists.txt | 2 - .../include/realm_ortho/map_tiler.h | 53 +- .../include/realm_ortho/tile_cache.h | 101 --- modules/realm_ortho/src/map_tiler.cpp | 44 +- modules/realm_ortho/src/tile_cache.cpp | 423 ------------- modules/realm_stages/CMakeLists.txt | 6 +- .../include/realm_stages/stage_base.h | 21 +- .../include/realm_stages/tileing.h | 102 ++- modules/realm_stages/src/stage_base.cpp | 5 + modules/realm_stages/src/tileing.cpp | 598 +++++++++++++++--- 10 files changed, 708 insertions(+), 647 deletions(-) delete mode 100644 modules/realm_ortho/include/realm_ortho/tile_cache.h delete mode 100644 modules/realm_ortho/src/tile_cache.cpp 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 7dba88be..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 @@ -62,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 @@ -88,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 @@ -177,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_cache.h b/modules/realm_ortho/include/realm_ortho/tile_cache.h deleted file mode 100644 index 972cac10..00000000 --- a/modules/realm_ortho/include/realm_ortho/tile_cache.h +++ /dev/null @@ -1,101 +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(); - - 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; - - 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); - -}; - -} - -#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 e873eb3c..590fc225 100644 --- a/modules/realm_ortho/src/map_tiler.cpp +++ b/modules/realm_ortho/src/map_tiler.cpp @@ -121,17 +121,45 @@ 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; @@ -217,18 +245,6 @@ cv::Rect2d MapTiler::computeTileBoundsMeters(const cv::Rect2i &idx_roi, int zoom 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; - - return wgs; -} - int MapTiler::computeZoomForPixelSize(double GSD, bool do_upscale) const { for (int i = 0; i < m_zoom_level_max; ++i) diff --git a/modules/realm_ortho/src/tile_cache.cpp b/modules/realm_ortho/src/tile_cache.cpp deleted file mode 100644 index 3d73c465..00000000 --- a/modules/realm_ortho/src/tile_cache.cpp +++ /dev/null @@ -1,423 +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::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); - } - - 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/tileing.h b/modules/realm_stages/include/realm_stages/tileing.h index f669b504..7eb17600 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 { @@ -91,7 +100,7 @@ class Tileing : public StageBase 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); @@ -103,12 +112,101 @@ 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>; + + 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 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/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 4d6398ed..e38fc29f 100644 --- a/modules/realm_stages/src/tileing.cpp +++ b/modules/realm_stages/src/tileing.cpp @@ -17,10 +17,14 @@ * 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; @@ -80,9 +84,7 @@ bool Tileing::process() // 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()); @@ -190,6 +192,7 @@ bool Tileing::process() map_3857->remove("elevated"); 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) { @@ -219,6 +222,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); @@ -229,17 +233,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; @@ -290,62 +292,13 @@ Tile::Ptr Tileing::blend(const Tile::Ptr &t1, const Tile::Ptr &t2) 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() @@ -403,7 +356,7 @@ void Tileing::initStageCallback() if (!m_map_tiler) { m_map_tiler = std::make_shared(true, m_generate_tms_tiles); - m_tile_cache = std::make_shared("tile_cache", 500, m_cache_path, false); + m_tile_cache = std::make_unique(this, 500, m_cache_path, true); if (m_delete_cache_on_init) { deleteCache(); } @@ -417,32 +370,513 @@ 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) + { + 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); + + // 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); + } + } + + cache_element.second->tile->data() = nullptr; + 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::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); + } + + 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 From 844a7bf015795ac18812dfed2d3db31a23ecae6f Mon Sep 17 00:00:00 2001 From: Zach Thorson Date: Tue, 28 Sep 2021 15:34:23 -0500 Subject: [PATCH 12/14] Load tiles into tiling cache on init - Added new parameter to tiling.yaml to control the option - All tiles are placed into the cache structure, but no data is loaded by default - Right now, it expects to find color_rgb, elevation_angle, and elevation data to work properly [ES-218] --- modules/realm_io/include/realm_io/utilities.h | 2 +- modules/realm_io/src/utilities.cpp | 17 ++- .../include/realm_stages/stage_settings.h | 1 + .../include/realm_stages/tileing.h | 5 + modules/realm_stages/src/tileing.cpp | 122 +++++++++++++++++- 5 files changed, 136 insertions(+), 11 deletions(-) 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_stages/include/realm_stages/stage_settings.h b/modules/realm_stages/include/realm_stages/stage_settings.h index bb9506f9..59cb9170 100644 --- a/modules/realm_stages/include/realm_stages/stage_settings.h +++ b/modules/realm_stages/include/realm_stages/stage_settings.h @@ -136,6 +136,7 @@ class TileingSettings : public StageSettings 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 7eb17600..a45edbfd 100644 --- a/modules/realm_stages/include/realm_stages/tileing.h +++ b/modules/realm_stages/include/realm_stages/tileing.h @@ -93,6 +93,9 @@ class Tileing : public StageBase /// 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; + /// The directory to store the output map tiles in, defaults to log directory std::string m_cache_path; @@ -142,6 +145,7 @@ class Tileing : public StageBase }; using CacheElementGrid = std::map>; + using CacheElementItem = std::map; public: TileCache(Tileing *tiling_stage, double sleep_time, std::string output_directory, bool verbose); @@ -161,6 +165,7 @@ class Tileing : public StageBase void flushAll(); void loadAll(); + void loadDiskCache(); void deleteCache(); void deleteCache(std::string layer); diff --git a/modules/realm_stages/src/tileing.cpp b/modules/realm_stages/src/tileing.cpp index e38fc29f..7aca2834 100644 --- a/modules/realm_stages/src/tileing.cpp +++ b/modules/realm_stages/src/tileing.cpp @@ -35,6 +35,7 @@ Tileing::Tileing(const StageSettings::Ptr &stage_set, double rate) 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_utm_reference(nullptr), m_map_tiler(nullptr), m_tile_cache(nullptr), @@ -357,9 +358,15 @@ void Tileing::initStageCallback() { 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(); } } @@ -441,7 +448,12 @@ bool TileCache::process() for (auto &cached_elements_zoom : m_cache) { - cv::Rect2i roi_prediction = m_roi_prediction.at(cached_elements_zoom.first); + // 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) @@ -635,7 +647,6 @@ Tile::Ptr TileCache::get(int tx, int ty, int zoom_level) { load(it_tile_xy->second); } - return it_tile_xy->second->tile; } @@ -681,7 +692,10 @@ void TileCache::flushAll() } } - cache_element.second->tile->data() = nullptr; + auto layers = cache_element.second->tile->data()->getAllLayerNames(); + for (auto layer : layers) { + cache_element.second->tile->data()->remove(layer); + } cache_element.second->tile->unlock(); } @@ -716,6 +730,108 @@ void TileCache::loadAll() } } + +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; + } + + 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 From 3456423768083bb78aafcc736f43a191ef2bbadc Mon Sep 17 00:00:00 2001 From: Zach Thorson Date: Tue, 28 Sep 2021 15:36:18 -0500 Subject: [PATCH 13/14] Fix cache prediction previous request not saving - The previous roi request wasn't being saved, causing the prediction to always just use the current roi. Fixed. [ES-218] --- modules/realm_stages/src/tileing.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/modules/realm_stages/src/tileing.cpp b/modules/realm_stages/src/tileing.cpp index 7aca2834..5de403a5 100644 --- a/modules/realm_stages/src/tileing.cpp +++ b/modules/realm_stages/src/tileing.cpp @@ -986,7 +986,8 @@ void TileCache::updatePrediction(int zoom_level, const cv::Rect2i &roi_current) it_roi_prediction->second.height = roi_current.height + (roi_current.height - it_roi_prev_request->second.height); } - it_roi_prev_request->second = roi_current; + // 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) From 76374b147dda20d47a932ae6469d771f29c51803 Mon Sep 17 00:00:00 2001 From: Zach Thorson Date: Wed, 29 Sep 2021 11:48:58 -0500 Subject: [PATCH 14/14] Publish cache loaded tiles to topic on start - When loading tiles from the disk, you wouldn't get an update that new tiles were present when loading from the cache. This update publishes the topic a single time with the loaded tiles when the tiling node is first started. - We wait until the node is started to ensure all listeners are set up first. [ES-218] --- .../include/realm_stages/tileing.h | 3 +++ modules/realm_stages/src/tileing.cpp | 20 ++++++++++++++++++- 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/modules/realm_stages/include/realm_stages/tileing.h b/modules/realm_stages/include/realm_stages/tileing.h index a45edbfd..82eddfce 100644 --- a/modules/realm_stages/include/realm_stages/tileing.h +++ b/modules/realm_stages/include/realm_stages/tileing.h @@ -96,6 +96,9 @@ class Tileing : public StageBase /// 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; diff --git a/modules/realm_stages/src/tileing.cpp b/modules/realm_stages/src/tileing.cpp index 5de403a5..4b7b32eb 100644 --- a/modules/realm_stages/src/tileing.cpp +++ b/modules/realm_stages/src/tileing.cpp @@ -36,6 +36,7 @@ Tileing::Tileing(const StageSettings::Ptr &stage_set, double rate) 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), @@ -79,6 +80,16 @@ 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) { @@ -821,6 +832,14 @@ void TileCache::loadDiskCache() 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); @@ -829,7 +848,6 @@ void TileCache::loadDiskCache() } LOG_F(INFO, "Loaded %d existing tiles from cache.", element_count); - } void TileCache::deleteCache()