/**************************************************************************** * * ViSP, open source Visual Servoing Platform software. * Copyright (C) 2005 - 2019 by Inria. All rights reserved. * * This software is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. * See the file LICENSE.txt at the root directory of this source * distribution for additional information about the GNU GPL. * * For using ViSP with software that can not be combined with the GNU * GPL, please contact Inria about acquiring a ViSP Professional * Edition License. * * See http://visp.inria.fr for more information. * * This software was developed at: * Inria Rennes - Bretagne Atlantique * Campus Universitaire de Beaulieu * 35042 Rennes Cedex * France * * If you have questions regarding the use of this file, please contact * Inria at visp@inria.fr * * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: * Image tools. * * Authors: * Fabien Spindler * *****************************************************************************/ #ifndef vpImageTools_H #define vpImageTools_H /*! \file vpImageTools.h \brief Various image tools; sub-image extraction, modification of the look up table, binarisation... */ #include #ifdef VISP_HAVE_PTHREAD #include #endif #include #include #include #include #include #include #include #include #include #if defined _OPENMP #include #endif /*! \class vpImageTools \ingroup group_core_image \brief Various image tools; sub-image extraction, modification of the look up table, binarisation... */ class VISP_EXPORT vpImageTools { public: enum vpImageInterpolationType { INTERPOLATION_NEAREST, /*!< Nearest neighbor interpolation. */ INTERPOLATION_LINEAR, /*!< Bi-linear interpolation (optimized by SIMD lib if enabled). */ INTERPOLATION_CUBIC, /*!< Bi-cubic interpolation. */ INTERPOLATION_AREA /*!< Area interpolation (optimized by SIMD lib if enabled). */ }; template static inline void binarise(vpImage &I, Type threshold1, Type threshold2, Type value1, Type value2, Type value3, bool useLUT = true); static void changeLUT(vpImage &I, unsigned char A, unsigned char newA, unsigned char B, unsigned char newB); template static void crop(const vpImage &I, double roi_top, double roi_left, unsigned int roi_height, unsigned int roi_width, vpImage &crop, unsigned int v_scale = 1, unsigned int h_scale = 1); static void columnMean(const vpImage &I, vpRowVector &result); template static void crop(const vpImage &I, const vpImagePoint &topLeft, unsigned int roi_height, unsigned int roi_width, vpImage &crop, unsigned int v_scale = 1, unsigned int h_scale = 1); template static void crop(const vpImage &I, const vpRect &roi, vpImage &crop, unsigned int v_scale = 1, unsigned int h_scale = 1); template static void crop(const unsigned char *bitmap, unsigned int width, unsigned int height, const vpRect &roi, vpImage &crop, unsigned int v_scale = 1, unsigned int h_scale = 1); static void extract(const vpImage &Src, vpImage &Dst, const vpRectOriented &r); static void extract(const vpImage &Src, vpImage &Dst, const vpRectOriented &r); template static void flip(const vpImage &I, vpImage &newI); template static void flip(vpImage &I); static void imageDifference(const vpImage &I1, const vpImage &I2, vpImage &Idiff); static void imageDifference(const vpImage &I1, const vpImage &I2, vpImage &Idiff); static void imageDifferenceAbsolute(const vpImage &I1, const vpImage &I2, vpImage &Idiff); static void imageDifferenceAbsolute(const vpImage &I1, const vpImage &I2, vpImage &Idiff); static void imageDifferenceAbsolute(const vpImage &I1, const vpImage &I2, vpImage &Idiff); static void imageAdd(const vpImage &I1, const vpImage &I2, vpImage &Ires, bool saturate = false); static void imageSubtract(const vpImage &I1, const vpImage &I2, vpImage &Ires, bool saturate = false); static void initUndistortMap(const vpCameraParameters &cam, unsigned int width, unsigned int height, vpArray2D &mapU, vpArray2D &mapV, vpArray2D &mapDu, vpArray2D &mapDv); static double interpolate(const vpImage &I, const vpImagePoint &point, const vpImageInterpolationType &method = INTERPOLATION_NEAREST); static void integralImage(const vpImage &I, vpImage &II, vpImage &IIsq); static double normalizedCorrelation(const vpImage &I1, const vpImage &I2, bool useOptimized = true); static void normalize(vpImage &I); static void remap(const vpImage &I, const vpArray2D &mapU, const vpArray2D &mapV, const vpArray2D &mapDu, const vpArray2D &mapDv, vpImage &Iundist); static void remap(const vpImage &I, const vpArray2D &mapU, const vpArray2D &mapV, const vpArray2D &mapDu, const vpArray2D &mapDv, vpImage &Iundist); template static void resize(const vpImage &I, vpImage &Ires, unsigned int width, unsigned int height, const vpImageInterpolationType &method = INTERPOLATION_NEAREST, unsigned int nThreads=0); template static void resize(const vpImage &I, vpImage &Ires, const vpImageInterpolationType &method = INTERPOLATION_NEAREST, unsigned int nThreads=0); static void templateMatching(const vpImage &I, const vpImage &I_tpl, vpImage &I_score, unsigned int step_u, unsigned int step_v, bool useOptimized = true); template static void undistort(const vpImage &I, const vpCameraParameters &cam, vpImage &newI, unsigned int nThreads=2); template static void undistort(const vpImage &I, vpArray2D mapU, vpArray2D mapV, vpArray2D mapDu, vpArray2D mapDv, vpImage &newI); template static void warpImage(const vpImage &src, const vpMatrix &T, vpImage &dst, const vpImageInterpolationType &interpolation=INTERPOLATION_NEAREST, bool fixedPointArithmetic=true, bool pixelCenter=false); #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) /*! @name Deprecated functions */ //@{ template vp_deprecated static void createSubImage(const vpImage &I, unsigned int i_sub, unsigned int j_sub, unsigned int nrow_sub, unsigned int ncol_sub, vpImage &S); template vp_deprecated static void createSubImage(const vpImage &I, const vpRect &rect, vpImage &S); //@} #endif private: // Cubic interpolation static float cubicHermite(const float A, const float B, const float C, const float D, const float t); template static Type getPixelClamped(const vpImage &I, float u, float v); static int coordCast(double x); // Linear interpolation static double lerp(double A, double B, double t); static float lerp(float A, float B, float t); static int64_t lerp2(int64_t A, int64_t B, int64_t t, int64_t t_1); static double normalizedCorrelation(const vpImage &I1, const vpImage &I2, const vpImage &II, const vpImage &IIsq, const vpImage &II_tpl, const vpImage &IIsq_tpl, unsigned int i0, unsigned int j0); template static void resizeBicubic(const vpImage &I, vpImage &Ires, unsigned int i, unsigned int j, float u, float v, float xFrac, float yFrac); template static void resizeBilinear(const vpImage &I, vpImage &Ires, unsigned int i, unsigned int j, float u, float v, float xFrac, float yFrac); template static void resizeNearest(const vpImage &I, vpImage &Ires, unsigned int i, unsigned int j, float u, float v); static void resizeSimdlib(const vpImage& Isrc, unsigned int resizeWidth, unsigned int resizeHeight, vpImage& Idst, int method); static void resizeSimdlib(const vpImage& Isrc, unsigned int resizeWidth, unsigned int resizeHeight, vpImage& Idst, int method); template static void warpNN(const vpImage &src, const vpMatrix &T, vpImage &dst, bool affine, bool centerCorner, bool fixedPoint); template static void warpLinear(const vpImage &src, const vpMatrix &T, vpImage &dst, bool affine, bool centerCorner, bool fixedPoint); static bool checkFixedPoint(unsigned int x, unsigned int y, const vpMatrix &T, bool affine); }; #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) /*! Crop a region of interest (ROI) in an image. \deprecated This fonction is deprecated. You should rather use crop(const vpImage &, unsigned int, unsigned int, unsigned int, unsigned int, vpImage &). \param I : Input image from which a sub image will be extracted. \param roi_top : ROI vertical position of the upper/left corner in the input image. \param roi_left : ROI horizontal position of the upper/left corner in the input image. \param roi_height : Cropped image height corresponding to the ROI height. \param roi_width : Cropped image width corresponding to the ROI height. \param crop : Cropped image. \sa crop(const vpImage &, unsigned int, unsigned int, unsigned int, unsigned int, vpImage &) */ template void vpImageTools::createSubImage(const vpImage &I, unsigned int roi_top, unsigned int roi_left, unsigned int roi_height, unsigned int roi_width, vpImage &crop) { vpImageTools::crop(I, roi_top, roi_left, roi_height, roi_width, crop); } /*! Crop an image region of interest. \deprecated This fonction is deprecated. You should rather use crop(const vpImage &, const vpRect &, vpImage &). \param I : Input image from which a sub image will be extracted. \param roi : Region of interest in image \e I corresponding to the cropped part of the image. \param crop : Cropped image. \sa crop(const vpImage &, const vpRect &, vpImage &) */ template void vpImageTools::createSubImage(const vpImage &I, const vpRect &roi, vpImage &crop) { vpImageTools::crop(I, roi, crop); } #endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS) /*! Crop a region of interest (ROI) in an image. The ROI coordinates and dimension are defined in the original image. Setting \e v_scale and \e h_scale to values different from 1 allows also to subsample the cropped image. \param I : Input image from which a sub image will be extracted. \param roi_top : ROI vertical position of the upper/left corner in the input image. \param roi_left : ROI horizontal position of the upper/left corner in the input image. \param roi_height : Cropped image height corresponding to the ROI height. \param roi_width : Cropped image width corresponding to the ROI height. \param crop : Cropped image. \param v_scale [in] : Vertical subsampling factor applied to the ROI. \param h_scale [in] : Horizontal subsampling factor applied to the ROI. \sa crop(const vpImage &, const vpRect &, vpImage &) */ template void vpImageTools::crop(const vpImage &I, double roi_top, double roi_left, unsigned int roi_height, unsigned int roi_width, vpImage &crop, unsigned int v_scale, unsigned int h_scale) { int i_min = (std::max)((int)(ceil(roi_top / v_scale)), 0); int j_min = (std::max)((int)(ceil(roi_left / h_scale)), 0); int i_max = (std::min)((int)(ceil((roi_top + roi_height)) / v_scale), (int)(I.getHeight() / v_scale)); int j_max = (std::min)((int)(ceil((roi_left + roi_width) / h_scale)), (int)(I.getWidth() / h_scale)); unsigned int i_min_u = (unsigned int)i_min; unsigned int j_min_u = (unsigned int)j_min; unsigned int r_width = (unsigned int)(j_max - j_min); unsigned int r_height = (unsigned int)(i_max - i_min); crop.resize(r_height, r_width); if (v_scale == 1 && h_scale == 1) { for (unsigned int i = 0; i < r_height; i++) { void *src = (void *)(I[i + i_min_u] + j_min_u); void *dst = (void *)crop[i]; memcpy(dst, src, r_width * sizeof(Type)); } } else if (h_scale == 1) { for (unsigned int i = 0; i < r_height; i++) { void *src = (void *)(I[(i + i_min_u) * v_scale] + j_min_u); void *dst = (void *)crop[i]; memcpy(dst, src, r_width * sizeof(Type)); } } else { for (unsigned int i = 0; i < r_height; i++) { for (unsigned int j = 0; j < r_width; j++) { crop[i][j] = I[(i + i_min_u) * v_scale][(j + j_min_u) * h_scale]; } } } } /*! Crop a region of interest (ROI) in an image. The ROI coordinates and dimension are defined in the original image. Setting \e v_scale and \e h_scale to values different from 1 allows also to subsample the cropped image. \param I : Input image from which a sub image will be extracted. \param topLeft : ROI position of the upper/left corner in the input image. \param roi_height : Cropped image height corresponding to the ROI height. \param roi_width : Cropped image width corresponding to the ROI height. \param crop : Cropped image. \param v_scale [in] : Vertical subsampling factor applied to the ROI. \param h_scale [in] : Horizontal subsampling factor applied to the ROI. \sa crop(const vpImage &, const vpRect &, vpImage &) */ template void vpImageTools::crop(const vpImage &I, const vpImagePoint &topLeft, unsigned int roi_height, unsigned int roi_width, vpImage &crop, unsigned int v_scale, unsigned int h_scale) { vpImageTools::crop(I, topLeft.get_i(), topLeft.get_j(), roi_height, roi_width, crop, v_scale, h_scale); } /*! Crop a region of interest (ROI) in an image. The ROI coordinates and dimension are defined in the original image. Setting \e v_scale and \e h_scale to values different from 1 allows also to subsample the cropped image. \param I : Input image from which a sub image will be extracted. \param roi : Region of interest in image \e I corresponding to the cropped part of the image. \param crop : Cropped image. \param v_scale [in] : Vertical subsampling factor applied to the ROI. \param h_scale [in] : Horizontal subsampling factor applied to the ROI. */ template void vpImageTools::crop(const vpImage &I, const vpRect &roi, vpImage &crop, unsigned int v_scale, unsigned int h_scale) { vpImageTools::crop(I, roi.getTop(), roi.getLeft(), (unsigned int)roi.getHeight(), (unsigned int)roi.getWidth(), crop, v_scale, h_scale); } /*! Crop a region of interest (ROI) in an image. The ROI coordinates and dimension are defined in the original image. Setting \e v_scale and \e h_scale to values different from 1 allows also to subsample the cropped image. \param bitmap : Pointer to the input image from which a sub image will be extracted. \param width, height : Size of the input image. \param roi : Region of interest corresponding to the cropped part of the image. \param crop : Cropped image. \param v_scale [in] : Vertical subsampling factor applied to the ROI. \param h_scale [in] : Horizontal subsampling factor applied to the ROI. */ template void vpImageTools::crop(const unsigned char *bitmap, unsigned int width, unsigned int height, const vpRect &roi, vpImage &crop, unsigned int v_scale, unsigned int h_scale) { int i_min = (std::max)((int)(ceil(roi.getTop() / v_scale)), 0); int j_min = (std::max)((int)(ceil(roi.getLeft() / h_scale)), 0); int i_max = (std::min)((int)(ceil((roi.getTop() + roi.getHeight()) / v_scale)), (int)(height / v_scale)); int j_max = (std::min)((int)(ceil((roi.getLeft() + roi.getWidth()) / h_scale)), (int)(width / h_scale)); unsigned int i_min_u = (unsigned int)i_min; unsigned int j_min_u = (unsigned int)j_min; unsigned int r_width = (unsigned int)(j_max - j_min); unsigned int r_height = (unsigned int)(i_max - i_min); crop.resize(r_height, r_width); if (v_scale == 1 && h_scale == 1) { for (unsigned int i = 0; i < r_height; i++) { void *src = (void *)(bitmap + ((i + i_min_u) * width + j_min_u) * sizeof(Type)); void *dst = (void *)crop[i]; memcpy(dst, src, r_width * sizeof(Type)); } } else if (h_scale == 1) { for (unsigned int i = 0; i < r_height; i++) { void *src = (void *)(bitmap + ((i + i_min_u) * width * v_scale + j_min_u) * sizeof(Type)); void *dst = (void *)crop[i]; memcpy(dst, src, r_width * sizeof(Type)); } } else { for (unsigned int i = 0; i < r_height; i++) { unsigned int i_src = (i + i_min_u) * width * v_scale + j_min_u * h_scale; for (unsigned int j = 0; j < r_width; j++) { void *src = (void *)(bitmap + (i_src + j * h_scale) * sizeof(Type)); void *dst = (void *)&crop[i][j]; memcpy(dst, src, sizeof(Type)); } } } } /*! Binarise an image. - Pixels whose values are less than \e threshold1 are set to \e value1 - Pixels whose values are greater then or equal to \e threshold1 and less then or equal to \e threshold2 are set to \e value2 - Pixels whose values are greater than \e threshold2 are set to \e value3 */ template inline void vpImageTools::binarise(vpImage &I, Type threshold1, Type threshold2, Type value1, Type value2, Type value3, bool useLUT) { if (useLUT) { std::cerr << "LUT not available for this type ! Will use the iteration method." << std::endl; } Type v; Type *p = I.bitmap; Type *pend = I.bitmap + I.getWidth() * I.getHeight(); for (; p < pend; p++) { v = *p; if (v < threshold1) *p = value1; else if (v > threshold2) *p = value3; else *p = value2; } } /*! Binarise an image. - Pixels whose values are less than \e threshold1 are set to \e value1 - Pixels whose values are greater then or equal to \e threshold1 and less then or equal to \e threshold2 are set to \e value2 - Pixels whose values are greater than \e threshold2 are set to \e value3 */ template <> inline void vpImageTools::binarise(vpImage &I, unsigned char threshold1, unsigned char threshold2, unsigned char value1, unsigned char value2, unsigned char value3, bool useLUT) { if (useLUT) { // Construct the LUT unsigned char lut[256]; for (unsigned int i = 0; i < 256; i++) { lut[i] = i < threshold1 ? value1 : (i > threshold2 ? value3 : value2); } I.performLut(lut); } else { unsigned char *p = I.bitmap; unsigned char *pend = I.bitmap + I.getWidth() * I.getHeight(); for (; p < pend; p++) { unsigned char v = *p; if (v < threshold1) *p = value1; else if (v > threshold2) *p = value3; else *p = value2; } } } #ifdef VISP_HAVE_PTHREAD #ifndef DOXYGEN_SHOULD_SKIP_THIS template class vpUndistortInternalType { public: Type *src; Type *dst; unsigned int width; unsigned int height; vpCameraParameters cam; unsigned int nthreads; unsigned int threadid; public: vpUndistortInternalType() : src(NULL), dst(NULL), width(0), height(0), cam(), nthreads(0), threadid(0) {} vpUndistortInternalType(const vpUndistortInternalType &u) { *this = u; } vpUndistortInternalType &operator=(const vpUndistortInternalType &u) { src = u.src; dst = u.dst; width = u.width; height = u.height; cam = u.cam; nthreads = u.nthreads; threadid = u.threadid; return *this; } static void *vpUndistort_threaded(void *arg); }; template void *vpUndistortInternalType::vpUndistort_threaded(void *arg) { vpUndistortInternalType *undistortSharedData = static_cast *>(arg); int offset = (int)undistortSharedData->threadid; int width = (int)undistortSharedData->width; int height = (int)undistortSharedData->height; int nthreads = (int)undistortSharedData->nthreads; double u0 = undistortSharedData->cam.get_u0(); double v0 = undistortSharedData->cam.get_v0(); double px = undistortSharedData->cam.get_px(); double py = undistortSharedData->cam.get_py(); double kud = undistortSharedData->cam.get_kud(); double invpx = 1.0 / px; double invpy = 1.0 / py; double kud_px2 = kud * invpx * invpx; double kud_py2 = kud * invpy * invpy; Type *dst = undistortSharedData->dst + (height / nthreads * offset) * width; Type *src = undistortSharedData->src; for (double v = height / nthreads * offset; v < height / nthreads * (offset + 1); v++) { double deltav = v - v0; // double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy)); double fr1 = 1.0 + kud_py2 * deltav * deltav; for (double u = 0; u < width; u++) { // computation of u,v : corresponding pixel coordinates in I. double deltau = u - u0; // double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx)); double fr2 = fr1 + kud_px2 * deltau * deltau; double u_double = deltau * fr2 + u0; double v_double = deltav * fr2 + v0; // computation of the bilinear interpolation // declarations int u_round = (int)(u_double); int v_round = (int)(v_double); if (u_round < 0.f) u_round = -1; if (v_round < 0.f) v_round = -1; double du_double = (u_double) - (double)u_round; double dv_double = (v_double) - (double)v_round; Type v01; Type v23; if ((0 <= u_round) && (0 <= v_round) && (u_round < ((width)-1)) && (v_round < ((height)-1))) { // process interpolation const Type *_mp = &src[v_round * width + u_round]; v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double)); _mp += width; v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double)); *dst = (Type)(v01 + ((v23 - v01) * dv_double)); } else { *dst = 0; } dst++; } } pthread_exit((void *)0); return NULL; } #endif // DOXYGEN_SHOULD_SKIP_THIS #endif // VISP_HAVE_PTHREAD /*! Undistort an image \param I : Input image to undistort. \param cam : Parameters of the camera causing distortion. \param undistI : Undistorted output image. The size of this image will be the same than the input image \e I. If the distortion parameter \f$k_{ud}\f$ is null, meaning that `cam.get_kud() == 0`, \e undistI is just a copy of \e I. \param nThreads : Number of threads to use if pthreads library is available. \warning This function works only with Types authorizing "+,-, multiplication by a scalar" operators. Since this function is time consuming, if you want to undistort multiple images, you should rather call initUndistortMap() once and then remap() to undistort the images. This will be less time consuming. \sa initUndistortMap(), remap() */ template void vpImageTools::undistort(const vpImage &I, const vpCameraParameters &cam, vpImage &undistI, unsigned int nThreads) { #ifdef VISP_HAVE_PTHREAD // // Optimized version using pthreads // unsigned int width = I.getWidth(); unsigned int height = I.getHeight(); undistI.resize(height, width); double kud = cam.get_kud(); // if (kud == 0) { if (std::fabs(kud) <= std::numeric_limits::epsilon()) { // There is no need to undistort the image undistI = I; return; } unsigned int nthreads = nThreads; pthread_attr_t attr; pthread_t *callThd = new pthread_t[nthreads]; pthread_attr_init(&attr); pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE); vpUndistortInternalType *undistortSharedData; undistortSharedData = new vpUndistortInternalType[nthreads]; for (unsigned int i = 0; i < nthreads; i++) { // Each thread works on a different set of data. // vpTRACE("create thread %d", i); undistortSharedData[i].src = I.bitmap; undistortSharedData[i].dst = undistI.bitmap; undistortSharedData[i].width = I.getWidth(); undistortSharedData[i].height = I.getHeight(); undistortSharedData[i].cam = cam; undistortSharedData[i].nthreads = nthreads; undistortSharedData[i].threadid = i; pthread_create(&callThd[i], &attr, &vpUndistortInternalType::vpUndistort_threaded, &undistortSharedData[i]); } pthread_attr_destroy(&attr); /* Wait on the other threads */ for (unsigned int i = 0; i < nthreads; i++) { // vpTRACE("join thread %d", i); pthread_join(callThd[i], NULL); } delete[] callThd; delete[] undistortSharedData; #else // VISP_HAVE_PTHREAD (void)nThreads; // // optimized version without pthreads // unsigned int width = I.getWidth(); unsigned int height = I.getHeight(); undistI.resize(height, width); double u0 = cam.get_u0(); double v0 = cam.get_v0(); double px = cam.get_px(); double py = cam.get_py(); double kud = cam.get_kud(); // if (kud == 0) { if (std::fabs(kud) <= std::numeric_limits::epsilon()) { // There is no need to undistort the image undistI = I; return; } double invpx = 1.0 / px; double invpy = 1.0 / py; double kud_px2 = kud * invpx * invpx; double kud_py2 = kud * invpy * invpy; Type *dst = undistI.bitmap; for (double v = 0; v < height; v++) { double deltav = v - v0; // double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy)); double fr1 = 1.0 + kud_py2 * deltav * deltav; for (double u = 0; u < width; u++) { // computation of u,v : corresponding pixel coordinates in I. double deltau = u - u0; // double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx)); double fr2 = fr1 + kud_px2 * deltau * deltau; double u_double = deltau * fr2 + u0; double v_double = deltav * fr2 + v0; // printf("[%g][%g] %g %g : ", u, v, u_double, v_double ); // computation of the bilinear interpolation // declarations int u_round = (int)(u_double); int v_round = (int)(v_double); if (u_round < 0.f) u_round = -1; if (v_round < 0.f) v_round = -1; double du_double = (u_double) - (double)u_round; double dv_double = (v_double) - (double)v_round; Type v01; Type v23; if ((0 <= u_round) && (0 <= v_round) && (u_round < (((int)width) - 1)) && (v_round < (((int)height) - 1))) { // process interpolation const Type *_mp = &I[(unsigned int)v_round][(unsigned int)u_round]; v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double)); _mp += width; v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double)); *dst = (Type)(v01 + ((v23 - v01) * dv_double)); // printf("R %d G %d B %d\n", dst->R, dst->G, dst->B); } else { *dst = 0; } dst++; } } #endif // VISP_HAVE_PTHREAD #if 0 // non optimized version int width = I.getWidth(); int height = I.getHeight(); undistI.resize(height,width); double u0 = cam.get_u0(); double v0 = cam.get_v0(); double px = cam.get_px(); double py = cam.get_py(); double kd = cam.get_kud(); if (kd == 0) { // There is no need to undistort the image undistI = I; return; } for(int v = 0 ; v < height; v++){ for(int u = 0; u < height; u++){ double r2 = vpMath::sqr(((double)u - u0)/px) + vpMath::sqr(((double)v-v0)/py); double u_double = ((double)u - u0)*(1.0+kd*r2) + u0; double v_double = ((double)v - v0)*(1.0+kd*r2) + v0; undistI[v][u] = I.getPixelBI((float)u_double,(float)v_double); } } #endif } /*! Undistort an image. \param I : Input image to undistort. \param mapU : Map that contains at each destination coordinate the u-coordinate in the source image. \param mapV : Map that contains at each destination coordinate the v-coordinate in the source image. \param mapDu : Map that contains at each destination coordinate the \f$ \Delta u \f$ for the interpolation. \param mapDv : Map that contains at each destination coordinate the \f$ \Delta v \f$ for the interpolation. \param newI : Undistorted output image. The size of this image will be the same as the input image \e I. \note To undistort a fisheye image, you have to first call initUndistortMap() function to calculate maps and then call undistort() with input maps. */ template void vpImageTools::undistort(const vpImage &I, vpArray2D mapU, vpArray2D mapV, vpArray2D mapDu, vpArray2D mapDv, vpImage &newI) { remap(I, mapU, mapV, mapDu, mapDv, newI); } /*! Flip vertically the input image and give the result in the output image. \param I : Input image to flip. \param newI : Output image which is the flipped input image. */ template void vpImageTools::flip(const vpImage &I, vpImage &newI) { unsigned int height = 0, width = 0; height = I.getHeight(); width = I.getWidth(); newI.resize(height, width); for (unsigned int i = 0; i < height; i++) { memcpy(newI.bitmap + i * width, I.bitmap + (height - 1 - i) * width, width * sizeof(Type)); } } /*! Flip vertically the input image. \param I : Input image which is flipped and modified in output. The following example shows how to use this function: \code #include #include #include int main() { vpImage I; #ifdef _WIN32 std::string filename("C:/temp/ViSP-images/Klimt/Klimt.ppm"); #else std::string filename("/local/soft/ViSP/ViSP-images/Klimt/Klimt.ppm"); #endif // Read an image from the disk vpImageIo::read(I, filename); // Flip the image vpImageTools::flip(I); // Write the image in a PGM P5 image file format vpImageIo::write(I, "Klimt-flip.ppm"); } \endcode */ template void vpImageTools::flip(vpImage &I) { unsigned int height = 0, width = 0; unsigned int i = 0; vpImage Ibuf; height = I.getHeight(); width = I.getWidth(); Ibuf.resize(1, width); for (i = 0; i < height / 2; i++) { memcpy(Ibuf.bitmap, I.bitmap + i * width, width * sizeof(Type)); memcpy(I.bitmap + i * width, I.bitmap + (height - 1 - i) * width, width * sizeof(Type)); memcpy(I.bitmap + (height - 1 - i) * width, Ibuf.bitmap, width * sizeof(Type)); } } template Type vpImageTools::getPixelClamped(const vpImage &I, float u, float v) { int x = vpMath::round(u); int y = vpMath::round(v); x = (std::max)(0, (std::min)(x, static_cast(I.getWidth())-1)); y = (std::max)(0, (std::min)(y, static_cast(I.getHeight())-1)); return I[y][x]; } // Reference: // http://blog.demofox.org/2015/08/15/resizing-images-with-bicubic-interpolation/ template void vpImageTools::resizeBicubic(const vpImage &I, vpImage &Ires, unsigned int i, unsigned int j, float u, float v, float xFrac, float yFrac) { // 1st row Type p00 = getPixelClamped(I, u - 1, v - 1); Type p01 = getPixelClamped(I, u + 0, v - 1); Type p02 = getPixelClamped(I, u + 1, v - 1); Type p03 = getPixelClamped(I, u + 2, v - 1); // 2nd row Type p10 = getPixelClamped(I, u - 1, v + 0); Type p11 = getPixelClamped(I, u + 0, v + 0); Type p12 = getPixelClamped(I, u + 1, v + 0); Type p13 = getPixelClamped(I, u + 2, v + 0); // 3rd row Type p20 = getPixelClamped(I, u - 1, v + 1); Type p21 = getPixelClamped(I, u + 0, v + 1); Type p22 = getPixelClamped(I, u + 1, v + 1); Type p23 = getPixelClamped(I, u + 2, v + 1); // 4th row Type p30 = getPixelClamped(I, u - 1, v + 2); Type p31 = getPixelClamped(I, u + 0, v + 2); Type p32 = getPixelClamped(I, u + 1, v + 2); Type p33 = getPixelClamped(I, u + 2, v + 2); float col0 = cubicHermite(p00, p01, p02, p03, xFrac); float col1 = cubicHermite(p10, p11, p12, p13, xFrac); float col2 = cubicHermite(p20, p21, p22, p23, xFrac); float col3 = cubicHermite(p30, p31, p32, p33, xFrac); float value = cubicHermite(col0, col1, col2, col3, yFrac); Ires[i][j] = vpMath::saturate(value); } template <> inline void vpImageTools::resizeBicubic(const vpImage &I, vpImage &Ires, unsigned int i, unsigned int j, float u, float v, float xFrac, float yFrac) { // 1st row vpRGBa p00 = getPixelClamped(I, u - 1, v - 1); vpRGBa p01 = getPixelClamped(I, u + 0, v - 1); vpRGBa p02 = getPixelClamped(I, u + 1, v - 1); vpRGBa p03 = getPixelClamped(I, u + 2, v - 1); // 2nd row vpRGBa p10 = getPixelClamped(I, u - 1, v + 0); vpRGBa p11 = getPixelClamped(I, u + 0, v + 0); vpRGBa p12 = getPixelClamped(I, u + 1, v + 0); vpRGBa p13 = getPixelClamped(I, u + 2, v + 0); // 3rd row vpRGBa p20 = getPixelClamped(I, u - 1, v + 1); vpRGBa p21 = getPixelClamped(I, u + 0, v + 1); vpRGBa p22 = getPixelClamped(I, u + 1, v + 1); vpRGBa p23 = getPixelClamped(I, u + 2, v + 1); // 4th row vpRGBa p30 = getPixelClamped(I, u - 1, v + 2); vpRGBa p31 = getPixelClamped(I, u + 0, v + 2); vpRGBa p32 = getPixelClamped(I, u + 1, v + 2); vpRGBa p33 = getPixelClamped(I, u + 2, v + 2); for (int c = 0; c < 3; c++) { float col0 = cubicHermite(static_cast(reinterpret_cast(&p00)[c]), static_cast(reinterpret_cast(&p01)[c]), static_cast(reinterpret_cast(&p02)[c]), static_cast(reinterpret_cast(&p03)[c]), xFrac); float col1 = cubicHermite(static_cast(reinterpret_cast(&p10)[c]), static_cast(reinterpret_cast(&p11)[c]), static_cast(reinterpret_cast(&p12)[c]), static_cast(reinterpret_cast(&p13)[c]), xFrac); float col2 = cubicHermite(static_cast(reinterpret_cast(&p20)[c]), static_cast(reinterpret_cast(&p21)[c]), static_cast(reinterpret_cast(&p22)[c]), static_cast(reinterpret_cast(&p23)[c]), xFrac); float col3 = cubicHermite(static_cast(reinterpret_cast(&p30)[c]), static_cast(reinterpret_cast(&p31)[c]), static_cast(reinterpret_cast(&p32)[c]), static_cast(reinterpret_cast(&p33)[c]), xFrac); float value = cubicHermite(col0, col1, col2, col3, yFrac); reinterpret_cast(&Ires[i][j])[c] = vpMath::saturate(value); } } template void vpImageTools::resizeBilinear(const vpImage &I, vpImage &Ires, unsigned int i, unsigned int j, float u, float v, float xFrac, float yFrac) { int u0 = static_cast(u); int v0 = static_cast(v); int u1 = (std::min)(static_cast(I.getWidth()) - 1, u0 + 1); int v1 = v0; int u2 = u0; int v2 = (std::min)(static_cast(I.getHeight()) - 1, v0 + 1); int u3 = u1; int v3 = v2; float col0 = lerp(I[v0][u0], I[v1][u1], xFrac); float col1 = lerp(I[v2][u2], I[v3][u3], xFrac); float value = lerp(col0, col1, yFrac); Ires[i][j] = vpMath::saturate(value); } template <> inline void vpImageTools::resizeBilinear(const vpImage &I, vpImage &Ires, unsigned int i, unsigned int j, float u, float v, float xFrac, float yFrac) { int u0 = static_cast(u); int v0 = static_cast(v); int u1 = (std::min)(static_cast(I.getWidth()) - 1, u0 + 1); int v1 = v0; int u2 = u0; int v2 = (std::min)(static_cast(I.getHeight()) - 1, v0 + 1); int u3 = u1; int v3 = v2; for (int c = 0; c < 3; c++) { float col0 = lerp(static_cast(reinterpret_cast(&I[v0][u0])[c]), static_cast(reinterpret_cast(&I[v1][u1])[c]), xFrac); float col1 = lerp(static_cast(reinterpret_cast(&I[v2][u2])[c]), static_cast(reinterpret_cast(&I[v3][u3])[c]), xFrac); float value = lerp(col0, col1, yFrac); reinterpret_cast(&Ires[i][j])[c] = vpMath::saturate(value); } } template void vpImageTools::resizeNearest(const vpImage &I, vpImage &Ires, unsigned int i, unsigned int j, float u, float v) { Ires[i][j] = getPixelClamped(I, u, v); } /*! Resize the image using one interpolation method (by default it uses the nearest neighbor interpolation). \param I : Input image. \param Ires : Output image resized to \e width, \e height. \param width : Resized width. \param height : Resized height. \param method : Interpolation method. \param nThreads : Number of threads to use if OpenMP is available (zero will let OpenMP uses the optimal number of threads). \warning The input \e I and output \e Ires images must be different objects. \note The SIMD lib is used to accelerate processing on x86 and ARM architecture for: - unsigned char and vpRGBa image types - and only with INTERPOLATION_AREA and INTERPOLATION_LINEAR methods */ template void vpImageTools::resize(const vpImage &I, vpImage &Ires, unsigned int width, unsigned int height, const vpImageInterpolationType &method, unsigned int nThreads) { Ires.resize(height, width); vpImageTools::resize(I, Ires, method, nThreads); } /*! Resize the image using one interpolation method (by default it uses the nearest neighbor interpolation). \param I : Input image. \param Ires : Output image resized (you have to init the image \e Ires at the desired size). \param method : Interpolation method. \param nThreads : Number of threads to use if OpenMP is available (zero will let OpenMP uses the optimal number of threads). \warning The input \e I and output \e Ires images must be different objects. \note The SIMD lib is used to accelerate processing on x86 and ARM architecture for: - unsigned char and vpRGBa image types - and only with INTERPOLATION_AREA and INTERPOLATION_LINEAR methods */ template void vpImageTools::resize(const vpImage &I, vpImage &Ires, const vpImageInterpolationType &method, unsigned int #if defined _OPENMP nThreads #endif ) { if (I.getWidth() < 2 || I.getHeight() < 2 || Ires.getWidth() < 2 || Ires.getHeight() < 2) { std::cerr << "Input or output image is too small!" << std::endl; return; } if (method == INTERPOLATION_AREA) { std::cerr << "INTERPOLATION_AREA is not implemented for this type." << std::endl; return; } const float scaleY = I.getHeight() / static_cast(Ires.getHeight()); const float scaleX = I.getWidth() / static_cast(Ires.getWidth()); const float half = 0.5f; #if defined _OPENMP if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } #pragma omp parallel for schedule(dynamic) #endif for (int i = 0; i < static_cast(Ires.getHeight()); i++) { const float v = (i + half) * scaleY - half; const int v0 = static_cast(v); const float yFrac = v - v0; for (unsigned int j = 0; j < Ires.getWidth(); j++) { const float u = (j + half) * scaleX - half; const int u0 = static_cast(u); const float xFrac = u - u0; if (method == INTERPOLATION_NEAREST) { resizeNearest(I, Ires, static_cast(i), j, u, v); } else if (method == INTERPOLATION_LINEAR) { resizeBilinear(I, Ires, static_cast(i), j, u0, v0, xFrac, yFrac); } else if (method == INTERPOLATION_CUBIC) { resizeBicubic(I, Ires, static_cast(i), j, u, v, xFrac, yFrac); } } } } template <> inline void vpImageTools::resize(const vpImage &I, vpImage &Ires, const vpImageInterpolationType &method, unsigned int #if defined _OPENMP nThreads #endif ) { if (I.getWidth() < 2 || I.getHeight() < 2 || Ires.getWidth() < 2 || Ires.getHeight() < 2) { std::cerr << "Input or output image is too small!" << std::endl; return; } if (method == INTERPOLATION_AREA) { resizeSimdlib(I, Ires.getWidth(), Ires.getHeight(), Ires, INTERPOLATION_AREA); } else if (method == INTERPOLATION_LINEAR) { resizeSimdlib(I, Ires.getWidth(), Ires.getHeight(), Ires, INTERPOLATION_LINEAR); } else { const float scaleY = I.getHeight() / static_cast(Ires.getHeight()); const float scaleX = I.getWidth() / static_cast(Ires.getWidth()); const float half = 0.5f; #if defined _OPENMP if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } #pragma omp parallel for schedule(dynamic) #endif for (int i = 0; i < static_cast(Ires.getHeight()); i++) { float v = (i + half) * scaleY - half; float yFrac = v - static_cast(v); for (unsigned int j = 0; j < Ires.getWidth(); j++) { float u = (j + half) * scaleX - half; float xFrac = u - static_cast(u); if (method == INTERPOLATION_NEAREST) { resizeNearest(I, Ires, static_cast(i), j, u, v); } else if (method == INTERPOLATION_CUBIC) { resizeBicubic(I, Ires, static_cast(i), j, u, v, xFrac, yFrac); } } } } } template <> inline void vpImageTools::resize(const vpImage &I, vpImage &Ires, const vpImageInterpolationType &method, unsigned int #if defined _OPENMP nThreads #endif ) { if (I.getWidth() < 2 || I.getHeight() < 2 || Ires.getWidth() < 2 || Ires.getHeight() < 2) { std::cerr << "Input or output image is too small!" << std::endl; return; } if (method == INTERPOLATION_AREA) { resizeSimdlib(I, Ires.getWidth(), Ires.getHeight(), Ires, INTERPOLATION_AREA); } else if (method == INTERPOLATION_LINEAR) { resizeSimdlib(I, Ires.getWidth(), Ires.getHeight(), Ires, INTERPOLATION_LINEAR); } else { const float scaleY = I.getHeight() / static_cast(Ires.getHeight()); const float scaleX = I.getWidth() / static_cast(Ires.getWidth()); const float half = 0.5f; #if defined _OPENMP if (nThreads > 0) { omp_set_num_threads(static_cast(nThreads)); } #pragma omp parallel for schedule(dynamic) #endif for (int i = 0; i < static_cast(Ires.getHeight()); i++) { float v = (i + half) * scaleY - half; float yFrac = v - static_cast(v); for (unsigned int j = 0; j < Ires.getWidth(); j++) { float u = (j + half) * scaleX - half; float xFrac = u - static_cast(u); if (method == INTERPOLATION_NEAREST) { resizeNearest(I, Ires, static_cast(i), j, u, v); } else if (method == INTERPOLATION_CUBIC) { resizeBicubic(I, Ires, static_cast(i), j, u, v, xFrac, yFrac); } } } } } /*! Apply a warping (affine or perspective) transformation to an image. \param src : Input image. \param T : Transformation / warping matrix, a `2x3` matrix for an affine transformation or a `3x3` matrix for a perspective transformation (homography). \param dst : Output image, if empty it will be of the same size than src and zero-initialized. \param interpolation : Interpolation method (only INTERPOLATION_NEAREST and INTERPOLATION_LINEAR are accepted, if INTERPOLATION_CUBIC is passed, INTERPOLATION_NEAREST will be used instead). \param fixedPointArithmetic : If true and if `pixelCenter` is false, fixed-point arithmetic is used if possible. Otherwise (e.g. the input image is too big) it fallbacks to the default implementation. \param pixelCenter : If true, pixel coordinates are at (0.5, 0.5), otherwise at (0,0). Fixed-point arithmetic cannot be used with `pixelCenter` option. */ template void vpImageTools::warpImage(const vpImage &src, const vpMatrix &T, vpImage &dst, const vpImageInterpolationType &interpolation, bool fixedPointArithmetic, bool pixelCenter) { if ((T.getRows() != 2 && T.getRows() != 3) || T.getCols() != 3) { std::cerr << "Input transformation must be a (2x3) or (3x3) matrix." << std::endl; return; } if (src.getSize() == 0) { return; } const bool affine = (T.getRows() == 2); const bool interp_NN = (interpolation == INTERPOLATION_NEAREST) || (interpolation == INTERPOLATION_CUBIC); if (dst.getSize() == 0) { dst.resize(src.getHeight(), src.getWidth(), Type(0)); } vpMatrix M = T; if (affine) { double D = M[0][0] * M[1][1] - M[0][1] * M[1][0]; D = !vpMath::nul(D, std::numeric_limits::epsilon()) ? 1.0 / D : 0; double A11 = M[1][1] * D, A22 = M[0][0] * D; M[0][0] = A11; M[0][1] *= -D; M[1][0] *= -D; M[1][1] = A22; double b1 = -M[0][0] * M[0][2] - M[0][1] * M[1][2]; double b2 = -M[1][0] * M[0][2] - M[1][1] * M[1][2]; M[0][2] = b1; M[1][2] = b2; } else { M = T.inverseByLU(); } if (fixedPointArithmetic && !pixelCenter) { fixedPointArithmetic = checkFixedPoint(0, 0, M, affine) && checkFixedPoint(dst.getWidth()-1, 0, M, affine) && checkFixedPoint(0, dst.getHeight()-1, M, affine) && checkFixedPoint(dst.getWidth() - 1, dst.getHeight() - 1, M, affine); } if (interp_NN) { //nearest neighbor interpolation warpNN(src, M, dst, affine, pixelCenter, fixedPointArithmetic); } else { //bilinear interpolation warpLinear(src, M, dst, affine, pixelCenter, fixedPointArithmetic); } } template void vpImageTools::warpNN(const vpImage &src, const vpMatrix &T, vpImage &dst, bool affine, bool centerCorner, bool fixedPoint) { if (fixedPoint && !centerCorner) { const int nbits = 16; const int32_t precision = 1 << nbits; const float precision_1 = 1 / static_cast(precision); int32_t a0_i32 = static_cast(T[0][0] * precision); int32_t a1_i32 = static_cast(T[0][1] * precision); int32_t a2_i32 = static_cast(T[0][2] * precision); int32_t a3_i32 = static_cast(T[1][0] * precision); int32_t a4_i32 = static_cast(T[1][1] * precision); int32_t a5_i32 = static_cast(T[1][2] * precision); int32_t a6_i32 = T.getRows() == 3 ? static_cast(T[2][0] * precision) : 0; int32_t a7_i32 = T.getRows() == 3 ? static_cast(T[2][1] * precision) : 0; int32_t a8_i32 = T.getRows() == 3 ? static_cast(T[2][2] * precision) : 1; int32_t height_1_i32 = static_cast((src.getHeight() - 1) * precision) + 0x8000; int32_t width_1_i32 = static_cast((src.getWidth() - 1) * precision) + 0x8000; if (affine) { for (unsigned int i = 0; i < dst.getHeight(); i++) { int32_t xi = a2_i32; int32_t yi = a5_i32; for (unsigned int j = 0; j < dst.getWidth(); j++) { if (yi >= 0 && yi < height_1_i32 && xi >= 0 && xi < width_1_i32) { float x_ = (xi >> nbits) + (xi & 0xFFFF) * precision_1; float y_ = (yi >> nbits) + (yi & 0xFFFF) * precision_1; int x = vpMath::round(x_); int y = vpMath::round(y_); dst[i][j] = src[y][x]; } xi += a0_i32; yi += a3_i32; } a2_i32 += a1_i32; a5_i32 += a4_i32; } } else { for (unsigned int i = 0; i < dst.getHeight(); i++) { int64_t xi = a2_i32; int64_t yi = a5_i32; int64_t wi = a8_i32; for (unsigned int j = 0; j < dst.getWidth(); j++) { if (wi != 0 && yi >= 0 && yi <= (static_cast(src.getHeight()) - 1)*wi && xi >= 0 && xi <= (static_cast(src.getWidth()) - 1)*wi) { float w_ = (wi >> nbits) + (wi & 0xFFFF) * precision_1; float x_ = ((xi >> nbits) + (xi & 0xFFFF) * precision_1) / w_; float y_ = ((yi >> nbits) + (yi & 0xFFFF) * precision_1) / w_; int x = vpMath::round(x_); int y = vpMath::round(y_); dst[i][j] = src[y][x]; } xi += a0_i32; yi += a3_i32; wi += a6_i32; } a2_i32 += a1_i32; a5_i32 += a4_i32; a8_i32 += a7_i32; } } } else { double a0 = T[0][0]; double a1 = T[0][1]; double a2 = T[0][2]; double a3 = T[1][0]; double a4 = T[1][1]; double a5 = T[1][2]; double a6 = affine ? 0.0 : T[2][0]; double a7 = affine ? 0.0 : T[2][1]; double a8 = affine ? 1.0 : T[2][2]; for (unsigned int i = 0; i < dst.getHeight(); i++) { for (unsigned int j = 0; j < dst.getWidth(); j++) { double x = a0 * (centerCorner ? j + 0.5 : j) + a1 * (centerCorner ? i + 0.5 : i) + a2; double y = a3 * (centerCorner ? j + 0.5 : j) + a4 * (centerCorner ? i + 0.5 : i) + a5; double w = a6 * (centerCorner ? j + 0.5 : j) + a7 * (centerCorner ? i + 0.5 : i) + a8; if (vpMath::nul(w, std::numeric_limits::epsilon())) { w = 1.0; } int x_ = centerCorner ? coordCast(x / w) : vpMath::round(x / w); int y_ = centerCorner ? coordCast(y / w) : vpMath::round(y / w); if (x_ >= 0 && x_ < static_cast(src.getWidth()) && y_ >= 0 && y_ < static_cast(src.getHeight())) { dst[i][j] = src[y_][x_]; } } } } } template void vpImageTools::warpLinear(const vpImage &src, const vpMatrix &T, vpImage &dst, bool affine, bool centerCorner, bool fixedPoint) { if (fixedPoint && !centerCorner) { const int nbits = 16; const int64_t precision = 1 << nbits; const float precision_1 = 1 / static_cast(precision); const int64_t precision2 = 1ULL << (2 * nbits); const float precision_2 = 1 / static_cast(precision2); int64_t a0_i64 = static_cast(T[0][0] * precision); int64_t a1_i64 = static_cast(T[0][1] * precision); int64_t a2_i64 = static_cast(T[0][2] * precision); int64_t a3_i64 = static_cast(T[1][0] * precision); int64_t a4_i64 = static_cast(T[1][1] * precision); int64_t a5_i64 = static_cast(T[1][2] * precision); int64_t a6_i64 = T.getRows() == 3 ? static_cast(T[2][0] * precision) : 0; int64_t a7_i64 = T.getRows() == 3 ? static_cast(T[2][1] * precision) : 0; int64_t a8_i64 = T.getRows() == 3 ? static_cast(T[2][2] * precision) : 1; int64_t height_i64 = static_cast(src.getHeight() * precision); int64_t width_i64 = static_cast(src.getWidth() * precision); if (affine) { for (unsigned int i = 0; i < dst.getHeight(); i++) { int64_t xi_ = a2_i64; int64_t yi_ = a5_i64; for (unsigned int j = 0; j < dst.getWidth(); j++) { if (yi_ >= 0 && yi_ < height_i64 && xi_ >= 0 && xi_ < width_i64) { const int64_t xi_lower = xi_ & (~0xFFFF); const int64_t yi_lower = yi_ & (~0xFFFF); const int64_t t = yi_ - yi_lower; const int64_t t_1 = precision - t; const int64_t s = xi_ - xi_lower; const int64_t s_1 = precision - s; const int x_ = static_cast(xi_ >> nbits); const int y_ = static_cast(yi_ >> nbits); if (y_ < static_cast(src.getHeight())-1 && x_ < static_cast(src.getWidth())-1) { const Type val00 = src[y_][x_]; const Type val01 = src[y_][x_+1]; const Type val10 = src[y_+1][x_]; const Type val11 = src[y_+1][x_+1]; const int64_t interp_i64 = static_cast(s_1*t_1*val00 + s*t_1*val01 + s_1*t*val10 + s*t*val11); const float interp = (interp_i64 >> (nbits*2)) + (interp_i64 & 0xFFFFFFFF) * precision_2; dst[i][j] = vpMath::saturate(interp); } else if (y_ < static_cast(src.getHeight())-1) { const Type val00 = src[y_][x_]; const Type val10 = src[y_+1][x_]; const int64_t interp_i64 = static_cast(t_1*val00 + t*val10); const float interp = (interp_i64 >> nbits) + (interp_i64 & 0xFFFF) * precision_1; dst[i][j] = vpMath::saturate(interp); } else if (x_ < static_cast(src.getWidth())-1) { const Type val00 = src[y_][x_]; const Type val01 = src[y_][x_+1]; const int64_t interp_i64 = static_cast(s_1*val00 + s*val01); const float interp = (interp_i64 >> nbits) + (interp_i64 & 0xFFFF) * precision_1; dst[i][j] = vpMath::saturate(interp); } else { dst[i][j] = src[y_][x_]; } } xi_ += a0_i64; yi_ += a3_i64; } a2_i64 += a1_i64; a5_i64 += a4_i64; } } else { for (unsigned int i = 0; i < dst.getHeight(); i++) { int64_t xi = a2_i64; int64_t yi = a5_i64; int64_t wi = a8_i64; for (unsigned int j = 0; j < dst.getWidth(); j++) { if (wi != 0 && yi >= 0 && yi <= (static_cast(src.getHeight()) - 1)*wi && xi >= 0 && xi <= (static_cast(src.getWidth()) - 1)*wi) { const float wi_ = (wi >> nbits) + (wi & 0xFFFF) * precision_1; const float xi_ = ((xi >> nbits) + (xi & 0xFFFF) * precision_1) / wi_; const float yi_ = ((yi >> nbits) + (yi & 0xFFFF) * precision_1) / wi_; const int x_ = static_cast(xi_); const int y_ = static_cast(yi_); const float t = yi_ - y_; const float s = xi_ - x_; if (y_ < static_cast(src.getHeight()) - 1 && x_ < static_cast(src.getWidth()) - 1) { const Type val00 = src[y_][x_]; const Type val01 = src[y_][x_ + 1]; const Type val10 = src[y_ + 1][x_]; const Type val11 = src[y_ + 1][x_ + 1]; const float col0 = lerp(val00, val01, s); const float col1 = lerp(val10, val11, s); const float interp = lerp(col0, col1, t); dst[i][j] = vpMath::saturate(interp); } else if (y_ < static_cast(src.getHeight()) - 1) { const Type val00 = src[y_][x_]; const Type val10 = src[y_ + 1][x_]; const float interp = lerp(val00, val10, t); dst[i][j] = vpMath::saturate(interp); } else if (x_ < static_cast(src.getWidth()) - 1) { const Type val00 = src[y_][x_]; const Type val01 = src[y_][x_ + 1]; const float interp = lerp(val00, val01, s); dst[i][j] = vpMath::saturate(interp); } else { dst[i][j] = src[y_][x_]; } } xi += a0_i64; yi += a3_i64; wi += a6_i64; } a2_i64 += a1_i64; a5_i64 += a4_i64; a8_i64 += a7_i64; } } } else { double a0 = T[0][0]; double a1 = T[0][1]; double a2 = T[0][2]; double a3 = T[1][0]; double a4 = T[1][1]; double a5 = T[1][2]; double a6 = affine ? 0.0 : T[2][0]; double a7 = affine ? 0.0 : T[2][1]; double a8 = affine ? 1.0 : T[2][2]; for (unsigned int i = 0; i < dst.getHeight(); i++) { for (unsigned int j = 0; j < dst.getWidth(); j++) { double x = a0 * (centerCorner ? j + 0.5 : j) + a1 * (centerCorner ? i + 0.5 : i) + a2; double y = a3 * (centerCorner ? j + 0.5 : j) + a4 * (centerCorner ? i + 0.5 : i) + a5; double w = a6 * (centerCorner ? j + 0.5 : j) + a7 * (centerCorner ? i + 0.5 : i) + a8; if (vpMath::nul(w, std::numeric_limits::epsilon())) { w = 1; } x = x / w - (centerCorner ? 0.5 : 0); y = y / w - (centerCorner ? 0.5 : 0); int x_lower = static_cast(x); int y_lower = static_cast(y); if (y_lower >= static_cast(src.getHeight()) || x_lower >= static_cast(src.getWidth()) || y < 0 || x < 0) { continue; } double s = x - x_lower; double t = y - y_lower; if (y_lower < static_cast(src.getHeight())-1 && x_lower < static_cast(src.getWidth())-1) { const Type val00 = src[y_lower][x_lower]; const Type val01 = src[y_lower][x_lower + 1]; const Type val10 = src[y_lower + 1][x_lower]; const Type val11 = src[y_lower + 1][x_lower + 1]; const double col0 = lerp(val00, val01, s); const double col1 = lerp(val10, val11, s); const double interp = lerp(col0, col1, t); dst[i][j] = vpMath::saturate(interp); } else if (y_lower < static_cast(src.getHeight())-1) { const Type val00 = src[y_lower][x_lower]; const Type val10 = src[y_lower + 1][x_lower]; const double interp = lerp(val00, val10, t); dst[i][j] = vpMath::saturate(interp); } else if (x_lower < static_cast(src.getWidth())-1) { const Type val00 = src[y_lower][x_lower]; const Type val01 = src[y_lower][x_lower + 1]; const double interp = lerp(val00, val01, s); dst[i][j] = vpMath::saturate(interp); } else { dst[i][j] = src[y_lower][x_lower]; } } } } } template <> inline void vpImageTools::warpLinear(const vpImage &src, const vpMatrix &T, vpImage &dst, bool affine, bool centerCorner, bool fixedPoint) { if (fixedPoint && !centerCorner) { const int nbits = 16; const int64_t precision = 1 << nbits; const float precision_1 = 1 / static_cast(precision); const int64_t precision2 = 1ULL << (2 * nbits); const float precision_2 = 1 / static_cast(precision2); int64_t a0_i64 = static_cast(T[0][0] * precision); int64_t a1_i64 = static_cast(T[0][1] * precision); int64_t a2_i64 = static_cast(T[0][2] * precision); int64_t a3_i64 = static_cast(T[1][0] * precision); int64_t a4_i64 = static_cast(T[1][1] * precision); int64_t a5_i64 = static_cast(T[1][2] * precision); int64_t a6_i64 = T.getRows() == 3 ? static_cast(T[2][0] * precision) : 0; int64_t a7_i64 = T.getRows() == 3 ? static_cast(T[2][1] * precision) : 0; int64_t a8_i64 = precision; int64_t height_i64 = static_cast(src.getHeight() * precision); int64_t width_i64 = static_cast(src.getWidth() * precision); if (affine) { for (unsigned int i = 0; i < dst.getHeight(); i++) { int64_t xi = a2_i64; int64_t yi = a5_i64; for (unsigned int j = 0; j < dst.getWidth(); j++) { if (yi >= 0 && yi < height_i64 && xi >= 0 && xi < width_i64) { const int64_t xi_lower = xi & (~0xFFFF); const int64_t yi_lower = yi & (~0xFFFF); const int64_t t = yi - yi_lower; const int64_t t_1 = precision - t; const int64_t s = xi - xi_lower; const int64_t s_1 = precision - s; const int x_ = static_cast(xi >> nbits); const int y_ = static_cast(yi >> nbits); if (y_ < static_cast(src.getHeight())-1 && x_ < static_cast(src.getWidth())-1) { const vpRGBa val00 = src[y_][x_]; const vpRGBa val01 = src[y_][x_+1]; const vpRGBa val10 = src[y_+1][x_]; const vpRGBa val11 = src[y_+1][x_+1]; const int64_t interpR_i64 = static_cast(s_1*t_1*val00.R + s * t_1*val01.R + s_1 * t*val10.R + s * t*val11.R); const float interpR = (interpR_i64 >> (nbits*2)) + (interpR_i64 & 0xFFFFFFFF) * precision_2; const int64_t interpG_i64 = static_cast(s_1*t_1*val00.G + s * t_1*val01.G + s_1 * t*val10.G + s * t*val11.G); const float interpG = (interpG_i64 >> (nbits * 2)) + (interpG_i64 & 0xFFFFFFFF) * precision_2; const int64_t interpB_i64 = static_cast