Subversion Repositories seema-scanner

#include <opencv2/aruco/charuco.hpp>

static void drawDetectedMarkersCharuco(cv::InputOutputArray _image, cv::InputArrayOfArrays _corners,

                                cv::InputArray _ids, cv::Scalar borderColor) {

    using namespace cv;

    using namespace std;

    CV_Assert(_image.getMat().total() != 0 &&

              (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));

    CV_Assert((_corners.total() == _ids.total()) || _ids.total() == 0);

    // calculate colors

    Scalar textColor, cornerColor;

    textColor = cornerColor = borderColor;

    swap(textColor.val[0], textColor.val[1]);     // text color just sawp G and R

    swap(cornerColor.val[1], cornerColor.val[2]); // corner color just sawp G and B

    int nMarkers = (int)_corners.total();

    for(int i = 0; i < nMarkers; i++) {

        Mat currentMarker = _corners.getMat(i);

        CV_Assert(currentMarker.total() == 4 && currentMarker.type() == CV_32FC2);

        // draw marker sides

        for(int j = 0; j < 4; j++) {

            Point2f p0, p1;

            p0 = currentMarker.ptr< Point2f >(0)[j];

            p1 = currentMarker.ptr< Point2f >(0)[(j + 1) % 4];

            line(_image, p0, p1, borderColor, 2, LINE_AA);

        }

        // draw first corner mark

        rectangle(_image, currentMarker.ptr< Point2f >(0)[0] - Point2f(3, 3),

                  currentMarker.ptr< Point2f >(0)[0] + Point2f(3, 3), cornerColor, 1, LINE_AA);

        // draw ID

        if(_ids.total() != 0) {

            Point2f cent(0, 0);

            for(int p = 0; p < 4; p++)

                cent += currentMarker.ptr< Point2f >(0)[p];

            cent = cent / 4.;

            stringstream s;

            s << _ids.getMat().ptr< int >(0)[i];

            putText(_image, s.str(), cent, FONT_HERSHEY_SIMPLEX, 1, textColor, 2, LINE_AA);

        }

    }

}

static void drawDetectedCornersCharuco(cv::InputOutputArray _image, cv::InputArray _charucoCorners,

                                       cv::InputArray _charucoIds, cv::Scalar cornerColor) {

    using namespace cv;

    using namespace std;

    CV_Assert(_image.getMat().total() != 0 &&

              (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));

    CV_Assert((_charucoCorners.getMat().total() == _charucoIds.getMat().total()) ||

              _charucoIds.getMat().total() == 0);

    unsigned int nCorners = (unsigned int)_charucoCorners.getMat().total();

    for(unsigned int i = 0; i < nCorners; i++) {

        Point2f corner = _charucoCorners.getMat().at< Point2f >(i);

        // draw first corner mark

        rectangle(_image, corner - Point2f(3, 3), corner + Point2f(3, 3), cornerColor, 2, LINE_AA);

        // draw ID

        if(_charucoIds.total() != 0) {

            int id = _charucoIds.getMat().at< int >(i);

            stringstream s;

            s << id;

            putText(_image, s.str(), corner + Point2f(5, -5), FONT_HERSHEY_SIMPLEX, 1.0,

                    cornerColor, 4, cv::LINE_AA);

        }

    }

}

void SMCalibrationWorker::cameraCalibrationCharuco(std::vector<SMCalibrationSet> calibrationData){

    QSettings settings;

    // Number of saddle points on calibration pattern

    cv::Size checkerCount(cv::Size(settings.value("calibration/patternSizeX", 22).toInt(), settings.value("calibration/patternSizeY", 13).toInt()));

    const float checkerSize = settings.value("calibration/squareSize", 10.0).toFloat();

    float markerLength = 0.8*checkerSize;

    unsigned int nSets = calibrationData.size();

    // 2D Points collected for OpenCV's calibration procedures

    std::vector< std::vector<cv::Point2f> > qc0, qc1;

    std::vector< std::vector<cv::Point2f> > qc0Stereo, qc1Stereo;

    // 3D object points

    std::vector< std::vector<cv::Point3f> > Q0, Q1, QStereo;

    // Charuco board and dictionary

    cv::Ptr<cv::aruco::Dictionary> dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);

//    // The dictionary from the standalone Aruco library used to generate the original chessboards

//    std::vector<uint64_t> ARUCO_MIP_36h12 = {0xd2b63a09dUL,0x6001134e5UL,0x1206fbe72UL,0xff8ad6cb4UL,0x85da9bc49UL,0xb461afe9cUL,0x6db51fe13UL,0x5248c541fUL,0x8f34503UL,0x8ea462eceUL,0xeac2be76dUL,0x1af615c44UL,0xb48a49f27UL,0x2e4e1283bUL,0x78b1f2fa8UL,0x27d34f57eUL,0x89222fff1UL,0x4c1669406UL,0xbf49b3511UL,0xdc191cd5dUL,0x11d7c3f85UL,0x16a130e35UL,0xe29f27effUL,0x428d8ae0cUL,0x90d548477UL,0x2319cbc93UL,0xc3b0c3dfcUL,0x424bccc9UL,0x2a081d630UL,0x762743d96UL,0xd0645bf19UL,0xf38d7fd60UL,0xc6cbf9a10UL,0x3c1be7c65UL,0x276f75e63UL,0x4490a3f63UL,0xda60acd52UL,0x3cc68df59UL,0xab46f9daeUL,0x88d533d78UL,0xb6d62ec21UL,0xb3c02b646UL,0x22e56d408UL,0xac5f5770aUL,0xaaa993f66UL,0x4caa07c8dUL,0x5c9b4f7b0UL,0xaa9ef0e05UL,0x705c5750UL,0xac81f545eUL,0x735b91e74UL,0x8cc35cee4UL,0xe44694d04UL,0xb5e121de0UL,0x261017d0fUL,0xf1d439eb5UL,0xa1a33ac96UL,0x174c62c02UL,0x1ee27f716UL,0x8b1c5ece9UL,0x6a05b0c6aUL,0xd0568dfcUL,0x192d25e5fUL,0x1adbeccc8UL,0xcfec87f00UL,0xd0b9dde7aUL,0x88dcef81eUL,0x445681cb9UL,0xdbb2ffc83UL,0xa48d96df1UL,0xb72cc2e7dUL,0xc295b53fUL,0xf49832704UL,0x9968edc29UL,0x9e4e1af85UL,0x8683e2d1bUL,0x810b45c04UL,0x6ac44bfe2UL,0x645346615UL,0x3990bd598UL,0x1c9ed0f6aUL,0xc26729d65UL,0x83993f795UL,0x3ac05ac5dUL,0x357adff3bUL,0xd5c05565UL,0x2f547ef44UL,0x86c115041UL,0x640fd9e5fUL,0xce08bbcf7UL,0x109bb343eUL,0xc21435c92UL,0x35b4dfce4UL,0x459752cf2UL,0xec915b82cUL,0x51881eed0UL,0x2dda7dc97UL,0x2e0142144UL,0x42e890f99UL,0x9a8856527UL,0x8e80d9d80UL,0x891cbcf34UL,0x25dd82410UL,0x239551d34UL,0x8fe8f0c70UL,0x94106a970UL,0x82609b40cUL,0xfc9caf36UL,0x688181d11UL,0x718613c08UL,0xf1ab7629UL,0xa357bfc18UL,0x4c03b7a46UL,0x204dedce6UL,0xad6300d37UL,0x84cc4cd09UL,0x42160e5c4UL,0x87d2adfa8UL,0x7850e7749UL,0x4e750fc7cUL,0xbf2e5dfdaUL,0xd88324da5UL,0x234b52f80UL,0x378204514UL,0xabdf2ad53UL,0x365e78ef9UL,0x49caa6ca2UL,0x3c39ddf3UL,0xc68c5385dUL,0x5bfcbbf67UL,0x623241e21UL,0xabc90d5ccUL,0x388c6fe85UL,0xda0e2d62dUL,0x10855dfe9UL,0x4d46efd6bUL,0x76ea12d61UL,0x9db377d3dUL,0xeed0efa71UL,0xe6ec3ae2fUL,0x441faee83UL,0xba19c8ff5UL,0x313035eabUL,0x6ce8f7625UL,0x880dab58dUL,0x8d3409e0dUL,0x2be92ee21UL,0xd60302c6cUL,0x469ffc724UL,0x87eebeed3UL,0x42587ef7aUL,0x7a8cc4e52UL,0x76a437650UL,0x999e41ef4UL,0x7d0969e42UL,0xc02baf46bUL,0x9259f3e47UL,0x2116a1dc0UL,0x9f2de4d84UL,0xeffac29UL,0x7b371ff8cUL,0x668339da9UL,0xd010aee3fUL,0x1cd00b4c0UL,0x95070fc3bUL,0xf84c9a770UL,0x38f863d76UL,0x3646ff045UL,0xce1b96412UL,0x7a5d45da8UL,0x14e00ef6cUL,0x5e95abfd8UL,0xb2e9cb729UL,0x36c47dd7UL,0xb8ee97c6bUL,0xe9e8f657UL,0xd4ad2ef1aUL,0x8811c7f32UL,0x47bde7c31UL,0x3adadfb64UL,0x6e5b28574UL,0x33e67cd91UL,0x2ab9fdd2dUL,0x8afa67f2bUL,0xe6a28fc5eUL,0x72049cdbdUL,0xae65dac12UL,0x1251a4526UL,0x1089ab841UL,0xe2f096ee0UL,0xb0caee573UL,0xfd6677e86UL,0x444b3f518UL,0xbe8b3a56aUL,0x680a75cfcUL,0xac02baea8UL,0x97d815e1cUL,0x1d4386e08UL,0x1a14f5b0eUL,0xe658a8d81UL,0xa3868efa7UL,0x3668a9673UL,0xe8fc53d85UL,0x2e2b7edd5UL,0x8b2470f13UL,0xf69795f32UL,0x4589ffc8eUL,0x2e2080c9cUL,0x64265f7dUL,0x3d714dd10UL,0x1692c6ef1UL,0x3e67f2f49UL,0x5041dad63UL,0x1a1503415UL,0x64c18c742UL,0xa72eec35UL,0x1f0f9dc60UL,0xa9559bc67UL,0xf32911d0dUL,0x21c0d4ffcUL,0xe01cef5b0UL,0x4e23a3520UL,0xaa4f04e49UL,0xe1c4fcc43UL,0x208e8f6e8UL,0x8486774a5UL,0x9e98c7558UL,0x2c59fb7dcUL,0x9446a4613UL,0x8292dcc2eUL,0x4d61631UL,0xd05527809UL,0xa0163852dUL,0x8f657f639UL,0xcca6c3e37UL,0xcb136bc7aUL,0xfc5a83e53UL,0x9aa44fc30UL,0xbdec1bd3cUL,0xe020b9f7cUL,0x4b8f35fb0UL,0xb8165f637UL,0x33dc88d69UL,0x10a2f7e4dUL,0xc8cb5ff53UL,0xde259ff6bUL,0x46d070dd4UL,0x32d3b9741UL,0x7075f1c04UL,0x4d58dbea0UL};

//    int tau_MIP_3612 = 12;

//    // shuffle according to aruco_create_markermap code

//    std::srand(0);

//    std::random_shuffle(ARUCO_MIP_36h12.begin(),ARUCO_MIP_36h12.end());

//    cv::Mat bytesList(250, 5, CV_8UC4);

//    for(int i=0; i<250; i++){

//        cv::Mat bits(6, 6, CV_8UC1);

//        for(int b=0; b<36; b++){

//            bits.at<char>(b) = getBit(ARUCO_MIP_36h12[i], b+1);

//        }

//        //std::cout << bits << std::endl;

//        cv::Mat bytes = cv::aruco::Dictionary::getByteListFromBits(bits);

//        bytes.copyTo(bytesList.row(i));

//    }

//    cv::Ptr<cv::aruco::Dictionary> dict = cv::makePtr<cv::aruco::Dictionary>(new cv::aruco::Dictionary(bytesList, 6, (tau_MIP_3612-1)/2));

//    for(int i=0; i<250; i++){

//        cv::Mat img;

//        dict->drawMarker(i, 100, img, 1);

//        cv::imwrite(QString("id%1.png").arg(i).toStdString(), img);

//    }

    cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(checkerCount.width+1, checkerCount.height+1, checkerSize, markerLength, dict);

//    cv::Mat boardImg;

//    board->draw(cv::Size((saddlePointCountX+1)*100, (saddlePointCountY+1)*100), boardImg, 0, 1);

//    cv::Mat boardImgRes;

//    cv::resize(boardImg, boardImgRes, cv::Size((saddlePointCountX+1)*10, (saddlePointCountY+1)*10), 0, 0, cv::INTER_NEAREST);

//    cv::imwrite("boardImg.png", boardImg);

//    cv::imwrite("boardImgRes.png", boardImgRes);

    cv::Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();

    parameters->adaptiveThreshWinSizeMin = 3;

    parameters->adaptiveThreshWinSizeMax = 153;

    parameters->adaptiveThreshWinSizeStep = 3;

//    parameters->adaptiveThreshConstant = 20;

//    parameters->minMarkerPerimeterRate = 0.01;

    parameters->maxMarkerPerimeterRate = 5.0;

//    parameters->minCornerDistanceRate = 0.10;

//    parameters->perspectiveRemovePixelPerCell = 6;

//    parameters->maxErroneousBitsInBorderRate = 0.8;

//    parameters->minOtsuStdDev = 1;

//    parameters->errorCorrectionRate = 0.1;

    // Loop through calibration sets

    for(unsigned int i=0; i<nSets; i++){

        SMCalibrationSet calibrationSetI = calibrationData[i];

        if(!calibrationSetI.selected)

            continue;

        // Camera 0

        std::vector<std::vector<cv::Point2f>> mc0, rejecti0;

        std::vector<int> mi0;

        cv::Mat gray0;

        cv::cvtColor(calibrationSetI.frame0, gray0, CV_BayerBG2GRAY);

        // Extract checker corners

        cv::aruco::detectMarkers(gray0, dict, mc0, mi0, parameters);

        cv::aruco::refineDetectedMarkers(gray0, board, mc0, mi0, rejecti0);

        cv::Mat cci0, cidsi0;

        cv::aruco::interpolateCornersCharuco(mc0, mi0, gray0, board, cci0, cidsi0);

        bool success0 = (cci0.rows >= 4);

        if(success0){

            cv::Mat color0;

            cv::cvtColor(calibrationSetI.frame0, color0, CV_BayerBG2RGB);

            // Draw colored chessboard

            drawDetectedMarkersCharuco(color0, mc0, mi0, cv::Scalar(0, 255, 0));

            drawDetectedCornersCharuco(color0, cci0, cidsi0, cv::Scalar(0, 255, 0));

            calibrationSetI.frame0Result = color0;

        } else {

            std::cerr << "Could not detect Charuco in camera 0, id " << calibrationSetI.id << std::endl;

        }

        // Camera 1

        std::vector<std::vector<cv::Point2f>> mc1, rejecti1;

        std::vector<int> mi1;

        cv::Mat gray1;

        cv::cvtColor(calibrationSetI.frame1, gray1, CV_BayerBG2GRAY);

        // Extract checker corners

        cv::aruco::detectMarkers(gray1, dict, mc1, mi1, parameters);

        cv::aruco::refineDetectedMarkers(gray1, board, mc1, mi1, rejecti1);

        cv::Mat cci1, cidsi1;

        cv::aruco::interpolateCornersCharuco(mc1, mi1, gray1, board, cci1, cidsi1);

        bool success1 = (cci1.rows >= 4);

        if(success1){

            cv::Mat color1;

            cv::cvtColor(calibrationSetI.frame1, color1, CV_BayerBG2RGB);

            // Draw colored chessboard

            drawDetectedMarkersCharuco(color1, mc1, mi1, cv::Scalar(0, 255, 0));

            drawDetectedCornersCharuco(color1, cci1, cidsi1, cv::Scalar(0, 255, 0));

            calibrationSetI.frame1Result = color1;

        } else {

            std::cerr << "Could not detect Charuco in camera 1, id " << calibrationSetI.id << std::endl;

        }

        emit newCheckerboardResult(calibrationSetI.id, calibrationSetI);

        if(success0){

            std::vector<cv::Point2f> qc0i;

            std::vector<cv::Point3f> Q0i;

            for(int j=0; j<cci0.rows; j++){

                qc0i.push_back(cci0.at<cv::Point2f>(j));

                int id = cidsi0.at<int>(j);

                Q0i.push_back(board->chessboardCorners[id]);

            }

            qc0.push_back(qc0i);

            Q0.push_back(Q0i);

        }

        if(success1){

            std::vector<cv::Point2f> qc1i;

            std::vector<cv::Point3f> Q1i;

            for(int j=0; j<cci1.rows; j++){

                qc1i.push_back(cci1.at<cv::Point2f>(j));

                int id = cidsi1.at<int>(j);

                Q1i.push_back(board->chessboardCorners[id]);

            }

            qc1.push_back(qc1i);

            Q1.push_back(Q1i);

        }

        if(success0 && success1){

            std::vector<cv::Point2f> qc0iStereo, qc1iStereo;

            std::vector<cv::Point3f> QiStereo;

            int j0 = 0;

            int j1 = 0;

            while(j0<cci0.rows && j1<cci1.rows){

                int id0 = cidsi0.at<int>(j0);

                int id1 = cidsi1.at<int>(j1);

                if(id0 < id1)

                    j0++;

                else if (id1 < id0)

                    j1++;

                else{

                    assert(id0 == id1);

                    qc0iStereo.push_back(cci0.at<cv::Point2f>(j0));

                    qc1iStereo.push_back(cci1.at<cv::Point2f>(j1));

                    QiStereo.push_back(board->chessboardCorners[id0]);

                    j0++;

                    j1++;

                }

            }

            if(QiStereo.size() > 0){

                qc0Stereo.push_back(qc0iStereo);

                qc1Stereo.push_back(qc1iStereo);

                QStereo.push_back(QiStereo);

            }

        }

    }

    size_t nValidSets = qc0Stereo.size();

    if(nValidSets < 2){

        std::cerr << "Not enough valid calibration sequences!" << std::endl;

        emit done();

        return;

    }

    // calibrate the cameras

    SMCalibrationParameters cal;

    cal.frameWidth = calibrationData[0].frame0.cols;

    cal.frameHeight = calibrationData[0].frame0.rows;

    cv::Size frameSize(cal.frameWidth, cal.frameHeight);

    // determine only k1, k2 for lens distortion

    int flags = cv::CALIB_FIX_ASPECT_RATIO + cv::CALIB_FIX_K2 + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT;

    // Note: several of the output arguments below must be cv::Mat, otherwise segfault

    std::vector<cv::Mat> cam_rvecs0, cam_tvecs0;

    cal.cam0_error = cv::calibrateCamera(Q0, qc0, frameSize, cal.K0, cal.k0, cam_rvecs0, cam_tvecs0, cal.cam0_intrinsic_std, cal.cam0_extrinsic_std, cal.cam0_errors_per_view, flags,

                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));

    std::vector<cv::Mat> cam_rvecs1, cam_tvecs1;

    cal.cam1_error = cv::calibrateCamera(Q1, qc1, frameSize, cal.K1, cal.k1, cam_rvecs1, cam_tvecs1, cal.cam1_intrinsic_std, cal.cam1_extrinsic_std, cal.cam1_errors_per_view, flags,

                                         cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 100, DBL_EPSILON));

    // stereo calibration

    int flags_stereo = cv::CALIB_FIX_INTRINSIC;// + cv::CALIB_FIX_K2 + cv::CALIB_FIX_K3 + cv::CALIB_ZERO_TANGENT_DIST + cv::CALIB_FIX_PRINCIPAL_POINT + cv::CALIB_FIX_ASPECT_RATIO;

    cv::Mat E, F, R1, T1;

    #if CV_MAJOR_VERSION < 3

        cal.stereo_error = cv::stereoCalibrate(QStereo, qc0Stereo, qc1Stereo, cal.K0, cal.k0, cal.K1, cal.k1,

                                               frameSize, R1, T1, E, F,

                                               cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, DBL_EPSILON),

                                               flags_stereo);

    #else

        cal.stereo_error = cv::stereoCalibrate(QStereo, qc0Stereo, qc1Stereo, cal.K0, cal.k0, cal.K1, cal.k1,

                                               frameSize, R1, T1, E, F, flags_stereo,

                                               cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 200, DBL_EPSILON));

    #endif

    cal.R1 = R1;

    cal.T1 = T1;

    cal.E = E;

    cal.F = F;

    // Print to log

    std::stringstream out;

    out << "## Camera Calibration ##" << std::endl

        << "No. images used for intrinsics of cam0: " << qc0.size() << std::endl

        << "No. images used for intrinsics of cam1: " << qc1.size() << std::endl

        << "No. images used for stereo calibration: " << qc0Stereo.size() << std::endl;

    cal.printCamera(out);

    out << std::endl << std::endl;

    emit logMessage(QString::fromStdString(out.str()));

    // save to (reentrant qsettings object)

    settings.setValue("calibration/parameters", QVariant::fromValue(cal));

    emit done();

}