36 #include <visp3/core/vpCPUFeatures.h>
37 #include <visp3/core/vpImageConvert.h>
38 #include <visp3/core/vpImageTools.h>
39 #include <visp3/core/vpImageException.h>
41 #if defined(VISP_HAVE_SIMDLIB)
42 #include <Simd/SimdLib.hpp>
101 unsigned char B_star)
110 double factor =
static_cast<double>((B_star - A_star) /
static_cast<double>((B - A)));
113 unsigned int i_width = I.
getWidth();
114 for (
unsigned int i = 0; i < i_height; ++i) {
115 for (
unsigned int j = 0; j < i_width; ++j) {
125 I[i][j] =
static_cast<unsigned char>(A_star + (factor * (v - A)));
154 #if defined(VISP_HAVE_SIMDLIB)
157 for (
unsigned int i = 0; i < I1.
getSize(); ++i) {
159 Idiff.
bitmap[i] =
static_cast<unsigned char>(std::max<unsigned char>(std::min<unsigned char>(diff, 255), 0));
181 "Cannot compute image difference. The two images "
182 "(%ux%u) and (%ux%u) have not the same size",
190 #if defined(VISP_HAVE_SIMDLIB)
191 SimdImageDifference(
reinterpret_cast<unsigned char *
>(I1.
bitmap),
reinterpret_cast<unsigned char *
>(I2.
bitmap),
192 I1.
getSize() * 4,
reinterpret_cast<unsigned char *
>(Idiff.
bitmap));
194 for (
unsigned int i = 0; i < I1.
getSize() * 4; ++i) {
229 for (
unsigned int b = 0; b < n; ++b) {
253 for (
unsigned int b = 0; b < n; ++b) {
282 for (
unsigned int b = 0; b < n; ++b) {
319 #if defined(VISP_HAVE_SIMDLIB)
320 typedef Simd::View<Simd::Allocator> View;
325 Simd::OperationBinary8u(img1, img2, imgAdd,
326 saturate ? SimdOperationBinary8uSaturatedAddition : SimdOperationBinary8uAddition);
328 unsigned char *ptr_I1 = I1.
bitmap;
329 unsigned char *ptr_I2 = I2.
bitmap;
330 unsigned char *ptr_Ires = Ires.
bitmap;
331 for (
unsigned int cpt = 0; cpt < Ires.
getSize(); cpt++, ++ptr_I1, ++ptr_I2, ++ptr_Ires) {
332 *ptr_Ires = saturate ? vpMath::saturate<unsigned char>((
short int)*ptr_I1 + (
short int)*ptr_I2) : *ptr_I1 + *ptr_I2;
361 #if defined(VISP_HAVE_SIMDLIB)
362 typedef Simd::View<Simd::Allocator> View;
367 Simd::OperationBinary8u(img1, img2, imgAdd,
368 saturate ? SimdOperationBinary8uSaturatedSubtraction : SimdOperationBinary8uSubtraction);
370 unsigned char *ptr_I1 = I1.
bitmap;
371 unsigned char *ptr_I2 = I2.
bitmap;
372 unsigned char *ptr_Ires = Ires.
bitmap;
373 for (
unsigned int cpt = 0; cpt < Ires.
getSize(); cpt++, ++ptr_I1, ++ptr_I2, ++ptr_Ires) {
374 *ptr_Ires = saturate ?
375 vpMath::saturate<unsigned char>(
static_cast<short int>(*ptr_I1) -
static_cast<short int>(*ptr_I2)) :
396 mapU.
resize(height, width,
false,
false);
397 mapV.
resize(height, width,
false,
false);
398 mapDu.
resize(height, width,
false,
false);
399 mapDv.
resize(height, width,
false,
false);
402 bool is_KannalaBrandt =
405 float u0 =
static_cast<float>(cam.
get_u0());
406 float v0 =
static_cast<float>(cam.
get_v0());
407 float px =
static_cast<float>(cam.
get_px());
408 float py =
static_cast<float>(cam.
get_py());
410 std::vector<double> dist_coefs;
412 if (!is_KannalaBrandt) {
413 kud =
static_cast<float>(cam.
get_kud());
419 if ((!is_KannalaBrandt) && (std::fabs(
static_cast<double>(kud)) <= std::numeric_limits<double>::epsilon())) {
421 for (
unsigned int i = 0; i < height; ++i) {
422 for (
unsigned int j = 0; j < width; ++j) {
423 mapU[i][j] =
static_cast<int>(j);
424 mapV[i][j] =
static_cast<int>(i);
434 float kud_px2 = 0., kud_py2 = 0., deltau_px, deltav_py = 0;
436 float deltav, deltau;
437 float u_float, v_float;
438 int u_round, v_round;
440 double theta, theta_d;
441 double theta2, theta4, theta6, theta8;
446 if (!is_KannalaBrandt) {
447 kud_px2 = kud * invpx * invpx;
448 kud_py2 = kud * invpy * invpy;
451 for (
unsigned int v = 0; v < height; ++v) {
454 if (!is_KannalaBrandt) {
455 fr1 = 1.0f + (kud_py2 * deltav * deltav);
458 deltav_py = deltav * invpy;
461 for (
unsigned int u = 0; u < width; ++u) {
464 if (!is_KannalaBrandt) {
465 fr2 = fr1 + (kud_px2 * deltau * deltau);
467 u_float = (deltau * fr2) + u0;
468 v_float = (deltav * fr2) + v0;
472 deltau_px = deltau * invpx;
478 theta6 = theta2 * theta4;
481 theta_d = theta * (1 + (dist_coefs[0] * theta2) + (dist_coefs[1] * theta4) + (dist_coefs[2] * theta6) +
482 (dist_coefs[3] * theta8));
485 scale = (std::fabs(r) < std::numeric_limits<double>::epsilon()) ? 1.0 : (theta_d / r);
486 u_float =
static_cast<float>((deltau * scale) + u0);
487 v_float =
static_cast<float>((deltav * scale) + v0);
490 u_round =
static_cast<int>(u_float);
491 v_round =
static_cast<int>(v_float);
493 mapU[v][u] = u_round;
494 mapV[v][u] = v_round;
496 mapDu[v][u] = u_float - u_round;
497 mapDv[v][u] = v_float - v_round;
516 std::cerr <<
"Error, input image is empty." << std::endl;
524 unsigned int ii_width = II.
getWidth();
525 for (
unsigned int i = 1; i < ii_height; ++i) {
526 for (
unsigned int j = 1; j < ii_width; ++j) {
527 II[i][j] = (I[i - 1][j - 1] + II[i - 1][j] + II[i][j - 1]) - II[i - 1][j - 1];
528 IIsq[i][j] = (
vpMath::sqr(I[i - 1][j - 1]) + IIsq[i - 1][j] + IIsq[i][j - 1]) - IIsq[i - 1][j - 1];
544 "Error: in vpImageTools::normalizedCorrelation(): "
545 "image dimension mismatch between I1=%ux%u and I2=%ux%u",
556 #if defined(VISP_HAVE_SIMDLIB)
557 SimdNormalizedCorrelation(I1.
bitmap, a, I2.
bitmap, b, I1.
getSize(), a2, b2, ab, useOptimized);
559 for (
unsigned int cpt = 0; cpt < I1.
getSize(); ++cpt) {
567 return ab / sqrt(a2 * b2);
581 for (
unsigned int i = 0; i < height; ++i) {
582 for (
unsigned int j = 0; j < width; ++j) {
586 for (
unsigned int j = 0; j < width; ++j) {
599 unsigned int i_width = I.
getWidth();
600 for (
unsigned int i = 0; i < i_height; ++i) {
601 for (
unsigned int j = 0; j < i_width; ++j) {
602 I(i, j, I(i, j) / s);
621 int x1 =
static_cast<int>(floor(point.
get_i()));
622 int x2 =
static_cast<int>(ceil(point.
get_i()));
623 int y1 =
static_cast<int>(floor(point.
get_j()));
624 int y2 =
static_cast<int>(ceil(point.
get_j()));
631 v1 = ((x2 - point.
get_i()) * I(x1, y1)) + ((point.
get_i() - x1) * I(x2, y1));
632 v2 = ((x2 - point.
get_i()) * I(x1, y2)) + ((point.
get_i() - x1) * I(x2, y2));
637 return ((y2 - point.
get_j()) * v1) + ((point.
get_j() - y1) * v2);
641 "vpImageTools::interpolate(): bi-cubic interpolation is not implemented.");
662 double cos_t = cos(t);
663 double sin_t = sin(t);
665 for (
unsigned int x = 0; x < x_d; ++x) {
666 double x_cos_t = x * cos_t;
667 double x_sin_t = x * sin_t;
668 for (
unsigned int y = 0; y < y_d; ++y) {
670 static_cast<unsigned char>(
interpolate(src,
vpImagePoint(x1 + x_cos_t + (y * sin_t), (y1 - x_sin_t) + (y * cos_t)),
689 double cos_t = cos(t);
690 double sin_t = sin(t);
692 for (
unsigned int x = 0; x < x_d; ++x) {
693 double x_cos_t = x * cos_t;
694 double x_sin_t = x * sin_t;
695 for (
unsigned int y = 0; y < y_d; ++y) {
722 std::cerr <<
"Error, input image is empty." << std::endl;
727 std::cerr <<
"Error, template image is empty." << std::endl;
732 std::cerr <<
"Error, template image is bigger than input image." << std::endl;
751 const double sum2 = (((II_tpl[height_tpl][width_tpl] + II_tpl[0][0]) - II_tpl[0][width_tpl]) - II_tpl[height_tpl][0]);
752 const double mean2 = sum2 / I_tpl.
getSize();
753 unsigned int i_tpl_double_size = I_tpl_double.
getSize();
754 for (
unsigned int cpt = 0; cpt < i_tpl_double_size; ++cpt) {
755 I_tpl_double.
bitmap[cpt] -= mean2;
758 #if defined(_OPENMP) && (_OPENMP >= 200711)
759 #pragma omp parallel for schedule(dynamic)
760 for (
unsigned int i = 0; i < I.
getHeight() - height_tpl; i += step_v) {
761 for (
unsigned int j = 0; j < I.
getWidth() - width_tpl; j += step_u) {
767 int end =
static_cast<int>((I.
getHeight() - height_tpl) / step_v) + 1;
768 std::vector<unsigned int> vec_step_v(
static_cast<size_t>(end));
770 for (
unsigned int cpt = 0, idx = 0; cpt < (i_height - height_tpl); cpt += step_v, ++idx) {
771 vec_step_v[
static_cast<size_t>(idx)] = cpt;
774 #pragma omp parallel for schedule(dynamic)
776 for (
int cpt = 0; cpt < end; ++cpt) {
777 unsigned int i_width = I.
getWidth();
778 for (
unsigned int j = 0; j < (i_width - width_tpl); j += step_u) {
779 I_score[vec_step_v[cpt]][j] =
789 unsigned int i_width = I.
getWidth();
790 for (
unsigned int i = 0; i < (i_height - height_tpl); i += step_v) {
791 for (
unsigned int j = 0; j < (i_width - width_tpl); j += step_u) {
808 float vpImageTools::cubicHermite(
const float A,
const float B,
const float C,
const float D,
const float t)
810 float a = (((-A + (3.0f * B)) - (3.0f * C)) + D) / 2.0f;
811 float b = (A + (2.0f * C)) - (((5.0f * B) + D) / 2.0f);
812 float c = (-A + C) / 2.0f;
815 return (a * t * t * t) + (b * t * t) + (c * t) + d;
818 int vpImageTools::coordCast(
double x) {
return x < 0 ? -1 :
static_cast<int>(x); }
820 double vpImageTools::lerp(
double A,
double B,
double t) {
return (A * (1.0 - t)) + (B * t); }
822 float vpImageTools::lerp(
float A,
float B,
float t) {
return (A * (1.0f - t)) + (B * t); }
824 int64_t vpImageTools::lerp2(int64_t A, int64_t B, int64_t t, int64_t t_1) {
return (A * t_1) + (B * t); }
829 unsigned int i0,
unsigned int j0)
833 #if defined(VISP_HAVE_SIMDLIB)
836 for (
unsigned int i = 0; i < I2.
getHeight(); ++i) {
837 for (
unsigned int j = 0; j < I2.
getWidth(); ++j) {
838 ab += (I1[i0 + i][j0 + j]) * I2[i][j];
845 (((II[i0 + height_tpl][j0 + width_tpl] + II[i0][j0]) - II[i0][j0 + width_tpl]) - II[i0 + height_tpl][j0]);
846 const double sum2 = (((II_tpl[height_tpl][width_tpl] + II_tpl[0][0]) - II_tpl[0][width_tpl]) - II_tpl[height_tpl][0]);
855 return ab / sqrt(a2 * b2);
874 #pragma omp parallel for schedule(dynamic)
876 for (
int i_ = 0; i_ < static_cast<int>(I.
getHeight()); ++i_) {
877 const unsigned int i =
static_cast<unsigned int>(i_);
878 unsigned int i_width = I.
getWidth();
879 for (
unsigned int j = 0; j < i_width; ++j) {
881 int u_round = mapU[i][j];
882 int v_round = mapV[i][j];
884 float du = mapDu[i][j];
885 float dv = mapDv[i][j];
887 if ((0 <= u_round) && (0 <= v_round) && (u_round < (
static_cast<int>(I.
getWidth()) - 1)) &&
888 (v_round < (static_cast<int>(I.
getHeight()) - 1))) {
890 float col0 = lerp(I[v_round][u_round], I[v_round][u_round + 1], du);
891 float col1 = lerp(I[v_round + 1][u_round], I[v_round + 1][u_round + 1], du);
892 float value = lerp(col0, col1, dv);
894 Iundist[i][j] =
static_cast<unsigned char>(value);
919 #pragma omp parallel for schedule(dynamic)
921 for (
int i = 0; i < static_cast<int>(I.
getHeight()); ++i) {
922 #if defined(VISP_HAVE_SIMDLIB)
924 mapV.
data, mapDu.
data, mapDv.
data,
reinterpret_cast<unsigned char *
>(Iundist.
bitmap));
926 const unsigned int i_ =
static_cast<unsigned int>(i);
927 for (
unsigned int j = 0; j < I.
getWidth(); ++j) {
929 int u_round = mapU[i_][j];
930 int v_round = mapV[i_][j];
932 float du = mapDu[i_][j];
933 float dv = mapDv[i_][j];
935 if (0 <= u_round && 0 <= v_round && u_round <
static_cast<int>(I.
getWidth()) - 1
936 && v_round < static_cast<int>(I.
getHeight()) - 1) {
938 float col0 = lerp(I[v_round][u_round].R, I[v_round][u_round + 1].R, du);
939 float col1 = lerp(I[v_round + 1][u_round].R, I[v_round + 1][u_round + 1].R, du);
940 float value = lerp(col0, col1, dv);
942 Iundist[i][j].R =
static_cast<unsigned char>(value);
944 col0 = lerp(I[v_round][u_round].G, I[v_round][u_round + 1].G, du);
945 col1 = lerp(I[v_round + 1][u_round].G, I[v_round + 1][u_round + 1].G, du);
946 value = lerp(col0, col1, dv);
948 Iundist[i][j].G =
static_cast<unsigned char>(value);
950 col0 = lerp(I[v_round][u_round].B, I[v_round][u_round + 1].B, du);
951 col1 = lerp(I[v_round + 1][u_round].B, I[v_round + 1][u_round + 1].B, du);
952 value = lerp(col0, col1, dv);
954 Iundist[i][j].B =
static_cast<unsigned char>(value);
956 col0 = lerp(I[v_round][u_round].A, I[v_round][u_round + 1].A, du);
957 col1 = lerp(I[v_round + 1][u_round].A, I[v_round + 1][u_round + 1].A, du);
958 value = lerp(col0, col1, dv);
960 Iundist[i][j].A =
static_cast<unsigned char>(value);
970 #if defined(VISP_HAVE_SIMDLIB)
971 void vpImageTools::resizeSimdlib(
const vpImage<vpRGBa> &Isrc,
unsigned int resizeWidth,
unsigned int resizeHeight,
974 Idst.
resize(resizeHeight, resizeWidth);
976 typedef Simd::View<Simd::Allocator> View;
980 Simd::Resize(src, dst, method ==
INTERPOLATION_LINEAR ? SimdResizeMethodBilinear : SimdResizeMethodArea);
986 Idst.
resize(resizeHeight, resizeWidth);
988 typedef Simd::View<Simd::Allocator> View;
992 Simd::Resize(src, dst, method ==
INTERPOLATION_LINEAR ? SimdResizeMethodBilinear : SimdResizeMethodArea);
996 bool vpImageTools::checkFixedPoint(
unsigned int x,
unsigned int y,
const vpMatrix &T,
bool affine)
1000 double a2 = T[0][2];
1001 double a3 = T[1][0];
1002 double a4 = T[1][1];
1003 double a5 = T[1][2];
1004 double a6 = affine ? 0.0 : T[2][0];
1005 double a7 = affine ? 0.0 : T[2][1];
1006 double a8 = affine ? 1.0 : T[2][2];
1008 double w = (a6 * x) + (a7 * y) + a8;
1009 double x2 = ((a0 * x) + (a1 * y) + a2) / w;
1010 double y2 = ((a3 * x) + (a4 * y) + a5) / w;
1012 const double limit = 1 << 15;
1027 "Error in vpImageTools::inMask(): image (%dx%d) and mask (%dx%d) size doesn't match",
1032 int cpt_in_mask = 0;
1033 int size_ =
static_cast<int>(I.
getSize());
1034 #if defined(_OPENMP)
1035 #pragma omp parallel for reduction(+:cpt_in_mask)
1037 for (
int i = 0; i < size_; ++i) {
1038 if (mask.
bitmap[i] == 0) {
1039 I_mask.
bitmap[i] = black;
1060 "Error in vpImageTools::inMask(): image (%dx%d) and mask (%dx%d) size doesn't match",
1064 int cpt_in_mask = 0;
1065 int size_ =
static_cast<int>(I.
getSize());
1066 #if defined(_OPENMP)
1067 #pragma omp parallel for reduction(+:cpt_in_mask)
1069 for (
int i = 0; i < size_; ++i) {
1070 if (mask.
bitmap[i] == 0) {
1098 const vpColVector &hsv_range,
unsigned char *mask,
unsigned int size)
1100 if ((hue ==
nullptr) || (saturation ==
nullptr) || (value ==
nullptr)) {
1102 "Error in vpImageTools::inRange(): hsv pointer are empty"));
1104 else if (hsv_range.
size() != 6) {
1106 "Error in vpImageTools::inRange(): wrong values vector size (%d)", hsv_range.
size()));
1108 unsigned char h_low =
static_cast<unsigned char>(hsv_range[0]);
1109 unsigned char h_high =
static_cast<unsigned char>(hsv_range[1]);
1110 unsigned char s_low =
static_cast<unsigned char>(hsv_range[2]);
1111 unsigned char s_high =
static_cast<unsigned char>(hsv_range[3]);
1112 unsigned char v_low =
static_cast<unsigned char>(hsv_range[4]);
1113 unsigned char v_high =
static_cast<unsigned char>(hsv_range[5]);
1114 int size_ =
static_cast<int>(size);
1115 int cpt_in_range = 0;
1116 #if defined(_OPENMP)
1117 #pragma omp parallel for reduction(+:cpt_in_range)
1119 for (
int i = 0; i < size_; ++i) {
1120 if ((h_low <= hue[i]) && (hue[i] <= h_high) &&
1121 (s_low <= saturation[i]) && (saturation[i] <= s_high) &&
1122 (v_low <= value[i]) && (value[i] <= v_high)) {
1130 return cpt_in_range;
1151 const std::vector<int> &hsv_range,
unsigned char *mask,
unsigned int size)
1153 if ((hue ==
nullptr) || (saturation ==
nullptr) || (value ==
nullptr)) {
1155 "Error in vpImageTools::inRange(): hsv pointer are empty"));
1157 else if (hsv_range.size() != 6) {
1159 "Error in vpImageTools::inRange(): wrong values vector size (%d)", hsv_range.size()));
1161 unsigned char h_low =
static_cast<unsigned char>(hsv_range[0]);
1162 unsigned char h_high =
static_cast<unsigned char>(hsv_range[1]);
1163 unsigned char s_low =
static_cast<unsigned char>(hsv_range[2]);
1164 unsigned char s_high =
static_cast<unsigned char>(hsv_range[3]);
1165 unsigned char v_low =
static_cast<unsigned char>(hsv_range[4]);
1166 unsigned char v_high =
static_cast<unsigned char>(hsv_range[5]);
1167 int size_ =
static_cast<int>(size);
1168 int cpt_in_range = 0;
1169 #if defined(_OPENMP)
1170 #pragma omp parallel for reduction(+:cpt_in_range)
1172 for (
int i = 0; i < size_; ++i) {
1173 if ((h_low <= hue[i]) && (hue[i] <= h_high) &&
1174 (s_low <= saturation[i]) && (saturation[i] <= s_high) &&
1175 (v_low <= value[i]) && (value[i] <= v_high)) {
1183 return cpt_in_range;
Implementation of a generic 2D array used as base class for matrices and vectors.
Type * data
Address of the first element of the data array.
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
unsigned int size() const
Return the number of elements of the 2D array.
Generic class defining intrinsic camera parameters.
std::vector< double > getKannalaBrandtDistortionCoefficients() const
vpCameraParametersProjType
@ ProjWithKannalaBrandtDistortion
Projection with Kannala-Brandt distortion model.
vpCameraParametersProjType get_projModel() const
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
@ dimensionError
Bad dimension.
@ notImplementedError
Not implemented.
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Error that can be emitted by the vpImage class and its derivatives.
@ notInitializedError
Image not initialized.
@ incorrectInitializationError
Wrong image initialization.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
double getSum(const vpImage< bool > *p_mask=nullptr, unsigned int *nbValidPoints=nullptr) const
Compute the sum of image intensities.
unsigned int getWidth() const
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
unsigned int getSize() const
Type * bitmap
points toward the bitmap
unsigned int getHeight() const
double getMeanValue(const vpImage< bool > *p_mask=nullptr, unsigned int *nbValidPoints=nullptr) const
Return the mean value of the bitmap.
static Type maximum(const Type &a, const Type &b)
static double sqr(double x)
static Type abs(const Type &x)
static int round(double x)
static Type minimum(const Type &a, const Type &b)
Implementation of a matrix and operations on matrices.
unsigned char B
Blue component.
unsigned char R
Red component.
unsigned char G
Green component.
unsigned char A
Additionnal component.
Defines an oriented rectangle in the plane.
double getHeight() const
Get the rectangle height.
double getOrientation() const
Get the rectangle orientation (rad).
double getWidth() const
Get the rectangle width.
vpImagePoint getTopLeft() const
Get the top-left corner.
Defines a rectangle in the plane.
Implementation of row vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)