Program Listing for File layered_maps.cpp
↰ Return to documentation for file (src/tuw_geometry/layered_maps.cpp)
/***************************************************************************
* Software License Agreement (BSD License) *
* Copyright (C) 2016 by Horatiu George Todoran <todorangrg@gmail.com> *
* *
* 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 <opencv2/core/core_c.h>
#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
#include <tuw_geometry/layered_maps.hpp>
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<float_t>(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<float_t>(mapPos_row_i, mapPos_col_i);
const double f10 = mapLayer(_layer).at<float_t>(mapPos_row_i + 1, mapPos_col_i);
const double f01 = mapLayer(_layer).at<float_t>(mapPos_row_i, mapPos_col_i + 1);
const double f11 = mapLayer(_layer).at<float_t>(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<Point2D> & _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<float_t>(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;
}
}