39 #include <visp3/core/vpCPUFeatures.h>
40 #include <visp3/core/vpImageConvert.h>
41 #include <visp3/core/vpImageTools.h>
43 #include <Simd/SimdLib.hpp>
101 unsigned char B_star)
110 double factor = (double)(B_star - A_star) / (double)(B - A);
112 for (
unsigned int i = 0; i < I.
getHeight(); i++)
113 for (
unsigned int j = 0; j < I.
getWidth(); j++) {
121 I[i][j] = (
unsigned char)(A_star + factor * (v - A));
168 "Cannot compute image difference. The two images "
169 "(%ux%u) and (%ux%u) have not the same size",
177 SimdImageDifference(
reinterpret_cast<unsigned char *
>(I1.
bitmap),
reinterpret_cast<unsigned char *
>(I2.
bitmap),
178 I1.
getSize() * 4,
reinterpret_cast<unsigned char *
>(Idiff.
bitmap));
203 for (
unsigned int b = 0; b < n; b++) {
227 for (
unsigned int b = 0; b < n; b++) {
256 for (
unsigned int b = 0; b < n; b++) {
293 typedef Simd::View<Simd::Allocator> View;
298 Simd::OperationBinary8u(img1, img2, imgAdd,
299 saturate ? SimdOperationBinary8uSaturatedAddition : SimdOperationBinary8uAddition);
326 typedef Simd::View<Simd::Allocator> View;
331 Simd::OperationBinary8u(img1, img2, imgAdd,
332 saturate ? SimdOperationBinary8uSaturatedSubtraction : SimdOperationBinary8uSubtraction);
350 mapU.
resize(height, width,
false,
false);
351 mapV.
resize(height, width,
false,
false);
352 mapDu.
resize(height, width,
false,
false);
353 mapDv.
resize(height, width,
false,
false);
356 bool is_KannalaBrandt =
359 float u0 =
static_cast<float>(cam.
get_u0());
360 float v0 =
static_cast<float>(cam.
get_v0());
361 float px =
static_cast<float>(cam.
get_px());
362 float py =
static_cast<float>(cam.
get_py());
364 std::vector<double> dist_coefs;
366 if (!is_KannalaBrandt)
367 kud =
static_cast<float>(cam.
get_kud());
372 if (!is_KannalaBrandt && std::fabs(
static_cast<double>(kud)) <= std::numeric_limits<double>::epsilon()) {
374 for (
unsigned int i = 0; i < height; i++) {
375 for (
unsigned int j = 0; j < width; j++) {
376 mapU[i][j] =
static_cast<int>(j);
377 mapV[i][j] =
static_cast<int>(i);
387 float kud_px2 = 0., kud_py2 = 0., deltau_px, deltav_py;
389 float deltav, deltau;
390 float u_float, v_float;
391 int u_round, v_round;
393 double theta, theta_d;
394 double theta2, theta4, theta6, theta8;
399 if (!is_KannalaBrandt) {
400 kud_px2 = kud * invpx * invpx;
401 kud_py2 = kud * invpy * invpy;
404 for (
unsigned int v = 0; v < height; v++) {
407 if (!is_KannalaBrandt)
408 fr1 = 1.0f + kud_py2 * deltav * deltav;
410 deltav_py = deltav * invpy;
412 for (
unsigned int u = 0; u < width; u++) {
415 if (!is_KannalaBrandt) {
416 fr2 = fr1 + kud_px2 * deltau * deltau;
418 u_float = deltau * fr2 + u0;
419 v_float = deltav * fr2 + v0;
423 deltau_px = deltau * invpx;
429 theta6 = theta2 * theta4;
432 theta_d = theta * (1 + dist_coefs[0] * theta2 + dist_coefs[1] * theta4 + dist_coefs[2] * theta6 +
433 dist_coefs[3] * theta8);
436 scale = (std::fabs(r) < std::numeric_limits<double>::epsilon()) ? 1.0 : theta_d / r;
437 u_float =
static_cast<float>(deltau * scale + u0);
438 v_float =
static_cast<float>(deltav * scale + v0);
441 u_round =
static_cast<int>(u_float);
442 v_round =
static_cast<int>(v_float);
444 mapU[v][u] = u_round;
445 mapV[v][u] = v_round;
447 mapDu[v][u] = u_float - u_round;
448 mapDv[v][u] = v_float - v_round;
467 std::cerr <<
"Error, input image is empty." << std::endl;
474 for (
unsigned int i = 1; i < II.
getHeight(); i++) {
475 for (
unsigned int j = 1; j < II.
getWidth(); j++) {
476 II[i][j] = I[i - 1][j - 1] + II[i - 1][j] + II[i][j - 1] - II[i - 1][j - 1];
477 IIsq[i][j] =
vpMath::sqr(I[i - 1][j - 1]) + IIsq[i - 1][j] + IIsq[i][j - 1] - IIsq[i - 1][j - 1];
493 "Error: in vpImageTools::normalizedCorrelation(): "
494 "image dimension mismatch between I1=%ux%u and I2=%ux%u",
505 SimdNormalizedCorrelation(I1.
bitmap, a, I2.
bitmap, b, I1.
getSize(), a2, b2, ab, useOptimized);
507 return ab / sqrt(a2 * b2);
521 for (
unsigned int i = 0; i < height; ++i)
522 for (
unsigned int j = 0; j < width; ++j)
524 for (
unsigned int j = 0; j < width; ++j)
535 for (
unsigned int i = 0; i < I.
getHeight(); ++i)
536 for (
unsigned int j = 0; j < I.
getWidth(); ++j)
537 I(i, j, I(i, j) / s);
554 int x1 = (int)floor(point.
get_i());
555 int x2 = (int)ceil(point.
get_i());
556 int y1 = (int)floor(point.
get_j());
557 int y2 = (int)ceil(point.
get_j());
563 v1 = (x2 - point.
get_i()) * I(x1, y1) + (point.
get_i() - x1) * I(x2, y1);
564 v2 = (x2 - point.
get_i()) * I(x1, y2) + (point.
get_i() - x1) * I(x2, y2);
568 return (y2 - point.
get_j()) * v1 + (point.
get_j() - y1) * v2;
572 "vpImageTools::interpolate(): bi-cubic interpolation is not implemented.");
594 for (
unsigned int x = 0; x < x_d; ++x) {
595 for (
unsigned int y = 0; y < y_d; ++y) {
617 for (
unsigned int x = 0; x < x_d; ++x) {
618 for (
unsigned int y = 0; y < y_d; ++y) {
645 std::cerr <<
"Error, input image is empty." << std::endl;
650 std::cerr <<
"Error, template image is empty." << std::endl;
655 std::cerr <<
"Error, template image is bigger than input image." << std::endl;
674 const double sum2 = (II_tpl[height_tpl][width_tpl] + II_tpl[0][0] - II_tpl[0][width_tpl] - II_tpl[height_tpl][0]);
675 const double mean2 = sum2 / I_tpl.
getSize();
676 for (
unsigned int cpt = 0; cpt < I_tpl_double.
getSize(); cpt++) {
677 I_tpl_double.
bitmap[cpt] -= mean2;
680 #if defined _OPENMP && _OPENMP >= 200711
681 #pragma omp parallel for schedule(dynamic)
682 for (
unsigned int i = 0; i < I.
getHeight() - height_tpl; i += step_v) {
683 for (
unsigned int j = 0; j < I.
getWidth() - width_tpl; j += step_u) {
689 int end = (int)((I.
getHeight() - height_tpl) / step_v) + 1;
690 std::vector<unsigned int> vec_step_v((
size_t)end);
691 for (
unsigned int cpt = 0, idx = 0; cpt < I.
getHeight() - height_tpl; cpt += step_v, idx++) {
692 vec_step_v[(size_t)idx] = cpt;
695 #pragma omp parallel for schedule(dynamic)
697 for (
int cpt = 0; cpt < end; cpt++) {
698 for (
unsigned int j = 0; j < I.
getWidth() - width_tpl; j += step_u) {
699 I_score[vec_step_v[cpt]][j] =
707 for (
unsigned int i = 0; i < I.
getHeight() - height_tpl; i += step_v) {
708 for (
unsigned int j = 0; j < I.
getWidth() - width_tpl; j += step_u) {
725 float vpImageTools::cubicHermite(
const float A,
const float B,
const float C,
const float D,
const float t)
727 float a = (-A + 3.0f * B - 3.0f * C + D) / 2.0f;
728 float b = A + 2.0f * C - (5.0f * B + D) / 2.0f;
729 float c = (-A + C) / 2.0f;
732 return a * t * t * t + b * t * t + c * t + d;
735 int vpImageTools::coordCast(
double x) {
return x < 0 ? -1 :
static_cast<int>(x); }
737 double vpImageTools::lerp(
double A,
double B,
double t) {
return A * (1.0 - t) + B * t; }
739 float vpImageTools::lerp(
float A,
float B,
float t) {
return A * (1.0f - t) + B * t; }
741 int64_t vpImageTools::lerp2(int64_t A, int64_t B, int64_t t, int64_t t_1) {
return A * t_1 + B * t; }
746 unsigned int i0,
unsigned int j0)
753 (II[i0 + height_tpl][j0 + width_tpl] + II[i0][j0] - II[i0][j0 + width_tpl] - II[i0 + height_tpl][j0]);
754 const double sum2 = (II_tpl[height_tpl][width_tpl] + II_tpl[0][0] - II_tpl[0][width_tpl] - II_tpl[height_tpl][0]);
763 return ab / sqrt(a2 * b2);
782 #pragma omp parallel for schedule(dynamic)
784 for (
int i_ = 0; i_ < static_cast<int>(I.
getHeight()); i_++) {
785 const unsigned int i =
static_cast<unsigned int>(i_);
786 for (
unsigned int j = 0; j < I.
getWidth(); j++) {
788 int u_round = mapU[i][j];
789 int v_round = mapV[i][j];
791 float du = mapDu[i][j];
792 float dv = mapDv[i][j];
794 if (0 <= u_round && 0 <= v_round && u_round <
static_cast<int>(I.
getWidth()) - 1 &&
795 v_round <
static_cast<int>(I.
getHeight()) - 1) {
797 float col0 = lerp(I[v_round][u_round], I[v_round][u_round + 1], du);
798 float col1 = lerp(I[v_round + 1][u_round], I[v_round + 1][u_round + 1], du);
799 float value = lerp(col0, col1, dv);
801 Iundist[i][j] =
static_cast<unsigned char>(value);
825 #pragma omp parallel for schedule(dynamic)
827 for (
int i = 0; i < static_cast<int>(I.
getHeight()); i++) {
829 mapV.
data, mapDu.
data, mapDv.
data,
reinterpret_cast<unsigned char *
>(Iundist.
bitmap));
833 void vpImageTools::resizeSimdlib(
const vpImage<vpRGBa> &Isrc,
unsigned int resizeWidth,
unsigned int resizeHeight,
836 Idst.
resize(resizeHeight, resizeWidth);
838 typedef Simd::View<Simd::Allocator> View;
842 Simd::Resize(src, dst, method ==
INTERPOLATION_LINEAR ? SimdResizeMethodBilinear : SimdResizeMethodArea);
848 Idst.
resize(resizeHeight, resizeWidth);
850 typedef Simd::View<Simd::Allocator> View;
854 Simd::Resize(src, dst, method ==
INTERPOLATION_LINEAR ? SimdResizeMethodBilinear : SimdResizeMethodArea);
857 bool vpImageTools::checkFixedPoint(
unsigned int x,
unsigned int y,
const vpMatrix &T,
bool affine)
865 double a6 = affine ? 0.0 : T[2][0];
866 double a7 = affine ? 0.0 : T[2][1];
867 double a8 = affine ? 1.0 : T[2][2];
869 double w = a6 * x + a7 * y + a8;
870 double x2 = (a0 * x + a1 * y + a2) / w;
871 double y2 = (a3 * x + a4 * y + a5) / w;
873 const double limit = 1 << 15;
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)
Generic class defining intrinsic camera parameters.
std::vector< double > getKannalaBrandtDistortionCoefficients() const
vpCameraParametersProjType
@ ProjWithKannalaBrandtDistortion
vpCameraParametersProjType get_projModel() const
error that can be emited by ViSP classes.
@ dimensionError
Bad dimension.
@ notImplementedError
Not implemented.
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Error that can be emited by the vpImage class and its derivates.
@ incorrectInitializationError
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Type getMeanValue() const
Return the mean value of the bitmap.
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
static double sqr(double x)
static Type abs(const Type &x)
static int round(double x)
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)