39 #ifndef vpImageTools_H
40 #define vpImageTools_H
50 #include <visp3/core/vpImage.h>
52 #ifdef VISP_HAVE_PTHREAD
56 #include <visp3/core/vpCameraParameters.h>
57 #include <visp3/core/vpImageException.h>
58 #include <visp3/core/vpMath.h>
59 #include <visp3/core/vpRect.h>
60 #include <visp3/core/vpRectOriented.h>
80 INTERPOLATION_NEAREST,
86 static inline void binarise(
vpImage<Type> &I, Type threshold1, Type threshold2, Type value1, Type value2, Type value3,
87 const bool useLUT =
true);
92 static void crop(
const vpImage<Type> &I,
double roi_top,
double roi_left,
unsigned int roi_height,
93 unsigned int roi_width,
vpImage<Type> &crop,
unsigned int v_scale = 1,
unsigned int h_scale = 1);
99 vpImage<Type> &crop,
unsigned int v_scale = 1,
unsigned int h_scale = 1);
100 template <
class Type>
102 unsigned int h_scale = 1);
103 template <
class Type>
104 static void crop(
const unsigned char *bitmap,
unsigned int width,
unsigned int height,
const vpRect &roi,
105 vpImage<Type> &crop,
unsigned int v_scale = 1,
unsigned int h_scale = 1);
124 const bool saturate =
false);
129 static void initUndistortMap(
const vpCameraParameters &cam,
unsigned int width,
unsigned int height,
134 const vpImageInterpolationType &method = INTERPOLATION_NEAREST);
139 const bool useOptimized =
true);
148 template <
class Type>
150 const vpImageInterpolationType &method = INTERPOLATION_NEAREST);
152 template <
class Type>
154 const vpImageInterpolationType &method = INTERPOLATION_NEAREST);
157 vpImage<double> &I_score,
const unsigned int step_u,
const unsigned int step_v,
158 const bool useOptimized =
true);
160 template <
class Type>
162 unsigned int nThreads=2);
164 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
169 template <
class Type>
170 vp_deprecated
static void createSubImage(
const vpImage<Type> &I,
unsigned int i_sub,
unsigned int j_sub,
171 unsigned int nrow_sub,
unsigned int ncol_sub,
vpImage<Type> &S);
173 template <
class Type>
180 static float cubicHermite(
const float A,
const float B,
const float C,
const float D,
const float t);
182 template <
class Type>
static Type getPixelClamped(
const vpImage<Type> &I,
const float u,
const float v);
185 static float lerp(
const float A,
const float B,
const float t);
189 const vpImage<double> &IIsq_tpl,
const unsigned int i0,
const unsigned int j0);
191 template <
class Type>
193 const float u,
const float v,
const float xFrac,
const float yFrac);
195 template <
class Type>
197 const float u,
const float v,
const float xFrac,
const float yFrac);
199 template <
class Type>
201 const float u,
const float v);
204 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
223 template <
class Type>
225 unsigned int roi_height,
unsigned int roi_width,
vpImage<Type> &crop)
250 #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
271 template <
class Type>
273 unsigned int roi_width,
vpImage<Type> &crop,
unsigned int v_scale,
unsigned int h_scale)
275 int i_min = (std::max)((
int)(ceil(roi_top / v_scale)), 0);
276 int j_min = (std::max)((
int)(ceil(roi_left / h_scale)), 0);
277 int i_max = (std::min)((
int)(ceil((roi_top + roi_height)) / v_scale), (
int)(I.
getHeight() / v_scale));
278 int j_max = (std::min)((
int)(ceil((roi_left + roi_width) / h_scale)), (
int)(I.
getWidth() / h_scale));
280 unsigned int i_min_u = (
unsigned int)i_min;
281 unsigned int j_min_u = (
unsigned int)j_min;
283 unsigned int r_width = (
unsigned int)(j_max - j_min);
284 unsigned int r_height = (
unsigned int)(i_max - i_min);
286 crop.resize(r_height, r_width);
288 if (v_scale == 1 && h_scale == 1) {
289 for (
unsigned int i = 0; i < r_height; i++) {
290 void *src = (
void *)(I[i + i_min_u] + j_min_u);
291 void *dst = (
void *)
crop[i];
292 memcpy(dst, src, r_width *
sizeof(Type));
294 }
else if (h_scale == 1) {
295 for (
unsigned int i = 0; i < r_height; i++) {
296 void *src = (
void *)(I[(i + i_min_u) * v_scale] + j_min_u);
297 void *dst = (
void *)
crop[i];
298 memcpy(dst, src, r_width *
sizeof(Type));
301 for (
unsigned int i = 0; i < r_height; i++) {
302 for (
unsigned int j = 0; j < r_width; j++) {
303 crop[i][j] = I[(i + i_min_u) * v_scale][(j + j_min_u) * h_scale];
327 template <
class Type>
329 unsigned int roi_width,
vpImage<Type> &crop,
unsigned int v_scale,
unsigned int h_scale)
351 template <
class Type>
353 unsigned int h_scale)
376 template <
class Type>
378 vpImage<Type> &crop,
unsigned int v_scale,
unsigned int h_scale)
380 int i_min = (std::max)((
int)(ceil(roi.
getTop() / v_scale)), 0);
381 int j_min = (std::max)((
int)(ceil(roi.
getLeft() / h_scale)), 0);
382 int i_max = (std::min)((
int)(ceil((roi.
getTop() + roi.
getHeight()) / v_scale)), (
int)(height / v_scale));
383 int j_max = (std::min)((
int)(ceil((roi.
getLeft() + roi.
getWidth()) / h_scale)), (
int)(width / h_scale));
385 unsigned int i_min_u = (
unsigned int)i_min;
386 unsigned int j_min_u = (
unsigned int)j_min;
388 unsigned int r_width = (
unsigned int)(j_max - j_min);
389 unsigned int r_height = (
unsigned int)(i_max - i_min);
391 crop.resize(r_height, r_width);
393 if (v_scale == 1 && h_scale == 1) {
394 for (
unsigned int i = 0; i < r_height; i++) {
395 void *src = (
void *)(bitmap + ((i + i_min_u) * width + j_min_u) *
sizeof(Type));
396 void *dst = (
void *)
crop[i];
397 memcpy(dst, src, r_width *
sizeof(Type));
399 }
else if (h_scale == 1) {
400 for (
unsigned int i = 0; i < r_height; i++) {
401 void *src = (
void *)(bitmap + ((i + i_min_u) * width * v_scale + j_min_u) *
sizeof(Type));
402 void *dst = (
void *)
crop[i];
403 memcpy(dst, src, r_width *
sizeof(Type));
406 for (
unsigned int i = 0; i < r_height; i++) {
407 unsigned int i_src = (i + i_min_u) * width * v_scale + j_min_u * h_scale;
408 for (
unsigned int j = 0; j < r_width; j++) {
409 void *src = (
void *)(bitmap + (i_src + j * h_scale) *
sizeof(Type));
410 void *dst = (
void *)&
crop[i][j];
411 memcpy(dst, src,
sizeof(Type));
429 template <
class Type>
431 Type value3,
const bool useLUT)
434 std::cerr <<
"LUT not available for this type ! Will use the iteration method." << std::endl;
440 for (; p < pend; p++) {
444 else if (v > threshold2)
465 unsigned char value1,
unsigned char value2,
unsigned char value3,
const bool useLUT)
469 unsigned char lut[256];
470 for (
unsigned int i = 0; i < 256; i++) {
471 lut[i] = i < threshold1 ? value1 : (i > threshold2 ? value3 : value2);
476 unsigned char *p = I.
bitmap;
478 for (; p < pend; p++) {
479 unsigned char v = *p;
482 else if (v > threshold2)
490 #ifdef VISP_HAVE_PTHREAD
492 #ifndef DOXYGEN_SHOULD_SKIP_THIS
493 template <
class Type>
class vpUndistortInternalType
501 unsigned int nthreads;
502 unsigned int threadid;
505 vpUndistortInternalType() : src(NULL), dst(NULL), width(0), height(0), cam(), nthreads(0), threadid(0) {}
507 vpUndistortInternalType(
const vpUndistortInternalType<Type> &u) { *
this = u; }
508 vpUndistortInternalType &operator=(
const vpUndistortInternalType<Type> &u)
515 nthreads = u.nthreads;
516 threadid = u.threadid;
521 static void *vpUndistort_threaded(
void *arg);
524 template <
class Type>
void *vpUndistortInternalType<Type>::vpUndistort_threaded(
void *arg)
526 vpUndistortInternalType<Type> *undistortSharedData =
static_cast<vpUndistortInternalType<Type> *
>(arg);
527 int offset = (int)undistortSharedData->threadid;
528 int width = (
int)undistortSharedData->width;
529 int height = (int)undistortSharedData->height;
530 int nthreads = (
int)undistortSharedData->nthreads;
532 double u0 = undistortSharedData->cam.get_u0();
533 double v0 = undistortSharedData->cam.get_v0();
534 double px = undistortSharedData->cam.get_px();
535 double py = undistortSharedData->cam.get_py();
536 double kud = undistortSharedData->cam.get_kud();
538 double invpx = 1.0 / px;
539 double invpy = 1.0 / py;
541 double kud_px2 = kud * invpx * invpx;
542 double kud_py2 = kud * invpy * invpy;
544 Type *dst = undistortSharedData->dst + (height / nthreads * offset) * width;
545 Type *src = undistortSharedData->src;
547 for (
double v = height / nthreads * offset; v < height / nthreads * (offset + 1); v++) {
548 double deltav = v - v0;
550 double fr1 = 1.0 + kud_py2 * deltav * deltav;
552 for (
double u = 0; u < width; u++) {
554 double deltau = u - u0;
556 double fr2 = fr1 + kud_px2 * deltau * deltau;
558 double u_double = deltau * fr2 + u0;
559 double v_double = deltav * fr2 + v0;
564 int u_round = (int)(u_double);
565 int v_round = (int)(v_double);
570 double du_double = (u_double) - (
double)u_round;
571 double dv_double = (v_double) - (
double)v_round;
574 if ((0 <= u_round) && (0 <= v_round) && (u_round < ((width)-1)) && (v_round < ((height)-1))) {
576 const Type *_mp = &src[v_round * width + u_round];
577 v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
579 v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
580 *dst = (Type)(v01 + ((v23 - v01) * dv_double));
588 pthread_exit((
void *)0);
591 #endif // DOXYGEN_SHOULD_SKIP_THIS
592 #endif // VISP_HAVE_PTHREAD
620 template <
class Type>
622 unsigned int nThreads)
624 #ifdef VISP_HAVE_PTHREAD
631 undistI.
resize(height, width);
636 if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
642 unsigned int nthreads = nThreads;
644 pthread_t *callThd =
new pthread_t[nthreads];
645 pthread_attr_init(&attr);
646 pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
648 vpUndistortInternalType<Type> *undistortSharedData;
649 undistortSharedData =
new vpUndistortInternalType<Type>[nthreads];
651 for (
unsigned int i = 0; i < nthreads; i++) {
654 undistortSharedData[i].src = I.
bitmap;
655 undistortSharedData[i].dst = undistI.
bitmap;
656 undistortSharedData[i].width = I.
getWidth();
657 undistortSharedData[i].height = I.
getHeight();
658 undistortSharedData[i].cam = cam;
659 undistortSharedData[i].nthreads = nthreads;
660 undistortSharedData[i].threadid = i;
661 pthread_create(&callThd[i], &attr, &vpUndistortInternalType<Type>::vpUndistort_threaded, &undistortSharedData[i]);
663 pthread_attr_destroy(&attr);
666 for (
unsigned int i = 0; i < nthreads; i++) {
668 pthread_join(callThd[i], NULL);
672 delete[] undistortSharedData;
673 #else // VISP_HAVE_PTHREAD
680 undistI.
resize(height, width);
689 if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
695 double invpx = 1.0 / px;
696 double invpy = 1.0 / py;
698 double kud_px2 = kud * invpx * invpx;
699 double kud_py2 = kud * invpy * invpy;
701 Type *dst = undistI.
bitmap;
702 for (
double v = 0; v < height; v++) {
703 double deltav = v - v0;
705 double fr1 = 1.0 + kud_py2 * deltav * deltav;
707 for (
double u = 0; u < width; u++) {
709 double deltau = u - u0;
711 double fr2 = fr1 + kud_px2 * deltau * deltau;
713 double u_double = deltau * fr2 + u0;
714 double v_double = deltav * fr2 + v0;
721 int u_round = (int)(u_double);
722 int v_round = (int)(v_double);
727 double du_double = (u_double) - (
double)u_round;
728 double dv_double = (v_double) - (
double)v_round;
731 if ((0 <= u_round) && (0 <= v_round) && (u_round < (((
int)width) - 1)) && (v_round < (((
int)height) - 1))) {
733 const Type *_mp = &I[(
unsigned int)v_round][(
unsigned int)u_round];
734 v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
736 v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
737 *dst = (Type)(v01 + ((v23 - v01) * dv_double));
745 #endif // VISP_HAVE_PTHREAD
752 undistI.
resize(height,width);
766 for(
int v = 0 ; v < height; v++){
767 for(
int u = 0; u < height; u++){
770 double u_double = ((double)u - u0)*(1.0+kd*r2) + u0;
771 double v_double = ((double)v - v0)*(1.0+kd*r2) + v0;
772 undistI[v][u] = I.getPixelBI((
float)u_double,(
float)v_double);
787 unsigned int height = 0, width = 0;
791 newI.
resize(height, width);
793 for (
unsigned int i = 0; i < height; i++) {
794 memcpy(newI.
bitmap + i * width, I.
bitmap + (height - 1 - i) * width, width *
sizeof(Type));
831 unsigned int height = 0, width = 0;
839 for (i = 0; i < height / 2; i++) {
840 memcpy(Ibuf.
bitmap, I.
bitmap + i * width, width *
sizeof(Type));
842 memcpy(I.
bitmap + i * width, I.
bitmap + (height - 1 - i) * width, width *
sizeof(Type));
843 memcpy(I.
bitmap + (height - 1 - i) * width, Ibuf.
bitmap, width *
sizeof(Type));
847 template <
class Type> Type vpImageTools::getPixelClamped(
const vpImage<Type> &I,
const float u,
const float v)
852 else if (u > (
float)I.
getWidth() - 1.)
869 template <
class Type>
871 const unsigned int j,
const float u,
const float v,
const float xFrac,
875 Type p00 = getPixelClamped(I, u - 1, v - 1);
876 Type p01 = getPixelClamped(I, u + 0, v - 1);
877 Type p02 = getPixelClamped(I, u + 1, v - 1);
878 Type p03 = getPixelClamped(I, u + 2, v - 1);
881 Type p10 = getPixelClamped(I, u - 1, v + 0);
882 Type p11 = getPixelClamped(I, u + 0, v + 0);
883 Type p12 = getPixelClamped(I, u + 1, v + 0);
884 Type p13 = getPixelClamped(I, u + 2, v + 0);
887 Type p20 = getPixelClamped(I, u - 1, v + 1);
888 Type p21 = getPixelClamped(I, u + 0, v + 1);
889 Type p22 = getPixelClamped(I, u + 1, v + 1);
890 Type p23 = getPixelClamped(I, u + 2, v + 1);
893 Type p30 = getPixelClamped(I, u - 1, v + 2);
894 Type p31 = getPixelClamped(I, u + 0, v + 2);
895 Type p32 = getPixelClamped(I, u + 1, v + 2);
896 Type p33 = getPixelClamped(I, u + 2, v + 2);
898 float col0 = cubicHermite(p00, p01, p02, p03, xFrac);
899 float col1 = cubicHermite(p10, p11, p12, p13, xFrac);
900 float col2 = cubicHermite(p20, p21, p22, p23, xFrac);
901 float col3 = cubicHermite(p30, p31, p32, p33, xFrac);
902 float value = cubicHermite(col0, col1, col2, col3, yFrac);
903 Ires[i][j] = vpMath::saturate<Type>(value);
908 const unsigned int j,
const float u,
const float v,
const float xFrac,
912 vpRGBa p00 = getPixelClamped(I, u - 1, v - 1);
913 vpRGBa p01 = getPixelClamped(I, u + 0, v - 1);
914 vpRGBa p02 = getPixelClamped(I, u + 1, v - 1);
915 vpRGBa p03 = getPixelClamped(I, u + 2, v - 1);
918 vpRGBa p10 = getPixelClamped(I, u - 1, v + 0);
919 vpRGBa p11 = getPixelClamped(I, u + 0, v + 0);
920 vpRGBa p12 = getPixelClamped(I, u + 1, v + 0);
921 vpRGBa p13 = getPixelClamped(I, u + 2, v + 0);
924 vpRGBa p20 = getPixelClamped(I, u - 1, v + 1);
925 vpRGBa p21 = getPixelClamped(I, u + 0, v + 1);
926 vpRGBa p22 = getPixelClamped(I, u + 1, v + 1);
927 vpRGBa p23 = getPixelClamped(I, u + 2, v + 1);
930 vpRGBa p30 = getPixelClamped(I, u - 1, v + 2);
931 vpRGBa p31 = getPixelClamped(I, u + 0, v + 2);
932 vpRGBa p32 = getPixelClamped(I, u + 1, v + 2);
933 vpRGBa p33 = getPixelClamped(I, u + 2, v + 2);
935 for (
int c = 0; c < 3; c++) {
936 float col0 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p00)[c]),
937 static_cast<float>(reinterpret_cast<unsigned char *>(&p01)[c]),
938 static_cast<float>(reinterpret_cast<unsigned char *>(&p02)[c]),
939 static_cast<float>(reinterpret_cast<unsigned char *>(&p03)[c]), xFrac);
940 float col1 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p10)[c]),
941 static_cast<float>(reinterpret_cast<unsigned char *>(&p11)[c]),
942 static_cast<float>(reinterpret_cast<unsigned char *>(&p12)[c]),
943 static_cast<float>(reinterpret_cast<unsigned char *>(&p13)[c]), xFrac);
944 float col2 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p20)[c]),
945 static_cast<float>(reinterpret_cast<unsigned char *>(&p21)[c]),
946 static_cast<float>(reinterpret_cast<unsigned char *>(&p22)[c]),
947 static_cast<float>(reinterpret_cast<unsigned char *>(&p23)[c]), xFrac);
948 float col3 = cubicHermite(static_cast<float>(reinterpret_cast<unsigned char *>(&p30)[c]),
949 static_cast<float>(reinterpret_cast<unsigned char *>(&p31)[c]),
950 static_cast<float>(reinterpret_cast<unsigned char *>(&p32)[c]),
951 static_cast<float>(reinterpret_cast<unsigned char *>(&p33)[c]), xFrac);
952 float value = cubicHermite(col0, col1, col2, col3, yFrac);
954 reinterpret_cast<unsigned char *>(&Ires[i][j])[c] = vpMath::saturate<unsigned char>(value);
958 template <
class Type>
960 const unsigned int j,
const float u,
const float v,
const float xFrac,
963 unsigned int u0 = (
unsigned int)u;
964 unsigned int v0 = (
unsigned int)v;
966 unsigned int u1 = (std::min)(I.
getWidth() - 1, (
unsigned int)u + 1);
967 unsigned int v1 = v0;
969 unsigned int u2 = u0;
970 unsigned int v2 = (std::min)(I.
getHeight() - 1, (
unsigned int)v + 1);
972 unsigned int u3 = u1;
973 unsigned int v3 = v2;
975 float col0 = lerp(I[v0][u0], I[v1][u1], xFrac);
976 float col1 = lerp(I[v2][u2], I[v3][u3], xFrac);
977 float value = lerp(col0, col1, yFrac);
979 Ires[i][j] = vpMath::saturate<Type>(value);
984 const unsigned int j,
const float u,
const float v,
const float xFrac,
987 unsigned int u0 = (
unsigned int)u;
988 unsigned int v0 = (
unsigned int)v;
990 unsigned int u1 = (std::min)(I.
getWidth() - 1, (
unsigned int)u + 1);
991 unsigned int v1 = v0;
993 unsigned int u2 = u0;
994 unsigned int v2 = (std::min)(I.
getHeight() - 1, (
unsigned int)v + 1);
996 unsigned int u3 = (std::min)(I.
getWidth() - 1, (
unsigned int)u + 1);
997 unsigned int v3 = (std::min)(I.
getHeight() - 1, (
unsigned int)v + 1);
999 for (
int c = 0; c < 3; c++) {
1000 float col0 = lerp(static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v0][u0])[c]),
1001 static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v1][u1])[c]), xFrac);
1002 float col1 = lerp(static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v2][u2])[c]),
1003 static_cast<float>(reinterpret_cast<const unsigned char *>(&I[v3][u3])[c]), xFrac);
1004 float value = lerp(col0, col1, yFrac);
1006 reinterpret_cast<unsigned char *>(&Ires[i][j])[c] = vpMath::saturate<unsigned char>(value);
1010 template <
class Type>
1012 const unsigned int j,
const float u,
const float v)
1014 Ires[i][j] = getPixelClamped(I, u, v);
1029 template <
class Type>
1033 Ires.
resize(height, width);
1048 template <
class Type>
1052 std::cerr <<
"Input or output image is too small!" << std::endl;
1064 for (
unsigned int i = 0; i < Ires.
getHeight(); i++) {
1065 float v = i * scaleY;
1066 float yFrac = v - (int)v;
1068 for (
unsigned int j = 0; j < Ires.
getWidth(); j++) {
1069 float u = j * scaleX;
1070 float xFrac = u - (int)u;
1073 resizeNearest(I, Ires, i, j, u, v);
1075 resizeBilinear(I, Ires, i, j, u, v, xFrac, yFrac);
1077 resizeBicubic(I, Ires, i, j, u, v, xFrac, yFrac);