카메라의 위치 및 자세 파악하기

카메라 캘리브레이션

실제 3차원 공간을 카메라로 찍게 되면 2차원의 이미지로 변하게 된다.

3차원의 점들이 이미지 상에서 어디에 맺히는지는 영상을 찍을 당시의 카메라의 위치 및 방향에 따라 결정된다.

즉 카메라의 위치와 방향이 중요!

실제 이미지에서 영향을 받는 요소

• 카메라의 사용된 렌즈
• 렌즈와 이미지 센서간의 거리
• 렌즈와 이미지 센서간의 각도

따라서 ,

• 3차원 점들이 영상에 투영된 위치를 구할때
• 반대로 영상좌표에서부터 3차원 공간좌표를 복구할 때

위의 동작을 수행 할 때 내부 요인을 제거해야만 정확한 계산이 가능해진다.

카메라에는 내부(intrinsic) 파라미터와 외부(extrinsic) 파라미터가 존재한다.

내부파라미터 (Instrinsic parameters)

카메라의 내부파라미터

• 초첨거리 Focal length : fx, fy
• 주점 Principal point : cx, cy
• 비대칭계수 Skew coefficient : skew_c = tana

이러한 계수가 있으며 표현은 다음 행렬식과 같다.

이러한 파라미터는 체크보드를 통해서 구해 낼수 있다.

위 체크보드는 세로 6 x 가로 9 의 crosspoint 체크보드이다.

체커보드를 다양한 각도에서 촬영하여 opencv의 cailibration 함수를 통해 내부파라미터와 외곡 파라미터를 얻을 수 있다.

카메라는 아래와 같이 L515 카메라를 사용

카메라와 월드좌표계간 포즈를 추출하기 위한 내부파라미터는 ros를 통해 구동

그중 /camera_info 토픽 메시지를 이용하여 확인하였다.

realsense 카메라의 컬러 이미지 정보를 받기

#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include <opencv2/opencv.hpp>   // Include OpenCV API

int main(int argc, char * argv[]) try
{
    // Declare depth colorizer for pretty visualization of depth data
    rs2::colorizer color_map;

    // Declare RealSense pipeline, encapsulating the actual device and sensors
    rs2::pipeline pipe;
    // Start streaming with default recommended configuration
    pipe.start();

    using namespace cv;
    const auto window_name = "Display Image";
    const auto color_name = "Display color_Image";

    namedWindow(window_name, WINDOW_AUTOSIZE);
    namedWindow(color_name, WINDOW_AUTOSIZE);

    while (waitKey(1) < 0 && getWindowProperty(window_name, WND_PROP_AUTOSIZE) >= 0)
    {
        rs2::frameset data = pipe.wait_for_frames(); // Wait for next set of frames from the camera
        rs2::frame depth = data.get_depth_frame().apply_filter(color_map);

        rs2::frame color = data.get_color_frame();

        // Query frame size (width and height)
        const int w = depth.as<rs2::video_frame>().get_width();
        const int h = depth.as<rs2::video_frame>().get_height();

        // Create OpenCV matrix of size (w,h) from the colorized depth data
        Mat image(Size(w, h), CV_8UC3, (void*)depth.get_data(), Mat::AUTO_STEP);

        const int color_w = color.as<rs2::video_frame>().get_width();
        const int color_h = color.as<rs2::video_frame>().get_height();

        Mat color_image(Size(color_w, color_h), CV_8UC3, (void*)color.get_data(), Mat::AUTO_STEP);
        cvtColor(color_image, color_image, COLOR_BGR2RGB); //rs 카메라의 색 형태를 변환

        // Update the window with new data
        imshow(window_name, image);
        imshow(color_name, color_image);

    }

    return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception& e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}

 

코드를 통해 이미지를 아래와 같이 확인 할 수 있다.

이제 이 이미지를 저장하여 월드 좌표계를 이미지에 투영 해야한다.

투영하기 위해서는 저장한 이미지의 해상도와 내부파라미터를 얻을 때 썻던 이미지의 해상도가 같아야한다.

월드 좌표계를 설정하기 위해서는 변하지않는 이미지의 고정 포인트를 잡아야한다.

따라서 이미지를 저장해본다. 코드는 아래와 같다.

#include <opencv2/opencv.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include <stdio.h>
#include <iostream>

int main(int argc, char * argv[]) try
{
    rs2::pipeline pipe;
    
    // cv::glob(path, images);
    // Start streaming with default recommended configuration
    pipe.start();

    const auto color_name = "Display color_Image";
    cv::namedWindow(color_name, cv::WINDOW_AUTOSIZE);

    cv::Mat color, gray;
    int i = 0;

    while (cv::getWindowProperty(color_name, cv::WND_PROP_AUTOSIZE) >= 0)
    {
        rs2::frameset data = pipe.wait_for_frames(); // Wait for next set of frames from the camera
        rs2::frame color_img = data.get_color_frame();

        const int w = color_img.as<rs2::video_frame>().get_width();
        const int h = color_img.as<rs2::video_frame>().get_height();

        // Create OpenCV matrix of size (w,h) from the colorized depth data
        cv::Mat frame(cv::Size(w, h), CV_8UC3, (void*)color_img.get_data(), cv::Mat::AUTO_STEP);
        cv::cvtColor(frame, color, cv::COLOR_BGR2RGB);

        // Update the window with new data
        cv::imshow(color_name, color);
        
        int k = cv::waitKey(1);
        if((k%256) == 27){
            std::cout << "Escape hit, closing...." << std::endl;
            break;
        }
        else if((k%256) == 32){
            cv::Mat save_img;
            cv::cvtColor(frame, save_img, cv::COLOR_BGR2RGB);
            std::string image_name = "image_" + std::to_string(i) + ".jpg";
            cv::imwrite(image_name, save_img);
            std::cout << "save the " << image_name << std::endl;
            i++;
        }
    }
    cv::destroyAllWindows();
    return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception& e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}

스페이스를 입력받으면 image_0.jpg 를 저장하게 된다. 저장된 이미지는 아래와 같다.

이제 이미지에 대한 월드 좌표계를 계산하기 위해 이미지의 x, y 포인트를 지정한다.

지정된 이미지의 x,y를 월드 좌표계로 대응하도록 작성하면 카메라 좌표와 월드좌표계의

rotation matrix와 translate matrix를 구할 수 있다.

우선 좌표를 지정하기 위한 코드이다.

#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <stdio.h>
#include <iostream>

void CallBackFunc(int event, int x, int y, int flags, void* userdata)
{
    if (event == cv::EVENT_LBUTTONDOWN)
    {
        std::cout << "왼쪽 마우스 버튼 클릭.. 좌표 = (" << x << ", " << y << ")" << std::endl;
    }
}

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

    cv::Mat img_original, img_gray;

    img_original = cv::imread("./image_0.jpg", cv::IMREAD_COLOR);
    if (img_original.empty())
    {
        std::cout << "Could not open or find the image" << std::endl;
        return -1;
    }
    
    // Display image.
    cv::imshow("Output", img_original);
    
    // cv::imshow("color_name", img_original);
    cv::setMouseCallback("Output", CallBackFunc, NULL);
    
    cv::waitKey(0);
    cv::destroyAllWindows();

    return EXIT_SUCCESS;
}

이와 같이 코드를 작성하여

위와 같은 좌표를 얻을 수 있다.

각 좌표에 대한 월드 좌표계의 포인트를 지정하여 정한다.

체크보드 하나의 격자를 1로 잡고 중심점을 (0, 0, 0)으로 하여 월드좌표계를 구성하였다.

#include <opencv2/opencv.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include <stdio.h>
#include <iostream>

void CallBackFunc(int event, int x, int y, int flags, void* userdata)
{
    if (event == cv::EVENT_LBUTTONDOWN)
    {
        std::cout << "왼쪽 마우스 버튼 클릭.. 좌표 = (" << x << ", " << y << ")" << std::endl;
    }
}

int main(int argc, char * argv[]) try
{
    rs2::pipeline pipe;
    
    std::vector<cv::Point3f> objectPoints;
    objectPoints.push_back( cv::Point3f(-1, 1, 0));    
    objectPoints.push_back( cv::Point3f(1, 1, 0));  
    objectPoints.push_back( cv::Point3f(0, 0, 0));  
    objectPoints.push_back( cv::Point3f(-1, -1, 0));  
    objectPoints.push_back( cv::Point3f(1, -1, 0));  

    std::vector<cv::Point2f> imagePoints;
    imagePoints.push_back( cv::Point2f(611, 514));    
    imagePoints.push_back( cv::Point2f(675, 512));    
    imagePoints.push_back( cv::Point2f(645, 533));    
    imagePoints.push_back( cv::Point2f(615, 555));    
    imagePoints.push_back( cv::Point2f(682, 552));    

    // camera parameter
    double m[] = {899.929443359375, 0.0, 649.4876708984375, 
                  0.0, 899.6634521484375, 370.26641845703125, 
                  0.0, 0.0, 1.0};
    cv::Mat K(3, 3, CV_64FC1, m);
    
    // distortion param. For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3).
    // {0.1281878501176834, -0.43850672245025635, -0.0010290791979059577, -0.0008640193846076727, 0.39977091550827026}
    double d[] = {0.1281878501176834, -0.43850672245025635, -0.0010290791979059577, -0.0008640193846076727};
    cv::Mat distCoeffs(4, 1, CV_64FC1, d);

    std::cout << "Camera Matrix " << std::endl << K << std::endl ;

    cv::Mat img_original, img_gray;

    img_original = cv::imread("./image00.jpg", cv::IMREAD_COLOR);
    if (img_original.empty())
    {
        std::cout << "Could not open or find the image" << std::endl;
        return -1;
    }
    
    cv::Mat rvec, tvec;
    cv::solvePnP(objectPoints, imagePoints, K, distCoeffs, rvec, tvec);
    
    
    std::vector<cv::Point3d> end_point3D;
    std::vector<cv::Point2d> end_point2D;
    end_point3D.push_back(cv::Point3d(0,0,1));
    
    projectPoints(end_point3D, rvec, tvec, K, distCoeffs, end_point2D);
 
    for(int i=0; i < imagePoints.size(); i++)
    {
        cv::circle(img_original, imagePoints[i], 3, cv::Scalar(0,0,255), -1);
    }
 
    cv::line(img_original, imagePoints[2], end_point2D[0], cv::Scalar(255,0,0), 2);
 
    std::cout << "Rotation Vector " << std::endl << rvec << std::endl;
    std::cout << "Translation Vector" << std::endl << tvec << std::endl;
    std::cout <<  end_point2D << std::endl;

    cv::Mat R, T;
    cv::Rodrigues(rvec, R);
    T = tvec;

    std::cout << "Rotation Vector " << std::endl << R << std::endl;
 
    // Display image.
    cv::imshow("Output", img_original);
    
    // cv::imshow("color_name", img_original);
    // cv::setMouseCallback("color_name", CallBackFunc, NULL);
    
    
    int k = cv::waitKey(0);
    if((k%256) == 27){
        std::cout << "Escape hit, closing...." << std::endl;
    }
    cv::destroyAllWindows();

    return EXIT_SUCCESS;
} catch (const rs2::error & e)
{
    std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\\n    " << e.what() << std::endl;
    return EXIT_FAILURE;
}
catch (const std::exception& e)
{
    std::cerr << e.what() << std::endl;
    return EXIT_FAILURE;
}

이렇게해서 카메라와 월드좌표계간의 Translation 과 Rotation 구할 수 있다.