입문 Visual SLAM 제 5장 실습 코드 중국어 주석 번역

출처 : https://docs.google.com/document/d/1WXqqtUF2L7t6v9-qC6zXF48Muog2Y7-lEc6ojXHaTHY/

입문 Visual SLAM 14강 (제5장)

제 5장 내용은 핀홀 카메라 모델과 양안, RGB-D 카메라 모델에 대한 설명이 나오고, 이미지 왜곡에 대한 설명도 나온다.

 

제 4장을 공부했을 때에는 수식에 대한 내용이 많아 시간이 오래걸렸는데, 5장은 4장보다는 난이도가 조금 쉬웠다.

 

실습코드의 경우 ch5/imageBasics와 ch5/joinMap 두 가지로 나뉘어진다.

 

먼저 ch5/imageBasics는 다음과 같다.

 

내용

- 이미지를 읽는 데 걸리는 시간을 출력

- 왼쪽 상단 모서리에 있는 (100,100)까지의 블록을 검정색으로 만든다.(이미지 변경)

- 왼쪽 상단 모서리에 있는 (100,100)까지의 블록을 흰색으로 만든다.(이미지 복사이므로 새 window 생성)

 

 

실행 시 인자로 ubuntu.png가 존재하는 경로를 넣어줘야 한다.

 

#include <iostream>
#include <chrono>
using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

int main ( int argc, char** argv )
{
    // argv[1]로 지정된 이미지 읽기
    cv::Mat image;
    image = cv::imread ( argv[1] ); //cv::imread 함수는 지정된 경로에서 이미지를 읽습니다.
    // 이미지 파일이 올바르게 읽혀졌는지 확인
    if ( image.data == nullptr ) //데이터가 존재하지 않습니다. 파일이 존재하지 않을 수 있습니다.
    {
        cerr<<"문서"<<argv[1]<<"존재하지 않는다."<<endl;
        return 0;
    }
    
    // 파일을 성공적으로 읽었습니다. 먼저 몇 가지 기본 정보를 출력합니다.
    cout<<"이미지 너비는"<<image.cols<<",높이"<<image.rows<<",채널 수는"<<image.channels()<<endl;
    cv::imshow ( "image", image );      // cv::imshow로 이미지 표시
    cv::waitKey ( 0 );                  // 프로그램을 일시 중지하고 키 입력을 기다립니다.
    // 이미지 유형 결정
    if ( image.type() != CV_8UC1 && image.type() != CV_8UC3 )
    {
        // 이미지 유형이 요구 사항을 충족하지 않습니다.
        cout<<"컬러 또는 회색조 이미지를 입력하세요."<<endl;
        return 0;
    }

    // 이미지 순회. 다음 순회 방법은 임의 픽셀 액세스에도 사용할 수 있습니다.
    // 시간 알고리즘에 std::chrono 사용
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    for ( size_t y=0; y<image.rows; y++ )
    {
        // cv::Mat::ptr을 사용하여 이미지의 행 포인터를 가져옵니다.
        unsigned char* row_ptr = image.ptr<unsigned char> ( y );  // row_ptr은 행 y의 헤드 포인터입니다.
        for ( size_t x=0; x<image.cols; x++ )
        {
            // x,y에서 픽셀에 액세스
            unsigned char* data_ptr = &row_ptr[ x*image.channels() ]; // data_ptr은 액세스할 픽셀 데이터를 가리킵니다.
            // 픽셀의 각 채널을 출력합니다. 그레이 스케일 이미지인 경우 채널이 하나만 있습니다.
            for ( int c = 0; c != image.channels(); c++ )
            {
                unsigned char data = data_ptr[c]; // 데이터는 I(x,y)의 c번째 채널 값입니다.
            }
        }
    }
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"이미지를 가로지르는 시간："<<time_used.count()<<" 초."<<endl;

    // cv::Mat 복사에 대하여
    // 직접 할당은 데이터를 복사하지 않습니다.
    cv::Mat image_another = image;
    // image_another를 수정하면 이미지가 변경됩니다.
    image_another ( cv::Rect ( 0,0,100,100 ) ).setTo ( 0 ); // 왼쪽 상단 모서리에 있는 100*100 블록을 0으로 만듭니다.
    cv::imshow ( "image", image );
    cv::waitKey ( 0 );
    
    // 복제 기능을 사용하여 데이터 복사
    cv::Mat image_clone = image.clone();
    image_clone ( cv::Rect ( 0,0,100,100 ) ).setTo ( 255 );
    cv::imshow ( "image", image );
    cv::imshow ( "image_clone", image_clone );
    cv::waitKey ( 0 );

    // 잘라내기, 회전, 스케일링 등 이미지에 대한 기본 연산이 많지만 지면의 제약으로 일일이 소개하지 않았으며, 각 함수의 호출 방법은 공식 OpenCV 문서를 참조하시기 바랍니다.
    cv::destroyAllWindows();
    return 0;
}

 

실행 결과

키보드를 하나씩 입력하면 출력 화면이 변한다.

 

첫 번째 출력 화면

두 번째 출력 화면

왼쪽 상단 (100,100)까지의 블록이 검정색으로 변했다.

세 번째 출력 화면

왼쪽 상단 (100,100)까지의 블록이 흰색으로 변했다.

 

두 번째 실습 코드 ch5/joinMap은 다음과 같다.

코드 실행에 필요한 디펜던시를 설치하는 법은 책에 나와있으므로 참고 바랍니다.

 

#include <iostream>
#include <fstream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry> 
#include <boost/format.hpp>  // for formating strings
#include <pcl/point_types.h> 
#include <pcl/io/pcd_io.h> 
#include <pcl/visualization/pcl_visualizer.h>

int main( int argc, char** argv )
{
    vector<cv::Mat> colorImgs, depthImgs;    // 컬러맵과 뎁스맵
    vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> poses;         // 카메라 포즈
    
    ifstream fin("./pose.txt");
    if (!fin)
    {
        cerr<<"pose.txt가 있는 디렉토리에서 이 프로그램을 실행하십시오."<<endl;
        return 1;
    }
    
    for ( int i=0; i<5; i++ )
    {
        boost::format fmt( "./%s/%d.%s" ); //이미지 파일 형식
        colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
        depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 원시 이미지를 읽으려면 -1을 사용하십시오.
        
        double data[7] = {0};
        for ( auto& d:data )
            fin>>d;
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
        Eigen::Isometry3d T(q);
        T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
        poses.push_back( T );
    }
    
    // 포인트 클라우드 및 스티치 계산
    // 카메라 참조
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;
    
    cout<<"이미지를 포인트 클라우드로 변환..."<<endl;
    
    // 포인트 클라우드에서 사용되는 형식을 정의합니다. 여기서는 XYZRGB가 사용됩니다.
    typedef pcl::PointXYZRGB PointT; 
    typedef pcl::PointCloud<PointT> PointCloud;
    
    // 새 포인트 클라우드 생성
    PointCloud::Ptr pointCloud( new PointCloud ); 
    for ( int i=0; i<5; i++ )
    {
        cout<<"이미지 변환: "<<i+1<<endl; 
        cv::Mat color = colorImgs[i]; 
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];
        for ( int v=0; v<color.rows; v++ )
            for ( int u=0; u<color.cols; u++ )
            {
                unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 깊이 값
                if ( d==0 ) continue; // 0은 측정되지 않음을 의미합니다.
                Eigen::Vector3d point; 
                point[2] = double(d)/depthScale; 
                point[0] = (u-cx)*point[2]/fx;
                point[1] = (v-cy)*point[2]/fy; 
                Eigen::Vector3d pointWorld = T*point;
                
                PointT p ;
                p.x = pointWorld[0];
                p.y = pointWorld[1];
                p.z = pointWorld[2];
                p.b = color.data[ v*color.step+u*color.channels() ];
                p.g = color.data[ v*color.step+u*color.channels()+1 ];
                p.r = color.data[ v*color.step+u*color.channels()+2 ];
                pointCloud->points.push_back( p );
            }
    }
    
    pointCloud->is_dense = false;
    cout<<"공유 포인트 클라우드"<<pointCloud->size()<<"포인트들."<<endl;
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
    return 0;
}

 

실행 결과