36 #include <visp3/core/vpCPUFeatures.h>
37 #include <visp3/core/vpImageConvert.h>
38 #include <visp3/core/vpImageTools.h>
40 #include <Simd/SimdLib.hpp>
107 double factor = (double)(B_star - A_star) / (double)(B - A);
109 for (
unsigned int i = 0; i < I.
getHeight(); i++)
110 for (
unsigned int j = 0; j < I.
getWidth(); j++) {
118 I[i][j] = (
unsigned char)(A_star + factor * (v - A));
165 "Cannot compute image difference. The two images "
166 "(%ux%u) and (%ux%u) have not the same size",
174 SimdImageDifference(
reinterpret_cast<unsigned char *
>(I1.
bitmap),
reinterpret_cast<unsigned char *
>(I2.
bitmap),
175 I1.
getSize() * 4,
reinterpret_cast<unsigned char *
>(Idiff.
bitmap));
200 for (
unsigned int b = 0; b < n; b++) {
224 for (
unsigned int b = 0; b < n; b++) {
253 for (
unsigned int b = 0; b < n; b++) {
290 typedef Simd::View<Simd::Allocator> View;
295 Simd::OperationBinary8u(img1, img2, imgAdd,
296 saturate ? SimdOperationBinary8uSaturatedAddition : SimdOperationBinary8uAddition);
323 typedef Simd::View<Simd::Allocator> View;
328 Simd::OperationBinary8u(img1, img2, imgAdd,
329 saturate ? SimdOperationBinary8uSaturatedSubtraction : SimdOperationBinary8uSubtraction);
347 mapU.
resize(height, width,
false,
false);
348 mapV.
resize(height, width,
false,
false);
349 mapDu.
resize(height, width,
false,
false);
350 mapDv.
resize(height, width,
false,
false);
353 bool is_KannalaBrandt =
356 float u0 =
static_cast<float>(cam.
get_u0());
357 float v0 =
static_cast<float>(cam.
get_v0());
358 float px =
static_cast<float>(cam.
get_px());
359 float py =
static_cast<float>(cam.
get_py());
361 std::vector<double> dist_coefs;
363 if (!is_KannalaBrandt)
364 kud =
static_cast<float>(cam.
get_kud());
369 if (!is_KannalaBrandt && std::fabs(
static_cast<double>(kud)) <= std::numeric_limits<double>::epsilon()) {
371 for (
unsigned int i = 0; i < height; i++) {
372 for (
unsigned int j = 0; j < width; j++) {
373 mapU[i][j] =
static_cast<int>(j);
374 mapV[i][j] =
static_cast<int>(i);
384 float kud_px2 = 0., kud_py2 = 0., deltau_px, deltav_py = 0;
386 float deltav, deltau;
387 float u_float, v_float;
388 int u_round, v_round;
390 double theta, theta_d;
391 double theta2, theta4, theta6, theta8;
396 if (!is_KannalaBrandt) {
397 kud_px2 = kud * invpx * invpx;
398 kud_py2 = kud * invpy * invpy;
401 for (
unsigned int v = 0; v < height; v++) {
404 if (!is_KannalaBrandt)
405 fr1 = 1.0f + kud_py2 * deltav * deltav;
407 deltav_py = deltav * invpy;
409 for (
unsigned int u = 0; u < width; u++) {
412 if (!is_KannalaBrandt) {
413 fr2 = fr1 + kud_px2 * deltau * deltau;
415 u_float = deltau * fr2 + u0;
416 v_float = deltav * fr2 + v0;
420 deltau_px = deltau * invpx;
426 theta6 = theta2 * theta4;
429 theta_d = theta * (1 + dist_coefs[0] * theta2 + dist_coefs[1] * theta4 + dist_coefs[2] * theta6 +
430 dist_coefs[3] * theta8);
433 scale = (std::fabs(r) < std::numeric_limits<double>::epsilon()) ? 1.0 : theta_d / r;
434 u_float =
static_cast<float>(deltau * scale + u0);
435 v_float =
static_cast<float>(deltav * scale + v0);
438 u_round =
static_cast<int>(u_float);
439 v_round =
static_cast<int>(v_float);
441 mapU[v][u] = u_round;
442 mapV[v][u] = v_round;
444 mapDu[v][u] = u_float - u_round;
445 mapDv[v][u] = v_float - v_round;
464 std::cerr <<
"Error, input image is empty." << std::endl;
471 for (
unsigned int i = 1; i < II.
getHeight(); i++) {
472 for (
unsigned int j = 1; j < II.
getWidth(); j++) {
473 II[i][j] = I[i - 1][j - 1] + II[i - 1][j] + II[i][j - 1] - II[i - 1][j - 1];
474 IIsq[i][j] =
vpMath::sqr(I[i - 1][j - 1]) + IIsq[i - 1][j] + IIsq[i][j - 1] - IIsq[i - 1][j - 1];
490 "Error: in vpImageTools::normalizedCorrelation(): "
491 "image dimension mismatch between I1=%ux%u and I2=%ux%u",
502 SimdNormalizedCorrelation(I1.
bitmap, a, I2.
bitmap, b, I1.
getSize(), a2, b2, ab, useOptimized);
504 return ab / sqrt(a2 * b2);
518 for (
unsigned int i = 0; i < height; ++i)
519 for (
unsigned int j = 0; j < width; ++j)
521 for (
unsigned int j = 0; j < width; ++j)
532 for (
unsigned int i = 0; i < I.
getHeight(); ++i)
533 for (
unsigned int j = 0; j < I.
getWidth(); ++j)
534 I(i, j, I(i, j) / s);
551 int x1 = (int)floor(point.
get_i());
552 int x2 = (int)ceil(point.
get_i());
553 int y1 = (int)floor(point.
get_j());
554 int y2 = (int)ceil(point.
get_j());
560 v1 = (x2 - point.
get_i()) * I(x1, y1) + (point.
get_i() - x1) * I(x2, y1);
561 v2 = (x2 - point.
get_i()) * I(x1, y2) + (point.
get_i() - x1) * I(x2, y2);
565 return (y2 - point.
get_j()) * v1 + (point.
get_j() - y1) * v2;
569 "vpImageTools::interpolate(): bi-cubic interpolation is not implemented.");
591 for (
unsigned int x = 0; x < x_d; ++x) {
592 for (
unsigned int y = 0; y < y_d; ++y) {
614 for (
unsigned int x = 0; x < x_d; ++x) {
615 for (
unsigned int y = 0; y < y_d; ++y) {
642 std::cerr <<
"Error, input image is empty." << std::endl;
647 std::cerr <<
"Error, template image is empty." << std::endl;
652 std::cerr <<
"Error, template image is bigger than input image." << std::endl;
671 const double sum2 = (II_tpl[height_tpl][width_tpl] + II_tpl[0][0] - II_tpl[0][width_tpl] - II_tpl[height_tpl][0]);
672 const double mean2 = sum2 / I_tpl.
getSize();
673 for (
unsigned int cpt = 0; cpt < I_tpl_double.
getSize(); cpt++) {
674 I_tpl_double.
bitmap[cpt] -= mean2;
677 #if defined _OPENMP && _OPENMP >= 200711
678 #pragma omp parallel for schedule(dynamic)
679 for (
unsigned int i = 0; i < I.
getHeight() - height_tpl; i += step_v) {
680 for (
unsigned int j = 0; j < I.
getWidth() - width_tpl; j += step_u) {
686 int end = (int)((I.
getHeight() - height_tpl) / step_v) + 1;
687 std::vector<unsigned int> vec_step_v((
size_t)end);
688 for (
unsigned int cpt = 0, idx = 0; cpt < I.
getHeight() - height_tpl; cpt += step_v, idx++) {
689 vec_step_v[(size_t)idx] = cpt;
692 #pragma omp parallel for schedule(dynamic)
694 for (
int cpt = 0; cpt < end; cpt++) {
695 for (
unsigned int j = 0; j < I.
getWidth() - width_tpl; j += step_u) {
696 I_score[vec_step_v[cpt]][j] =
704 for (
unsigned int i = 0; i < I.
getHeight() - height_tpl; i += step_v) {
705 for (
unsigned int j = 0; j < I.
getWidth() - width_tpl; j += step_u) {
722 float vpImageTools::cubicHermite(
const float A,
const float B,
const float C,
const float D,
const float t)
724 float a = (-A + 3.0f * B - 3.0f * C + D) / 2.0f;
725 float b = A + 2.0f * C - (5.0f * B + D) / 2.0f;
726 float c = (-A + C) / 2.0f;
729 return a * t * t * t + b * t * t + c * t + d;
732 int vpImageTools::coordCast(
double x) {
return x < 0 ? -1 :
static_cast<int>(x); }
734 double vpImageTools::lerp(
double A,
double B,
double t) {
return A * (1.0 - t) + B * t; }
736 float vpImageTools::lerp(
float A,
float B,
float t) {
return A * (1.0f - t) + B * t; }
738 int64_t vpImageTools::lerp2(int64_t A, int64_t B, int64_t t, int64_t t_1) {
return A * t_1 + B * t; }
743 unsigned int i0,
unsigned int j0)
750 (II[i0 + height_tpl][j0 + width_tpl] + II[i0][j0] - II[i0][j0 + width_tpl] - II[i0 + height_tpl][j0]);
751 const double sum2 = (II_tpl[height_tpl][width_tpl] + II_tpl[0][0] - II_tpl[0][width_tpl] - II_tpl[height_tpl][0]);
760 return ab / sqrt(a2 * b2);
779 #pragma omp parallel for schedule(dynamic)
781 for (
int i_ = 0; i_ < static_cast<int>(I.
getHeight()); i_++) {
782 const unsigned int i =
static_cast<unsigned int>(i_);
783 for (
unsigned int j = 0; j < I.
getWidth(); j++) {
785 int u_round = mapU[i][j];
786 int v_round = mapV[i][j];
788 float du = mapDu[i][j];
789 float dv = mapDv[i][j];
791 if (0 <= u_round && 0 <= v_round && u_round <
static_cast<int>(I.
getWidth()) - 1 &&
792 v_round <
static_cast<int>(I.
getHeight()) - 1) {
794 float col0 = lerp(I[v_round][u_round], I[v_round][u_round + 1], du);
795 float col1 = lerp(I[v_round + 1][u_round], I[v_round + 1][u_round + 1], du);
796 float value = lerp(col0, col1, dv);
798 Iundist[i][j] =
static_cast<unsigned char>(value);
822 #pragma omp parallel for schedule(dynamic)
824 for (
int i = 0; i < static_cast<int>(I.
getHeight()); i++) {
826 mapV.
data, mapDu.
data, mapDv.
data,
reinterpret_cast<unsigned char *
>(Iundist.
bitmap));
830 void vpImageTools::resizeSimdlib(
const vpImage<vpRGBa> &Isrc,
unsigned int resizeWidth,
unsigned int resizeHeight,
833 Idst.
resize(resizeHeight, resizeWidth);
835 typedef Simd::View<Simd::Allocator> View;
839 Simd::Resize(src, dst, method ==
INTERPOLATION_LINEAR ? SimdResizeMethodBilinear : SimdResizeMethodArea);
845 Idst.
resize(resizeHeight, resizeWidth);
847 typedef Simd::View<Simd::Allocator> View;
851 Simd::Resize(src, dst, method ==
INTERPOLATION_LINEAR ? SimdResizeMethodBilinear : SimdResizeMethodArea);
854 bool vpImageTools::checkFixedPoint(
unsigned int x,
unsigned int y,
const vpMatrix &T,
bool affine)
862 double a6 = affine ? 0.0 : T[2][0];
863 double a7 = affine ? 0.0 : T[2][1];
864 double a8 = affine ? 1.0 : T[2][2];
866 double w = a6 * x + a7 * y + a8;
867 double x2 = (a0 * x + a1 * y + a2) / w;
868 double y2 = (a3 * x + a4 * y + a5) / w;
870 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
Projection with Kannala-Brandt distortion model.
vpCameraParametersProjType get_projModel() const
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.
@ incorrectInitializationError
Wrong image initialization.
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)