dab_tracker_optical_calibration.cpp

/** \file dab_tracker_optical_calibration.cpp
*/

#include "dab_tracker_optical_calibration.h"
#include "ofxCvColorImage.h"
#include "ofxCvGrayscaleImage.h"
#include "ofxCvFloatImage.h"
#include "ofxCvShortImage.h"
#include "dab_tracker_serialize_helper.h"

using namespace dab;
using namespace dab::tracker;

cv::Size OpticalCalibration::sBoardSize = cv::Size(8, 6);
OpticalCalibration::Pattern OpticalCalibration::sBoardPattern = OpticalCalibration::CHESSBOARD;
float OpticalCalibration::sSquareSize = 25.0;
OpticalCalibration::Mode OpticalCalibration::sCalibrationMode = OpticalCalibration::CALIBRATED;
unsigned int OpticalCalibration::sCalibrationFrameCount = 20;
double OpticalCalibration::sCaptureInterval = 5000.0;

OpticalCalibration::OpticalCalibration(ofxCvImage* pInputImage)
: mInputImage(pInputImage)
, mBoardSize(sBoardSize)
, mBoardPattern(sBoardPattern)
, mSquareSize(sSquareSize)
, mCalibrationMode(sCalibrationMode)
, mCalibrationFrameCount(sCalibrationFrameCount)
, mCaptureInterval(sCaptureInterval)
, mPreviousCaptureTime(0.0)
, mCapturePatternNow(false)
{
    mFrameSize = { mInputImage->width, mInputImage->height };
    
    if(mCalibFixPrincipalPoint == true) mFlag |= cv::CALIB_FIX_PRINCIPAL_POINT;
    if(mCalibZeroTangentDist == true) mFlag |= cv::CALIB_ZERO_TANGENT_DIST;
    if(mAspectRatio > 0.0) mFlag |= cv::CALIB_FIX_ASPECT_RATIO;
    
    mCameraMatrix = cv::Mat::eye(3, 3, CV_64F);
    mDistCoeffs = cv::Mat::zeros(8, 1, CV_64F);
    
    if(dynamic_cast<ofxCvColorImage*>(mInputImage) != nullptr)
    {
        mOutputImage = new ofxCvColorImage();
    }
    else if(dynamic_cast<ofxCvGrayscaleImage*>(mInputImage) != nullptr)
    {
        mOutputImage = new ofxCvGrayscaleImage();
    }
    else if(dynamic_cast<ofxCvFloatImage*>(mInputImage) != nullptr )
    {
        mOutputImage = new ofxCvFloatImage();
    }
	else if (dynamic_cast<ofxCvShortImage*>(mInputImage) != nullptr)
	{
		mOutputImage = new ofxCvShortImage();
	}
    
    mOutputImage->allocate(mFrameSize[0], mFrameSize[1]);
    
    mSettingsFileName = "Tracker_OpticalCalibration_Settings.json";
}

OpticalCalibration::~OpticalCalibration()
{
    if(mOutputImage != nullptr) delete mOutputImage;
}

ofxCvImage*
OpticalCalibration::outputImage()
{
    return mOutputImage;
}

OpticalCalibration::Mode
OpticalCalibration::calibrationMode() const
{
	return mCalibrationMode;
}

std::array<float, 9>
OpticalCalibration::cameraMatrix() const
{
	std::array<float, 9> _camMatrix;
	for (int rI = 0, i = 0; rI < 3; ++rI)
	{
		const double* _camData = mCameraMatrix.ptr<double>(rI);

		for (int cI = 0; cI < 3; ++cI, ++i)
		{
			_camMatrix[i] = _camData[cI];
		}
	}

	return _camMatrix;
}

std::array<float, 8>
OpticalCalibration::distCoefficients() const
{
	std::array<float, 8> _distCoefficients;
	for (int rI = 0, i = 0; rI < 8; ++rI, ++i)
	{
		const double* _distData = mDistCoeffs.ptr<double>(rI);
		_distCoefficients[i] = _distData[0];
	}
	return _distCoefficients;
}

void 
OpticalCalibration::startCalibration()
{
	mImagePoints.clear();
	mCalibrationMode = CALIBRATING;
}

void 
OpticalCalibration::captureCalibationPattern()
{
	if (mCalibrationMode != CALIBRATING) return;

	mCapturePatternNow = true;
}

void 
OpticalCalibration::setCameraMatrix(const std::array<float, 9>& pCameraMatrix)
{
	for(int r=0, i=0; r<3; ++r)
	{
		for(int c=0; c<3; ++c, ++i)
		{
			mCameraMatrix.at<double>(r, c) = pCameraMatrix[i];
		}
	}
}

void 
OpticalCalibration::setDistCoefficients(const std::array<float, 8>& pDistCoefficients)
{
	for(int r=0, i=0; r<8; ++r, ++i)
	{
		mDistCoeffs.at<double>(r, 0) = pDistCoefficients[i];
	}
}

void
OpticalCalibration::setupGui(ofxDatGui* pGui)
{
    mGuiFolder = pGui->addFolder("OpticalCalibration");
    
    mBoardSizeXControl = mGuiFolder->addTextInput("Pattern Count X", std::to_string(mBoardSize.width));
    mBoardSizeXControl->setInputType(ofxDatGuiInputType::NUMERIC);
    mBoardSizeXControl->onTextInputEvent(this, &OpticalCalibration::onTextInputEvent);

    mBoardSizeYControl = mGuiFolder->addTextInput("Pattern Count Y", std::to_string(mBoardSize.height));
    mBoardSizeYControl->setInputType(ofxDatGuiInputType::NUMERIC);
    mBoardSizeYControl->onTextInputEvent(this, &OpticalCalibration::onTextInputEvent);
    
    mSquareSizeControl = mGuiFolder->addTextInput("Pattern Size", std::to_string(mSquareSize));
    mSquareSizeControl->setInputType(ofxDatGuiInputType::NUMERIC);
    mSquareSizeControl->onTextInputEvent(this, &OpticalCalibration::onTextInputEvent);
    
    std::vector<std::string> boardPatternOptions = { "CHESSBOARD", "CIRCLES_GRID", "ASYMMETRIC_CIRCLES_GRID" };
    mBoardPatternControl = new ofxDatGuiDropdown("Board Pattern", boardPatternOptions);
    mGuiFolder->attachItem(mBoardPatternControl);
    mBoardPatternControl->onDropdownEvent(this, &OpticalCalibration::onDropdownEvent);
    
    mCalibrationFrameCountControl = mGuiFolder->addTextInput("Capture Count", std::to_string(mCalibrationFrameCount));
    mCalibrationFrameCountControl->setInputType(ofxDatGuiInputType::NUMERIC);
    mCalibrationFrameCountControl->onTextInputEvent(this, &OpticalCalibration::onTextInputEvent);
    
    mCaptureIntervalControl = mGuiFolder->addTextInput("Capture Interval", std::to_string(mCaptureInterval));
    mCaptureIntervalControl->setInputType(ofxDatGuiInputType::NUMERIC);
    mCaptureIntervalControl->onTextInputEvent(this, &OpticalCalibration::onTextInputEvent);
    
    mAspectRatioControl = mGuiFolder->addTextInput("Aspect Ratio", std::to_string(mAspectRatio));
    mAspectRatioControl->setInputType(ofxDatGuiInputType::NUMERIC);
    mAspectRatioControl->onTextInputEvent(this, &OpticalCalibration::onTextInputEvent);
    
    mFlipVerticalControl = new ofxDatGuiToggle("Flip Vertical", mFlipVertical);
    mGuiFolder->attachItem(mFlipVerticalControl);
	mFlipVerticalControl->onButtonEvent(this, &OpticalCalibration::onButtonEvent);

	mStartCalibrationControl = new ofxDatGuiButton("Start Calibration");
	mGuiFolder->attachItem(mStartCalibrationControl);
	mStartCalibrationControl->onButtonEvent(this, &OpticalCalibration::onButtonEvent);

	mCapturePatternNowControl = new ofxDatGuiToggle("Capture Pattern Now");
	mGuiFolder->attachItem(mCapturePatternNowControl);
	mCapturePatternNowControl->onButtonEvent(this, &OpticalCalibration::onButtonEvent);
}

void
OpticalCalibration::update() throw (dab::Exception)
{
    cv::Mat inputMat;
    
    if(dynamic_cast<ofxCvColorImage*>(mInputImage) != nullptr)
    {
		cv::Mat colorMat = cv::cvarrToMat(mInputImage->getCvImage(), false);
		cvtColor(colorMat, inputMat, CV_RGB2GRAY);
    }
    else
    {
		inputMat = cv::cvarrToMat(mInputImage->getCvImage(), false);
    }

    if( mFlipVertical == true )  cv::flip( inputMat, inputMat, 0 );

    
    if(mCalibrationMode == CALIBRATING)
    {
        int currentCalibrationFrameCount = mImagePoints.size();
        
        //std::cout << "cur CalibFrameCount " << currentCalibrationFrameCount << " target CalibFrameCount " << mCalibrationFrameCount << "\n";
        
        if(currentCalibrationFrameCount >= mCalibrationFrameCount)
        {
            runCalibration();
            mCalibrationMode = CALIBRATED;
            mStartCalibrationControl->setLabel("Calibrate");
        }
        else
        {
			double captureTime = ofGetElapsedTimeMillis();

			if (mCapturePatternNow == true || (mCaptureInterval > 0.0 && captureTime - mPreviousCaptureTime > mCaptureInterval ) )
			{

				std::vector<cv::Point2f> patternPoints;
				bool patternFound = detectCalibrationPattern(inputMat, patternPoints);
            
				//std::cout << "patternFound " << patternFound << "\n";
            
				if (patternFound == true)
				{
					//std::cout << "add points to image points\n";   
					mPreviousCaptureTime = captureTime;
					mCapturePatternNow = false;
					mCapturePatternNowControl->setChecked(mCapturePatternNow);
					mImagePoints.push_back(patternPoints);
				}
            }
            
            mStartCalibrationControl->setLabel("Capture Frame " + std::to_string(mImagePoints.size()) + " out of " + std::to_string(mCalibrationFrameCount));
        }
    }
    
    if(mCalibrationMode == CALIBRATED)
    {
        cv::Mat inputMat = cv::cvarrToMat(mInputImage->getCvImage(), false);
        cv::Mat outputMat = cv::cvarrToMat(mOutputImage->getCvImage(), false);
        cv::undistort(inputMat, outputMat, mCameraMatrix, mDistCoeffs);

//        void cvUndistort2(const CvArr* src, CvArr* dst, const CvMat* camera_matrix, const CvMat* distortion_coeffs, const CvMat* new_camera_matrix=0 )¶
    }
    else
    {
        cvCopy(mInputImage->getCvImage(), mOutputImage->getCvImage());
    }
    
    mOutputImage->flagImageChanged();
}

void
OpticalCalibration::display() throw (dab::Exception)
{
    if(mDisplay == false) return;
    mOutputImage->draw(mDisplayPosition[0], mDisplayPosition[1], mDisplaySize[0], mDisplaySize[1]);
    
    if(mCalibrationMode == CALIBRATING && mImagePoints.size() > 0)
    {
        std::vector<cv::Point2f>& mostRecentImagePoints = mImagePoints.back();
        int pointCount = mostRecentImagePoints.size();
        
        std::array<float, 2> displayPosition = { static_cast<float>(mDisplayPosition[0]), static_cast<float>(mDisplayPosition[1]) };
        std::array<float,2> displayScale = { static_cast<float>(mDisplaySize[0]) / static_cast<float>(mInputImage->width), static_cast<float>(mDisplaySize[1]) / static_cast<float>(mInputImage->height) };
        
        glPushMatrix();
        glTranslated(displayPosition[0], displayPosition[1], 0.0);
        glScaled(displayScale[0], displayScale[1], 1.0);
        ofSetColor(0, 255, 0, 255);
        
        for(int pI=0; pI<pointCount; ++pI)
        {
            ofDrawCircle(mostRecentImagePoints[pI].x, mostRecentImagePoints[pI].y, 10.0);

        }
        
        glPopMatrix();
        ofSetColor(255, 255, 255, 255);
    }
}

void
OpticalCalibration::saveSettings() throw (dab::Exception)
{
    SerializeHelper& serializeHelper = SerializeHelper::get();
    Json::Value jsonData;
    
    try
    {
        std::array<int, 2> _boardSize = { mBoardSize.height, mBoardSize.width };
        serializeHelper.addValue<int, 2>(jsonData, "BoardSize", _boardSize);
        
        if(mBoardPattern == CHESSBOARD) serializeHelper.addValue(jsonData, "BoardPattern", "CHESSBOARD");
        else if(mBoardPattern == CIRCLES_GRID) serializeHelper.addValue(jsonData, "BoardPattern", "CIRCLES_GRID");
        else if(mBoardPattern == ASYMMETRIC_CIRCLES_GRID) serializeHelper.addValue(jsonData, "BoardPattern", "ASYMMETRIC_CIRCLES_GRID");
        else serializeHelper.addValue(jsonData, "BoardPattern", "NOT_EXISTING");
        
        serializeHelper.addValue(jsonData, "FrameCount", mCalibrationFrameCount);
        serializeHelper.addValue(jsonData, "SquareSize", mSquareSize);
        serializeHelper.addValue(jsonData, "AspectRatio", mAspectRatio);
        serializeHelper.addValue(jsonData, "CaptureInterval", mCaptureInterval);
        serializeHelper.addValue(jsonData, "CalibZeroTangentDist", mCalibZeroTangentDist);
        serializeHelper.addValue(jsonData, "CalibFixPrincipalPoint", mCalibFixPrincipalPoint);
        serializeHelper.addValue(jsonData, "FlipVertical", mFlipVertical);
        
        std::array<float, 9> _camDataArray;
        for(int rI=0, i=0; rI<3; ++rI)
        {
            const double* _camData = mCameraMatrix.ptr<double>(rI);
            
            for(int cI=0; cI<3; ++cI, ++i)
            {
                _camDataArray[i] = _camData[cI];
            }
        }
        serializeHelper.addValue<float, 9>(jsonData, "CameraMatrix", _camDataArray);
        
        std::array<float, 8> _distCoeffsArray;
        for(int rI=0, i=0; rI<8; ++rI, ++i)
        {
            const double* _distData = mDistCoeffs.ptr<double>(rI);
            _distCoeffsArray[i] = _distData[0];
        }
        serializeHelper.addValue<float, 8>(jsonData, "DistCoeffs", _distCoeffsArray);

		Json::Value cameraTransformsData;
		int cameraTransformCount = mCameraRotations.size();

		for(int tI=0; tI< cameraTransformCount; ++tI)
		{ 
			Json::Value cameraTransformData;

			std::array<float, 3> _transArray;
			std::array<float, 3> _rotArray;

			const double* _transData = mCameraTranslations[tI].ptr<double>(0);
			const double* _rotData = mCameraRotations[tI].ptr<double>(0);

			for (int i = 0; i < 3; ++i)
			{
				_transArray[i] = _transData[i];
				_rotArray[i] = _rotData[i];
			}
			
			serializeHelper.addValue<float, 3>(cameraTransformData, "CameraTranslation", _transArray);
			serializeHelper.addValue<float, 3>(cameraTransformData, "CameraRotation", _rotArray);

			cameraTransformsData.append(cameraTransformData);
		}

		jsonData["CameraTransforms"] = cameraTransformsData;

        serializeHelper.save(jsonData, mSettingsFileName);
    }
    catch(Exception& e)
    {
        e += Exception("TRACKER ERROR: failed to save VideoCrop settings", __FILE__, __FUNCTION__, __LINE__);
        throw e;
    }
}

void
OpticalCalibration::restoreSettings() throw (dab::Exception)
{
    SerializeHelper& serializeHelper = SerializeHelper::get();
    Json::Value jsonData;
    
    try
    {
        serializeHelper.restore(jsonData, mSettingsFileName);
        
        std::array<int, 2> _boardSize;
        serializeHelper.getValue<int, 2>(jsonData, "BoardSize", _boardSize);
        mBoardSize.width = _boardSize[0];
        mBoardSize.height = _boardSize[1];
        
        std::string _boardPattern;
        serializeHelper.getValue(jsonData, "BoardPattern", _boardPattern);
        if(_boardPattern == "CHESSBOARD") mBoardPattern == CHESSBOARD;
        else if(_boardPattern == "CIRCLES_GRID") mBoardPattern == CIRCLES_GRID;
        else if(_boardPattern == "ASYMMETRIC_CIRCLES_GRID") mBoardPattern == ASYMMETRIC_CIRCLES_GRID;
        else mBoardPattern = NOT_EXISTING;

        int _mCalibrationFrameCount;
        serializeHelper.getValue(jsonData, "FrameCount", _mCalibrationFrameCount);
        mCalibrationFrameCount = _mCalibrationFrameCount;
        serializeHelper.getValue(jsonData, "SquareSize", mSquareSize);
        serializeHelper.getValue(jsonData, "AspectRatio", mAspectRatio);
        serializeHelper.getValue(jsonData, "CaptureInterval", mCaptureInterval);
        serializeHelper.getValue(jsonData, "CalibZeroTangentDist", mCalibZeroTangentDist);
        serializeHelper.getValue(jsonData, "CalibFixPrincipalPoint", mCalibFixPrincipalPoint);
        serializeHelper.getValue(jsonData, "FlipVertical", mFlipVertical);

        std::array<float, 9> _camDataArray;
        serializeHelper.getValue<float, 9>(jsonData, "CameraMatrix", _camDataArray);
        
        for(int r=0, i=0; r<3; ++r)
        {
            for(int c=0; c<3; ++c, ++i)
            {
                mCameraMatrix.at<double>(r, c) = _camDataArray[i];
            }
        }


        std::array<float, 8> _distCoeffsArray;
        serializeHelper.getValue<float, 8>(jsonData, "DistCoeffs", _distCoeffsArray);
        
        for(int r=0, i=0; r<8; ++r, ++i)
        {
            mDistCoeffs.at<double>(r, 0) = _distCoeffsArray[i];
        }


		mCameraRotations.clear();
		mCameraTranslations.clear();

		const Json::Value& cameraTransformsData = jsonData["CameraTransforms"];
		unsigned int cameraTransformCount = cameraTransformsData.size();

		for (int tI = 0; tI < cameraTransformCount; ++tI)
		{
			const Json::Value& cameraTransformData = cameraTransformsData[tI];

			std::array<float, 3> _transArray;
			std::array<float, 3> _rotArray;
			cv::Mat transMat = cv::Mat::zeros(3, 1, CV_64F);
			cv::Mat rotMat = cv::Mat::zeros(3, 1, CV_64F);

			serializeHelper.getValue<float, 3>(cameraTransformData, "CameraTranslation", _transArray);
			serializeHelper.getValue<float, 3>(cameraTransformData, "CameraRotation", _rotArray);

			for (int i = 0; i < 3; ++i)
			{
				transMat.at<double>(i, 0) = _transArray[i];
				rotMat.at<double>(i, 0) = _rotArray[i];
			}

			mCameraTranslations.push_back(transMat);
			mCameraRotations.push_back(rotMat);
		}

		//// debug
		//std::cout << "rvecs " << mCameraRotations.size() << "\n";
		//for (int i = 0; i < mCameraRotations.size(); ++i)
		//{
		//	std::cout << "i " << i << " rvec s " << mCameraRotations[i].size() << " d " << mCameraRotations[i] << "\n";
		//}
		//std::cout << "tvecs " << mCameraTranslations.size() << "\n";
		//for (int i = 0; i < mCameraTranslations.size(); ++i)
		//{
		//	std::cout << "i " << i << " tvec s " << mCameraTranslations[i].size() << " d " << mCameraTranslations[i] << "\n";
		//}
		//// debug done

        // update gui
        mBoardSizeXControl->setText(std::to_string(mBoardSize.width));
        mBoardSizeYControl->setText(std::to_string(mBoardSize.height));
        mSquareSizeControl->setText(std::to_string(mSquareSize));
        if(mBoardPattern == CHESSBOARD) mBoardPatternControl->select(0);
        else if(mBoardPattern == CIRCLES_GRID) mBoardPatternControl->select(1);
        else if(mBoardPattern == ASYMMETRIC_CIRCLES_GRID) mBoardPatternControl->select(2);
        mCalibrationFrameCountControl->setText(std::to_string(mCalibrationFrameCount));
        mCaptureIntervalControl->setText(std::to_string(mCaptureInterval));
        mAspectRatioControl->setText(std::to_string(mAspectRatio));
        mFlipVerticalControl->setChecked(mFlipVertical);

    }
    catch(Exception& e)
    {
        e += Exception("TRACKER ERROR: failed to load OpticalCalibration settings", __FILE__, __FUNCTION__, __LINE__);
        throw e;
    }
}

bool
OpticalCalibration::detectCalibrationPattern(cv::Mat& pInputImage, std::vector<cv::Point2f>& pPatternPoints)
{
    bool patternFound;
    
    if(mBoardPattern == CHESSBOARD)
    {
        patternFound = cv::findChessboardCorners( pInputImage, mBoardSize, pPatternPoints, cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK | cv::CALIB_CB_NORMALIZE_IMAGE);
        
        //std::cout << "chess patternFound " << patternFound << "\n";
    }
    else if(mBoardPattern == CIRCLES_GRID)
    {
        patternFound = cv::findCirclesGrid( pInputImage, mBoardSize, pPatternPoints );
    }
    else if(mBoardPattern == ASYMMETRIC_CIRCLES_GRID)
    {
        patternFound = cv::findCirclesGrid( pInputImage, mBoardSize, pPatternPoints, cv::CALIB_CB_ASYMMETRIC_GRID );
    }
    else
    {
        patternFound = false;
    }
    
    if(patternFound == true)                // If done with success,
    {
        // improve the found corners' coordinate accuracy for chessboard
        if( mBoardPattern == CHESSBOARD)
        {
//            cv::Mat inputGray;
//            cvtColor(pInputImage, inputGray, cv::COLOR_BGR2GRAY);
//            cv::cornerSubPix( inputGray, pPatternPoints, cv::Size(11,11), cv::Size(-1,-1), cv::TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
            
            cv::cornerSubPix( pInputImage, pPatternPoints, cv::Size(11,11), cv::Size(-1,-1), cv::TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
        }
    }
    
    return patternFound;
}

bool
OpticalCalibration::runCalibration()
{
	mCameraRotations.clear();
	mCameraTranslations.clear();

    std::vector<float> reprojErrs;
    double totalAvgErr = 0.0;
    
    mCameraMatrix = cv::Mat::eye(3, 3, CV_64F);
    if( mAspectRatio > 0.0 ) mCameraMatrix.at<double>(0,0) = 1.0;
    
    mDistCoeffs = cv::Mat::zeros(8, 1, CV_64F);
    vector< std::vector<cv::Point3f> > objectPoints(1);
    
    calcBoardCornerPositions(objectPoints[0]);
    
    objectPoints.resize(mImagePoints.size(),objectPoints[0]);
    
    //Find intrinsic and extrinsic camera parameters
    double rms = cv::calibrateCamera(objectPoints, mImagePoints, cv::Size(mFrameSize[0], mFrameSize[1]), mCameraMatrix, mDistCoeffs, mCameraRotations, mCameraTranslations, mFlag|cv::CALIB_FIX_K4|cv::CALIB_FIX_K5);
    
    std::cout << "Re-projection error reported by calibrateCamera: "<< rms << "\n";
    
    bool success = cv::checkRange(mCameraMatrix) && cv::checkRange(mDistCoeffs);
    
    totalAvgErr = computeReprojectionErrors(objectPoints, mCameraRotations, mCameraTranslations, reprojErrs);
    
    std::cout << (success ? "Calibration succeeded" : "Calibration failed")
    << ". avg re projection error = "  << totalAvgErr;
    
    if(success == false) return false;

	//std::cout << "rvecs " << mCameraRotations.size() << "\n";
	//for (int i = 0; i < mCameraRotations.size(); ++i)
	//{
	//	std::cout << "i " << i << " rvec s " << mCameraRotations[i].size() << " d " << mCameraRotations[i]  << "\n";
	//}
	//std::cout << "tvecs " << mCameraTranslations.size() << "\n";
	//for (int i = 0; i < mCameraTranslations.size(); ++i)
	//{
	//	std::cout << "i " << i << " tvec s " << mCameraTranslations[i].size() << " d " << mCameraTranslations[i]  << "\n";
	//}
}

double
OpticalCalibration::computeReprojectionErrors(const vector<vector<cv::Point3f> >& pObjectPoints,const std::vector<cv::Mat>& pRvecs, const std::vector<cv::Mat>& pTvecs, std::vector<float>& pPerViewErrors)
{
    std::vector<cv::Point2f> imagePoints2;
    int i, totalPoints = 0;
    double totalErr = 0, err;
    
    pPerViewErrors.resize(pObjectPoints.size());
    
    for( i = 0; i < (int)pObjectPoints.size(); ++i )
    {
        cv::projectPoints( cv::Mat(pObjectPoints[i]), pRvecs[i], pTvecs[i], mCameraMatrix,
                      mDistCoeffs, imagePoints2);
        err = cv::norm(cv::Mat(mImagePoints[i]), cv::Mat(imagePoints2), CV_L2);

        int n = (int)pObjectPoints[i].size();
        pPerViewErrors[i] = (float) std::sqrt(err*err/n);
        totalErr        += err*err;
        totalPoints     += n;
    }

    return std::sqrt(totalErr/totalPoints);
}

void
OpticalCalibration::calcBoardCornerPositions(vector<cv::Point3f>& pCorners)
{
    pCorners.clear();
    
    if(mBoardPattern == CHESSBOARD || mBoardPattern == CIRCLES_GRID)
    {
        for( int i = 0; i < mBoardSize.height; ++i )
        {
            for( int j = 0; j < mBoardSize.width; ++j )
            {
                pCorners.push_back(cv::Point3f(float( j*mSquareSize ), float( i*mSquareSize ), 0));
            }
        }
    }
    else if(mBoardPattern == ASYMMETRIC_CIRCLES_GRID)
    {
        for( int i = 0; i < mBoardSize.height; i++ )
        {
            for( int j = 0; j < mBoardSize.width; j++ )
            {
                pCorners.push_back(cv::Point3f(float((2*j + i % 2)*mSquareSize), float(i*mSquareSize), 0));
            }
        }
    }
}

void
OpticalCalibration::onTextInputEvent(ofxDatGuiTextInputEvent e)
{
    if(e.target == mBoardSizeXControl)
    {
        mBoardSize.width= std::stoi(e.text);

        //std::cout << "mBoardSize.width " << mBoardSize.width  << "\n";
    }
    else if(e.target == mBoardSizeYControl)
    {
        mBoardSize.height = std::stoi(e.text);
        
        //std::cout << "height " << mBoardSize.height  << "\n";
    }
    else if(e.target == mSquareSizeControl)
    {
        mSquareSize = std::stof(e.text);

        //std::cout << "mSquareSize " << mSquareSize  << "\n";
    }
    else if(e.target == mCalibrationFrameCountControl)
    {
        mCalibrationFrameCount = std::stoi(e.text);
        
        //std::cout << "mCalibrationFrameCount " << mCalibrationFrameCount  << "\n";
    }
    else if(e.target == mCaptureIntervalControl)
    {
        mCaptureInterval = std::stof(e.text);
        
        //std::cout << "mCaptureInterval " << mCaptureInterval  << "\n";
    }
    else if(e.target == mAspectRatioControl)
    {
        mAspectRatio = std::stof(e.text);
        
        //std::cout << "mAspectRatio " << mAspectRatio  << "\n";
    }
}

void
OpticalCalibration::onDropdownEvent(ofxDatGuiDropdownEvent e)
{
    if(e.target == mBoardPatternControl)
    {
        std::string option = e.target->getLabel();
        
        if(option == "CHESSBOARD")
        {
            mBoardPattern = CHESSBOARD;
            
            //std::cout << "board pattern: CHESSBOARD\n";
        }
        else if(option == "CIRCLES_GRID")
        {
            mBoardPattern = CIRCLES_GRID;
            
            //std::cout << "board pattern: CIRCLES_GRID\n";
        }
        else if(option == "ASYMMETRIC_CIRCLES_GRID")
        {
            mBoardPattern = ASYMMETRIC_CIRCLES_GRID;
            
            //std::cout << "board pattern: ASYMMETRIC_CIRCLES_GRID\n";
        }
    }
}

void
OpticalCalibration::onButtonEvent(ofxDatGuiButtonEvent e)
{
    if(e.target == mFlipVerticalControl)
    {
		mFlipVertical = static_cast<ofxDatGuiToggle*>(e.target)->getChecked();        
        
		//std::cout << "mFlipVertical " << mFlipVertical  << "\n";
    }
    else if(e.target == mStartCalibrationControl)
    {
        mImagePoints.clear();
        mCalibrationMode = CALIBRATING;
        
        //std::cout << "start calibration\n";
    }
	else if (e.target == mCapturePatternNowControl)
	{
		mCapturePatternNow = static_cast<ofxDatGuiToggle*>(e.target)->getChecked();

		//std::cout << "start calibration\n";
	}
}