#include <opencv2/opencv.hpp>

using namespace std;

using namespace cv;

//Global vars

Mat camera_matrix(3,3,CV_64F);

Mat dist_coeffs(1,4,CV_64F);

Mat imain = Mat::zeros( 480, 480, CV_8UC3 );

Mat imaout = Mat::zeros( 480, 480, CV_8UC3 );;

void resetUndistortCoeffs()

{

    setIdentity(camera_matrix);

    camera_matrix.at<double>(0,0) = (imain.cols -1)/2.0;

    camera_matrix.at<double>(0,2) = (imain.cols -1)/2.0;

    camera_matrix.at<double>(1,1) = (imain.rows -1)/2.0;

    camera_matrix.at<double>(1,2) = (imain.rows -1)/2.0;

    dist_coeffs = Scalar::all(0.0);

}

//=============================================================

// main procedure

//=============================================================

int main(int argc, char *argv[])

{       

    imshow("Original image", imain);

    waitKey(0);

    //Init camera_matrix and dist_coeffs

    resetUndistortCoeffs();

    FileStorage fs2("testcvUndistort1.yml", FileStorage::WRITE);

    fs2 << "camera_matrix" << camera_matrix << "dist_coeffs" << dist_coeffs;

    fs2.release();

    //Do undistort

    undistort(imain, imaout, camera_matrix, dist_coeffs);   //<<----- Crash occurrs inside this func

    //See results

    imshow("Imatge undistorted", imaout);

    waitKey(0);

}