algoLib/sourceCode/binocularMarkCam.cpp

#include <vector>
#include "SG_baseDataType.h"
#include "SG_baseAlgo_Export.h"
#include "binocularMarkCam_Export.h"
#include <opencv2/opencv.hpp>
#include "aruco.hpp"
#include "aruco/charuco.hpp"
#include <limits>

#define _OUTPUT_DEBUG_DATA  0

//version 1.0.0 : <20><>ʼ<EFBFBD>汾
std::string	m_strVersion = "1.0.0";
//<2F><><EFBFBD>汾<EFBFBD><E6B1BE>
const char* wd_charuco3DMarkVersion(void)
{
	return m_strVersion.c_str();
}

/*Breif<69><66><EFBFBD><EFBFBD>ά<EFBFBD><CEAC><EFBFBD>궨<EFBFBD><EAB6A8><EFBFBD>ǵ㺯<C7B5><E3BAAF>*/
void detectCharucoCorners(const cv::Mat& img,
    const SWD_BQ_CharucoMarkInfo markInfo,
    const SWD_BQ_MarkBoardInfo boardInfo,
    std::vector<int>& markerIdsAll,
    std::vector<std::vector<cv::Point2f> >& markerCornersAll,
    std::vector<int>& charucoIds,
    std::vector<std::vector<cv::Point2f>>& charucoCorners)
{
    cv::Mat gray;
    if (img.channels() == 3)
        cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);
    else
        gray = img.clone();

    enum cv::aruco::PredefinedDictionaryType dictType;
    if (1 == markInfo.dictType)
        dictType = cv::aruco::DICT_6X6_1000;
    else
        dictType = cv::aruco::DICT_6X6_1000;  //Ŀǰֻ֧<D6BB><D6A7>DICT_6X6_1000

    cv::aruco::Dictionary aruco_dict = cv::aruco::getPredefinedDictionary(dictType);
    cv::aruco::DetectorParameters params = cv::aruco::DetectorParameters();
    params.cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE;

    cv::Ptr<cv::aruco::Dictionary> ptrDictionary = cv::makePtr<cv::aruco::Dictionary>(aruco_dict);
    cv::Ptr<cv::aruco::DetectorParameters> ptrParams = cv::makePtr<cv::aruco::DetectorParameters>(params);
#if 0
    // <20>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD><C8B6>ԣ<EFBFBD>
    ptrParams->adaptiveThreshWinSizeMin = 3;
    ptrParams->adaptiveThreshWinSizeMax = 23;
    ptrParams->adaptiveThreshWinSizeStep = 10;
    ptrParams->minMarkerPerimeterRate = 0.01; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD>ı<EFBFBD><C4B1><EFBFBD>
#endif
    //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>нǵ<D0BD>
    cv::aruco::detectMarkers(gray, ptrDictionary, markerCornersAll, markerIdsAll, ptrParams);

    //<2F><><EFBFBD><EFBFBD>Mark(<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ<EFBFBD>ı궨<C4B1>壩
    for (int board = 0; board < boardInfo.totalBoardNum; board++)
    {
        int startId = board * boardInfo.boardIdInterval;
        int endId = startId + boardInfo.boardChaucoIDNum - 1;

        std::vector<int> ids;
        for (int i = 0; i < boardInfo.boardChaucoIDNum; i++)
            ids.push_back(startId + i);

        //<2F><><EFBFBD><EFBFBD>
        std::vector<int> markerIds;
        std::vector<std::vector<cv::Point2f> > markerCorners;
        for (int i = 0; i < markerIdsAll.size(); i++)
        {
            if ((markerIdsAll[i] >= startId) && (markerIdsAll[i] <= endId))
            {
                markerIds.push_back(markerIdsAll[i]);
                markerCorners.push_back(markerCornersAll[i]);
            }
        }
        if (markerIds.size() > 0)
        {
            std::vector<int> board_charucoIds;
            std::vector<cv::Point2f> board_charucoCorners;

            cv::aruco::CharucoBoard board = cv::aruco::CharucoBoard(markInfo.patternSize, markInfo.checkerSize, markInfo.markerSize, aruco_dict, ids);
            cv::Ptr<cv::aruco::CharucoBoard> ptrBoard = cv::makePtr<cv::aruco::CharucoBoard>(board);
            cv::aruco::interpolateCornersCharuco(
                markerCorners,
                markerIds,
                gray,
                ptrBoard,
                board_charucoCorners,
                board_charucoIds);
            if (board_charucoIds.size() > 0)
            {
                // <20><><EFBFBD><EFBFBD><EFBFBD>ؾ<EFBFBD>ȷ<EFBFBD><C8B7>
                cv::cornerSubPix(gray, board_charucoCorners, cv::Size(11, 11), cv::Size(-1, -1),
                    cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 50, 0.1));

                charucoIds.push_back(startId);
                charucoCorners.push_back(board_charucoCorners);
            }
        }
    }
    return;
}

SVzNL3DPoint _getStereoPoints3d(cv::Point2f left, cv::Point2f right, const cv::Mat& Q, double offset = 0)
{
    const double cx = Q.at<double>(0, 3);
    const double cy = Q.at<double>(1, 3);
    const double f = Q.at<double>(2, 3);
    const double t = 1.0 / Q.at<double>(3, 2);
    //std::cout << "Cx: " << cx << ", Cy: " << cy << ", f: " << f << ", t: " << t << std::endl;
    SVzNL3DPoint result;
    double dis = left.x - right.x + offset;
    result.x = (left.x + cx) * t / dis;
    result.y = (left.y + cy) * t / dis;
    result.z = f * t / dis;
    //std::cout << "L: " << left[_i] << ", R: " << right[_i] << ", dis: " << dis << std::endl;
    return result;
}

typedef struct
{
    int markID;
    std::vector<cv::Point2f> left;
    std::vector<cv::Point2f> right;
}_MarkerPairing;

//<2F><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>Mark3D<33><44>Ϣ
void wd_BQ_getCharuco3DMark(
    cv::Mat& leftImage,
    cv::Mat& rightImage,
    cv::Mat& cameraMatrixL,
    cv::Mat& distCoeffsL,
    cv::Mat& cameraMatrixR,
    cv::Mat& distCoeffsR,
    cv::Mat& R1,
    cv::Mat& R2,
    cv::Mat& P1,
    cv::Mat& P2,
    cv::Mat& Q,
    SWD_BQ_CharucoMarkInfo markInfo,
    SWD_BQ_MarkBoardInfo boardInfo,
    double disparityOffset,
	std::vector<SWD_charuco3DMark>& marks)
{
    std::vector<int> markerIdsLeft;
    std::vector<std::vector<cv::Point2f> > markerCornersLeft;  
    std::vector<int> charucoIdsLeft;
    std::vector<std::vector<cv::Point2f>> charucoCornersLeft;//ÿ<><C3BF>Mark<72><6B>4<EFBFBD><34><EFBFBD>ǵ<EFBFBD><C7B5><EFBFBD>Ӧ4<D3A6><34><EFBFBD><EFBFBD>ά<EFBFBD><CEAC>
    detectCharucoCorners(leftImage,
        markInfo,
        boardInfo,
        markerIdsLeft,
        markerCornersLeft,
        charucoIdsLeft,
        charucoCornersLeft);

    std::vector<int> markerIdsRight;
    std::vector<std::vector<cv::Point2f> > markerCornersRight;
    std::vector<int> charucoIdsRight;
    std::vector<std::vector<cv::Point2f>> charucoCornersRight;
    detectCharucoCorners(rightImage,
        markInfo,
        boardInfo,
        markerIdsRight,
        markerCornersRight,
        charucoIdsRight,
        charucoCornersRight);

#if _OUTPUT_DEBUG_DATA
    cv::Mat imageCopyLeft = leftImage.clone();
    if (markerIdsLeft.size() > 0)
        cv::aruco::drawDetectedMarkers(imageCopyLeft, markerCornersLeft, markerIdsLeft);
    if (charucoIdsLeft.size() > 0)
        cv::aruco::drawDetectedCornersCharuco(imageCopyLeft, charucoCornersLeft, charucoIdsLeft, cv::Scalar(0, 255, 255));
    char markFilenameL[256];
    sprintf_s(markFilenameL, "markersLeft.bmp");
    cv::imwrite(markFilenameL, imageCopyLeft);

    cv::Mat imageCopyRight = leftImage.clone();
    if (markerIdsRight.size() > 0)
        cv::aruco::drawDetectedMarkers(imageCopyRight, markerCornersRight, markerIdsRight);
    if (charucoIdsRight.size() > 0)
        cv::aruco::drawDetectedCornersCharuco(imageCopyRight, charucoCornersRight, charucoIdsRight, cv::Scalar(0, 255, 255));
    char markFilenameR[256];
    sprintf_s(markFilenameR, "markersRight.bmp");
    cv::imwrite(markFilenameR, imageCopyRight);
#endif

    //Marker<65><72><EFBFBD><EFBFBD>
    std::vector<_MarkerPairing> stereoMarks;
    for (int i = 0, i_max = (int)charucoIdsLeft.size(); i < i_max; i++)
    {
        for (int j = 0, j_max = (int)charucoIdsRight.size(); j < j_max; j++)
        {
            if ( (charucoIdsLeft[i] == charucoIdsRight[j]) && (charucoCornersLeft[i].size() == charucoCornersRight[j].size()))
            {
                _MarkerPairing a_pair;
                a_pair.markID = charucoIdsLeft[i];
                a_pair.left.insert(a_pair.left.end(), charucoCornersLeft[i].begin(), charucoCornersLeft[i].end());
                a_pair.right.insert(a_pair.right.end(), charucoCornersRight[j].begin(), charucoCornersRight[j].end());
                stereoMarks.push_back(a_pair);
                break;
            }
        }
    }

    for (int i = 0, i_max = (int)stereoMarks.size(); i < i_max; i++)
    {
        //˫ĿУ<C4BF><D0A3>
        std::vector<cv::Point2f> rectCornersLeft;
        cv::undistortPoints(stereoMarks[i].left, rectCornersLeft, cameraMatrixL, distCoeffsL, R1, P1);
        std::vector<cv::Point2f> rectCornersRight;
        cv::undistortPoints(stereoMarks[i].right, rectCornersRight, cameraMatrixR, distCoeffsR, R2, P2);

        std::vector<SVzNL3DPoint> corners;
        for (int m = 0; m < rectCornersLeft.size(); m++)
        {
            double abs_diff = abs(rectCornersLeft[m].y - rectCornersRight[m].y);
            if (abs_diff < 1.0) //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
            {
                SVzNL3DPoint a_pt3d = _getStereoPoints3d(rectCornersLeft[m], rectCornersRight[m], Q, disparityOffset);
                corners.push_back(a_pt3d);
            }
        }
        if (corners.size() == 4)
        {
            SWD_charuco3DMark a_3dMark;
            a_3dMark.markID = stereoMarks[i].markID;
            a_3dMark.mark3D.x = (corners[0].x + corners[1].x + corners[2].x + corners[3].x) / 4;
            a_3dMark.mark3D.y = (corners[0].y + corners[1].y + corners[2].y + corners[3].y) / 4;
            a_3dMark.mark3D.z = (corners[0].z + corners[1].z + corners[2].z + corners[3].z) / 4;
            marks.push_back(a_3dMark);
        }
    }
    return;
}