How to write this interpolate function using OpenCV?

edit

I want to optimize this code, in particular this function:

bool interpolate(const Mat &im, float ofsx, float ofsy, float a11, float a12, float a21, float a22, Mat &res)
{         
   bool ret = false;
   // input size (-1 for the safe bilinear interpolation)
   const int width = im.cols-1;
   const int height = im.rows-1;
   // output size
   const int halfWidth  = res.cols >> 1;
   const int halfHeight = res.rows >> 1;
   int dim = res.rows * res.cols;
   float *out = res.ptr<float>(0);
   for (int j=-halfHeight; j<=halfHeight; ++j)
   {
      const float rx = ofsx + j * a12;
      const float ry = ofsy + j * a22;
      #pragma omp simd
      for(int i=-halfWidth; i<=halfWidth; ++i)
      {
         float wx = rx + i * a11;
         float wy = ry + i * a21;
         const int x = (int) floor(wx);
         const int y = (int) floor(wy);
         if (x >= 0 && y >= 0 && x < width && y < height)
         {
            // compute weights
            wx -= x; wy -= y;
            // bilinear interpolation
            *out++ = 
               (1.0f - wy) * ((1.0f - wx) * im.at<float>(y,x)   + wx * im.at<float>(y,x+1)) +
               (       wy) * ((1.0f - wx) * im.at<float>(y+1,x) + wx * im.at<float>(y+1,x+1));
         } else {
            *out++ = 0;
            ret =  true; // touching boundary of the input            
         }
      }
   }
   return ret;
}

Can anybody please help to find an equivalent function in OpenCV for the code above? I'm not expert on image processing, so I don't really know what the fucntion above does in details, but I think this is a warp-affine transformation, even though I don't really know how to define an OpenCV equivalent.

It seems that the input images are the blurred image, the original image or small patches of it. The output result is always a small patch.

Here are some samples:

Sample 1:

Args:

ofx=175.497 ofsy=315.06 a11=1.69477 a12=0.0671724 a21=0.0679493 a22=1.56309

Input image:

Output image:

Sample 2:

Args:

ofx=572.121 ofsy=326.659 a11=0.871508 a12=0 a21=0.346405 a22=1.14744

Input image:

Output image:

Sample 3:

ofx=66.571 ofsy=148.991 a11=1.12027 a12=0.126609 a21=0.126609 a22=2.53436

Input image:

Output image:

These are different sections of the original code where the function is called:

  // warp input according to current shape matrix
  interpolate(wrapper.prevBlur, lx, ly, u11*ratio, u12*ratio, u21*ratio, u22*ratio, img);

  Mat smoothed(patchImageSize, patchImageSize, CV_32FC1, (void *)&workspace.front());
  // interpolate with det == 1
  if (!interpolate(img, x, y, a11, a12, a21, a22, smoothed))
  {

  // subsample with corresponding scale
  interpolate(smoothed, (float)(patchImageSize>>1), (float)(patchImageSize>>1), imageToPatchScale, 0, 0, imageToPatchScale, patch);


  // ok, do the interpolation
  interpolate(img, x, y, a11, a12, a21, a22, patch);

This is what happens when I try to use warpAffine in the following way:

  // warp input according to current shape matrix
  interpolate(wrapper.prevBlur, lx, ly, u11*ratio, u12*ratio, u21*ratio, u22*ratio, img);

  Mat warp_mat( 2, 3, CV_32FC1 );
  Mat myPatch(img.rows, img.cols, CV_32FC1);
  warp_mat.at<float>(0,2) = lx;
  warp_mat.at<float>(1,2) = ly;
  warp_mat.at<float ...