.. _program_listing_file__tmp_ws_src_grid_map_grid_map_sdf_include_grid_map_sdf_distance_transform_dt.hpp: Program Listing for File dt.hpp =============================== |exhale_lsh| :ref:`Return to documentation for file ` (``/tmp/ws/src/grid_map/grid_map_sdf/include/grid_map_sdf/distance_transform/dt.hpp``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp /* Copyright (C) 2006 Pedro Felzenszwalb This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ /* distance transform */ #ifndef GRID_MAP_SDF__DISTANCE_TRANSFORM__DT_HPP_ #define GRID_MAP_SDF__DISTANCE_TRANSFORM__DT_HPP_ #include #include "grid_map_sdf/distance_transform/image.hpp" #include "grid_map_sdf/distance_transform/imconv.hpp" namespace distance_transform { const double INF = 1E20; /* dt of 1d function using squared distance */ static float * dt(float * f, int n) { float * d = new float[n]; int * v = new int[n]; float * z = new float[n + 1]; int k = 0; v[0] = 0; z[0] = -INF; z[1] = +INF; for (int q = 1; q <= n - 1; q++) { float s = ((f[q] + square(q)) - (f[v[k]] + square(v[k]))) / (2 * q - 2 * v[k]); while (s <= z[k]) { k--; s = ((f[q] + square(q)) - (f[v[k]] + square(v[k]))) / (2 * q - 2 * v[k]); } k++; v[k] = q; z[k] = s; z[k + 1] = +INF; } k = 0; for (int q = 0; q <= n - 1; q++) { while (z[k + 1] < q) { k++; } d[q] = square(q - v[k]) + f[v[k]]; } delete[] v; delete[] z; return d; } /* dt of 2d function using squared distance */ static void dt(image * im) { int width = im->width(); int height = im->height(); float * f = new float[std::max(width, height)]; // transform along columns for (int x = 0; x < width; x++) { for (int y = 0; y < height; y++) { f[y] = imRef(im, x, y); } float * d = dt(f, height); for (int y = 0; y < height; y++) { imRef(im, x, y) = d[y]; } delete[] d; } // transform along rows for (int y = 0; y < height; y++) { for (int x = 0; x < width; x++) { f[x] = imRef(im, x, y); } float * d = dt(f, width); for (int x = 0; x < width; x++) { imRef(im, x, y) = d[x]; } delete[] d; } delete[] f; } /* dt of binary image using squared distance */ static image * dt(image * im, uchar on = 1) { int width = im->width(); int height = im->height(); image * out = new image(width, height, false); for (int y = 0; y < height; y++) { for (int x = 0; x < width; x++) { if (imRef(im, x, y) == on) { imRef(out, x, y) = 0; } else { imRef(out, x, y) = INF; } } } dt(out); return out; } } // namespace distance_transform #endif // GRID_MAP_SDF__DISTANCE_TRANSFORM__DT_HPP_