[OPENCV] pose estimation using solvePnP

solvePnP

opencv함수로 chessboard의 좌표계를 구성하여 chessboard 좌표계까지의 변환 행렬을 얻을 수 있다. 물론, 카메라 calibration 및 그 외의 전과정은 미리 다 해둔 것으로 가정.
* 정확하게는 chessboard와 카메라 사이의 translation vector와 rotation vector가 반환된다. homography matrix를 얻고싶다면 위의 값을 잘 이용해보자.

아래의 동영상을 보면 쉽게 이해 될 것이다.

이와 같은 pose estimation은 써먹을 곳이 많다. 예로들면,
움직이는 물체를 추적하고 이동거리를 알고 싶을 때 pixel단위가 아닌 mm단위로 이동거리를 알아낼 수 있다. 이와 마찬가지로 지면과 카메라 간의 변환 행렬을 구하여 실제 거리를 재는등의 목적으로 사용할 수 있을 것 같다. 

장점

1. 오차가 상당히 없는 듯하다.
2. 데이터를 실험적으로 뽑아 낸 것은 아니지만 나의 감각에 의하면 오차가 커봐야 1cm터 내외 일 듯 하다.
*위의 영상에서는 chessboard 좌표계의 원점에서 부터 x,y,z 축으로 5cm의 선을 그리도록 하였다.

단점

1. findChessboard 과정에서 corner를 못 찾게 되면 상당한 시간이 지연된다.
2. 최소 3x3 chessboard가 필요하다.

핵심 코드

struct CamParameter
{
    cv::Mat cameraMatrix;
    cv::Mat distCoeffs;
};

void solvePnP(Mat& img, CamParameter camParam)
{
    vector<Point3f> objectPoint;
    vector<Point2f> imagePoint = findChessboard(img);

    if(imagePoint.size() != CHESS_COLS*CHESS_ROWS)
        return;

    // createObjPoints;
    for( int j = 0; j < rows; j++ )
        for( int k = 0; k < cols; k++ )
            objectPoints.push_back(cv::Point3f(float( k*square_size ), float( j*square_size ), 0));

    cv::Mat rvecs;
    cv::Mat tvecs;

    cv::solvePnP(
                objectPoint,
                imagePoint,
                camParam.cameraMatrix,
                camParam.distCoeffs,
                rvecs,tvecs);

    cout << "rvecs\n";
    cout << rvecs << endl;

    cout << "tvecs\n";
    cout << tvecs << endl;

    vector<cv::Point3f> obj_pts;
    obj_pts.push_back( Point3f(0, 0, 0));
    obj_pts.push_back( Point3f(50, 0, 0));
    obj_pts.push_back( Point3f(0, 50, 0));
    obj_pts.push_back( Point3f(0, 0, 50));

    projectPoints(
                obj_pts,
                rvecs,tvecs,
                camParam.cameraMatrix,
                camParam.distCoeffs,
                imagePoint);

    line(img, imagePoint.at(0), imagePoint.at(1), Scalar(0, 0, 255), 5, 8, 0);
    line(img, imagePoint.at(0), imagePoint.at(2), Scalar(0, 255, 0), 5, 8, 0);
    line(img, imagePoint.at(0), imagePoint.at(3), Scalar(255, 0, 0), 5, 8, 0);
}

핵심 코드만 올렸으나 충분히 위의 코드로 사용법은 알수 있을 것이라 생각된다.
예제는 opencv.org에 많으니 자세한 것은 생략한다.