ORB SLAM 웹캠 코드

오랜만에 글을 써볼 차례이다.

맨날 한다한다 하고 안하는거같아서 마음잡고 후딱 웹캠코드를 짜보려한다.

 

일단 코드는 저의 깃허브에 올려놓았으니 사용하시면 될거 같습니다.

https://github.com/JongBeomBaek/ORB_SLAM2_WEBCAM

 

JongBeomBaek/ORB_SLAM2_WEBCAM

 

집에 보통은 뎁스카메라가 없기 때문에 monocular(single) 카메라로 

실시간 웹캠 코드를 작성하려고한다.

---

코드를 설명하기 전에 준비해야할 것들을 알려드리고자한다.

1. 카메라 세팅

2. 캘리브레이션 된 intrinsic parameter  파일

 

1. 카메라 세팅

카메라를 연결됬는지 확인하는 방법은 터미널에 

 

$ ls /dev/video*

명령어를 입력해보면 카메라가 잡히는대로

/dev/video0    이렇게 잡히는걸 볼 수 있을 것이다. (여러개면 뒤에 0말고 숫자가 더 커지면서 잡힘)

 

2. 캘리브레이션 된 intrinsic parameter  파일

카메라마다  내부 파라미터가 다르다 이 개념은 이야기하자면 길어지므로 인터넷에 캘리브레이션 코드를 사용해서

yaml 파일을 만들어야한다.

 

https://robot-vision-develop-story.tistory.com/5

ORB SLAM2 설치 및 실행

이전 글에서도 보시면 실행시킬때 Examples/Monocular/TUM1.yaml

이 들어간걸 확인 할 수 있다.

 

이파일을 열어보면  focal length부터 lens distortion 파라미터까지

다 들어있다.

 

이렇게 준비가 되었다면 이제 본격적으로 바뀐 코드를 알아보러 가볼까요~

---

크게 달라진부분은 

ORB_SLAM2/Example/Monocular/mono_tum.cc  코드이다.

(나중에 stereo 나 RGB-D를 사용하고싶으시면 example 에서 다른폴더에 .cc파일을 응용해서 건드리면 된다.)

 

//Main loop 라고 되있는 코드인데

이미지 시퀀스를 가져와서 저장된 타임스탬프 대로 이미지를 불러오는 방식이다.

(실제 찍은 환경에서 사진사이의 시간 간격을 모르기 때문에) 

이것을 웹캠을 불러오는 식으로 사용하려고한다.

 

먼저말씀드릴것은 저는 타임스탬프를 실시간으로 활용하려고하기때문에 

리눅스에 있는 monotonic time을 사용하려고 한다.

(그냥 컴퓨터 부팅부터 시작하는 시계라고 생각하면 된다.

알고리즘이 성능 평가가 아니기 때문에  프로세서 타임이 아닌 실제 타임으로 빌드해야한다.)

 

이부분에 대한 코드는 사실 원래 코드에도있는데 

시퀀스를 맞추기 위해 sleep할 시간을 정하려고 들어가 있다.

 

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif

이부분인데 둘다 같은 함수인데 시스템 dependency에 따라 사용하는 함수가 다르기 때문이다.

 

 

---

 

나머지는 그냥 보셔도 이해가 될정도로 크게 달라진 부분이 없을 것이다.

아마 인터넷에 떠도는 cpp opencv  웹캠 예제에서 ORB SLAM 코드만 추가된 것일 뿐이다.

 

/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/


#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>

#include<opencv2/core/core.hpp>

#include<System.h>

#define _WEBCAM_BUILD_


using namespace std;

void LoadImages(const string &strFile, vector<string> &vstrImageFilenames,
                vector<double> &vTimestamps);

int main(int argc, char **argv)
{
#ifdef _WEBCAM_BUILD_
    if(argc != 3)
    {
        cerr << endl << "argc:" << argc << "!= 3"<< endl;
    }

    cv::VideoCapture cap(0);

    if (!cap.isOpened()) {
        cerr << endl << "Could not open camera feed." << endl;
        return -1;
    }
    ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true);
    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;

#ifdef COMPILEDWITHC11
    std::chrono::steady_clock::time_point initT = std::chrono::steady_clock::now();
#else
    std::chrono::monotonic_clock::time_point initT = std::chrono::monotonic_clock::now();
#endif

    // Main loop
    while(true)//cv::waitKey(0) != 27)
    {
        //Create a new Mat
        cv::Mat frame;

        //Send the captured frame to the new Mat
        cap >> frame;

        if(frame.empty())
            break;
#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point nowT = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point nowT = std::chrono::monotonic_clock::now();
#endif
        // Pass the image to the SLAM system
        SLAM.TrackMonocular(frame, std::chrono::duration_cast<std::chrono::duration<double> >(nowT-initT).count());
    }
    // Stop all threads
    SLAM.Shutdown();

    //slam->SaveSeperateKeyFrameTrajectoryTUM("KeyFrameTrajectory-1.txt", "KeyFrameTrajectory-2.txt", "KeyFrameTrajectory-3.txt");
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

#else
    if(argc != 4)
    {
        cerr << endl << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_sequence" << endl;
        return 1;
    }

    // Retrieve paths to images
    vector<string> vstrImageFilenames;
    vector<double> vTimestamps;
    string strFile = string(argv[3])+"/rgb.txt";
    LoadImages(strFile, vstrImageFilenames, vTimestamps);

    int nImages = vstrImageFilenames.size();

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

    // Vector for tracking time statistics
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);

    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;
    cout << "Images in the sequence: " << nImages << endl << endl;

    ORB_SLAM2::System SLAM(argv[1], argv[2], ORB_SLAM2::System::MONOCULAR, true);

    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;

    // Main loop
    cv::Mat im;
    for(int ni=0; ni<nImages; ni++)
    {
        // Read image from file
        im = cv::imread(string(argv[3])+"/"+vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];

        if(im.empty())
        {
            cerr << endl << "Failed to load image at: "
                 << string(argv[3]) << "/" << vstrImageFilenames[ni] << endl;
            return 1;
        }

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif

        // Pass the image to the SLAM system
        SLAM.TrackMonocular(im,tframe);

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif

        double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

        vTimesTrack[ni]=ttrack;

        // Wait to load the next frame
        double T=0;
        if(ni<nImages-1)
            T = vTimestamps[ni+1]-tframe;
        else if(ni>0)
            T = tframe-vTimestamps[ni-1];

        if(ttrack<T)
            usleep((T-ttrack)*1e6);
    }

    // Stop all threads
    SLAM.Shutdown();

    // Tracking time statistics
    sort(vTimesTrack.begin(),vTimesTrack.end());
    float totaltime = 0;
    for(int ni=0; ni<nImages; ni++)
    {
        totaltime+=vTimesTrack[ni];
    }
    cout << "-------" << endl << endl;
    cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
    cout << "mean tracking time: " << totaltime/nImages << endl;

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
#endif
    return 0;
}

void LoadImages(const string &strFile, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
    ifstream f;
    f.open(strFile.c_str());

    // skip first three lines
    string s0;
    getline(f,s0);
    getline(f,s0);
    getline(f,s0);

    while(!f.eof())
    {
        string s;
        getline(f,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            double t;
            string sRGB;
            ss >> t;
            vTimestamps.push_back(t);
            ss >> sRGB;
            vstrImageFilenames.push_back(sRGB);
        }
    }
}

 

여러분의 시간은 소중하니까 바뀐 코드 부분만 복붙할 수 있도록 여기에도 올려봅니다~

mono_tum.cc 에 복붙하시면 되요~

 

#define _WEBCAM_BUILD_
이부분만 주석하면 원래 이미지 시퀀스로 돌리는 파일이다.

 

리눅스에서 c++코드로 좋은 IDE로 Qt가 있는데 cmake 파일을 불러와 

비쥬얼 스튜디오에서 솔루션 파일읽듯 전체 프로그램을 불러와주고 빌드도 쉽게 된다.

(활용 권장~ ROS하시려는분은 패키지 달린 Qt로 깔아요 TMI..)

 

없으신 분들은 이전 글에서 나온 명령어인

 

$ chmod +x build.sh

$ sh build.sh

만 해도 된다.

 

마지막으로 실행 명령어 ORB_SLAM2 폴더에서

$ ./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt (여러분이 캘리브레이션한 yaml파일)

만약에 파일이 없는 경우 

일단은 기존에 있던 파일로 한다.

 

$ ./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt ./Examples/Monocular/TUM1.yaml

 

단! 참고해야할 점은 ORB 슬램 자체가 ORB 피쳐를 통한 Sparse matching으로

깊이 값을 알아 내는 것이기 때문에 초기에 세팅하는 initialization 과정부터 잘 안될 수 있다.

intrinsic 파라미터가 다르니까 ㅠㅠ...

 

위 사진은 테이블 위에서 테스트 해본 사진이다.

카메라 모델과 다른 calibration파일을 사용해서 초기 initializing이 잘안되기 때문에

feature를 뽑기 좋은 키보드 위에서 테스트 해보았다.