calibrate.cpp

#include <iostream>
#include <sstream>
#include <string>
#include <ctime>
#include <cstdio>

#include <ftl/calibrate.hpp>

#include <opencv2/core.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>

#include <glog/logging.h>

using namespace cv;
using namespace std;

using ftl::Calibrate;


void Calibrate::Settings::write(FileStorage& fs) const                        //Write serialization for this class
{
    fs << "{"
              << "BoardSize_Width"  << boardSize.width
              << "BoardSize_Height" << boardSize.height
              << "Square_Size"         << squareSize
              << "Calibrate_Pattern" << patternToUse
              << "Calibrate_NrOfFrameToUse" << nrFrames
              << "Calibrate_FixAspectRatio" << aspectRatio
              << "Calibrate_AssumeZeroTangentialDistortion" << calibZeroTangentDist
              << "Calibrate_FixPrincipalPointAtTheCenter" << calibFixPrincipalPoint

              << "Write_DetectedFeaturePoints" << writePoints
              << "Write_extrinsicParameters"   << writeExtrinsics
              << "Write_gridPoints" << writeGrid
              //<< "Write_outputFileName"  << outputFileName

              //<< "Show_UndistortedImage" << showUndistorsed

              << "Input_FlipAroundHorizontalAxis" << flipVertical
              << "Input_Delay" << delay
              //<< "Input" << input
       << "}";
}
void Calibrate::Settings::read(const nlohmann::json& node)                          //Read serialization for this class
{
    boardSize.width = node["board_size"][0];
    boardSize.height = node["board_size"][1];
    //node["Calibrate_Pattern"] >> patternToUse;
    squareSize = node["square_size"];
    nrFrames = node["num_frames"];
    aspectRatio = node["fix_aspect_ratio"];
    //node["Write_DetectedFeaturePoints"] >> writePoints;
    //node["Write_extrinsicParameters"] >> writeExtrinsics;
    //node["Write_gridPoints"] >> writeGrid;
    //node["Write_outputFileName"] >> outputFileName;
    calibZeroTangentDist = node["assume_zero_tangential_distortion"];
    calibFixPrincipalPoint = node["fix_principal_point_at_center"];
    useFisheye =  node["use_fisheye_model"];
    flipVertical = node["flip_vertical"];
    //node["Show_UndistortedImage"] >> showUndistorsed;
    //node["Input"] >> input;
    delay = node["frame_delay"];
    fixK1 = node["fix_k1"];
    fixK2 = node["fix_k2"];
    fixK3 = node["fix_k3"];
    fixK4 = node["fix_k4"];
    fixK5 = node["fix_k5"];

    validate();
}
void Calibrate::Settings::validate()
{
    goodInput = true;
    if (boardSize.width <= 0 || boardSize.height <= 0)
    {
        LOG(ERROR) << "Invalid Board size: " << boardSize.width << " " << boardSize.height;
        goodInput = false;
    }
    if (squareSize <= 10e-6)
    {
        LOG(ERROR) << "Invalid square size " << squareSize;
        goodInput = false;
    }
    if (nrFrames <= 0)
    {
        LOG(ERROR) << "Invalid number of frames " << nrFrames;
        goodInput = false;
    }

    flag = 0;
    if(calibFixPrincipalPoint) flag |= CALIB_FIX_PRINCIPAL_POINT;
    if(calibZeroTangentDist)   flag |= CALIB_ZERO_TANGENT_DIST;
    if(aspectRatio)            flag |= CALIB_FIX_ASPECT_RATIO;
    if(fixK1)                  flag |= CALIB_FIX_K1;
    if(fixK2)                  flag |= CALIB_FIX_K2;
    if(fixK3)                  flag |= CALIB_FIX_K3;
    if(fixK4)                  flag |= CALIB_FIX_K4;
    if(fixK5)                  flag |= CALIB_FIX_K5;

    if (useFisheye) {
        // the fisheye model has its own enum, so overwrite the flags
        flag = fisheye::CALIB_FIX_SKEW | fisheye::CALIB_RECOMPUTE_EXTRINSIC;
        if(fixK1)                   flag |= fisheye::CALIB_FIX_K1;
        if(fixK2)                   flag |= fisheye::CALIB_FIX_K2;
        if(fixK3)                   flag |= fisheye::CALIB_FIX_K3;
        if(fixK4)                   flag |= fisheye::CALIB_FIX_K4;
        if (calibFixPrincipalPoint) flag |= fisheye::CALIB_FIX_PRINCIPAL_POINT;
    }

    /*calibrationPattern = NOT_EXISTING;
    if (!patternToUse.compare("CHESSBOARD")) calibrationPattern = CHESSBOARD;
    if (!patternToUse.compare("CIRCLES_GRID")) calibrationPattern = CIRCLES_GRID;
    if (!patternToUse.compare("ASYMMETRIC_CIRCLES_GRID")) calibrationPattern = ASYMMETRIC_CIRCLES_GRID;
    if (calibrationPattern == NOT_EXISTING)
    {
        LOG(ERROR) << " Camera calibration mode does not exist: " << patternToUse;
        goodInput = false;
    }*/
    atImageList = 0;

}
Mat Calibrate::_nextImage(size_t cam)
{
    Mat result;
    if (cam == 0) {
    	local_->left(result);
    } else if (cam == 1 && local_->isStereo()) {
    	local_->right(result);
    }
    return result;
}

bool Calibrate::Settings::readStringList( const string& filename, vector<string>& l )
{
    l.clear();
    FileStorage fs(filename, FileStorage::READ);
    if( !fs.isOpened() )
        return false;
    FileNode n = fs.getFirstTopLevelNode();
    if( n.type() != FileNode::SEQ )
        return false;
    FileNodeIterator it = n.begin(), it_end = n.end();
    for( ; it != it_end; ++it )
        l.push_back((string)*it);
    return true;
}

bool Calibrate::Settings::isListOfImages( const string& filename)
{
    string s(filename);
    // Look for file extension
    if( s.find(".xml") == string::npos && s.find(".yaml") == string::npos && s.find(".yml") == string::npos )
        return false;
    else
        return true;
}

enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };

bool runCalibration(Calibrate::Settings& s, Size imageSize, Mat&  cameraMatrix, Mat& distCoeffs,
                           vector<vector<Point2f> > imagePoints, float grid_width, bool release_object);


Calibrate::Calibrate(ftl::LocalSource *s, nlohmann::json &config) : local_(s) {
    /*FileStorage fs(cal, FileStorage::READ); // Read the settings
    if (!fs.isOpened())
    {
        LOG(ERROR) << "Could not open the configuration file: \"" << cal << "\"";
        return;
    }*/
    //fs["Settings"] >> settings_;
    settings_.read(config);
    //fs.release();
    
    if (!settings_.goodInput)
    {
        LOG(ERROR) << "Invalid input detected. Application stopping.";
        return;
    }
    
    map1_.resize(2);
    map2_.resize(2);
    
    // TODO Load existing calibration if available...
    
    calibrated_ = _loadCalibration();
    
    if (calibrated_) {
    	LOG(INFO) << "Calibration loaded from file";
    }
}

bool Calibrate::_loadCalibration() {
	// Capture one frame to get size;
	Mat l,r;
	local_->get(l,r);
	Size img_size = l.size();
	float scale = 1.0f;

    Rect roi1, roi2;
    
    // reading intrinsic parameters
    FileStorage fs(FTL_CONFIG_ROOT "/intrinsics.yml", FileStorage::READ);
    if(!fs.isOpened())
    {
        LOG(WARNING) << "Calibration file not found";
        return false;
    }

    Mat M1, D1, M2, D2;
    fs["M1"] >> M1;
    fs["D1"] >> D1;
    fs["M2"] >> M2;
    fs["D2"] >> D2;

    M1 *= scale;
    M2 *= scale;

    fs.open(FTL_CONFIG_ROOT "/extrinsics.yml", FileStorage::READ);
    if(!fs.isOpened())
    {
        LOG(WARNING) << "Calibration file not found";
        return false;
    }

    Mat R, T, R1, P1, R2, P2;
    fs["R"] >> R;
    fs["T"] >> T;
    fs["R1"] >> R1;
    fs["R2"] >> R2;
    fs["P1"] >> P1;
    fs["P2"] >> P2;
    fs["Q"] >> Q_;

    stereoRectify( M1, D1, M2, D2, img_size, R, T, R1, R2, P1, P2, Q_, CALIB_ZERO_DISPARITY, -1, img_size, &roi1, &roi2 );

    Mat map11, map12, map21, map22;
    initUndistortRectifyMap(M1, D1, R1, P1, img_size, CV_16SC2, map1_[0], map2_[0]);
    initUndistortRectifyMap(M2, D2, R2, P2, img_size, CV_16SC2, map1_[1], map2_[1]);
    return true;
}

bool Calibrate::recalibrate() {
	vector<vector<Point2f> > imagePoints[2];
    Mat cameraMatrix[2], distCoeffs[2];
    Size imageSize[2];
    
    // TODO Ensure both left+right use same frames
    
	bool r = _recalibrate(imagePoints, cameraMatrix, distCoeffs, imageSize);
	//if (local_->isStereo()) r &= _recalibrate(1, imagePoints[1], cameraMatrix[1], distCoeffs[1], imageSize[1]);
	
	if (r) calibrated_ = true;
	
	if (r && local_->isStereo()) {
		int nimages = static_cast<int>(imagePoints[0].size());
		auto squareSize = settings_.squareSize;
		vector<vector<Point3f>> objectPoints;
		objectPoints.resize(nimages);

		for(auto i = 0; i < nimages; i++ )
		{
		    for(auto j = 0; j < settings_.boardSize.height; j++ )
		        for(auto  k = 0; k < settings_.boardSize.width; k++ )
		            objectPoints[i].push_back(Point3f(k*squareSize, j*squareSize, 0));
		}
		
		Mat R, T, E, F;
		
		LOG(INFO) << "Running stereo calibration...";
    
		double rms = stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1],
		                cameraMatrix[0], distCoeffs[0],
		                cameraMatrix[1], distCoeffs[1],
		                imageSize[0], R, T, E, F,
		                CALIB_FIX_ASPECT_RATIO +
		                CALIB_ZERO_TANGENT_DIST +
		                CALIB_USE_INTRINSIC_GUESS +
		                CALIB_SAME_FOCAL_LENGTH +
		                CALIB_RATIONAL_MODEL +
		                CALIB_FIX_K3 + CALIB_FIX_K4 + CALIB_FIX_K5,
		                TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 100, 1e-5) );
		LOG(INFO) << "... done with RMS error=" << rms;
		
		// save intrinsic parameters
		FileStorage fs(FTL_CONFIG_ROOT "/intrinsics.yml", FileStorage::WRITE);
		if( fs.isOpened() )
		{
		    fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] <<
		        "M2" << cameraMatrix[1] << "D2" << distCoeffs[1];
		    fs.release();
		}
		else
		    LOG(ERROR) << "Error: can not save the intrinsic parameters";

		Mat R1, R2, P1, P2;
		Rect validRoi[2];

		stereoRectify(cameraMatrix[0], distCoeffs[0],
		              cameraMatrix[1], distCoeffs[1],
		              imageSize[0], R, T, R1, R2, P1, P2, Q_,
		              CALIB_ZERO_DISPARITY, 1, imageSize[0], &validRoi[0], &validRoi[1]);

		fs.open(FTL_CONFIG_ROOT "/extrinsics.yml", FileStorage::WRITE);
		if( fs.isOpened() )
		{
		    fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q_;
		    fs.release();
		}
		else
		    LOG(ERROR) << "Error: can not save the extrinsic parameters";
		    
		    
		//Precompute maps for cv::remap()
    	initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize[0], CV_16SC2, map1_[0], map2_[0]);
    	initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize[0], CV_16SC2, map1_[1], map2_[1]);

	} else {
		int cam = 0;
		if (settings_.useFisheye)
		{
		    Mat newCamMat;
		    fisheye::estimateNewCameraMatrixForUndistortRectify(cameraMatrix[cam], distCoeffs[cam], imageSize[cam],
		                                                        Matx33d::eye(), newCamMat, 1);
		    fisheye::initUndistortRectifyMap(cameraMatrix[cam], distCoeffs[cam], Matx33d::eye(), newCamMat, imageSize[cam],
		                                     CV_16SC2, map1_[cam], map2_[cam]);
		}
		else
		{
		    initUndistortRectifyMap(
		        cameraMatrix[cam], distCoeffs[cam], Mat(),
		        getOptimalNewCameraMatrix(cameraMatrix[cam], distCoeffs[cam], imageSize[cam], 1, imageSize[cam], 0), imageSize[cam],
		        CV_16SC2, map1_[cam], map2_[cam]);
		}
	}
	
	return r;
}

bool Calibrate::_recalibrate(vector<vector<Point2f>> *imagePoints,
		Mat *cameraMatrix, Mat *distCoeffs, Size *imageSize) {
	int winSize = 11; //parser.get<int>("winSize");

    float grid_width = settings_.squareSize * (settings_.boardSize.width - 1);
    bool release_object = false;

    //vector<vector<Point2f> > imagePoints;
    //Mat cameraMatrix, distCoeffs;
    //Size imageSize;
    int mode = CAPTURING;
    double prevTimestamp = 0.0;
    const Scalar RED(0,0,255), GREEN(0,255,0);
    
    int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;

    if(!settings_.useFisheye) {
        // fast check erroneously fails with high distortions like fisheye
        chessBoardFlags |= CALIB_CB_FAST_CHECK;
    }
    
    //! [get_input]
    for(;;)
    {
        Mat view[2];
        //bool blinkOutput = false;

		local_->get(view[0],view[1]);

        //view = _nextImage(cam);
        LOG(INFO) << "Grabbing calibration image...";
        
        if (view[0].empty() || (local_->isStereo() && view[1].empty()) || imagePoints[0].size() >= (size_t)settings_.nrFrames) {
        	settings_.outputFileName = FTL_CONFIG_ROOT "/cam0.xml";
        	bool r = runCalibration(settings_, imageSize[0],
        					cameraMatrix[0], distCoeffs[0], imagePoints[0],
        					grid_width, release_object);
        					
        	if (local_->isStereo()) {
		    	settings_.outputFileName = FTL_CONFIG_ROOT "/cam1.xml";
		    	r &= runCalibration(settings_, imageSize[1],
		    					cameraMatrix[1], distCoeffs[1], imagePoints[1],
		    					grid_width, release_object);
        	}
        					
        	if (!r && view[0].empty()) {
        		LOG(ERROR) << "Not enough frames to calibrate";
        		return false;
        	} else if (r) {
        		LOG(INFO) << "Calibration successful";
        		break;
        	}
            
        }

        imageSize[0] = view[0].size();  // Format input image.
        imageSize[1] = view[1].size();
        //if( settings_.flipVertical )    flip( view[cam], view[cam], 0 );

        //! [find_pattern]
        vector<Point2f> pointBuf[2];

        bool found1,found2;

		found1 = findChessboardCorners( view[0], settings_.boardSize, pointBuf[0], chessBoardFlags);
		found2 = !local_->isStereo() || findChessboardCorners( view[1], settings_.boardSize, pointBuf[1], chessBoardFlags);
      
        if (found1 && found2 && local_->getTimestamp()-prevTimestamp > settings_.delay)                // If done with success,
        {
			prevTimestamp = local_->getTimestamp();
              // improve the found corners' coordinate accuracy for chessboard
                    Mat viewGray;
                    cvtColor(view[0], viewGray, COLOR_BGR2GRAY);
                    cornerSubPix( viewGray, pointBuf[0], Size(winSize,winSize),
                        Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.0001 ));
                imagePoints[0].push_back(pointBuf[0]);
                  
                if (local_->isStereo()) {      
                    cvtColor(view[1], viewGray, COLOR_BGR2GRAY);
                    cornerSubPix( viewGray, pointBuf[1], Size(winSize,winSize),
                        Size(-1,-1), TermCriteria( TermCriteria::EPS+TermCriteria::COUNT, 30, 0.0001 ));
					imagePoints[1].push_back(pointBuf[1]);
                }


                // Draw the corners.
                drawChessboardCorners( view[0], settings_.boardSize, Mat(pointBuf[0]), found1 );
                if (local_->isStereo()) drawChessboardCorners( view[1], settings_.boardSize, Mat(pointBuf[1]), found2 );
        } else {
        	LOG(WARNING) << "No calibration pattern found";
        }

        imshow("Left", view[0]);
        if (local_->isStereo()) imshow("Right", view[1]);
        char key = (char)waitKey(50);

        if( key  == 27 )
            break;
    }
    
	return true;
}

bool Calibrate::undistort(cv::Mat &l, cv::Mat &r) {
	local_->get(l,r);
	if (!calibrated_) return false;
	if (l.empty()) return false;
	remap(l, l, map1_[0], map2_[0], INTER_LINEAR);
	if (local_->isStereo()) remap(r, r, map1_[1], map2_[1], INTER_LINEAR);
	return true;
}

bool Calibrate::rectified(cv::Mat &l, cv::Mat &r) {
	return undistort(l,r);
}

bool Calibrate::isCalibrated() {
	return calibrated_;
}

//! [compute_errors]
static double computeReprojectionErrors( const vector<vector<Point3f> >& objectPoints,
                                         const vector<vector<Point2f> >& imagePoints,
                                         const vector<Mat>& rvecs, const vector<Mat>& tvecs,
                                         const Mat& cameraMatrix , const Mat& distCoeffs,
                                         vector<float>& perViewErrors, bool fisheye)
{
    vector<Point2f> imagePoints2;
    size_t totalPoints = 0;
    double totalErr = 0, err;
    perViewErrors.resize(objectPoints.size());

    for(size_t i = 0; i < objectPoints.size(); ++i )
    {
        if (fisheye)
        {
            fisheye::projectPoints(objectPoints[i], imagePoints2, rvecs[i], tvecs[i], cameraMatrix,
                                   distCoeffs);
        }
        else
        {
            projectPoints(objectPoints[i], rvecs[i], tvecs[i], cameraMatrix, distCoeffs, imagePoints2);
        }
        err = norm(imagePoints[i], imagePoints2, NORM_L2);

        size_t n = objectPoints[i].size();
        perViewErrors[i] = (float) std::sqrt(err*err/n);
        totalErr        += err*err;
        totalPoints     += n;
    }

    return std::sqrt(totalErr/totalPoints);
}
//! [compute_errors]
//! [board_corners]
static void calcBoardCornerPositions(Size boardSize, float squareSize, vector<Point3f>& corners,
                                     Calibrate::Settings::Pattern patternType /*= Settings::CHESSBOARD*/)
{
    corners.clear();

    switch(patternType)
    {
    case Calibrate::Settings::CHESSBOARD:
    case Calibrate::Settings::CIRCLES_GRID:
        for( int i = 0; i < boardSize.height; ++i )
            for( int j = 0; j < boardSize.width; ++j )
                corners.push_back(Point3f(j*squareSize, i*squareSize, 0));
        break;

    case Calibrate::Settings::ASYMMETRIC_CIRCLES_GRID:
        for( int i = 0; i < boardSize.height; i++ )
            for( int j = 0; j < boardSize.width; j++ )
                corners.push_back(Point3f((2*j + i % 2)*squareSize, i*squareSize, 0));
        break;
    default:
        break;
    }
}
//! [board_corners]
static bool _runCalibration( Calibrate::Settings& s, Size& imageSize, Mat& cameraMatrix, Mat& distCoeffs,
                            vector<vector<Point2f> > imagePoints, vector<Mat>& rvecs, vector<Mat>& tvecs,
                            vector<float>& reprojErrs,  double& totalAvgErr, vector<Point3f>& newObjPoints,
                            float grid_width, bool release_object)
{
    //! [fixed_aspect]
    cameraMatrix = Mat::eye(3, 3, CV_64F);
    if( s.flag & CALIB_FIX_ASPECT_RATIO )
        cameraMatrix.at<double>(0,0) = s.aspectRatio;
    //! [fixed_aspect]
    if (s.useFisheye) {
        distCoeffs = Mat::zeros(4, 1, CV_64F);
    } else {
        distCoeffs = Mat::zeros(8, 1, CV_64F);
    }

    vector<vector<Point3f> > objectPoints(1);
    calcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0], Calibrate::Settings::CHESSBOARD);
    objectPoints[0][s.boardSize.width - 1].x = objectPoints[0][0].x + grid_width;
    newObjPoints = objectPoints[0];

    objectPoints.resize(imagePoints.size(),objectPoints[0]);

    //Find intrinsic and extrinsic camera parameters
    double rms;

    if (s.useFisheye) {
        Mat _rvecs, _tvecs;
        rms = fisheye::calibrate(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, _rvecs,
                                 _tvecs, s.flag);

        rvecs.reserve(_rvecs.rows);
        tvecs.reserve(_tvecs.rows);
        for(int i = 0; i < int(objectPoints.size()); i++){
            rvecs.push_back(_rvecs.row(i));
            tvecs.push_back(_tvecs.row(i));
        }
    } else {
        int iFixedPoint = -1;
        if (release_object)
            iFixedPoint = s.boardSize.width - 1;
        rms = calibrateCameraRO(objectPoints, imagePoints, imageSize, iFixedPoint,
                                cameraMatrix, distCoeffs, rvecs, tvecs, newObjPoints,
                                s.flag | CALIB_USE_LU);
    }

    /*if (release_object) {
        cout << "New board corners: " << endl;
        cout << newObjPoints[0] << endl;
        cout << newObjPoints[s.boardSize.width - 1] << endl;
        cout << newObjPoints[s.boardSize.width * (s.boardSize.height - 1)] << endl;
        cout << newObjPoints.back() << endl;
    }*/

    LOG(INFO) << "Re-projection error reported by calibrateCamera: "<< rms;

    bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);

    objectPoints.clear();
    objectPoints.resize(imagePoints.size(), newObjPoints);
    totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints, rvecs, tvecs, cameraMatrix,
                                            distCoeffs, reprojErrs, s.useFisheye);

    return ok;
}

//! [run_and_save]
bool runCalibration(Calibrate::Settings& s, Size imageSize, Mat& cameraMatrix, Mat& distCoeffs,
                           vector<vector<Point2f> > imagePoints, float grid_width, bool release_object)
{
    vector<Mat> rvecs, tvecs;
    vector<float> reprojErrs;
    double totalAvgErr = 0;
    vector<Point3f> newObjPoints;

    bool ok = _runCalibration(s, imageSize, cameraMatrix, distCoeffs, imagePoints, rvecs, tvecs, reprojErrs,
                             totalAvgErr, newObjPoints, grid_width, release_object);
    LOG(INFO) << (ok ? "Calibration succeeded" : "Calibration failed")
         << ". avg re projection error = " << totalAvgErr;

    return ok;
}
//! [run_and_save]