Odometry(오드메트리)란 일반적으로는 엔코더나 가속도 센서로부터 얻어지는 이동 변화량입니다만, RGB-D 카메라를 이용한 오드메트리(Visual Odometry)라고 하면 카메라의 이동량(회전 행렬과 평행 이동 벡터)입니다. SLAM은 이 외에도 맵 최적화나 폐루프 검출 처리가 없어야 합니다.

샘플 프로그램

OpenCV 샘플 프로그램에서 rgbdodometry.cpp를 실행해 보았습니다.

포함 파일

#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/contrib/contrib.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <cstdio>
#include <iostream>
#include <ctime>

라이브러리 설정

#include <opencv_lib.hpp>

네임스페이스

using namespace cv;
using namespace std;

Depth에서 3차원 점군을 생성하는 함수

static
void cvtDepth2Cloud(const Mat& depth, Mat& cloud, const Mat& cameraMatrix)
{
    const float inv_fx = 1.f / cameraMatrix.at<float>(0, 0);
    const float inv_fy = 1.f / cameraMatrix.at<float>(1, 1);
    const float ox = cameraMatrix.at<float>(0, 2);
    const float oy = cameraMatrix.at<float>(1, 2);
    cloud.create(depth.size(), CV_32FC3);
    for (int y = 0; y < cloud.rows; y++)
    {
        Point3f* cloud_ptr = (Point3f*)cloud.ptr(y);
        const float* depth_prt = (const float*)depth.ptr(y);
        for (int x = 0; x < cloud.cols; x++)
        {
            float z = depth_prt[x];
            cloud_ptr[x].x = (x - ox) * z * inv_fx;
            cloud_ptr[x].y = (y - oy) * z * inv_fy;
            cloud_ptr[x].z = z;
        }
    }
}

변환 행렬(4x4)로 3차원 점군을 변환하는 함수
※카메라 0(Image0)에서 카메라 1(Image1)로 변환

template<class ImageElemType>
static void warpImage(const Mat& image, const Mat& depth,
    const Mat& Rt, const Mat& cameraMatrix, const Mat& distCoeff,
    Mat& warpedImage)
{
    const Rect rect = Rect(0, 0, image.cols, image.rows);

    vector<Point2f> points2d;
    Mat cloud, transformedCloud;

    cvtDepth2Cloud(depth, cloud, cameraMatrix);
    perspectiveTransform(cloud, transformedCloud, Rt);
    projectPoints(transformedCloud.reshape(3, 1), Mat::eye(3, 3, CV_64FC1), Mat::zeros(3, 1, CV_64FC1), cameraMatrix, distCoeff, points2d);

    Mat pointsPositions(points2d);
    pointsPositions = pointsPositions.reshape(2, image.rows);

    warpedImage.create(image.size(), image.type());
    warpedImage = Scalar::all(0);

    Mat zBuffer(image.size(), CV_32FC1, FLT_MAX);
    for (int y = 0; y < image.rows; y++)
    {
        for (int x = 0; x < image.cols; x++)
        {
            const Point3f p3d = transformedCloud.at<Point3f>(y, x);
            const Point p2d = pointsPositions.at<Point2f>(y, x);
            if (!cvIsNaN(cloud.at<Point3f>(y, x).z) && cloud.at<Point3f>(y, x).z > 0 &&
                rect.contains(p2d) && zBuffer.at<float>(p2d) > p3d.z)
            {
                warpedImage.at<ImageElemType>(p2d) = image.at<ImageElemType>(y, x);
                zBuffer.at<float>(p2d) = p3d.z;
            }
        }
    }
}

메인 함수

int main(int argc, char** argv)
{

①vals는 카메라의 내부 파라미터(Kinect 출하 시의 내부 파라미터)이며, 초점 거리(fx, fy), 화상 중심(cx, cy)으로 하면, fx = 525, fy = 525, cx = 319.5, cy = 239.5 . 카메라 왜곡(distCoeff) 없음.

    float vals[] = { 525., 0., 3.1950000000000000e+02,
        0., 525., 2.3950000000000000e+02,
        0., 0., 1. };

    const Mat cameraMatrix = Mat(3, 3, CV_32FC1, vals);
    const Mat distCoeff(1, 5, CV_32FC1, Scalar(0));

②화상 세트(RGB와 Depth)를 2개 읽어들여,
- 강체 변환
- 회전 행렬만
벡터
를 구하는 옵션(-rbm, -r, -t)을 붙인다.

    if (argc != 5 && argc != 6)
    {
        cout << "Format: image0 depth0 image1 depth1 [transformationType]" << endl;
        cout << "Depth file must be 16U image stored depth in mm." << endl;
        cout << "Transformation types:" << endl;
        cout << "   -rbm - rigid body motion (default)" << endl;
        cout << "   -r   - rotation rotation only" << endl;
        cout << "   -t   - translation only" << endl;
        return -1;
    }

    Mat colorImage0 = imread(argv[1]);
    Mat depth0 = imread(argv[2], -1);

    Mat colorImage1 = imread(argv[3]);
    Mat depth1 = imread(argv[4], -1);

    if (colorImage0.empty() || depth0.empty() || colorImage1.empty() || depth1.empty())
    {
        cout << "Data (rgb or depth images) is empty.";
        return -1;
    }

    int transformationType = RIGID_BODY_MOTION;
    if (argc == 6)
    {
        string ttype = argv[5];
        if (ttype == "-rbm")
        {
            transformationType = RIGID_BODY_MOTION;
        }
        else if (ttype == "-r")
        {
            transformationType = ROTATION;
        }
        else if (ttype == "-t")
        {
            transformationType = TRANSLATION;
        }
        else
        {
            cout << "Unsupported transformation type." << endl;
            return -1;
        }
    }

③ 컬러 이미지는 그레이스케일로 변환하고, Depth는 미터로 변환한다. 여기서는 0~4m의 Depth 데이터만을 사용하고 있다.

    Mat grayImage0, grayImage1, depthFlt0, depthFlt1/*in meters*/;
    cvtColor(colorImage0, grayImage0, COLOR_BGR2GRAY);
    cvtColor(colorImage1, grayImage1, COLOR_BGR2GRAY);
    depth0.convertTo(depthFlt0, CV_32FC1, 1. / 1000);
    depth1.convertTo(depthFlt1, CV_32FC1, 1. / 1000);

④TickMeter tm;는 처리시간을 측정한다. tm.start();에서 tm.stop();까지의 시간을 측정할 수 있다. tm.getTimeMilli()로 시간 [ms]를 표시한다. 그리고는, 변환 행렬을 구하기 위한 설정 (iterCounts, minGradMagnitudes).

    TickMeter tm;
    Mat Rt;

    vector<int> iterCounts(4);
    iterCounts[0] = 7;
    iterCounts[1] = 7;
    iterCounts[2] = 7;
    iterCounts[3] = 10;

    vector<float> minGradMagnitudes(4);
    minGradMagnitudes[0] = 12;
    minGradMagnitudes[1] = 5;
    minGradMagnitudes[2] = 3;
    minGradMagnitudes[3] = 1;

    const float minDepth = 0.f; //in meters
    const float maxDepth = 4.f; //in meters
    const float maxDepthDiff = 0.07f; //in meters

⑤ 오도메트리를 추정하는 함수(Direct Method)와 변환 행렬의 표시

    tm.start();
    bool isFound = cv::RGBDOdometry(Rt, Mat(),
        grayImage0, depthFlt0, Mat(),
        grayImage1, depthFlt1, Mat(),
        cameraMatrix, minDepth, maxDepth, maxDepthDiff,
        iterCounts, minGradMagnitudes, transformationType);
    tm.stop();

    cout << "Rt = " << Rt << endl;
    cout << "Time = " << tm.getTimeSec() << " sec." << endl;

    if (!isFound)
    {
        cout << "Rigid body motion cann't be estimated for given RGBD data." << endl;
        return -1;
    }

⑥ Image0을 Image1로 변환한 이미지 warpedImage0을 표시하고 종료.

    Mat warpedImage0;
    warpImage<Point3_<uchar> >(colorImage0, depthFlt0, Rt, cameraMatrix, distCoeff, warpedImage0);

    imshow("image0", colorImage0);
    imshow("warped_image0", warpedImage0);
    imshow("image1", colorImage1);
    waitKey();

    return 0;
}

샘플 이미지 세트는 OpenCV에 있습니다.

\opencv249\sources\samples\cpp\rgbdodometry\

PPL로 병렬화 된 소스 코드을 github에 올립니다. 속도는 전혀 바뀌지 않습니다 w 실행할 때, Debugging의 설정 (커맨드 라인으로부터 인수를 준다)를 실시해 주세요.

C:\opencv249\sources\samples\cpp\rgbdodometry\image_00000.png C:\opencv249\sources\samples\cpp\rgbdodometry\depth_00000.png C:\opencv249\sources\samples\cpp\rgbdodometry\image_00002.png C:\opencv249\sources\samples\cpp\rgbdodometry\depth_00002.png -rbm

운영 환경

Windows8.1(RAM 8GB, 2 cores @ 2.1GHz)

OpenCV2.4.9

Visual Studio 2013

처리 시간

rgbdodometry

ppl로 병렬화 (이중 루프 정도는 속도는 변하지 않습니다)