#include "opencv2\core.

hpp"
#include "opencv2\imgcodes.hpp"
#include "opencv2\imgproc.hpp"
#include "opencv2\highgui.hpp"
#include "opencv2\aruco.hpp"
#include "opencv2\calib3d.hpp"

#include <sstream>
#include <iostream>
#include <fstream>

using namespace std;

using namespace cv;

const float calibrationSquareDimension = 0.01905f; //meters

const float arucoSquareDimension =0.1016f; //meters
const Size chessboardDimension = Size(6.9)

void createArucoMakers()
{
Mat outputMarker;

Ptr<aruco ::Dictionary =
aruco::getPredefineDictionary( aruco::PREDEFINE_DICTIONARY)

for (int i=0; i<50; i++)

{
aruco::drawMarker(markerDictionary, i, 500,outputMaeker,1)
ostringstream convert;
string imageName= "4x4Marker_";
convert << imageName << i << ".jpg";
imwrite(convert.str(),outputMarker);
}
}
void createKnownBoardPosition(Size boardSize, float squareEdgeLength,
vector<Point3f>& corners)
{
for (int i=0; i < boardSize.height; i++ )
{
for ( int j=0; j < boardSize.width; j++)
{
corners.push_back(Point3f( j* squareEdgeLength, i*squareEdgeLength, 0.0f));
}
}
}

void getChessboardCorners ( vector<Mat> images,

vector<vector<Point2f>>&allFoundConcerns, bool showResults=False)
{
for (vector<Mat>::iterator iter = images.begin(); iter != images.end(;
iter++))
{
vector<Point2f> pointBuf;
bool found =findChessboardCorners(*iter, Size(9,6),pointBuf,
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_VB_NORMALIZE_IMAGE);
if (found)
{
allFoundConcerns.push_back(pointBuf);
}
 if (showResults)
{
drawChessboardCorners(*iter,Size(9,6), pointBuf, found);
imshow("Looking for Corners",*iter);
waiKey(0);
}

void cameraCalibration(vector<Mat> calibrationImages, Size boardSize, float

squareEdgeLength, Mat& cameraMatrix,Mat& distanceCore)
{
vector<vector<Point2f>> checkerboardImageSpacePoints;
getChessboardCorners(calibrationUmages, checkerboardImageSpacePoints,
false);

vector<vector<Point3f>> worldSpaceCornerPoints(1);
createKnownBoardPosition(boardSize, squareEdgeLength,
worldSpaceCornerPoints[0]);
worldSpaceCornerPoints.resize(checkerboardImageSpacePoints.size(),
worldSpaceCornerPoints[0]);

vector<Mat> rVector, tVector;

distanceCoefficients = Mat::zeros(8,1, CV_64F);

calibrateCamera(worldSpaceCornerPoints, checkerboardImageSpacePoints,
boardSize, cameraMatrix, distanceCoefficients, rVector,tVector);
}

bool saveCameraCalibration(string name, Mat cameraMatrix, Mat

distanceCoefficients)
{
ofstream outStream(name);
if (outStream)
{
uint16_t rows = cameraMatrix.rows;
uint16_t colums= cameraMatrix.cols;

for(int r =0; r< rows; r++)

{
for (int c=0; c< colums; c++)
{
double value =cameraMatrix.at<double>(r,c);
outStream << value <<endl;
}
}
outStream.close();
return true;

}
}

int main(int argv,char**argc)

{
Mat frame;
 Mat drawToFrame;

Mat cameraMatrix = Mat::eye(3,3,CV_64);

Mat distanceCoefficients;
vector<Mat> savedImages;

vector<vector<Point2f>> markerCorners, rejectedCanndidates;

VideoCapture vid(0);
if (!vid.isOpened())
{
return;
}

int framePerSecond =20;

nameWindow("Wedcam", CV_WINDOW_AUTOSIZE);

While (true)
{
if (!vid.read(frame))
break;

vector<Vec2f> foundPoints;
bool found = false;

found = findChessboardCorners(frame, chessboardDimension, foundPoints,

CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_VB_NORMALIZE_IMAGE)
frame.copyTo(drawToFrame);
drawChessboardCorners(drawToFrame, chessboardDimension,
foundPoints,found);
if (found)
imshow("Wedcam",drawToFrame);
else imshow("Wedcam",frame);
char character = waitKey( 1000/ framePerSecond);

switch(character)
{
case ' ':
//saving image
if (found)
{
Mat temp;
frame.copyTo(temp);
savedImages.push_back(temp);

}
break;
case 13:
//start calibration
if (savedImages.size() > 15)
{
cameraCalibration(saveImages, chessboardDimension,
calibrationSquareDimension, cameraMatrix, distanceCoefficients);
saveCameraCalibration("I loveCameraCalibration", cameraMatrix,
distanceCoefficients);
}
break;
 case 27:
//exit
return 0;
break;
}
}
return 0;