Camera 이미지 좌표계

최근 이미지 데이터를 가지고 뭘 해야되는 상황이 생겨서 관련 내용을 정리하려 한다.
주로 공부한 부분은 좌표계와 변환에 관한 내용이다.

공부한 내용은 구글 검색과 다크 프로그래머님의 블로그를 많이 참조하였다(그저 갓갓..).

카메라와 월드 좌표계

이미지에서 사용하는 좌표계는 라이다에서와 다른 점은 가장 크게는 2가지인것 같다.
1. 2차원이다.
2. 좌측 상단 꼭짓점이 원점이다.

...

Code

라이다 포인트 클라우드를 이미지에 투영하는걸 해보고 있는데, 가장 큰 문제는 두개 센서가 어떻게 붙어있는지 위치를 모른다.
즉, 노가다로 끼워 맞추면서 찾는 과정을 진행하고 있다...

int main(int argc, char** argv) 
{
	// PointCloud 데이터 불러오기
    pcl::PointCloud<pcl::PointXYZI>::Ptr frame(new pcl::PointCloud<pcl::PointXYZI>);
    reader.read("***.pcd", *frame);
    
	// 라이다 포인트 클라우드를 projection할 이미지 불러오기
    cv::Mat labeled_image = cv::imread("***.png");
    // Intrinsic parameter를 알고있기 때문에 image resize가 필요하다
    cv::resize(labeled_image, labeled_image, cv::Size(2028, 1520));
    
    // camera parameter를 읽어오는 함수
    read_camera_param();

	// camera img 처리의 가장 기본이라는 undistort. 기울어진 이미지를 평형하게(?) 맞춰주는 과정이었다
    cv::Mat undistortedImage_0;
    cv::undistort(labeled_image, undistortedImage_0, cameraMatrix_0, distortionCoeffs_0);
    
    // 노가다 param1
    double tr_x = 0.0;
    double tr_y = 0.0;
    double tr_z = 0.0;
    // 라이다의 좌표계를 월드 좌표계라고 가정하는 경우, 카메라의 상대위치를 알고 싶다.
    // 라이다 -> 월드 -> 카메라의 순서라면 월드2카메라의 행렬은 카메라2월드의 역행렬
    cv::Mat c2w = (cv::Mat_<double>(4,4) << 0, 0, 1, tr_x, -1, 0, 0, tr_y, 0, -1, 0, tr_z, 0, 0, 0, 1);
    cv::Mat w2c;
    cv::invert(c2w, w2c);

	// 노가다 param2
    double deg_x = 0;
    double deg_y = 0;
    double deg_z = 0;

	// 카메라 좌표에 맞도록 라이다 포인트 변환
    std::vector<cv::Point3f> lidar_points_to_camera;
    for(const auto& lidar_point : frame->points)
    {
        cv::Mat lidar_point_mat = (cv::Mat_<double>(4, 1) << lidar_point.x, lidar_point.y, lidar_point.z, 1.0);
        cv::Mat transformed_point_mat = get_ratation_matrix_z(*) * get_ratation_matrix_x(*) * get_ratation_matrix_y(*) * w2c * lidar_point_mat;
        cv::Point3f transformed_point
        (
            transformed_point_mat.at<double>(0, 0),
            transformed_point_mat.at<double>(1, 0),
            transformed_point_mat.at<double>(2, 0)
        );

		// 이미지 기준 전방에 해당하는 포인트들만 투영
        if(transformed_point.z > 0)
            lidar_points_to_camera.push_back(transformed_point);
    }
    std::cout << "vector points : " << lidar_points_to_camera.size() << std::endl;

    std::vector<cv::Point2f> imagePoints;
    cv::projectPoints(lidar_points_to_camera, cv::Vec3d(0, 0, 0), cv::Vec3d(0, 0, 0), cameraMatrix_0, distortionCoeffs_0, imagePoints);

    for (const auto& imagePoint : imagePoints)
    {
        cv::circle(undistortedImage_0, imagePoint, 3, cv::Scalar(255, 0, 255), -1);
    }

    cv::imwrite("/home/dj/paper_ws/src/data_maker/projection_test.png", undistortedImage_0);
    return 0;
}

Reference

https://darkpgmr.tistory.com/category/%EC%98%81%EC%83%81%EC%B2%98%EB%A6%AC