Искажение изображения с синусом с кубической интерполяцией

Я пытаюсь сделать искажение с помощью синуса / косинуса по оси Y с кубической интерполяцией со следующим кодом, я сохраняю новый размер нового изображения, что и старое изображение. И результаты не выглядят хорошими. Я делаю интерполяцию только по оси Y. Я сделал расчет на основе этого математический

Mat cubic(Mat old, int freq){
int height = old.size().height;
int width = old.size().width;
int p0,p1,p2,p3;
int Bp0,Bp1,Bp2,Bp3;
int Gp0,Gp1,Gp2,Gp3;
int Rp0,Rp1,Rp2,Rp3;
unsigned char B,G,R;

double Ba,Bb,Bc,Bd;
double Ga,Gb,Gc,Gd;
double Ra,Rb,Rc,Rd;
double fi,fj;
Mat res(height,width,CV_8UC3,cv::Scalar(100));
for (int i = 0; i < width; i++) {
for (int j = 0; j < height; j++) {
fi = i;
fj = (j - (((height/30)*(sin((M_PI*i)/(freq/2)))))) + sqrt(height) ;
//Get the indexes of the for neighbors
if(fj > 20 &  fj < height){
p1 = floor(fj);
p2 = ceil(fj);
p0 = p1-1;
p3 = p2+1;
//get the color values
Vec3b p0Color = old.at<Vec3b>(Point(floor(fi),p0));
Vec3b p1Color = old.at<Vec3b>(Point(floor(fi),p1));
Vec3b p2Color = old.at<Vec3b>(Point(floor(fi),p2));
Vec3b p3Color = old.at<Vec3b>(Point(floor(fi),p3));
//cout <<" fj:" <<fj<< " p0: " << p0 <<" p1: "<< p1 << " p2: "<<p2 <<" p3:" <<p3;

Bp0 = p0Color.val[0]; Bp1 = p1Color.val[0]; Bp2 = p2Color.val[0]; Bp3 = p3Color.val[0];
Ba = -0.5*Bp0 + 1.5*Bp1 - 1.5*Bp2 + 0.5*Bp3;
Bb = Bp0 - 2.5*Bp1 + 2*Bp2 -0.5*Bp3;
Bc = -0.5*Bp0 + 0.5*Bp2;
Bd = Bp1;

Gp0 = p0Color.val[1]; Gp1 = p1Color.val[1]; Gp2 = p2Color.val[1]; Gp3 = p3Color.val[1];
Ga = -0.5*Gp0 + 1.5*Gp1 - 1.5*Gp2 + 0.5*Gp3;
Gb = Gp0 - 2.5*Gp1 + 2*Gp2 -0.5*Gp3;
Gc = -0.5*Gp0 + 0.5*Gp2;
Gd = Gp1;

Rp0 = p0Color.val[2]; Rp1 = p1Color.val[2]; Rp2 = p2Color.val[2]; Rp3 = p3Color.val[2];
Ra = -0.5*Rp0 + 1.5*Rp1 - 1.5*Rp2 + 0.5*Rp3;
Rb = Rp0 - 2.5*Rp1 + 2*Rp2 -0.5*Rp3;
Rc = -0.5*Rp0 + 0.5*Rp2;
Rd = Rp1;

B = floor(Ba*pow(fj,3) + Bb*pow(fj,2) + Bc*fj + Bd);
G = floor(Ga*pow(fj,3) + Gb*pow(fj,2) + Gc*fj + Gd);
R = floor(Ra*pow(fj,3) + Rb*pow(fj,2) + Rc*fj + Rd);
if(B==0){
cout <<"list:" << Ba <<" " << Bb <<" "<< Bc <<" " << Bd;
}
Vec3b finalColor(B,G,R);
res.at<Vec3b>(j,i) = finalColor;
}
}
}
namedWindow( "cubic", CV_WINDOW_AUTOSIZE );
imshow( "cubic", res);
return res;

}
Происхождение фото

Билинейная интерполяция

Кубическая интерполяция

Спасибо