.. _program_listing_file_src_tuw_geometry_layered_maps.cpp: Program Listing for File layered_maps.cpp ========================================= |exhale_lsh| :ref:`Return to documentation for file ` (``src/tuw_geometry/layered_maps.cpp``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp /*************************************************************************** * Software License Agreement (BSD License) * * Copyright (C) 2016 by Horatiu George Todoran * * * * Redistribution and use in source and binary forms, with or without * * modification, are permitted provided that the following conditions * * are met: * * * * 1. Redistributions of source code must retain the above copyright * * notice, this list of conditions and the following disclaimer. * * 2. Redistributions in binary form must reproduce the above copyright * * notice, this list of conditions and the following disclaimer in * * the documentation and/or other materials provided with the * * distribution. * * 3. Neither the name of the copyright holder nor the names of its * * contributors may be used to endorse or promote products derived * * from this software without specific prior written permission. * * * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY * * WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * * POSSIBILITY OF SUCH DAMAGE. * ***************************************************************************/ #include #include #include #include using namespace tuw; using namespace std; using namespace cv; LayeredMaps::LayeredMaps() {} void LayeredMaps::initLayers( int width_pixel, int height_pixel, double min_x, double max_x, double min_y, double max_y, double rotation) { init(width_pixel, height_pixel, min_x, max_x, min_y, max_y, rotation); initLayers(); } size_t LayeredMaps::sizeLayers() const {return mapLayers_.size();} void LayeredMaps::resizeLayers(const size_t & _n) {mapLayers_.resize(_n);} void LayeredMaps::initLayers() { for (size_t i = 0; i < mapLayers_.size(); i++) { mapLayers_[i].create(width(), height(), CV_32FC1); clearLayer(i); } } void LayeredMaps::clearLayers() { for (size_t i = 0; i < mapLayers_.size(); i++) { clearLayer(i); } } void LayeredMaps::clearLayer(const size_t & _layer) {mapLayers_[_layer].setTo(1);} cv::Mat & LayeredMaps::mapLayer(const size_t & _layer) {return mapLayers_[_layer];} const cv::Mat & LayeredMaps::mapLayer(const size_t & _layer) const {return mapLayers_[_layer];} double LayeredMaps::getVal( const size_t & _layer, const Point2D & _worldPos, Interpolation _interpolationType) const { double retVal = -100; Point2D mapPos = w2m(_worldPos); if (!mapPos.inside(0, 0, mapLayer(_layer).rows - 1, mapLayer(_layer).cols - 1)) { return retVal; } const double mapPos_row_d = mapPos.y(), mapPos_col_d = mapPos.x(); const int mapPos_row_i = int(mapPos_row_d), mapPos_col_i = int(mapPos_col_d); switch (_interpolationType) { case SIMPLE: retVal = mapLayer(_layer).at(mapPos_row_i, mapPos_col_i); break; case BILINEAR: //if((mapPos_y_i+1 >= mapLayer(layer).rows)||(mapPos_x_i+1 >= mapLayer(layer).cols)||(mapPos_y_i <0)||(mapPos_x_i <0)){throw 0;} const double f00 = mapLayer(_layer).at(mapPos_row_i, mapPos_col_i); const double f10 = mapLayer(_layer).at(mapPos_row_i + 1, mapPos_col_i); const double f01 = mapLayer(_layer).at(mapPos_row_i, mapPos_col_i + 1); const double f11 = mapLayer(_layer).at(mapPos_row_i + 1, mapPos_col_i + 1); const double mapPosRed_row = mapPos_row_d - mapPos_row_i, mapPosRed_col = mapPos_col_d - mapPos_col_i; retVal = f00 * (1 - mapPosRed_row) * (1 - mapPosRed_col) + f10 * mapPosRed_row * (1 - mapPosRed_col) + f01 * (1 - mapPosRed_row) * mapPosRed_col + f11 * mapPosRed_row * mapPosRed_col; break; } return retVal; } float distance2probabilitySigmoid(float d, float threshold) { float p = 0; if (d < threshold) { p = tanh((2 * M_PI * -d) / threshold + M_PI) * 0.5 + 0.5; } return p; } float distance2probabilityTriangle(float d, float threshold) { float p = 0; if (d < threshold) { p = 1. - d / threshold; } return p; } void LayeredMaps::computeDistanceField( Mat & _mDst, vector & _pSrc, const double & _radius, bool _flipDistance, bool connectPoints) const { Mat srcMap; srcMap.create(width(), height(), CV_32FC1); srcMap.setTo(1); Scalar colour(0); if (connectPoints) { for (size_t i = 1; i < _pSrc.size(); i++) { line(srcMap, _pSrc[i - 1], _pSrc[i], colour, 8); } } else { for (size_t i = 0; i < _pSrc.size(); i++) { circle(srcMap, _pSrc[i], 1, colour, 1, 8); } } computeDistanceField(_mDst, srcMap, _radius, _flipDistance); } void LayeredMaps::computeDistanceField( Mat & _mDst, Mat & _mSrc, const double & _radius, bool _flipDistance) const { Mat srcMap; _mSrc.convertTo(srcMap, CV_8U); int maskSize0 = cv::DIST_MASK_PRECISE; int voronoiType = -1; int distType0 = cv::DIST_L2; //CV_DIST_L1; int maskSize = voronoiType >= 0 ? cv::DIST_MASK_5 : maskSize0; int distType = voronoiType >= 0 ? cv::DIST_L2 : distType0; Mat destMap_f, labels; // destMap_f.create ( width(), height(), CV_32F ); if (voronoiType < 0) { cv::distanceTransform(srcMap, destMap_f, distType, maskSize); } else { cv::distanceTransform(srcMap, destMap_f, labels, distType, maskSize, voronoiType); } // destMap_f.convertTo(destMap_ui, CV_8U ); CV_Assert(destMap_f.depth() == CV_32F); int channels = destMap_f.channels(); int nRows = destMap_f.rows; int nCols = destMap_f.cols * channels; if (destMap_f.isContinuous()) { nCols *= nRows; nRows = 1; } float threshold = _radius * scale_x(); float_t * p_d; for (int i = 0; i < nRows; ++i) { p_d = destMap_f.ptr(i); for (int j = 0; j < nCols; ++j) { if (_flipDistance) { p_d[j] = (float_t)(distance2probabilityTriangle(p_d[j], threshold)); } else { p_d[j] = (float_t)((1. - distance2probabilityTriangle(p_d[j], threshold))); } } } if ((_mDst.channels() != 1) && (_mDst.channels() != 5)) { cvtColor(destMap_f, _mDst, cv::COLOR_GRAY2BGR); } else { _mDst = destMap_f; } }