| 1 |
/* +------------------------------------------------------------------------+ |
| 2 |
| Mobile Robot Programming Toolkit (MRPT) | |
| 3 |
| https://www.mrpt.org/ | |
| 4 |
| | |
| 5 |
| Copyright (c) 2005-2026, Individual contributors, see AUTHORS file | |
| 6 |
| See: https://www.mrpt.org/Authors - All rights reserved. | |
| 7 |
| Released under BSD License. See: https://www.mrpt.org/License | |
| 8 |
+------------------------------------------------------------------------+ */ |
| 9 |
|
| 10 |
#include "img-precomp.h" // Precompiled headers |
| 11 |
// |
| 12 |
#include <mrpt/core/round.h> |
| 13 |
#include <mrpt/img/CMappedImage.h> |
| 14 |
|
| 15 |
using namespace mrpt; |
| 16 |
using namespace mrpt::img; |
| 17 |
using namespace mrpt::math; |
| 18 |
|
| 19 |
/*--------------------------------------------------------------- |
| 20 |
Constructor |
| 21 |
---------------------------------------------------------------*/ |
| 22 |
CMappedImage::CMappedImage( |
| 23 |
CImage::Ptr img, double x0, double x1, double y0, double y1, TInterpolationMethod method) : |
| 24 |
m_img(img), m_x0(x0), m_x1(x1), m_y0(y0), m_y1(y1), m_pixel_size(0), m_method(method) |
| 25 |
{ |
| 26 |
if (m_img->isColor()) |
| 27 |
{ |
| 28 |
auto new_img = CImage::Create(); |
| 29 |
m_img->grayscale(*new_img); |
| 30 |
m_img = new_img; |
| 31 |
} |
| 32 |
changeCoordinates(x0, x1, y0, y1); |
| 33 |
} |
| 34 |
|
| 35 |
/*--------------------------------------------------------------- |
| 36 |
changeCoordinates |
| 37 |
---------------------------------------------------------------*/ |
| 38 |
void CMappedImage::changeCoordinates(double x0, double x1, double y0, double y1) |
| 39 |
{ |
| 40 |
MRPT_START |
| 41 |
ASSERT_(x0 != x1); |
| 42 |
ASSERT_(y0 != y1); |
| 43 |
|
| 44 |
m_x0 = x0; |
| 45 |
m_x1 = x1; |
| 46 |
m_y0 = y0; |
| 47 |
m_y1 = y1; |
| 48 |
|
| 49 |
if (y1 < 0 || x1 < 0) |
| 50 |
{ |
| 51 |
m_x1 = m_img->getWidth() - 1; |
| 52 |
m_y1 = m_img->getHeight() - 1; |
| 53 |
} |
| 54 |
|
| 55 |
ASSERT_(m_img->getWidth() > 0 && m_img->getHeight()); |
| 56 |
|
| 57 |
m_pixel_size = (m_x1 - m_x0) / m_img->getWidth(); |
| 58 |
|
| 59 |
MRPT_END |
| 60 |
} |
| 61 |
|
| 62 |
/*--------------------------------------------------------------- |
| 63 |
getPixel |
| 64 |
---------------------------------------------------------------*/ |
| 65 |
double CMappedImage::getPixel(double x, double y) const |
| 66 |
{ |
| 67 |
// Image size: |
| 68 |
const size_t W = m_img->getWidth(); |
| 69 |
const size_t H = m_img->getHeight(); |
| 70 |
|
| 71 |
// the sub-pixel pixel coordinates: |
| 72 |
const double px = (x - m_x0) / m_pixel_size; |
| 73 |
const double py = (y - m_y0) / m_pixel_size; |
| 74 |
|
| 75 |
if (px < 0 || py < 0 || px > W || py > H) |
| 76 |
{ |
| 77 |
return 0; |
| 78 |
} // Out of image |
| 79 |
|
| 80 |
switch (m_method) |
| 81 |
{ |
| 82 |
case IMG_INTERP_NN: |
| 83 |
{ |
| 84 |
// The closest pixel: |
| 85 |
const unsigned int px0 = mrpt::round(px); |
| 86 |
const unsigned int py0 = mrpt::round(py); |
| 87 |
return static_cast<double>((*m_img).at<uint8_t>(px0, py0)); |
| 88 |
} |
| 89 |
break; |
| 90 |
case IMG_INTERP_LINEAR: |
| 91 |
{ |
| 92 |
// See: http://en.wikipedia.org/wiki/Bilinear_interpolation |
| 93 |
|
| 94 |
// The four pixels around: |
| 95 |
const int px0 = (int)floor(px); |
| 96 |
const int px1 = (int)ceil(px); |
| 97 |
const int py0 = (int)floor(py); |
| 98 |
const int py1 = (int)ceil(py); |
| 99 |
|
| 100 |
const auto P11 = static_cast<double>((*m_img).at<uint8_t>(px0, py0)); |
| 101 |
const auto P12 = static_cast<double>((*m_img).at<uint8_t>(px0, py1)); |
| 102 |
const auto P21 = static_cast<double>((*m_img).at<uint8_t>(px1, py0)); |
| 103 |
const auto P22 = static_cast<double>((*m_img).at<uint8_t>(px1, py1)); |
| 104 |
|
| 105 |
const double R1 = P11 * (px1 - px) /* /(px1-px0)*/ + P21 * (px - px0) /* /(px1-px0) */; |
| 106 |
const double R2 = P12 * (px1 - px) /* /(px1-px0)*/ + P22 * (px - px0) /* /(px1-px0) */; |
| 107 |
|
| 108 |
return R1 * (py1 - py) + R2 * (py - py0); |
| 109 |
} |
| 110 |
break; |
| 111 |
|
| 112 |
case IMG_INTERP_CUBIC: |
| 113 |
case IMG_INTERP_AREA: |
| 114 |
default: |
| 115 |
THROW_EXCEPTION( |
| 116 |
"The selected interpolation method is not supported in this " |
| 117 |
"method."); |
| 118 |
}; |
| 119 |
} |