CloudMonocular.cpp

/** * 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 <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <System.h>
#include <ARCHandler.h>

#include <opencv2/core/core.hpp>

using namespace std;

int main(int argc, char **argv)
{
    if (argc != 5)
    {
        cerr << endl
             << "Usage: ./cloud path_to_vocabulary path_to_settings path_to_sample_image num_of_images" << endl;
        return 1;
    }

    int nImages = atoi(argv[4]);

    // 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);

    // Initialize ARCHandler Stream
    std::cout << std::endl
              << "-------" << std::endl;
    std::cout << "Initializing ARCHandler Stream ..." << std::endl;

    cout << endl
         << "-------" << endl;
    cout << "Start processing ..." << endl;
    cout << "Images to be processed: " << nImages << endl
         << endl;

    // Main loop
    cv::Mat im;
    // Dummy to replace im. TODO: remove this
    im = cv::imread(argv[3], IMREAD_UNCHANGED);
    // nImages = 2;
    for (int ni = 0; ni < nImages; ni++)
    {
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();

        // Pass the image to the SLAM system
        SLAM.TrackMonocularRemote(im);

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

        vTimesTrack[ni] = ttrack;
    }

    // Stop all threads
    SLAM.Shutdown();

    // Save camera trajectory
    // SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

    // Get the stored map points as a tuple of length 3 (x, y, z)
    std::vector<ORB_SLAM2::MapPoint *> mapPoints = SLAM.GetMap()->GetAllMapPoints();
    std::vector<std::tuple<float, float, float>> mapPointsTuple;
    std::ofstream mapPointsFile("mapPoints.txt");

    for (ORB_SLAM2::MapPoint *mp : mapPoints)
    {
        cv::Mat pos = mp->GetWorldPos();
        mapPointsFile << pos.at<float>(0) << " " << pos.at<float>(1) << " " << pos.at<float>(2) << std::endl;
    }
    

    mapPointsFile.close();

    return 0;
}