Reverse the Fish-eye Distortion(I've used openCV with VC++) - c

Question

I've made a simulation of fish eye distortion.
I want to develop a reverse program that can convert the distorted image to normal image.
I've tried to use undistortPonts() function but couldn't understand the input(dist-coefficient).
cv.UndistortPoints(distorted, undistorted, intrinsics, dist_coeffs)
My code for fish eye distortion:
#include "stdio.h"
#include <cv.h>
#include <highgui.h>
#include <math.h>
#include <iostream>
void sampleImage(const IplImage* arr, float idx0, float idx1, CvScalar& res)
{
if(idx0<0 || idx1<0 || idx0>(cvGetSize(arr).height-1) || idx1>(cvGetSize(arr).width-1))
{
res.val[0]=0;
res.val[1]=0;
res.val[2]=0;
res.val[3]=0;
return;
}
float idx0_fl=floor(idx0);
float idx0_cl=ceil(idx0);
float idx1_fl=floor(idx1);
float idx1_cl=ceil(idx1);
CvScalar s1=cvGet2D(arr,(int)idx0_fl,(int)idx1_fl);
CvScalar s2=cvGet2D(arr,(int)idx0_fl,(int)idx1_cl);
CvScalar s3=cvGet2D(arr,(int)idx0_cl,(int)idx1_cl);
CvScalar s4=cvGet2D(arr,(int)idx0_cl,(int)idx1_fl);
float x = idx0 - idx0_fl;
float y = idx1 - idx1_fl;
res.val[0]= s1.val[0]*(1-x)*(1-y) + s2.val[0]*(1-x)*y + s3.val[0]*x*y + s4.val[0]*x*(1-y);
res.val[1]= s1.val[1]*(1-x)*(1-y) + s2.val[1]*(1-x)*y + s3.val[1]*x*y + s4.val[1]*x*(1-y);
res.val[2]= s1.val[2]*(1-x)*(1-y) + s2.val[2]*(1-x)*y + s3.val[2]*x*y + s4.val[2]*x*(1-y);
res.val[3]= s1.val[3]*(1-x)*(1-y) + s2.val[3]*(1-x)*y + s3.val[3]*x*y + s4.val[3]*x*(1-y);
}
float xscale;
float yscale;
float xshift;
float yshift;
float getRadialX(float x,float y,float cx,float cy,float k)
{
x = (x*xscale+xshift);
y = (y*yscale+yshift);
float res = x+((x-cx)*k*((x-cx)*(x-cx)+(y-cy)*(y-cy)));
return res;
}
float getRadialY(float x,float y,float cx,float cy,float k)
{
x = (x*xscale+xshift);
y = (y*yscale+yshift);
float res = y+((y-cy)*k*((x-cx)*(x-cx)+(y-cy)*(y-cy)));
return res;
}
float thresh = 1;
float calc_shift(float x1,float x2,float cx,float k)
{
float x3 = x1+(x2-x1)*0.5;
float res1 = x1+((x1-cx)*k*((x1-cx)*(x1-cx)));
float res3 = x3+((x3-cx)*k*((x3-cx)*(x3-cx)));
// std::cerr<<"x1: "<<x1<<" - "<<res1<<" x3: "<<x3<<" - "<<res3<<std::endl;
if(res1>-thresh && res1 < thresh)
return x1;
if(res3<0)
{
return calc_shift(x3,x2,cx,k);
}
else
{
return calc_shift(x1,x3,cx,k);
}
}
int main(int argc, char** argv)
{
IplImage* src = cvLoadImage( "D:\\2012 Projects\\FishEye\\Debug\\images\\grid1.bmp", 1 );
IplImage* dst = cvCreateImage(cvGetSize(src),src->depth,src->nChannels);
IplImage* dst2 = cvCreateImage(cvGetSize(src),src->depth,src->nChannels);
float K=0.002;
float centerX=(float)(src->width/2);
float centerY=(float)(src->height/2);
int width = cvGetSize(src).width;
int height = cvGetSize(src).height;
xshift = calc_shift(0,centerX-1,centerX,K);
float newcenterX = width-centerX;
float xshift_2 = calc_shift(0,newcenterX-1,newcenterX,K);
yshift = calc_shift(0,centerY-1,centerY,K);
float newcenterY = height-centerY;
float yshift_2 = calc_shift(0,newcenterY-1,newcenterY,K);
// scale = (centerX-xshift)/centerX;
xscale = (width-xshift-xshift_2)/width;
yscale = (height-yshift-yshift_2)/height;
std::cerr<<xshift<<" "<<yshift<<" "<<xscale<<" "<<yscale<<std::endl;
std::cerr<<cvGetSize(src).height<<std::endl;
std::cerr<<cvGetSize(src).width<<std::endl;
for(int j=0;j<cvGetSize(dst).height;j++)
{
for(int i=0;i<cvGetSize(dst).width;i++)
{
CvScalar s;
float x = getRadialX((float)i,(float)j,centerX,centerY,K);
float y = getRadialY((float)i,(float)j,centerX,centerY,K);
sampleImage(src,y,x,s);
cvSet2D(dst,j,i,s);
}
}
#if 0
cvNamedWindow( "Source1", 1 );
cvShowImage( "Source1", dst);
cvWaitKey(0);
#endif
cvSaveImage("D:\\2012 Projects\\FishEye\\Debug\\images\\grid3.bmp",dst,0);
cvNamedWindow( "Source1", 1 );
cvShowImage( "Source1", src);
cvWaitKey(0);
cvNamedWindow( "Distortion", 2 );
cvShowImage( "Distortion", dst);
cvWaitKey(0);
#if 0
for(int j=0;j<cvGetSize(src).height;j++)
{
for(int i=0;i<cvGetSize(src).width;i++)
{
CvScalar s;
sampleImage(src,j+0.25,i+0.25,s);
cvSet2D(dst,j,i,s);
}
}
cvNamedWindow( "Source1", 1 );
cvShowImage( "Source1", src);
cvWaitKey(0);
#endif
}

Answer 1

Actually, my original anwser was about the undistortion algorithm for individual points. If you want to undistort a complete image, there is a much simpler technique, as explained in this other thread:
Understanding of openCV undistortion
The outline of the algorithm (which is the one used in OpenCV function undistort()) is as follow. For each pixel of the destination lens-corrected image do:
Convert the pixel coordinates (u_dst, v_dst) to normalized coordinates (x', y') using the inverse of the calibration matrix K,
Apply your lens-distortion model, to obtain the distorted normalized coordinates (x'', y''),
Convert (x'', y'') to distorted pixel coordinates (u_src, v_src) using the calibration matrix K,
Use the interpolation method of your choice to find the intensity/depth associated with the pixel coordinates (u_src, v_src) in the source image, and assign this intensity/depth to the current destination pixel (u_dst, v_dst).
Original answer:
Here is the undistortion algorithm extracted from OpenCV function undistortPoints() :
void dist2norm(const cv::Point2d &pt_dist, cv::Point2d &pt_norm) const {
pt_norm.x = (pt_dist.x-Kcx)/Kfx;
pt_norm.y = (pt_dist.y-Kcy)/Kfy;
int niters=(Dk1!=0.?5:0);
double x0=pt_norm.x, y0=pt_norm.y;
for(int i=0; i<niters; ++i) {
double x2=pt_norm.x*pt_norm.x,
y2=pt_norm.y*pt_norm.y,
xy=pt_norm.x*pt_norm.y,
r2=x2+y2;
double icdist = 1./(1 + ((Dk3*r2 + Dk2)*r2 + Dk1)*r2);
double deltaX = 2*Dp1*xy + Dp2*(r2 + 2*x2);
double deltaY = Dp1*(r2 + 2*y2) + 2*Dp2*xy;
pt_norm.x = (x0-deltaX)*icdist;
pt_norm.y = (y0-deltaY)*icdist;
}
}
If you provide the coordinates of a point in the distorted image in argument pt_dist, it will calculate the normalized coordinates of the associated point and return them in pt_norm. Then, you can obtain the coordinates of the associated point in the undistorted image as
pt_undist = K . [pt_norm.x; pt_norm.y; 1]
where K is the camera matrix.
The standard lens distortion model used by OpenCV is explained at the beginning of this page:
where the distortion coefficients are (k1,k2,p1,p2,k3, k4,k5,k6) (most often we use k4=k5=k6=0).
I don't know what is your model for FishEye distortion, but you can surely adapt the above algorithm to your case. Otherwise, you may use a non-linear optimization algorithm (e.g. Levenberg-Marquardt or any other), to recover the undistorted coordinates from the distorted one.