#include <opencv2/opencv.hpp>

#define CORN_HOR 8
#define CORN_VER 5
#define COUNT CORN_HOR*CORN_VER
int main(int argc, char* argv[])
{
  CvSize board_sz=cvSize(CORN_HOR,CORN_VER);
  CvPoint2D32f corners[COUNT];
  int corner_count;

  IplImage *image=cvLoadImage("chess2.png");
  if (!image) return EXIT_FAILURE;
  CvSize imsize=cvGetSize(image);
  IplImage *gray=cvCreateImage(imsize,IPL_DEPTH_8U,1);

  int found=cvFindChessboardCorners(image,board_sz,corners,&corner_count,CV_CALIB_CB_ADAPTIVE_THRESH|CV_CALIB_CB_FILTER_QUADS);

  cvCvtColor(image,gray,CV_BGR2GRAY);
  cvFindCornerSubPix(gray,corners,corner_count,cvSize( 11, 11 ),cvSize(-1,-1 ),cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,0.1));

  IplImage *clone1=cvCloneImage(image);
  cvDrawChessboardCorners(clone1,board_sz,corners,corner_count,found);
  cvShowImage("chess detect",clone1);

  if (corner_count==COUNT)
  {
    CvMat *points=cvCreateMat(2*COUNT,2,CV_32FC1);
    CvMat *object=cvCreateMat(2*COUNT,3,CV_32FC1);
    CvMat *point_mat=cvCreateMat(2,1,CV_32SC1);
    for(int i=0;i<COUNT;i++) 
    {
      cvSet1D(points,i,cvScalar(corners[i].x,corners[i].y,0,0)); 
      cvSet1D(object,i,cvScalar(i/CORN_HOR,i%CORN_HOR,0,0)); 
      cvSet1D(points,i+COUNT,cvScalar(corners[i].x,corners[i].y,0,0)); 
      cvSet1D(object,i+COUNT,cvScalar(i/CORN_HOR,i%CORN_HOR,0,0)); 

    }
    cvSet1D(point_mat,0,cvScalarAll(COUNT));
    cvSet1D(point_mat,1,cvScalarAll(COUNT));

    CvMat *intrinsic =cvCreateMat(3,3,CV_32FC1);
    CvMat *distortion =cvCreateMat(5,1,CV_32FC1);

    cvInitIntrinsicParams2D(object,points,point_mat,imsize,intrinsic);

    cvCalibrateCamera2(object,points,point_mat,imsize,intrinsic,distortion,NULL,NULL,CV_CALIB_FIX_ASPECT_RATIO); 

    IplImage *map_x=cvCreateImage(imsize,IPL_DEPTH_32F,1);
    IplImage *map_y=cvCreateImage(imsize,IPL_DEPTH_32F,1);
    
    cvInitUndistortMap(intrinsic,distortion,map_x,map_y);

    IplImage *clone2=cvCloneImage(image); 
    cvRemap(image,clone2,map_x,map_y);

    cvShowImage("Undistord",clone2);

    cvReleaseImage(&clone2);
    cvReleaseImage(&map_x);
    cvReleaseImage(&map_y);
    cvReleaseMat(&intrinsic);
    cvReleaseMat(&distortion);
    cvReleaseMat(&points);
    cvReleaseMat(&object);
    cvReleaseMat(&point_mat);

  }
  cvWaitKey();
  cvReleaseImage(&clone1);
  cvReleaseImage(&image);
  cvReleaseImage(&gray);
  return EXIT_SUCCESS;
}
