ArUco Marker Detection 구현 및 Pose Estimation

계속 내용을 보완하고 추가할 예정입니다.

기준 마커(fiducial marker)는 일정한 포맷으로 만들어진 인공적인 랜드마크입니다. 증강 현실(Augmented Reality)을 구현하데 이용할 수 있습니다. 

영상에서 마커를 검출하여 마커의 4개의 코너를 구하면 카메라 자세 추정(camera pose estimation)을 할 수 있는데. 카메라 자세 추정이란 3차원 공간상에서의 카메라의 위치와 방향을 구하는 것입니다. 이 정보를 바탕으로 마커의 자세를 추정하여 마커 위에 가상의 사물을 띄웁니다. 이 방법을 사용하면 마커를 손에 들고 움직이더라도 항상 마커 위에 가상 사물을 띄울 수 있습니다.

ArUco marker는 기준 마커 중 한가지로 n x n 크기의 2차원 비트 패턴과 이를 둘러싸고 있는 검은색 태두리 영역으로 구성되어 있습니다. 검은색 테두리 영역은 마커를 빨리 인식하기 위한 것이며, 내부의 2차원 비트 패턴은 흰색 셀과 검정색 셀의 조합으로 마커의 고유 ID를  표현한 것으로 마커를 식별하는데 사용됩니다.  

OpenCV의 aruco 모듈에 ArUco marker의 생성, 검출 및 자세 추정을 위한 함수를 제공하고 있습니다. 아래 튜토리얼 사이트를 따라 해보면 쉽게 해 볼 수 있습니다.   

http://docs.opencv.org/3.1.0/d9/d6d/tutorial_table_of_content_aruco.html

aruco 모듈을 사용하려면 OpenCV's extra module을 포함하여 OpenCV라이브러리를 컴파일해야 합니다. 포함해서 컴파일 하는 방법은 아래 글들을 참고하세요.

[그래픽스&컴퓨터비전/개발환경] - Visual studio 2015용으로 opencv 3.1 라이브러리 컴파일 하기 ( opencv_contrib 모듈 포함 )

[그래픽스&컴퓨터비전/개발환경] - OpenCV 3.1을 Ubuntu 14.04에 설치

[그래픽스&컴퓨터비전/개발환경] - OpenCV 3.1을 Ubuntu 16.04에 설치

마커를 생성하는 것만 aruco 모듈에서 제공하는 예제 프로그램을 사용하고 이후 마커 검출 및 자세 추정 구현해보았습니다.

마커 생성

ArUco 마커를 생성하기 위한 예제 코드는 opencv_contrib/modules/aruco/samples/create_marker.cpp 경로에 있습니다. 

해당 경로로 이동한 후, 컴파일을 합니다. 

$ cd opencv_contrib/modules/aruco/samples

$ g++ -o create_marker create_marker.cpp `pkg-config opencv --cflags --libs`

아래처럼 옵션을 주고 실행해보면 marker1.png 파일이름으로 마커 이미지가 저장됩니다. 

$ ./create_marker -d=10 --id=0 --ms=200 --bb=1 marker1.png

d - 미리 정의된 dictionary의 인덱스로 10은 DICT_6X6_250입니다. 크기 6x6인 마커가 250개 포함된 dictionary를 사용합니다. 

      6x6은 2차원 비트 패턴의 크기를 의미하며 검은색 테두리 영역을 포함하면 8x8이 됩니다.

id - dictionary에 속한 마커들 중 사용할 마커의 id입니다.  실제 마커 id가 아니라 인덱스입니다.

      실제 마커 ID를 사용하는 것 보다 속한 dictionary내에서의 인덱스를 사용하는 것이 효율적이기 때문입니다. 

      DICT_6X6_250의 경우 0부터 249까지 id를 지정할 수 있습니다.  

ms - 마커 이미지의 크기를 픽셀단위로 지정합니다. 200의 경우 200x200 픽셀 크기의 이미지로 저장됩니다.

bb - 검은색 테두리 영역의 크기입니다. 내부 2차원 비트 패턴의 하나의 셀 크기의 배수로 지정합니다.

왼쪽 이미지는 마커 테두리의 크기를 1로 한 경우이고 오른쪽은 마커 테두리의 크기를 2로 한 경우입니다.

aruco 모듈에는 포함된 marker의 갯수 및 marker 크기 별로 dictionary가 정의되어 있습니다. 

dictionary내에 포함된 실제 마커의 ID는 opencv_contrib/modules/aruco/src/predefined_dictionaries.hpp에  배열 형태로 저장되어 있습니다.  마커가 회전을 대비해서( 0도, 90도, 180도, 270도)  하나의 마커 ID에 대해 4개의 해이밍 코드가 저장됩니다.  또한 마커 크기별로 1000개씩 배열을 선언해두고 있습니다. 예를 들어 DICT_6X6_50의 경우 앞에서 50개의 마커 ID를 사용하고 DICT_6X6_100은 앞에서 100개의 마커 ID를 사용합니다. 

DICT_4X4_50의 경우 미리 50개의 마커 ID를 정의해 놓은 것입니다. 4x4는 2차원 비트 패턴의 크기로 가로 4, 세로 4인 격자내에 존재 할 수 있는 16개의 셀을 사용하여 마커 ID를 표현하게 됩니다. 실제 마커 이미지는 검은색 테두리 영역까지 포함하여 6x6으로 생성이 됩니다. 

이후 구현에 사용하기 위해 dictionary DICT_6X6_250에 있는 ID 0번과 ID 41 마커 이미지 파일을 생성한 후,  두 개의 마커를  배경이 흰색인 이미지로 옮겨 저장했습니다.  이대로 사용하면 마커가 정면인 상태만 태스트 가능하므로 A4용지에 인쇄한 후,  일부러 비스듬하게 놓고 카메라로 찍었습니다.  

마커 검출

마커를 검출하면 다음 2가지 정보를 획득하게 됩니다. 

    -이미지 상에서 마커의 4개 코너 위치 (마커가 회전되더라도 각각의 코너는 고유하게 식별됨)

    -마커의 ID

1. 입력 이미지를 그레이스케일 이미지로 변환한 후, adaptive thresholding을 적용하여 이미지를 이진화했습니다. 예상과 달리 에지만 검출된 것은 adaptiveThreshold함수의 blocksize를 충분히 크게 지정해주지 않았기 때문입니다. 이 이미지를 바탕으로 contour 검출결과 마커 사각형에서 contour가 2번씩 검출되는 오류가 있었습니다. 

blocksize를 7 -> 91로 높여준 결과입니다. 마커가 제대로 이진화되었으나 A4용지 가장자리로 두껍께 흰색 영역이 생겼습니다.  contour 검출시 앞에서 생겼던 오류는 없어졌습니다. 

blocksize를 51로 주었을 때입니다. 마커 내부 영역에 제대로 이진화 안된 부분들이 남아있습니다. 

otsu 방법을 적용했을 때입니다. 결과가 더 좋아보입니다.  앞에서 사용한 방법과 결과를 비교해봐야 할듯합니다. 

opencv에선 adaptiveThreshold함수를 사용하여 구현되어 있습니다. 

    //이미지 파일을 불러온다.  
    Mat input_image = imread("test.jpg", IMREAD_COLOR);
 
    //그레이스케일 이미지로 변환한다.
    Mat input_gray_image;
    cvtColor(input_image, input_gray_image, CV_BGR2GRAY);
    
    //Adaptive Thresholding을 적용하여 이진화 한다. 
    Mat binary_image;
    adaptiveThreshold(input_gray_image, binary_image,
    255, ADAPTIVE_THRESH_GAUSSIAN_C, THRESH_BINARY_INV, 91, 7);
    //threshold(input_gray_image, binary_image, 125, 255, THRESH_BINARY_INV | THRESH_OTSU);

2. 이진화된 영상에서 contour를 추출합니다. 

adaptiveThreshold함수를 사용한 경우 79개가 검출되었습니다. 

otsu 방법을 사용한 경우에는 contour가 8개 검출되었고, 속도가 adaptiveThreshold를 사용할때 보다 빨랐습니다. 완성되면 웹캠 영상을 입력으로 사용할 거라서 otsu 방법을 사용하는게 나을듯 보입니다. 

    //contours를 찾는다.
    Mat contour_image = binary_image.clone();
    vector<vector<Point> > contours;
    findContours( contour_image, contours, RETR_LIST, CHAIN_APPROX_SIMPLE);

3. contour를 polygonal curve으로 근사화하여 4개의 점으로 구성된 것만 반시계방향으로 정렬한 후, 저장합니다.

이때 너무 작은 면적의 contour와 너무 큰 면적의 contour 제거합니다.(추후 웹캠 입력 영상의 크기와 웹캠과의 거리에 따른 마커의 크기 변화를 태스트해서 정할 필요가 있을 듯합니다. )

볼록다각형(Convex Polygon)이 아닌 경우도 제거합니다. 

처리 결과 2개의 contour만 남았고, 근사화된 점 위치를 저장해서 다음 단계에서 사용하도록 했습니다.

    //contour를 근사화한다.
    vector<vector<Point2f> > marker;
    vector<Point2f> approx;
 
    for (size_t i = 0; i < contours.size(); i++)
    {        
        approxPolyDP(Mat(contours[i]), approx, arcLength(Mat(contours[i]), true)*0.05, true);
        
        if (
            approx.size() == 4 && //사각형은 4개의 vertex를 가진다. 
            fabs(contourArea(Mat(approx))) > 1000  && //면적이 일정크기 이상이어야 한다.
            fabs(contourArea(Mat(approx))) < 50000 && //면적이 일정크기 이하여야 한다. 
            isContourConvex(Mat(approx)) //convex인지 검사한다.
            ) 
        {
 
            //drawContours(input_image, contours, i, Scalar(0, 255, 0), 1, LINE_AA);
 
            vector<cv::Point2f> points;
            for (int j = 0; j<4; j++)
                points.push_back(cv::Point2f(approx[j].x, approx[j].y));
 
            //반시계 방향으로 정렬
            cv::Point v1 = points[1] - points[0];
            cv::Point v2 = points[2] - points[0];
 
            double o = (v1.x * v2.y) - (v1.y * v2.x);
            if (o < 0.0)        
                swap(points[1], points[3]);
 
            marker.push_back(points);
                        
        }
    }

4. perspective transformation를 적용하여 정면에서 바라본 사각형으로 변환합니다.. 왼쪽 이미지는 입력영상에서의 모습이고 오른쪽은 변환 후 이미지입니다.

5. Otsu 방법으로 이진화를 적용하여 흰색과 검은색 영역으로 이진화합니다.

6. 이진화된 이미지를 격자로 분할합니다. 예를 들어 마커의 크기가 6x6이면 검은색 태두리를 포함해서 8x8=64개의 셀로 나누어집니다. 마커 주변의 테두리 영역에 해당되는 셀만 검사하여 흰색셀이 발견되면 마커 후보에서 제외시킵니다. 셀 내의 흰색 픽셀의 개수가 전체의 절반 이상이면 흰색셀로 간주합니다.

4~6까지의 코드입니다.

    vector<vector<Point2f> > detectedMarkers;
    vector<Mat> detectedMarkersImage;
    vector<Point2f> square_points;
 
    int marker_image_side_length = 80; //마커 6x6크기일때 검은색 테두리 영역 포함한 크기는 8x8
                                //이후 단계에서 이미지를 격자로 분할할 시 셀하나의 픽셀너비를 10으로 한다면
                                //마커 이미지의 한변 길이는 80
    square_points.push_back(cv::Point2f(0, 0));
    square_points.push_back(cv::Point2f(marker_image_side_length - 1, 0));
    square_points.push_back(cv::Point2f(marker_image_side_length - 1, marker_image_side_length - 1));
    square_points.push_back(cv::Point2f(0, marker_image_side_length - 1));
 
    Mat marker_image;
    for (int i = 0; i < marker.size(); i++)
    {
        vector<Point2f> m = marker[i];
 
        //Mat input_gray_image2 = input_gray_image.clone();
        //Mat markerSubImage = input_gray_image2(cv::boundingRect(m));
 
 
        //마커를 사각형형태로 바꿀 perspective transformation matrix를 구한다.
        Mat PerspectiveTransformMatrix = getPerspectiveTransform(m, square_points);
 
        //perspective transformation을 적용한다. 
        warpPerspective(input_gray_image, marker_image, PerspectiveTransformMatrix, Size(marker_image_side_length, marker_image_side_length));
 
        //otsu 방법으로 이진화를 적용한다. 
        threshold(marker_image, marker_image, 125, 255, THRESH_BINARY | THRESH_OTSU);
 
        
 
        //마커의 크기는 6, 검은색 태두리를 포함한 크기는 8
        //마커 이미지 테두리만 검사하여 전부 검은색인지 확인한다. 
        int cellSize = marker_image.rows / 8;
        int white_cell_count = 0;
        for (int y = 0; y<8; y++)
        {
            int inc = 7; // 첫번째 열과 마지막 열만 검사하기 위한 값
 
            if (y == 0 || y == 7) inc = 1; //첫번째 줄과 마지막줄은 모든 열을 검사한다. 
 
                                           
            for (int x = 0; x<8; x += inc)
            {
                int cellX = x * cellSize;
                int cellY = y * cellSize;
                cv::Mat cell = marker_image(Rect(cellX, cellY, cellSize, cellSize));
 
                int total_cell_count = countNonZero(cell);
                
                if (total_cell_count > (cellSize*cellSize) / 2)
                    white_cell_count++; //태두리에 흰색영역이 있다면, 셀내의 픽셀이 절반이상 흰색이면 흰색영역으로 본다 
                                    
            }
        }
 
        //검은색 태두리로 둘러쌓여 있는 것만 저장한다.
        if (white_cell_count == 0) {
            detectedMarkers.push_back(m);
            Mat img = marker_image.clone();
            detectedMarkersImage.push_back(img);
        }
    }
 

7. 이제 검은색 테두리를 제외한 내부의 6x6 영역 내에 있는 셀들에 포함된 흰색 픽셀의 개수를 카운트합니다. 셀 내의 흰색 픽셀의 개수가 전체의 절반 이상이면 흰색셀로 간주합니다. 흰색셀은 1로 검은셀은 0으로 표현하여 6x6 배열에 저장하여 비트 매트릭스를 만듭니다.

    vector<Mat> bitMatrixs;
    for (int i = 0; i < detectedMarkers.size(); i++)
    {
        Mat marker_image = detectedMarkersImage[i];
 
        //내부 6x6에 있는 정보를 비트로 저장하기 위한 변수
        Mat bitMatrix = Mat::zeros(6, 6, CV_8UC1);
 
        int cellSize = marker_image.rows / 8;
        for (int y = 0; y < 6; y++)
        {
            for (int x = 0; x < 6; x++)
            {
                int cellX = (x + 1)*cellSize;
                int cellY = (y + 1)*cellSize;
                Mat cell = marker_image(cv::Rect(cellX, cellY, cellSize, cellSize));
                
                int total_cell_count = countNonZero(cell);
 
 
                if (total_cell_count > (cellSize*cellSize) / 2)
                    bitMatrix.at<uchar>(y, x) = 1;
            }
        }
 
        bitMatrixs.push_back(bitMatrix);
        
    }

8. 비트 매트릭스를 바이트 리스트로 바꿉니다. 필요한 바이트 수는 ( 6 x 6  + 8 - 1 ) / 8로 계산해보면 5바이트임을 알 수 있습니다. 

5바이트를 저장할 수 있는 변수를 선언하고 비트 매트릭스를 왼쪽에서 오른쪽으로, 위에서 아래로 스캔하면서 해당 위치에 있는 비트를 변수에 저장합니다.  비트는 circular shift 연산을 사용하여 변수에 저장됩니다. 

처음에 비트매트릭스 (x,y)=(0,0)에 있는 비트값 0이 첫번째 바이트의 8번째 비트에 입력합니다.

그 다음 기존값을 왼쪽으로 시프트하고 (1,0)에 있는 비트값 1을 첫번째 바이트의 8번째 비트에 입력합니다.

이런식으로 기존값을 왼쪽으로 시프트하고 첫번째 바이트의 8번째 비트에 입력하는 것을 반복합니다.

첫번째 8비트를 모두 채웠으면, 두번째 바이트로 이동하여 앞 과정을 반복합니다.

6x6 비트 매트릭스를 5바이트에 모두 옮기고나서 각 바이트를 10진수로 표현한 것이 바이트 리스트입니다. 

각각 마커에 대해 구해진 바이트 리스트입니다. 

[ 96,  83, 122, 137,   1]

[ 30,  61, 216,  42,   6]

Mat getByteListFromBits(const Mat &bits) {
    // integer ceil
    int nbytes = (bits.cols * bits.rows + 8 - 1) / 8;
 
    Mat candidateByteList(1, nbytes, CV_8UC1, Scalar::all(0));
    unsigned char currentBit = 0;
    int currentByte = 0;
 
    uchar* rot0 = candidateByteList.ptr();
 
    for (int row = 0; row < bits.rows; row++) {
        for (int col = 0; col < bits.cols; col++) {
            // circular shift
            rot0[currentByte] <<= 1;
 
            // set bit
            rot0[currentByte] |= bits.at<uchar>(row, col);
 
            currentBit++;
            if (currentBit == 8) {
                // next byte
                currentBit = 0;
                currentByte++;
            }
        }
    }
    return candidateByteList;
}

9.  dictionary에는 마커 ID를 0도, 90도, 180도, 270도 방향에 대한  4개의 바이트 리스트로 저장하고 있습니다.

위에서 구한 바이트 리스트를 dictionary에서 찾아보면 마커 ID 0번의 첫번째 바이트 리스트와 마커 ID 41번의 첫번째 바이트 리스트와 일치하는 것을 확인할 수 있습니다. 몇번째 바이트 리스트와 일치하는 지 여부로 마커의 회전각도를 알아냅니다. 

{{ 30, 61, 216, 42, 6 }, { 227, 186, 70, 49, 9 }, { 101, 65, 187, 199, 8 }, { 152, 198, 37, 220, 7 }, }

{{ 96, 83, 122, 137, 1 }, { 100, 102, 44, 148, 6 }, { 137, 21, 236, 160, 6 }, { 98, 147, 70, 98, 6 }, }

opencv_contrib/modules/aruco/src/predefined_dictionaries.hpp에 선언 되어 있는 배열 DICT_6X6_1000_BYTES[][4][5]를 작성중인 코드로 가져오고 아래 한줄을 선언해주었습니다. 1000개중 250개만 dictionary에서 사용합니다. 

Mat dictionary = Mat(250, (6 * 6 + 7) / 8, CV_8UC4, (uchar*)DICT_6X6_1000_BYTES);

identiy함수를 사용하여 마커가 발견되었다면 회전 인덱스에 맞게 코너를 회전시킵니다. 

    vector<int> markerID;
    vector<vector<Point2f> > final_detectedMarkers;
    for (int i = 0; i < detectedMarkers.size(); i++)
    {
        Mat bitMatrix = bitMatrixs[i];
        vector<Point2f> m = detectedMarkers[i];
 
 
        int rotation;
        int marker_id;
        if (!identify(bitMatrix, marker_id, rotation ))
            cout << "발견안됨" << endl;
        else {
            
            if (rotation != 0) {
                //회전을 고려하여 코너를 정렬합니다. 
                //마커의 회전과 상관없이 마커 코너는 항상 같은 순서로 저장됩니다.
                std::rotate(m.begin(), m.begin() + 4 - rotation, m.end());
            }
            
            markerID.push_back(marker_id);
            final_detectedMarkers.push_back(m);
        }
    }

identify함수에서는 비트 매트릭스를 바이트 리스트로 변환한 후,  dictionary에 저장된 바이트 리스트와  비교하여 매치되는 마커 ID와 회전 인덱스를 구합니다.  매치여부는 간단하게 해밍 거리를 계산하여 최소가 될 경우로 했습니다.

 
bool identify(const Mat &onlyBits, int &idx, int &rotation ) {
    int markerSize = 6;
 
    //비트 매트릭스를 바이트 리스트로 변환합니다. 
    Mat candidateBytes = getByteListFromBits(onlyBits);
 
    idx = -1; // by default, not found
 
    //dictionary에서 가장 근접한 바이트 리스트를 찾습니다. 
    int MinDistance = markerSize * markerSize + 1;
    rotation = -1;
    for (int m = 0; m < dictionary.rows; m++) {
        
        //각 마커 ID
        for (unsigned int r = 0; r < 4; r++) {
            int currentHamming = hal::normHamming(
                dictionary.ptr(m) + r*candidateBytes.cols,
                candidateBytes.ptr(),
                candidateBytes.cols );
            
            //이전에 계산된 해밍 거리보다 작다면 
            if (currentHamming < MinDistance) {
                //현재 해밍 거리와 발견된 회전각도를 기록합니다. 
                MinDistance = currentHamming;
                rotation = r;
                idx = m;
            }
        }
    }
 
    //idx가 디폴트값 -1이 아니면 발견된 것
    return idx != -1;
}

이제 마커의 ID와 마커 코너점을 구했습니다.

마커가 회전하더라도  각 코너점은 고유하게 인식이 됩니다.

10. cornerSubPix 함수를 이용하여 코너점의 정확도를 높입니다. 

[453, 310;

 508, 456;

 322, 500;

 284, 350]

[452.66232, 309.4938;

 509.08487, 455.62674;

 321.51157, 500.82654;

 283.33392, 350.10324]

[370, 93;

 409, 197;

 255, 233;

 228, 125]

[370.41031, 92.603607;

 410.0723, 196.51663;

 254.17957, 233.44662;

 226.72998, 124.6369]

위 코드에서 cornerSubPix함수 호출하는 부분이 추가되었습니다. 

    vector<int> markerID;
    vector<vector<Point2f> > final_detectedMarkers;
    for (int i = 0; i < detectedMarkers.size(); i++)
    {
        Mat bitMatrix = bitMatrixs[i];
        vector<Point2f> m = detectedMarkers[i];
 
 
        int rotation;
        int marker_id;
        if (!identify(bitMatrix, marker_id, rotation ))
            cout << "발견안됨" << endl;
        else {
            
            if (rotation != 0) {
                //회전을 고려하여 코너를 정렬합니다. 
                //마커의 회전과 상관없이 마커 코너는 항상 같은 순서로 저장됩니다.
                std::rotate(m.begin(), m.begin() + 4 - rotation, m.end());
            }
            /*
            int sumx = 0, sumy = 0;
            for (int j = 0; j < 4; j++) {
                putText(input_image, to_string(j + 1), Point(m[j].x, m[j].y), CV_FONT_NORMAL, 1, Scalar(255, 0, 0), 1, 1);
                sumx += m[j].x;
                sumy += m[j].y;
            }

            putText(input_image, "id="+to_string(marker_id), Point( sumx/4 , sumy/4 ), CV_FONT_NORMAL, 1, Scalar(255, 0, 0), 1, 1);
            */
            cornerSubPix(input_gray_image, m, Size(5, 5), Size(-1, -1), TermCriteria(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, 30, 0.01));
 
            markerID.push_back(marker_id);
            final_detectedMarkers.push_back(m);
        }
    }

카메라 컬리브레이션

ChArUco Board를 이용하여 카메라 컬리브레이션을 했습니다. ArUco marker와 chess board를 합쳐서 만들어진 보드 입니다. 

opencv_contrib/modules/aruco/samples/create_board_charuco.cpp 경로에 있는 ChArUco Board 생성하는 코드를 Visual Studio에서 컴파일 후 실행하여 ChArUco Board이미지를 생성하였습니다. 옵션들은 미리 코드에서 입력해두었습니다.

#include <opencv2/highgui.hpp>
#include <opencv2/aruco/charuco.hpp>
 
using namespace cv;
 
namespace {
    const char* about = "Create a ChArUco board image";
    const char* keys =
        "{@outfile |<none> | Output image }"
        "{w        |       | Number of squares in X direction }"
        "{h        |       | Number of squares in Y direction }"
        "{sl       |       | Square side length (in pixels) }"
        "{ml       |       | Marker side length (in pixels) }"
        "{d        |       | dictionary: DICT_4X4_50=0, DICT_4X4_100=1, DICT_4X4_250=2,"
        "DICT_4X4_1000=3, DICT_5X5_50=4, DICT_5X5_100=5, DICT_5X5_250=6, DICT_5X5_1000=7, "
        "DICT_6X6_50=8, DICT_6X6_100=9, DICT_6X6_250=10, DICT_6X6_1000=11, DICT_7X7_50=12,"
        "DICT_7X7_100=13, DICT_7X7_250=14, DICT_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16}"
        "{m        |       | Margins size (in pixels). Default is (squareLength-markerLength) }"
        "{bb       | 1     | Number of bits in marker borders }"
        "{si       | false | show generated image }";
}
 
int main(int argc, char *argv[]) {
 
    int squaresX = 5;//가로방향 마커 갯수
    int squaresY = 7;//세로방향 마터 갯수
    int squareLength = 80; //검은색 테두리 포함한 정사각형의 한변 길이 , 픽셀단위
    int markerLength = 40;//마커 한 변의 길이, 픽셀단위
    int dictionaryId = 10; //DICT_6X6_250=10
    int margins = 10;//ChArUco board와 A4용지 사이의 흰색 여백 크기, 픽셀단위
    int borderBits = 1;//검은색 테두리 크기 
    bool showImage = false;
 
    String out = "board.jpg";
 
    
    Ptr<aruco::Dictionary> dictionary =
        aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId));
 
    Size imageSize;
    imageSize.width = squaresX * squareLength + 2 * margins;
    imageSize.height = squaresY * squareLength + 2 * margins;
 
    Ptr<aruco::CharucoBoard> board = aruco::CharucoBoard::create(squaresX, squaresY, (float)squareLength,
        (float)markerLength, dictionary);
 
    // show created board
    Mat boardImage;
    board->draw(imageSize, boardImage, margins, borderBits);
 
    if (showImage) {
        imshow("board", boardImage);
        waitKey(0);
    }
 
    imwrite(out, boardImage);
 
    return 0;
}

 ChArUco Board를 생성한 이미지 결과입니다.  카메라 컬리브레이션에 사용하기 위해서는 프린터로 인쇄해서 사용해야 합니다.

opencv_contrib/modules/aruco/samples/calibrate_camera_charuco.cpp에 있는 카메라 컬리브레이션 코드를 Visual Studio에서 컴파일 후, 실행하여 카메라 컬리브레이션을 진행하였습니다. 카메라 컬리브레이션을 진행하면 YAML형식의 택스트 결과 파일에 카메라 정보가 저장됩니다. 리눅스에서 결과파일을 생성하여 윈도우에서 불러오면 인코딩의 차이로  YAML 형식으로 저장된 카메라 정보를 불러오지를 못합니다. 

소스코드에 옵션들을 미리 넣어서 사용했습니다. 앞에서 만든 ChArUco board에 맞게 옵션을 입력해야 합니다. 

#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/imgproc.hpp>
#include <vector>
#include <iostream>
#include <ctime>
 
using namespace std;
using namespace cv;
 
/**
*/
static bool saveCameraParams(const string &filename, Size imageSize, float aspectRatio, int flags,
    const Mat &cameraMatrix, const Mat &distCoeffs, double totalAvgErr) {
    FileStorage fs(filename, FileStorage::WRITE);
    if (!fs.isOpened())
        return false;
 
    time_t tt;
    time(&tt);
    struct tm *t2 = localtime(&tt);
    char buf[1024];
    strftime(buf, sizeof(buf) - 1, "%c", t2);
 
    fs << "calibration_time" << buf;
 
    fs << "image_width" << imageSize.width;
    fs << "image_height" << imageSize.height;
 
    if (flags & CALIB_FIX_ASPECT_RATIO) fs << "aspectRatio" << aspectRatio;
 
    if (flags != 0) {
        sprintf(buf, "flags: %s%s%s%s",
            flags & CALIB_USE_INTRINSIC_GUESS ? "+use_intrinsic_guess" : "",
            flags & CALIB_FIX_ASPECT_RATIO ? "+fix_aspectRatio" : "",
            flags & CALIB_FIX_PRINCIPAL_POINT ? "+fix_principal_point" : "",
            flags & CALIB_ZERO_TANGENT_DIST ? "+zero_tangent_dist" : "");
    }
 
    fs << "flags" << flags;
 
    fs << "camera_matrix" << cameraMatrix;
    fs << "distortion_coefficients" << distCoeffs;
 
    fs << "avg_reprojection_error" << totalAvgErr;
 
    return true;
}
 
/**
*/
int main(int argc, char *argv[]) {
    
    int squaresX = 5;//인쇄한 보드의 가로방향 마커 갯수
    int squaresY = 7;//인쇄한 보드의 세로방향 마커 갯수
    float squareLength = 36;//검은색 테두리 포함한 정사각형의 한변 길이, mm단위로 입력
    float markerLength = 18;//인쇄물에서의 마커 한변의 길이, mm단위로 입력
    int dictionaryId = 10;//DICT_6X6_250=10
    string outputFile = "output.txt";
 
    int calibrationFlags = 0;
    float aspectRatio = 1;
 
 
    Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
    
    bool refindStrategy =true;
    int camId = 0;
    
 
 
    VideoCapture inputVideo;
    int waitTime;
 
    inputVideo.open(camId);
    waitTime = 10;
 
 
    Ptr<aruco::Dictionary> dictionary =
        aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId));
 
    // create charuco board object
    Ptr<aruco::CharucoBoard> charucoboard =
        aruco::CharucoBoard::create(squaresX, squaresY, squareLength, markerLength, dictionary);
    Ptr<aruco::Board> board = charucoboard.staticCast<aruco::Board>();
 
    // collect data from each frame
    vector< vector< vector< Point2f > > > allCorners;
    vector< vector< int > > allIds;
    vector< Mat > allImgs;
    Size imgSize;
 
    while (inputVideo.grab()) {
        Mat image, imageCopy;
        inputVideo.retrieve(image);
 
        vector< int > ids;
        vector< vector< Point2f > > corners, rejected;
 
        // detect markers
        aruco::detectMarkers(image, dictionary, corners, ids, detectorParams, rejected);
 
        // refind strategy to detect more markers
        if (refindStrategy) aruco::refineDetectedMarkers(image, board, corners, ids, rejected);
 
        // interpolate charuco corners
        Mat currentCharucoCorners, currentCharucoIds;
        if (ids.size() > 0)
            aruco::interpolateCornersCharuco(corners, ids, image, charucoboard, currentCharucoCorners,
                currentCharucoIds);
 
        // draw results
        image.copyTo(imageCopy);
        if (ids.size() > 0) aruco::drawDetectedMarkers(imageCopy, corners);
 
        if (currentCharucoCorners.total() > 0)
            aruco::drawDetectedCornersCharuco(imageCopy, currentCharucoCorners, currentCharucoIds);
 
        putText(imageCopy, "Press 'c' to add current frame. 'ESC' to finish and calibrate",
            Point(10, 20), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 0, 0), 2);
 
        imshow("out", imageCopy);
        char key = (char)waitKey(waitTime);
        if (key == 27) break;
        if (key == 'c' && ids.size() > 0) {
            cout << "Frame captured" << endl;
            allCorners.push_back(corners);
            allIds.push_back(ids);
            allImgs.push_back(image);
            imgSize = image.size();
        }
    }
 
    if (allIds.size() < 1) {
        cerr << "Not enough captures for calibration" << endl;
        return 0;
    }
 
    Mat cameraMatrix, distCoeffs;
    vector< Mat > rvecs, tvecs;
    double repError;
 
    if (calibrationFlags & CALIB_FIX_ASPECT_RATIO) {
        cameraMatrix = Mat::eye(3, 3, CV_64F);
        cameraMatrix.at< double >(0, 0) = aspectRatio;
    }
 
    // prepare data for calibration
    vector< vector< Point2f > > allCornersConcatenated;
    vector< int > allIdsConcatenated;
    vector< int > markerCounterPerFrame;
    markerCounterPerFrame.reserve(allCorners.size());
    for (unsigned int i = 0; i < allCorners.size(); i++) {
        markerCounterPerFrame.push_back((int)allCorners[i].size());
        for (unsigned int j = 0; j < allCorners[i].size(); j++) {
            allCornersConcatenated.push_back(allCorners[i][j]);
            allIdsConcatenated.push_back(allIds[i][j]);
        }
    }
 
    // calibrate camera using aruco markers
    double arucoRepErr;
    arucoRepErr = aruco::calibrateCameraAruco(allCornersConcatenated, allIdsConcatenated,
        markerCounterPerFrame, board, imgSize, cameraMatrix,
        distCoeffs, noArray(), noArray(), calibrationFlags);
 
    // prepare data for charuco calibration
    int nFrames = (int)allCorners.size();
    vector< Mat > allCharucoCorners;
    vector< Mat > allCharucoIds;
    vector< Mat > filteredImages;
    allCharucoCorners.reserve(nFrames);
    allCharucoIds.reserve(nFrames);
 
    for (int i = 0; i < nFrames; i++) {
        // interpolate using camera parameters
        Mat currentCharucoCorners, currentCharucoIds;
        aruco::interpolateCornersCharuco(allCorners[i], allIds[i], allImgs[i], charucoboard,
            currentCharucoCorners, currentCharucoIds, cameraMatrix,
            distCoeffs);
 
        allCharucoCorners.push_back(currentCharucoCorners);
        allCharucoIds.push_back(currentCharucoIds);
        filteredImages.push_back(allImgs[i]);
    }
 
    if (allCharucoCorners.size() < 4) {
        cerr << "Not enough corners for calibration" << endl;
        return 0;
    }
 
    // calibrate camera using charuco
    repError =
        aruco::calibrateCameraCharuco(allCharucoCorners, allCharucoIds, charucoboard, imgSize,
            cameraMatrix, distCoeffs, rvecs, tvecs, calibrationFlags);
 
    bool saveOk = saveCameraParams(outputFile, imgSize, aspectRatio, calibrationFlags,
        cameraMatrix, distCoeffs, repError);
    if (!saveOk) {
        cerr << "Cannot save output file" << endl;
        return 0;
    }
 
    cout << "Rep Error: " << repError << endl;
    cout << "Rep Error Aruco: " << arucoRepErr << endl;
    cout << "Calibration saved to " << outputFile << endl;
 
 
    return 0;
}

실행하면 카메라에서 찍은 영상이 보입니다. ChArUco board를 카메라에 비추어 마커들이 인식되었는지 확인 후, c키를 눌러줍니다.  보드 위치나 각도를 바꾸어가며 c키 누르는 것을 반복합니다.  최소 4번이상 반복한 후, ESC키를 눌러줍니다.  

아래같은 정보를 포함하고 있는 텍스트 파일이 생성됩니다. 

%YAML 1.0
---
calibration_time: "Fri Sep  9 09:08:25 2016"
image_width: 640
image_height: 480
flags: 0
camera_matrix: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 6.1443645574203697e+02, 0., 2.8512874154070664e+02, 0.,
       6.1557063632004963e+02, 2.5092914090822913e+02, 0., 0., 1. ]
distortion_coefficients: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [ 5.9924567649311936e-02, -1.0813526599267214e+00,
       3.9822786482964978e-03, -1.1893544690509423e-02,
       1.4847422278360058e+00 ]
avg_reprojection_error: 9.5317071003908871e-01

 

마커 자세 추정

카메라와 마커사이의 rotation 및 translation 벡터를 구하기 위해서 solvePnP함수를 사용하였습니다.

( #include "opencv2/calib3d/calib3d.hpp" 추가 )

markerID 41

rotation_vector

[1.934049351005919;

 1.550223573108547;

 -0.8446386827131184]

translation_vector

[0.5851968763294053;

 0.7985369246642258;

 3.34307109549722]

markerID 0

rotation_vector

[2.092875802469283;

 1.653450305374654;

 -0.6720463631651644]

translation_vector

[0.194568444358113;

 -0.6106960911100342;

 4.019592772908302]

마커 위에 좌표축을 출력해주기 위해 aruco::drawAxis함수를  사용했습니다. 

( #include <opencv2/aruco.hpp> 추가)

   //카메라 컬리브레이션으로 얻은 카메라 정보를 파일에서 읽어옴
    Mat camMatrix, distCoeffs;
    FileStorage fs("output.txt", FileStorage::READ);
    if (!fs.isOpened())
        return false;
    fs["camera_matrix"] >> camMatrix;
    fs["distortion_coefficients"] >> distCoeffs;
 
 
    vector<cv::Point3f> markerCorners3d;
    markerCorners3d.push_back(cv::Point3f(-0.5f, 0.5f, 0));
    markerCorners3d.push_back(cv::Point3f(0.5f, 0.5f, 0));
    markerCorners3d.push_back(cv::Point3f(0.5f, -0.5f, 0));
    markerCorners3d.push_back(cv::Point3f(-0.5f, -0.5f, 0));
        
 
    for (int i = 0; i < final_detectedMarkers.size(); i++)
    {
        vector<Point2f> m = final_detectedMarkers[i];
        
        //카메라와 마커사이의 rotation 및 translation 벡터를 구함
        Mat rotation_vector, translation_vector;
        solvePnP(markerCorners3d, m, camMatrix, distCoeffs, rotation_vector, translation_vector);    
 
        cout << "markerID " << markerID[i] << endl;
        cout << "rotation_vector" << endl << rotation_vector << endl;
        cout << "translation_vector" << endl << translation_vector << endl;
 
        //aruco 모듈에서 제공하는 함수를 이용하여 마커위에 좌표축을 그림
        aruco::drawAxis(input_image, camMatrix, distCoeffs, rotation_vector, translation_vector, 1.0);
    }