main.cpp

#include <iostream>
#include <string>
#include <random>    
#include <torch/torch.h>
#include <opencv2/opencv.hpp>

#include "utils.hpp"
#include "viewer.hpp"
#include "data_loader.hpp"
#include "visual_slam.hpp"


int main() {
    try {
        // Set fixed seed for reproducible results
        utils::setFixedSeed();
        
        // Parse command line arguments
        std::string dataset_folder = "/home/saqib/visual_slam/visual_odometry/stereo_rgb";
        std::string config_path = "/home/saqib/visual_slam/visual_odometry/config.yaml";
        
        std::cout << "Visual Odometry Demo Application" << std::endl;
        std::cout << "Dataset: " << dataset_folder << std::endl;
        
        // Initialize data loader
        DataLoader data_loader(dataset_folder);
        VisualSLAM visual_slam(config_path);
        Viewer viewer;

        std::thread viewer_thread(&Viewer::run3DViewer, &viewer);
        
        // Main processing loop
        cv::Mat left_img, right_img;
        size_t frame_count = 0;
        
        std::cout << "Starting image sequence playback..." << std::endl;
        std::cout << "Press 'q' to quit" << std::endl;
        
        while (data_loader.hasMoreFrames()) {
            if (!data_loader.loadNextFrame(left_img, right_img)) {
                std::cerr << "Failed to load frame " << frame_count << std::endl;
                continue;
            }

            const auto& slam_map = visual_slam.processFrame(left_img, right_img, frame_count);

            // print the pose of the last frame
            const auto& last_frame = slam_map.get_LastFrame();
            cv::Vec3f translation(last_frame.pose.at<float>(0,3), last_frame.pose.at<float>(1,3), last_frame.pose.at<float>(2,3));
            std::cout << "Frame " << frame_count << " pose: " << translation << std::endl;

            // viewer.update will return false if user pressed 'q' to quit
            if (!viewer.update(slam_map)) {
                std::cout << "User requested exit." << std::endl;
                break;
            }
            
            frame_count++;
        }

        std::cout << "\nPlayback completed. Processed " << frame_count << " frames." << std::endl;
        
        // Signal viewer to stop by setting running_ to false
        {
            std::lock_guard<std::mutex> lock(viewer.getMutex());
            viewer.setRunning(false);
        }
        
        // Wait for thread to finish
        if (viewer_thread.joinable()) {
            viewer_thread.join();
        }
        
        // Cleanup
        cv::destroyAllWindows();
        return 0;
        
    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << std::endl;
        return 1;
    }
}