32 #include <mrpt/3rdparty/do_opencv_includes.h> 37 #include <mexplus/mxarray.h> 75 :
std::runtime_error(s)
86 #define IMAGE_ALLOC_PERFLOG 0 88 #if IMAGE_ALLOC_PERFLOG 107 template <
typename RET = u
int32_t>
122 return std::numeric_limits<RET>::max();
124 template <
typename RET = u
int32_t>
139 return std::numeric_limits<RET>::max();
159 #endif // MRPT_HAS_OPENCV 166 unsigned int width,
unsigned int height,
TImageChannels nChannels)
170 resize(width, height, nChannels);
192 m_impl->img = img.clone();
202 CImage(img.m_impl->img, copy_type)
216 THROW_EXCEPTION(
"Operation not supported: build MRPT against OpenCV!");
224 out_img =
m_impl->img.clone();
236 THROW_EXCEPTION(
"Operation not supported: build MRPT against OpenCV!");
246 THROW_EXCEPTION(
"Operation not supported: build MRPT against OpenCV!");
262 if (static_cast<unsigned>(
m_impl->img.cols) == width &&
263 static_cast<unsigned>(
m_impl->img.rows) == height &&
264 m_impl->img.channels() == nChannels &&
271 #if IMAGE_ALLOC_PERFLOG 272 const std::string sLog =
mrpt::format(
"cvCreateImage %ux%u", width, height);
273 alloc_tims.enter(sLog.c_str());
280 static_cast<int>(height), static_cast<int>(width),
281 pixelDepth2CvDepth<int>(depth) + ((nChannels - 1) << CV_CN_SHIFT));
283 #if IMAGE_ALLOC_PERFLOG 284 alloc_tims.leave(sLog.c_str());
288 THROW_EXCEPTION(
"The MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
299 THROW_EXCEPTION(
"The MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
310 #ifdef HAVE_OPENCV_IMGCODECS 312 MRPT_TODO(
"add flag to reuse current img buffer");
314 m_impl->img = cv::imread(fileName, static_cast<cv::ImreadModes>(
isColor));
317 if (!newImg)
return false;
318 m_impl->img = cv::cvarrToMat(newImg);
320 if (
m_impl->img.empty())
return false;
324 THROW_EXCEPTION(
"The MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
336 #ifdef HAVE_OPENCV_IMGCODECS 337 const std::vector<int>
params = {cv::IMWRITE_JPEG_QUALITY, jpeg_quality};
340 int p[3] = {CV_IMWRITE_JPEG_QUALITY, jpeg_quality, 0};
341 _IplImage ipl =
m_impl->img;
342 return (0 != cvSaveImage(fileName.c_str(), &ipl, p));
345 THROW_EXCEPTION(
"The MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
357 cv::cvarrToMat(iplImage, c ==
DEEP_COPY ?
true :
false );
359 THROW_EXCEPTION(
"The MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
365 unsigned int width,
unsigned int height,
bool color,
366 unsigned char* rawpixels,
bool swapRedBlue)
374 auto* imgData =
m_impl->img.data;
375 const auto imgWidthStep =
m_impl->img.step[0];
377 if (color && swapRedBlue)
380 unsigned char* ptr_src = rawpixels;
381 auto* ptr_dest = imgData;
382 const int bytes_per_row_out = imgWidthStep;
384 for (
int h = height; h--;)
386 for (
unsigned int i = 0; i < width;
387 i++, ptr_src += 3, ptr_dest += 3)
389 unsigned char t0 = ptr_src[0], t1 = ptr_src[1], t2 = ptr_src[2];
394 ptr_dest += bytes_per_row_out - width * 3;
400 static_cast<size_t>(
m_impl->img.cols *
m_impl->img.channels()))
408 unsigned char* ptr_src = rawpixels;
409 auto* ptr_dest = imgData;
410 int bytes_per_row = width * (color ? 3 : 1);
411 int bytes_per_row_out = imgWidthStep;
412 for (
unsigned int y = 0; y < height; y++)
414 memcpy(ptr_dest, ptr_src, bytes_per_row);
415 ptr_src += bytes_per_row;
416 ptr_dest += bytes_per_row_out;
421 THROW_EXCEPTION(
"The MRPT has been compiled with MRPT_HAS_OPENCV=0 !");
427 unsigned int ucol,
unsigned int urow,
unsigned int uchannel)
const 431 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG) 436 const auto col =
static_cast<int>(ucol);
437 const auto row =
static_cast<int>(urow);
438 const auto channel =
static_cast<int>(uchannel);
440 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG) 442 if (row >=
m_impl->img.rows || col >=
m_impl->img.cols ||
443 channel >=
m_impl->img.channels())
446 "Pixel coordinates/channel out of bounds: row=%u/%u col=%u/%u " 453 (&
m_impl->img.at<uint8_t>(row,
m_impl->img.channels() * col)) + channel;
454 return const_cast<unsigned char*
>(p);
455 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG) 469 (&
m_impl->img.at<uint8_t>(row,
m_impl->img.channels() * col)) + channel;
470 return const_cast<uint8_t*
>(p);
477 unsigned int col,
unsigned int row, uint8_t channel)
const 509 const bool hasColor =
m_impl->img.empty() ? false :
isColor();
514 const int32_t width =
m_impl->img.cols;
515 const int32_t height =
m_impl->img.rows;
522 uint32_t imageSize = height *
m_impl->img.step[0];
524 int32_t depth =
m_impl->img.depth();
526 out << width << height << origin << imageSize
533 bool imageStoredAsZip =
false;
535 out << imageStoredAsZip;
537 if (imageSize > 0 &&
m_impl->img.data !=
nullptr)
538 out.WriteBuffer(
m_impl->img.data, imageSize);
551 out << width << height;
553 if (width >= 1 && height >= 1)
570 const int32_t neg_width = -width;
571 const int32_t neg_height = -height;
573 out << neg_width << neg_height;
576 const auto bytes_per_row = width * 3;
578 out.WriteBuffer(
m_impl->img.data, bytes_per_row * height);
596 "[CImage] Cannot deserialize image since MRPT has been " 597 "compiled without OpenCV");
614 uint32_t width, height, nChannels, imgLength;
615 uint8_t originTopLeft;
617 in >> width >> height >> nChannels >> originTopLeft >> imgLength;
619 resize(width, height, static_cast<TImageChannels>(nChannels));
628 std::vector<uint8_t> buf(nBytes);
666 int32_t width, height, origin, imageSize;
667 in >> width >> height >> origin >> imageSize;
676 static_cast<uint32_t>(width),
677 static_cast<uint32_t>(height),
CH_GRAY, depth);
679 static_cast<uint32_t>(imageSize),
680 static_cast<uint32_t>(height) *
m_impl->img.step[0]);
690 bool imageIsZIP =
true;
694 if (version == 4 && imageSize <= 16 * 1024)
708 "ZIP image deserialization not supported " 721 bool loadJPEG =
true;
725 int32_t width, height;
726 in >> width >> height;
728 if (width >= 1 && height >= 1)
736 if (width < 0 && height < 0)
739 const int32_t real_w = -width;
740 const int32_t real_h = -height;
745 const size_t bytes_per_row = img.cols * 3;
746 for (
int y = 0; y < img.rows; y++)
749 img.ptr<
void>(y), bytes_per_row);
750 if (nRead != bytes_per_row)
752 "Error: Truncated data stream " 753 "while parsing raw image?");
771 std::vector<uint8_t> buf(nBytes);
835 const int chCount =
m_impl->img.channels();
838 const std::array<const char*, 4> orderNames = {
"GRAY",
"",
"BGR",
"BGRA"};
839 return std::string(orderNames.at(chCount - 1));
849 return m_impl->img.step[0];
869 return m_impl->img.channels() == 3;
900 unsigned int col,
unsigned int row,
unsigned int channel)
const 904 return (*(*
this)(col, row, channel)) / 255.0f;
913 unsigned char* pixels = (*this)(col, row, 0);
914 return (pixels[0] * 0.3f + pixels[1] * 0.59f + pixels[2] * 0.11f) /
920 return (*(*
this)(col, row, 0 )) / 255.0f;
933 for (x = 0; x < cx; x++)
934 for (y = 0; y < cy; y++) maxPixel = max(maxPixel,
getAsFloat(x, y));
950 if (dest.size() != src.size() || dest.type() != src.type())
951 dest = cv::Mat(src.rows, src.cols, CV_8UC1);
954 #if MRPT_ARCH_INTEL_COMPATIBLE 955 if ((src.step[0] & 0x0f) == 0 && (dest.step[0] & 0x0f) == 0 &&
959 src.ptr<uint8_t>(), dest.ptr<uint8_t>(), src.cols, src.rows,
960 src.step[0], dest.step[0]);
966 cv::cvtColor(src, dest, CV_BGR2GRAY);
976 if (
m_impl->img.channels() == 1)
984 cv::Mat src =
m_impl->img;
986 if (src.data == ret.
m_impl->img.data) src = src.clone();
991 THROW_EXCEPTION(
"Operation not supported: build MRPT against OpenCV!");
1001 const int w = img.cols, h = img.rows;
1005 auto& img_out =
out.m_impl->img;
1008 #if MRPT_ARCH_INTEL_COMPATIBLE 1013 img.data, img_out.data, w, h, img.step[0], img_out.step[0]);
1022 img.data, img_out.data, w, h, img.step[0], img_out.step[0]);
1028 img.data, img_out.data, w, h, img.step[0], img_out.step[0]);
1039 THROW_EXCEPTION(
"Operation not supported: build MRPT against OpenCV!");
1051 unsigned int width,
unsigned int height,
unsigned int bytesPerRow,
1052 unsigned char* red,
unsigned char* green,
unsigned char* blue)
1060 for (
unsigned int y = 0; y < height; y++)
1063 auto* dest =
m_impl->img.ptr<uint8_t>(y);
1066 unsigned char* srcR = red + bytesPerRow * y;
1067 unsigned char* srcG = green + bytesPerRow * y;
1068 unsigned char* srcB = blue + bytesPerRow * y;
1070 for (
unsigned int x = 0; x < width; x++)
1072 *(dest++) = *(srcB++);
1073 *(dest++) = *(srcG++);
1074 *(dest++) = *(srcR++);
1086 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG) 1095 if (x >= 0 && y >= 0 && y < img.rows && x < img.cols)
1098 if (img.channels() == 1)
1100 img.ptr<uint8_t>(y)[x] = static_cast<uint8_t>(color);
1104 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG) 1107 auto* dest = &img.ptr<uint8_t>(y)[3 * x];
1108 const auto* src =
reinterpret_cast<uint8_t*
>(&color);
1116 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG) 1125 unsigned int width, [[maybe_unused]]
TPenStyle penStyle)
1131 m_impl->img, cv::Point(x0, y0), cv::Point(x1, y1),
1132 CV_RGB(color.
R, color.
G, color.
B), static_cast<int>(width));
1143 m_impl->img, cv::Point(x, y), radius, CV_RGB(color.
R, color.
G, color.
B),
1144 static_cast<int>(width));
1155 cv::Mat dest =
m_impl->img(roi);
1156 img.
m_impl->img.copyTo(dest);
1161 const CImage& patch,
const unsigned int col_,
const unsigned int row_)
1165 const auto& src =
m_impl->img;
1166 auto& dest = patch.
m_impl->img;
1168 src(cv::Rect(col_, row_, dest.cols, dest.rows)).copyTo(dest);
1173 CImage& patch,
const unsigned int col_,
const unsigned int row_,
1174 const unsigned int col_num,
const unsigned int row_num)
const 1178 const auto& src =
m_impl->img;
1179 auto& dest = patch.
m_impl->img;
1181 src(cv::Rect(col_, row_, col_num, row_num)).copyTo(dest);
1186 const CImage& img2,
int width_init,
int height_init)
const 1193 THROW_EXCEPTION(
"Correlation Error!, image to correlate out of bounds");
1196 float syy = 0.0f, sxy = 0.0f, sxx = 0.0f, m1 = 0.0f, m2 = 0.0f,
1200 for (
size_t i = 0; i < img2.
getHeight(); i++)
1202 for (
size_t j = 0; j < img2.
getWidth(); j++)
1215 for (
size_t i = 0; i < img2.
getHeight(); i++)
1217 for (
size_t j = 0; j < img2.
getWidth(); j++)
1219 x1 = *(*this)(j + width_init, i + height_init) -
1222 x2 = *img2(j, i) - m2;
1230 return sxy / sqrt(sxx * syy);
1245 CMatrixFloat& outMatrix,
bool doResize,
int x_min,
int y_min,
int x_max,
1246 int y_max,
bool normalize_01)
const 1252 const auto& img =
m_impl->img;
1255 if (x_max == -1) x_max = img.cols - 1;
1256 if (y_max == -1) y_max = img.rows - 1;
1258 ASSERT_(x_min >= 0 && x_min < img.cols && x_min < x_max);
1259 ASSERT_(y_min >= 0 && y_min < img.rows && y_min < y_max);
1261 int lx = (x_max - x_min + 1);
1262 int ly = (y_max - y_min + 1);
1264 if (doResize || outMatrix.
rows() < ly || outMatrix.
cols() < lx)
1265 outMatrix.
setSize(y_max - y_min + 1, x_max - x_min + 1);
1267 const bool is_color =
isColor();
1270 for (
int y = 0; y < ly; y++)
1272 const uint8_t* pixels = ptr<uint8_t>(x_min, y_min + y);
1273 for (
int x = 0; x < lx; x++)
1278 aux = *pixels++ * 0.3f;
1279 aux += *pixels++ * 0.59f;
1280 aux += *pixels++ * 0.11f;
1289 if (normalize_01) outMatrix *= (1.0f / 255);
1296 CMatrix_u8& outMatrix,
bool doResize,
int x_min,
int y_min,
int x_max,
1303 const auto& img =
m_impl->img;
1306 if (x_max == -1) x_max = img.cols - 1;
1307 if (y_max == -1) y_max = img.rows - 1;
1309 ASSERT_(x_min >= 0 && x_min < img.cols && x_min < x_max);
1310 ASSERT_(y_min >= 0 && y_min < img.rows && y_min < y_max);
1312 int lx = (x_max - x_min + 1);
1313 int ly = (y_max - y_min + 1);
1315 if (doResize || outMatrix.
rows() < ly || outMatrix.
cols() < lx)
1316 outMatrix.
setSize(y_max - y_min + 1, x_max - x_min + 1);
1318 const bool is_color =
isColor();
1321 for (
int y = 0; y < ly; y++)
1323 const uint8_t* pixels = ptr<uint8_t>(x_min, y_min + y);
1324 for (
int x = 0; x < lx; x++)
1328 unsigned int aux = *pixels++ * 3000;
1329 aux += *pixels++ * 5900;
1330 aux += *pixels++ * 1100;
1331 outMatrix.
coeffRef(y, x) =
static_cast<uint8_t
>(aux / 1000);
1335 outMatrix.
coeffRef(y, x) = (*pixels++);
1352 const auto& img =
m_impl->img;
1355 if (x_max == -1) x_max = img.cols - 1;
1356 if (y_max == -1) y_max = img.rows - 1;
1358 ASSERT_(x_min >= 0 && x_min < img.cols && x_min < x_max);
1359 ASSERT_(y_min >= 0 && y_min < img.rows && y_min < y_max);
1361 int lx = (x_max - x_min + 1);
1362 int ly = (y_max - y_min + 1);
1364 if (doResize ||
R.rows() < ly ||
R.cols() < lx)
R.setSize(ly, lx);
1365 if (doResize ||
G.rows() < ly ||
G.cols() < lx)
G.setSize(ly, lx);
1368 const bool is_color =
isColor();
1369 for (
int y = 0; y < ly; y++)
1371 const uint8_t* pixels = ptr<uint8_t>(x_min, y_min + y);
1372 for (
int x = 0; x < lx; x++)
1376 R.coeffRef(y, x) =
u8tof(*pixels++);
1377 G.coeffRef(y, x) =
u8tof(*pixels++);
1382 R.coeffRef(y, x) =
G.coeffRef(y, x) = B.
coeffRef(y, x) =
1401 const auto& img =
m_impl->img;
1404 if (x_max == -1) x_max = img.cols - 1;
1405 if (y_max == -1) y_max = img.rows - 1;
1407 ASSERT_(x_min >= 0 && x_min < img.cols && x_min < x_max);
1408 ASSERT_(y_min >= 0 && y_min < img.rows && y_min < y_max);
1410 int lx = (x_max - x_min + 1);
1411 int ly = (y_max - y_min + 1);
1413 if (doResize ||
R.rows() < ly ||
R.cols() < lx)
R.setSize(ly, lx);
1414 if (doResize ||
G.rows() < ly ||
G.cols() < lx)
G.setSize(ly, lx);
1417 const bool is_color =
isColor();
1418 for (
int y = 0; y < ly; y++)
1420 const uint8_t* pixels = ptr<uint8_t>(x_min, y_min + y);
1421 for (
int x = 0; x < lx; x++)
1425 R.coeffRef(y, x) = *pixels++;
1426 G.coeffRef(y, x) = *pixels++;
1431 R.coeffRef(y, x) =
G.coeffRef(y, x) = B.
coeffRef(y, x) =
1443 int v_search_ini,
int u_search_size,
int v_search_size,
float biasThisImg,
1444 float biasInImg)
const 1451 if (u_search_ini == -1) u_search_ini = 0;
1452 if (v_search_ini == -1) v_search_ini = 0;
1453 if (u_search_size == -1) u_search_size =
static_cast<int>(
getWidth());
1454 if (v_search_size == -1) v_search_size =
static_cast<int>(
getHeight());
1456 int u_search_end = u_search_ini + u_search_size - 1;
1457 int v_search_end = v_search_ini + v_search_size - 1;
1465 std::max(static_cast<size_t>(u_search_size), in_img.
getWidth());
1467 std::max(static_cast<size_t>(v_search_size), in_img.
getHeight());
1468 size_t lx = mrpt::round2up<size_t>(actual_lx);
1469 size_t ly = mrpt::round2up<size_t>(actual_ly);
1477 i2.fill(biasThisImg);
1481 i2,
false, u_search_ini, v_search_ini, u_search_ini + u_search_size - 1,
1482 v_search_ini + v_search_size - 1);
1490 CMatrixF I1_R, I1_I, I2_R, I2_I, ZEROS(ly, lx);
1495 for (y = 0; y < ly; y++)
1496 for (x = 0; x < lx; x++)
1498 float r1 = I1_R(y, x);
1499 float r2 = I2_R(y, x);
1501 float ii1 = I1_I(y, x);
1502 float ii2 = I2_I(y, x);
1505 I2_R(y, x) = (r1 * r2 + ii1 * ii2) / den;
1506 I2_I(y, x) = (ii2 * r1 - r2 * ii1) / den;
1513 out_corr.
setSize(actual_ly, actual_lx);
1514 for (y = 0; y < actual_ly; y++)
1515 for (x = 0; x < actual_lx; x++)
1516 out_corr(y, x) = sqrt(
square(res_R(y, x)) +
square(res_I(y, x)));
1527 const auto& img =
m_impl->img;
1530 const auto matrix_lx = outMatrix.
cols();
1531 const auto matrix_ly = outMatrix.
rows();
1538 unsigned char* min_pixels = (*this)(0, y % img.rows, 0);
1539 unsigned char* max_pixels = min_pixels + img.cols * 3;
1540 unsigned char* pixels = min_pixels;
1544 aux = *pixels++ * 0.30f;
1545 aux += *pixels++ * 0.59f;
1546 aux += *pixels++ * 0.11f;
1547 outMatrix(y, x) = aux;
1548 if (pixels >= max_pixels) pixels = min_pixels;
1556 unsigned char* min_pixels = (*this)(0, y % img.rows, 0);
1557 unsigned char* max_pixels = min_pixels + img.
cols;
1558 unsigned char* pixels = min_pixels;
1561 outMatrix(y, x) = *pixels++;
1562 if (pixels >= max_pixels) pixels = min_pixels;
1580 m_externalFile = fileName;
1581 m_imgIsExternalStorage =
true;
1594 if (!
m_impl->img.empty())
return;
1615 "Error loading externally-stored image from: %s",
1621 "Trying to access uninitialized image in a non " 1622 "externally-stored " 1669 cv::cvtColor(
m_impl->img,
m_impl->img, cv::COLOR_RGB2BGR);
1678 auto& srcImg =
m_impl->img;
1679 cv::Mat outImg(srcImg.rows, srcImg.cols, srcImg.type());
1681 auto mapXm =
static_cast<cv::Mat*
>(mapX);
1682 auto mapYm =
static_cast<cv::Mat*
>(mapX);
1684 cv::remap(srcImg, outImg, *mapXm, *mapYm, cv::INTER_CUBIC);
1699 "In-place undistort() not supported");
1701 auto& srcImg =
const_cast<cv::Mat&
>(
m_impl->img);
1706 const auto& dist = cameraParams.
dist;
1708 cv::Mat distM(1, dist.size(), CV_64F,
const_cast<double*
>(&dist[0]));
1709 cv::Mat inMat(3, 3, CV_64F);
1711 for (
int i = 0; i < 3; i++)
1712 for (
int j = 0; j < 3; j++) inMat.at<
double>(i, j) = intrMat(i, j);
1714 cv::undistort(srcImg, out_img.
m_impl->img, inMat, distM);
1723 auto srcImg =
const_cast<cv::Mat&
>(
m_impl->img);
1724 if (
this == &out_img)
1725 srcImg = srcImg.clone();
1729 cv::medianBlur(srcImg, out_img.
m_impl->img, W);
1737 auto srcImg =
const_cast<cv::Mat&
>(
m_impl->img);
1738 if (
this == &out_img)
1739 srcImg = srcImg.clone();
1743 cv::GaussianBlur(srcImg, out_img.
m_impl->img, cv::Size(W, H), sigma);
1748 CImage& out_img,
unsigned int width,
unsigned int height,
1754 auto srcImg =
m_impl->img;
1756 if (out_img.
m_impl->img.data == srcImg.data) srcImg = srcImg.clone();
1761 out_img.
m_impl->img = srcImg;
1768 srcImg, out_img.
m_impl->img, out_img.
m_impl->img.size(), 0, 0,
1774 CImage& out_img,
double ang,
unsigned int cx,
unsigned int cy,
1780 auto srcImg =
m_impl->img;
1782 if (out_img.
m_impl->img.data == srcImg.data) srcImg = srcImg.clone();
1790 double m[2 * 3] = {scale * cos(ang), -scale * sin(ang), 1.0 * cx,
1791 scale * sin(ang), scale * cos(ang), 1.0 * cy};
1792 cv::Mat M(2, 3, CV_64F, m);
1794 double dx = (srcImg.cols - 1) * 0.5;
1795 double dy = (srcImg.rows - 1) * 0.5;
1796 m[2] -= m[0] * dx + m[1] * dy;
1797 m[5] -= m[3] * dx + m[4] * dy;
1800 srcImg, out_img.
m_impl->img, M, out_img.
m_impl->img.size(),
1801 cv::INTER_LINEAR + cv::WARP_INVERSE_MAP, cv::BORDER_REPLICATE);
1806 std::vector<TPixelCoordf>& cornerCoords,
unsigned int check_size_x,
1807 unsigned int check_size_y,
unsigned int lines_width,
unsigned int r)
1815 unsigned int x, y, i;
1816 cv::Point prev_pt = cvPoint(0, 0);
1817 const int line_max = 8;
1820 line_colors[0] = CV_RGB(255, 0, 0);
1821 line_colors[1] = CV_RGB(255, 128, 0);
1822 line_colors[2] = CV_RGB(255, 128, 0);
1823 line_colors[3] = CV_RGB(200, 200, 0);
1824 line_colors[4] = CV_RGB(0, 255, 0);
1825 line_colors[5] = CV_RGB(0, 200, 200);
1826 line_colors[6] = CV_RGB(0, 0, 255);
1827 line_colors[7] = CV_RGB(255, 0, 255);
1833 const auto color = line_colors[y % line_max];
1837 pt.x = cvRound(cornerCoords[i].x);
1838 pt.y = cvRound(cornerCoords[i].y);
1840 if (i != 0) cv::line(img, prev_pt, pt, color, lines_width);
1843 img, cvPoint(pt.x - r, pt.y - r), cvPoint(pt.x + r, pt.y + r),
1844 color, lines_width);
1846 img, cvPoint(pt.x - r, pt.y + r), cvPoint(pt.x + r, pt.y - r),
1847 color, lines_width);
1849 if (r > 0) cv::circle(img, pt, r + 1, color);
1854 if (i == 0 || i == cornerCoords.size() - 1)
1879 if (&ret !=
this) ret = *
this;
1883 auto srcImg =
m_impl->img;
1885 if (srcImg.data == ret.
m_impl->img.data) srcImg = srcImg.clone();
1889 cv::cvtColor(srcImg, ret.
m_impl->img, cv::COLOR_GRAY2BGR);
1899 ASSERT_(im1.type() == im2.type());
1903 im1.copyTo(img(cv::Rect(0, 0, im1.cols, im1.rows)));
1904 im2.copyTo(img(cv::Rect(im1.cols, 0, im2.cols, im2.rows)));
1912 auto srcImg =
m_impl->img;
1913 if (
this != &out_img)
1915 auto outImg = out_img.
m_impl->img;
1917 if (srcImg.channels() == 1)
1918 cv::equalizeHist(srcImg, outImg);
1926 #if defined(__GNUC__) 1927 #define MRPT_DISABLE_FULL_OPTIMIZATION __attribute__((optimize("O1"))) 1929 #define MRPT_DISABLE_FULL_OPTIMIZATION 1932 template <
unsigned int HALF_WIN_SIZE>
1934 const uint8_t* in,
const int widthStep,
unsigned int x,
unsigned int y,
1935 int32_t& _gxx, int32_t& _gyy, int32_t& _gxy)
1937 const auto min_x = x - HALF_WIN_SIZE;
1938 const auto min_y = y - HALF_WIN_SIZE;
1944 const unsigned int WIN_SIZE = 1 + 2 * HALF_WIN_SIZE;
1946 unsigned int yy = min_y;
1947 for (
unsigned int iy = WIN_SIZE; iy; --iy, ++yy)
1949 const uint8_t* ptr = in + widthStep * yy + min_x;
1950 unsigned int xx = min_x;
1951 for (
unsigned int ix = WIN_SIZE; ix; --ix, ++xx, ++ptr)
1954 static_cast<int32_t
>(ptr[+1]) - static_cast<int32_t>(ptr[-1]);
1955 const int32_t dy =
static_cast<int32_t
>(ptr[+widthStep]) -
1956 static_cast<int32_t>(ptr[-widthStep]);
1968 const unsigned int x,
const unsigned int y,
1969 const unsigned int half_window_size)
const 1973 const auto& im1 =
m_impl->img;
1974 const auto img_w =
static_cast<unsigned int>(im1.cols),
1975 img_h = static_cast<unsigned int>(im1.rows);
1976 const int widthStep = im1.step[0];
1979 const unsigned int min_x = x - half_window_size;
1980 const unsigned int max_x = x + half_window_size;
1981 const unsigned int min_y = y - half_window_size;
1982 const unsigned int max_y = y + half_window_size;
1987 min_x < img_w && max_x < img_w && min_y < img_h && max_y < img_h,
1988 "Window is out of image bounds");
1996 const auto* img_data = im1.ptr<uint8_t>(0);
1997 switch (half_window_size)
2000 image_KLT_response_template<2>(
2001 img_data, widthStep, x, y, gxx, gyy, gxy);
2004 image_KLT_response_template<3>(
2005 img_data, widthStep, x, y, gxx, gyy, gxy);
2008 image_KLT_response_template<4>(
2009 img_data, widthStep, x, y, gxx, gyy, gxy);
2012 image_KLT_response_template<5>(
2013 img_data, widthStep, x, y, gxx, gyy, gxy);
2016 image_KLT_response_template<6>(
2017 img_data, widthStep, x, y, gxx, gyy, gxy);
2020 image_KLT_response_template<7>(
2021 img_data, widthStep, x, y, gxx, gyy, gxy);
2024 image_KLT_response_template<8>(
2025 img_data, widthStep, x, y, gxx, gyy, gxy);
2028 image_KLT_response_template<9>(
2029 img_data, widthStep, x, y, gxx, gyy, gxy);
2032 image_KLT_response_template<10>(
2033 img_data, widthStep, x, y, gxx, gyy, gxy);
2036 image_KLT_response_template<11>(
2037 img_data, widthStep, x, y, gxx, gyy, gxy);
2040 image_KLT_response_template<12>(
2041 img_data, widthStep, x, y, gxx, gyy, gxy);
2044 image_KLT_response_template<13>(
2045 img_data, widthStep, x, y, gxx, gyy, gxy);
2048 image_KLT_response_template<14>(
2049 img_data, widthStep, x, y, gxx, gyy, gxy);
2052 image_KLT_response_template<15>(
2053 img_data, widthStep, x, y, gxx, gyy, gxy);
2056 image_KLT_response_template<16>(
2057 img_data, widthStep, x, y, gxx, gyy, gxy);
2060 image_KLT_response_template<32>(
2061 img_data, widthStep, x, y, gxx, gyy, gxy);
2065 for (
unsigned int yy = min_y; yy <= max_y; yy++)
2067 const uint8_t* p = img_data + widthStep * yy + min_x;
2068 for (
unsigned int xx = min_x; xx <= max_x; xx++)
2070 const int32_t dx = p[+1] - p[-1];
2071 const int32_t dy = p[+widthStep] - p[-widthStep];
2080 const float K = 0.5f / ((max_y - min_y + 1) * (max_x - min_x + 1));
2081 const float Gxx = gxx * K;
2082 const float Gxy = gxy * K;
2083 const float Gyy = gyy * K;
2090 const float t = Gxx + Gyy;
2091 const float de = Gxx * Gyy - Gxy * Gxy;
2093 return 0.5f * (t - std::sqrt(t * t - 4.0f * de));
2108 std::fstream stream;
2109 stream.open(fileName.c_str(), std::fstream::in | std::fstream::binary);
2110 if (!stream.is_open())
2112 std::cerr <<
"[CImage::loadTGA] Couldn't open file '" << fileName
2119 stream.seekg(0, std::ios_base::beg);
2122 char dumpBuffer[12];
2123 char trueColorHeader[] =
"\0\0\2\0\0\0\0\0\0\0\0\0";
2124 stream.read(dumpBuffer, 12);
2125 if (memcmp(dumpBuffer, trueColorHeader, 12) != 0)
2127 std::cerr <<
"[CImage::loadTGA] Unsupported format or invalid file.\n";
2131 unsigned short width, height;
2134 stream.read((
char*)&width, 2);
2135 stream.read((
char*)&height, 2);
2139 std::cerr <<
"[CImage::loadTGA] Only 32 bpp format supported!\n";
2144 desc = stream.get();
2145 if (desc != 8 && desc != 32)
2147 std::cerr <<
"[CImage::loadTGA] Unsupported format or invalid file.\n";
2150 const bool origin_is_low_corner = (desc == 8);
2153 std::vector<uint8_t> bytes(width * height * 4);
2154 stream.read((
char*)&bytes[0], width * height * 4);
2162 for (
int r = 0; r < height; r++)
2164 const auto actual_row = origin_is_low_corner ? (height - 1 - r) : r;
2165 auto& img = out_RGB.
m_impl->img;
2166 auto data = img.ptr<uint8_t>(actual_row);
2168 auto& img_alpha = out_alpha.
m_impl->img;
2169 auto data_alpha = img_alpha.ptr<uint8_t>(actual_row);
2171 for (
unsigned int c = 0; c < width; c++)
2173 *
data++ = bytes[idx++];
2174 *
data++ = bytes[idx++];
2175 *
data++ = bytes[idx++];
2176 *data_alpha++ = bytes[idx++];
2183 #endif // MRPT_HAS_OPENCV 2191 #if MRPT_OPENCV_VERSION_NUM < 0x300 2193 *dest = cvIplImage(
m_impl->img);
2202 o <<
"(" << p.
x <<
"," << p.
y <<
")";
2207 o <<
"(" << p.
x <<
"," << p.
y <<
")";
void update_patch(const CImage &patch, const unsigned int col, const unsigned int row)
Update a part of this image with the "patch" given as argument.
void drawCircle(int x, int y, int radius, const mrpt::img::TColor &color=mrpt::img::TColor(255, 255, 255), unsigned int width=1) override
Draws a circle of a given radius.
void line(int x0, int y0, int x1, int y1, const mrpt::img::TColor color, unsigned int width=1, TPenStyle penStyle=psSolid) override
Draws a line.
Used in mrpt::img::CImage.
bool isEmpty() const
Returns true if the object is in the state after default constructor.
TPenStyle
Definition of pen styles.
bool drawChessboardCorners(std::vector< TPixelCoordf > &cornerCoords, unsigned int check_size_x, unsigned int check_size_y, unsigned int lines_width=1, unsigned int circles_radius=4)
Draw onto this image the detected corners of a chessboard.
void getAsMatrixTiled(mrpt::math::CMatrixFloat &outMatrix) const
Returns the image as a matrix, where the image is "tiled" (repeated) the required number of times to ...
void MRPT_DISABLE_FULL_OPTIMIZATION image_KLT_response_template(const uint8_t *in, const int widthStep, unsigned int x, unsigned int y, int32_t &_gxx, int32_t &_gyy, int32_t &_gxy)
#define THROW_EXCEPTION(msg)
std::string std::string format(std::string_view fmt, ARGS &&... args)
CImage scaleHalf(TInterpolationMethod interp) const
Returns a new image scaled down to half its original size.
void getAsMatrix(mrpt::math::CMatrixFloat &outMatrix, bool doResize=true, int x_min=0, int y_min=0, int x_max=-1, int y_max=-1, bool normalize_01=true) const
Returns the image as a matrix with pixel grayscale values in the range [0,1].
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void copyFromForceLoad(const CImage &o)
Copies from another image (shallow copy), and, if it is externally stored, the image file will be act...
void getAsRGBMatrices(mrpt::math::CMatrixFloat &outMatrixR, mrpt::math::CMatrixFloat &outMatrixG, mrpt::math::CMatrixFloat &outMatrixB, bool doResize=true, int x_min=0, int y_min=0, int x_max=-1, int y_max=-1) const
Returns the image as RGB matrices with pixel values in the range [0,1].
static bool DISABLE_JPEG_COMPRESSION()
copy_type_t
Define kind of copies.
CExceptionExternalImageNotFound(const std::string &s)
float getMaxAsFloat() const
Return the maximum pixel value of the image, as a float value in the range [0,1]. ...
void drawImage(int x, int y, const mrpt::img::CImage &img) override
Draws an image as a bitmap at a given position.
cv::Mat & asCvMatRef()
Get a reference to the internal cv::Mat, which can be resized, etc.
void flipHorizontal()
Flips the image horizontally.
TImageChannels getChannelCount() const
Returns the number of channels, typically 1 (GRAY) or 3 (RGB)
static constexpr TColor blue()
size_t getHeight() const override
Returns the height of the image in pixels.
mrpt::vision::TStereoCalibParams params
void scaleImage(CImage &out_img, unsigned int width, unsigned int height, TInterpolationMethod interp=IMG_INTERP_CUBIC) const
Scales this image to a new size, interpolating as needed, saving the new image in a different output ...
std::string getChannelsOrder() const
As of mrpt 2.0.0, this returns either "GRAY" or "BGR".
A pair (x,y) of pixel coordinates (subpixel resolution).
void setExternalStorage(const std::string &fileName) noexcept
By using this method the image is marked as referenced to an external file, which will be loaded only...
float correlate(const CImage &img2int, int width_init=0, int height_init=0) const
Computes the correlation coefficient (returned as val), between two images This function use grayscal...
void makeSureImageIsLoaded() const
Checks if the image is of type "external storage", and if so and not loaded yet, load it...
void filterGaussian(CImage &out_img, int W=3, int H=3, double sigma=1.0) const
Filter the image with a Gaussian filter with a window size WxH, replacing "this" image by the filtere...
void joinImagesHorz(const CImage &im1, const CImage &im2)
Joins two images side-by-side horizontally.
void unload() const noexcept
For external storage image objects only, this method unloads the image from memory (or does nothing i...
float getAsFloat(unsigned int col, unsigned int row, unsigned int channel) const
Returns the contents of a given pixel at the desired channel, in float format: [0,255]->[0,1] The coordinate origin is pixel(0,0)=top-left corner of the image.
#define MRPT_DISABLE_FULL_OPTIMIZATION
mrpt::pimpl< Impl > m_impl
bool supports(feature f) noexcept
Returns true if the current CPU (and OS) supports the given CPU feature.
void swap(CImage &o)
Efficiently swap of two images.
static std::string IMAGES_PATH_BASE(".")
void asCvMat(cv::Mat &out_img, copy_type_t copy_type) const
Makes a shallow or deep copy of this image into the provided cv::Mat.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
uint64_t getTotalBytesCount() const override
Returns the total size of the internal buffer.
void swapRB()
Swaps red and blue channels.
CImage colorImage() const
Returns a color (RGB) version of the grayscale image, or a shallow copy of itself if it is already a ...
void image_SSSE3_scale_half_3c8u(const uint8_t *in, uint8_t *out, int w, int h, size_t in_step, size_t out_step)
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates): ...
#define ASSERT_(f)
Defines an assertion mechanism.
bool loadFromFile(const std::string &fileName, int isColor=-1)
Load image from a file, whose format is determined from the extension (internally uses OpenCV)...
This base provides a set of functions for maths stuff.
static int SERIALIZATION_JPEG_QUALITY_value
RET pixelDepth2IPLCvDepth(PixelDepth d)
size_t getWidth() const override
Returns the width of the image in pixels.
float KLT_response(const unsigned int x, const unsigned int y, const unsigned int half_window_size) const
Compute the KLT response at a given pixel (x,y) - Only for grayscale images (for efficiency it avoids...
void resize(std::size_t width, std::size_t height, TImageChannels nChannels, PixelDepth depth=PixelDepth::D8U)
Changes the size of the image, erasing previous contents (does NOT scale its current content...
mrpt::system::CTimeLogger CTimeLogger
unsigned char * operator()(unsigned int col, unsigned int row, unsigned int channel=0) const
Returns a pointer to a given pixel information.
void normalize(CONTAINER &c, Scalar valMin, Scalar valMax)
Scales all elements such as the minimum & maximum values are shifted to the given values...
virtual void selectTextFont(const std::string &fontName)
Select the current font used when drawing text.
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
void * getRawBufferData()
Method for getting a pointer to the raw stored data.
This CStream derived class allow using a memory buffer as a CStream.
void internal_fromIPL(const IplImage *iplImage, copy_type_t c)
void saveToStreamAsJPEG(mrpt::io::CStream &out, const int jpeg_quality=95) const
Save image to binary stream as a JPEG (.jpg) compressed format.
void assignMemoryNotOwn(const void *data, const uint64_t nBytesInData)
Initilize the data in the stream from a block of memory which is NEITHER OWNED NOR COPIED by the obje...
void normalize()
Optimize the brightness range of an image without using histogram Only for one channel images...
uint64_t Seek(int64_t Offset, CStream::TSeekOrigin Origin=sFromBeginning) override
Introduces a pure virtual method for moving to a specified position in the streamed resource...
static PixelDepth cvDepth2PixelDepth(int64_t d)
A pair (x,y) of pixel coordinates (integer resolution).
void flipVertical()
Flips the image vertically.
uint8_t * get_unsafe(unsigned int col, unsigned int row, uint8_t channel=0) const
Access to pixels without checking boundaries - Use normally the () operator better, which checks the coordinates.
#define IMPLEMENTS_MEXPLUS_FROM(complete_type)
Parameters for the Brown-Conrady camera lens distortion model.
void getAsIplImage(IplImage *dest) const
(DEPRECATED, DO NOT USE - Kept here only to interface opencv 2.4)
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
void loadFromStreamAsJPEG(mrpt::io::CStream &in)
Reads the image from a binary stream containing a binary jpeg file.
void rectifyImageInPlace(void *mapX, void *mapY)
Rectify an image (undistorts and rectification) from a stereo pair according to a pair of precomputed...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#define ASSERT_ABOVEEQ_(__A, __B)
TImageSize getSize() const
Return the size of the image.
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
std::array< double, 8 > dist
[k1 k2 t1 t2 k3 k4 k5 k6] -> k_i: parameters of radial distortion, t_i: parameters of tangential dist...
void clear()
Resets the image to the state after a default ctor.
uint8_t * internal_get(int col, int row, uint8_t channel=0) const
struct mxArray_tag mxArray
Forward declaration for mxArray (avoid #including as much as possible to speed up compiling) ...
CImage grayscale() const
Returns a grayscale version of the image, or a shallow copy of itself if it is already a grayscale im...
void cross_correlation_FFT(const CImage &in_img, math::CMatrixFloat &out_corr, int u_search_ini=-1, int v_search_ini=-1, int u_search_size=-1, int v_search_size=-1, float biasThisImg=0, float biasInImg=0) const
Computes the correlation matrix between this image and another one.
void idft2_complex(const CMatrixFloat &in_real, const CMatrixFloat &in_imag, CMatrixFloat &out_real, CMatrixFloat &out_imag)
Compute the 2D inverse Discrete Fourier Transform (DFT).
return_t square(const num_t x)
Inline function for the square of a number.
void setPixel(int x, int y, size_t color) override
Changes the value of the pixel (x,y).
PixelDepth getPixelDepth() const
const_iterator end() const
This class is a "CSerializable" wrapper for "CMatrixFloat".
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void filterMedian(CImage &out_img, int W=3) const
Filter the image with a Median filter with a window size WxW, returning the filtered image in out_img...
void image_SSSE3_bgr_to_gray_8u(const uint8_t *in, uint8_t *out, int w, int h, size_t in_step, size_t out_step)
CImage()
Default constructor: initialize to empty image.
void image_SSE2_scale_half_1c8u(const uint8_t *in, uint8_t *out, int w, int h, size_t in_step, size_t out_step)
Virtual base class for "archives": classes abstracting I/O streams.
CImage makeDeepCopy() const
Returns a deep copy of this image.
bool isColor() const
Returns true if the image is RGB, false if it is grayscale.
TInterpolationMethod
Interpolation methods for images.
bool isOriginTopLeft() const
Returns true (as of MRPT v2.0.0, it's fixed)
mrpt::vision::TStereoCalibResults out
struct _IplImage IplImage
virtual mxArray * writeToMatlab() const
Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class.
bool m_imgIsExternalStorage
Set to true only when using setExternalStorage.
Deep copy: the copied object has a duplicate of all data, becoming independent.
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
float u8tof(const uint8_t v)
converts a uint8_t [0,255] into a float [0,1]
size_t getRowStride() const
Returns the row stride of the image: this is the number of bytes between two consecutive rows...
CImage scaleDouble(TInterpolationMethod interp) const
Returns a new image scaled up to double its original size.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void rotateImage(CImage &out_img, double ang, unsigned int cx, unsigned int cy, double scale=1.0) const
Rotates the image by the given angle around the given center point, with an optional scale factor...
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
void extract_patch(CImage &patch, const unsigned int col=0, const unsigned int row=0, const unsigned int width=1, const unsigned int height=1) const
Extract a patch from this image, saveing it into "patch" (its previous contents will be overwritten)...
constexpr RET pixelDepth2CvDepth(PixelDepth d)
pimpl< T > make_impl(Args &&... args)
void image_SSE2_scale_half_smooth_1c8u(const uint8_t *in, uint8_t *out, int w, int h, size_t in_step, size_t out_step)
virtual void textOut(int x0, int y0, const std::string &str, const mrpt::img::TColor color)
Renders 2D text using bitmap fonts.
MRPT_TODO("toPointCloud / calibration")
static int interpolationMethod2Cv(TInterpolationMethod i)
bool saveToFile(const std::string &fileName, int jpeg_quality=95) const
Save the image to a file, whose format is determined from the extension (internally uses OpenCV)...
TImageChannels
For use in mrpt::img::CImage.
#define ASSERT_BELOWEQ_(__A, __B)
static int SERIALIZATION_JPEG_QUALITY()
static bool DISABLE_JPEG_COMPRESSION_value
void undistort(CImage &out_img, const mrpt::img::TCamera &cameraParams) const
Undistort the image according to some camera parameters, and returns an output undistorted image...
std::ostream & operator<<(std::ostream &o, const TColor &c)
void dft2_complex(const CMatrixFloat &in_real, const CMatrixFloat &in_imag, CMatrixFloat &out_real, CMatrixFloat &out_imag)
Compute the 2D Discrete Fourier Transform (DFT) of a complex matrix, returning the real and imaginary...
std::string getExternalStorageFileAbsolutePath() const
Only if isExternallyStored() returns true.
This template class provides the basic functionality for a general 2D any-size, resizable container o...
std::string m_externalFile
The file name of a external storage image.
void clear()
Clear the contents of this container.
static void setImagesPathBase(const std::string &path)
void equalizeHist(CImage &out_img) const
Equalize the image histogram, saving the new image in the given output object.
#define THROW_TYPED_EXCEPTION_FMT(exceptionClass, _FORMAT_STRING,...)
void forceLoad() const
For external storage image objects only, this method makes sure the image is loaded in memory...
static bool loadTGA(const std::string &fileName, mrpt::img::CImage &out_RGB, mrpt::img::CImage &out_alpha)
Loads a TGA true-color RGBA image as two CImage objects, one for the RGB channels plus a separate gra...
static bool my_img_to_grayscale(const cv::Mat &src, cv::Mat &dest)
A class for storing images as grayscale or RGB bitmaps.
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
void loadFromMemoryBuffer(unsigned int width, unsigned int height, bool color, unsigned char *rawpixels, bool swapRedBlue=false)
Reads the image from raw pixels buffer in memory.
Scalar & coeffRef(int r, int c)
static struct FontData data
static const std::string & getImagesPathBase()
By default, ".".