main.cpp

#include <iostream>
#include <opencv2/opencv.hpp>
#include <unordered_set>
#include <vector>

#include "FeatureTracker.hpp"
#include "MapInitializer.hpp"
#include "MapManager.hpp"
#include "PerspectiveVisualizer.hpp"
#include "PoseEstimator.hpp"
#include "Visualizer.hpp"

using namespace std;
using namespace cv;

static cv::Scalar getLandmarkColor(const cv::Point3f &pt) {
  const float room_hw = 3.0f;
  const float room_d = 5.0f;
  const float room_hh = 2.0f;

  if (pt.z > room_d - 0.1f)
    return cv::Scalar(255, 255, 255); // front wall
  if (pt.z < 0.1f)
    return cv::Scalar(255, 200, 50); // back wall (cyan)
  if (pt.x < -room_hw + 0.1f)
    return cv::Scalar(50, 255, 255); // left wall (yellow)
  if (pt.x > room_hw - 0.1f)
    return cv::Scalar(50, 255, 50); // right wall (green)
  if (pt.y > room_hh - 0.1f)
    return cv::Scalar(150, 150, 150); // floor (grey)
  if (pt.y < -room_hh + 0.1f)
    return cv::Scalar(100, 100, 200); // ceiling (blue-grey)
  return cv::Scalar(50, 150, 255);    // table cluster (orange)
}

int main(int argc, char **argv) {
  cout << "=========================================================" << endl;
  cout << "      Modular Monocular Visual SLAM from Scratch" << endl;
  cout << "=========================================================" << endl;

  // 1. Default video path
  string video_path =
      "../../../temp_videos/ReadySetGo_1920x1080_30fps_x264_1200k.mp4";
  if (argc > 1) {
    video_path = argv[1];
  }

  VideoCapture cap(video_path);
  if (!cap.isOpened()) {
    cerr << "ERROR: Could not open video file: " << video_path << endl;
    return -1;
  }

  // Camera Intrinsic Matrix K for iPhone 15 Pro main (1x) camera
  // at 800x450 processing resolution.
  // Derived: 24mm equiv focal length → ~73.7° horizontal FoV
  //   fx = (800/2) / tan(73.7°/2) ≈ 531 px
  //   cx = 400 (image centre), cy = 225 (image centre)
  Mat K = (Mat_<double>(3, 3) << 531.0, 0.0, 400.0, 0.0, 531.0, 225.0, 0.0, 0.0,
           1.0);

  // 3. Initialize SLAM Modules
  // MapManager: translation threshold auto-tunes to scene depth; no rotation
  // trigger.
  FeatureTracker tracker(1000, 4.0);
  MapManager map_manager(0.3, 0.25);
  Visualizer visualizer;
  PerspectiveVisualizer perspective_visualizer;

  vector<Mat> trajectory; // Camera centres in world space

  // 4. Map Bootstrapping Phase
  cout << "System Bootstrapping: Tracking features to initialize 3D map..."
       << endl;
  Mat frame1;
  if (!cap.read(frame1)) {
    cerr << "ERROR: Failed to read first frame." << endl;
    return -1;
  }

  Mat display_frame1, gray1;
  double scale = 800.0 / frame1.cols;
  resize(frame1, display_frame1, Size(), scale, scale);
  cvtColor(display_frame1, gray1, COLOR_BGR2GRAY);

  vector<Point2f> points_init;
  tracker.processFrame(display_frame1, points_init);

  Mat prev_gray = gray1.clone();
  vector<Point2f> points_prev = points_init;

  Mat R_init = Mat::eye(3, 3, CV_64F);
  Mat t_init = Mat::zeros(3, 1, CV_64F);

  bool is_initialized = false;
  int bootstrap_frames = 30;
  int current_frame_id = 1;

  for (int i = 0; i < bootstrap_frames; ++i) {
    Mat frame;
    if (!cap.read(frame)) {
      cerr << "ERROR: Video ended before initialization completed." << endl;
      return -1;
    }
    current_frame_id++;

    Mat display_frame, gray;
    resize(frame, display_frame, Size(), scale, scale);
    cvtColor(display_frame, gray, COLOR_BGR2GRAY);

    vector<Point2f> points_next;
    vector<uchar> status;
    vector<float> err;
    calcOpticalFlowPyrLK(prev_gray, gray, points_prev, points_next, status, err,
                         Size(21, 21), 3);

    // Forward-Backward tracking check (same as test_slam_adaptive)
    vector<Point2f> points_back;
    vector<uchar> status_back;
    vector<float> err_back;
    calcOpticalFlowPyrLK(gray, prev_gray, points_next, points_back, status_back,
                         err_back, Size(21, 21), 3);
    for (size_t j = 0; j < status.size(); ++j) {
      if (status[j]) {
        if (!status_back[j] ||
            cv::norm(points_prev[j] - points_back[j]) > 2.0) {
          status[j] = 0;
        }
      }
    }

    vector<Point2f> good_prev, good_next;
    for (size_t j = 0; j < points_prev.size(); ++j) {
      if (status[j] && points_next[j].x >= 0 && points_next[j].x < gray.cols &&
          points_next[j].y >= 0 && points_next[j].y < gray.rows) {
        // Guard: only index points_init if j is still in range
        if (j < points_init.size())
          good_prev.push_back(points_init[j]);
        good_next.push_back(points_next[j]);
      }
    }
    points_init = good_prev;
    points_prev = good_next;

    // Reuse gray buffer via swap (avoids allocation)
    std::swap(prev_gray, gray);

    // Overlay status during initialization
    vector<bool> dummy_mask(points_prev.size(), false);
    visualizer.draw2D(display_frame, points_prev, dummy_mask, current_frame_id);
    waitKey(1);
  }

  // Run MapInitializer to bootstrap 3D coordinates.
  // Using strict mode (same as test_slam_adaptive): 1.0px RANSAC, 30 inliers.
  // If bootstrap fails the opening scene needs non-planar features (floor+wall).
  MapInitializer initializer(K, /*real_camera=*/false);
  vector<Point3f> points3D;
  vector<bool> inliers_init;
  Mat R_relative, t_relative;

  is_initialized = initializer.initialize(points_init, points_prev, R_relative,
                                          t_relative, points3D, inliers_init);

  if (!is_initialized) {
    cerr << "ERROR: Bootstrapping failed. Paradigm requires larger camera "
            "translation."
         << endl;
    return -1;
  }

  // Populate the first keyframe (Origin)
  vector<int> initial_3d_indices(points_init.size(), -1);
  vector<int> bootstrap_ids(points_init.size());
  for (size_t i = 0; i < points_init.size(); ++i) {
    bootstrap_ids[i] = static_cast<int>(i);
    if (inliers_init[i]) {
      int pt_idx = map_manager.addMapPoint(points3D[i]);
      initial_3d_indices[i] = pt_idx;
    }
  }

  // Add Keyframe 0 (Origin)
  map_manager.addKeyframe(gray1, R_init, t_init, points_init,
                          initial_3d_indices, bootstrap_ids);

  // Replenish features at frame 30 (Keyframe 1) — seed from points_prev
  // so existing IDs carry over (critical for Jaccard similarity)
  std::vector<Point2f> points_pool = points_prev;
  tracker.processFrame(prev_gray, points_pool);

  int next_feature_id = static_cast<int>(points3D.size());
  for (size_t i = points_prev.size(); i < points_pool.size(); ++i) {
    points_prev.push_back(points_pool[i]);
    points3D.push_back(cv::Point3f(0, 0, 0)); // placeholder
    inliers_init.push_back(false);
    initial_3d_indices.push_back(-1);
    bootstrap_ids.push_back(next_feature_id++);
  }

  // ── Re-estimate KF1 pose with PnP to fix scale inconsistency ────────────
  // t_relative from recoverPose is unit-norm; PnP gives metric scale.
  // Without this, triangulated 3D points land outside the walls.
  {
    PoseEstimator boot_estimator(K);
    std::vector<cv::Point3f> boot_3d;
    std::vector<cv::Point2f> boot_2d;
    for (size_t i = 0; i < points_prev.size(); ++i) {
      if (i < inliers_init.size() && inliers_init[i] &&
          initial_3d_indices[i] != -1) {
        boot_3d.push_back(points3D[i]);
        boot_2d.push_back(points_prev[i]);
      }
    }
    if (boot_3d.size() >= 8) {
      cv::Mat R_kf1, t_kf1;
      std::vector<int> kf1_inliers;
      if (boot_estimator.estimatePose(boot_3d, boot_2d, R_kf1, t_kf1,
                                      kf1_inliers)) {
        R_relative = R_kf1;
        t_relative = t_kf1;
        cout << "[SLAM] Bootstrap KF1 re-estimated via PnP ("
             << kf1_inliers.size() << " inliers). Metric scale applied."
             << endl;
      } else {
        cout << "[SLAM] Warning: PnP re-estimation of KF1 failed; using "
             << "unit-scale t_relative (may cause triangulation issues)."
             << endl;
      }
    }
  }

  // Add Keyframe 1 (Relative Translation)
  map_manager.addKeyframe(prev_gray, R_relative, t_relative, points_prev,
                          initial_3d_indices, bootstrap_ids);

  trajectory.push_back(t_init.clone());
  trajectory.push_back(-R_relative.t() * t_relative);

  vector<int> active_indices;
  for (size_t i = 0; i < points3D.size(); ++i) {
    active_indices.push_back(static_cast<int>(i));
  }

  // 5. Continuous Visual SLAM Tracking and Map Growth
  cout << endl
       << "Initialization Success! Entering continuous SLAM tracking..."
       << endl;
  PoseEstimator estimator(K);

  // ── Chirality pre-filter state ──────────────────────────────────────────
  // We store the last successfully accepted pose so we can transform candidate
  // 3D points into camera space and reject any that fall behind the camera
  // BEFORE they corrupt the PnP solver.
  Mat prev_R = R_relative.clone();
  Mat prev_t = t_relative.clone();
  bool have_prev_pose = true;

  Mat frame;
  int tracking_failures = 0;
  std::vector<cv::Point2f> final_good_next; // saved each frame for post-run viewer
  while (cap.read(frame)) {
    current_frame_id++;

    Mat display_frame, gray;
    resize(frame, display_frame, Size(), scale, scale);
    cvtColor(display_frame, gray, COLOR_BGR2GRAY);

    vector<Point2f> points_next;
    vector<uchar> status;
    vector<float> err;
    if (!points_prev.empty()) {
      calcOpticalFlowPyrLK(prev_gray, gray, points_prev, points_next, status,
                           err, Size(21, 21), 3);

      // Forward-Backward tracking check for robustness (same as test_slam_adaptive)
      vector<Point2f> points_back;
      vector<uchar> status_back;
      vector<float> err_back;
      calcOpticalFlowPyrLK(gray, prev_gray, points_next, points_back,
                           status_back, err_back, Size(21, 21), 3);
      for (size_t j = 0; j < status.size(); ++j) {
        if (status[j]) {
          if (!status_back[j] ||
              cv::norm(points_prev[j] - points_back[j]) > 2.0) {
            status[j] = 0;
          }
        }
      }
    }

    // Filter and compile active 3D-to-2D correspondences for PnP localization
    vector<Point3f> active_3d_points;
    vector<Point2f> active_2d_points;
    vector<Point2f> good_next;
    vector<int> good_indices;
    vector<int> good_next_to_active_pnp_map;

    int new_untracked_count =
        0; // Features tracked optically but not yet in the map

    for (size_t j = 0; j < points_prev.size(); ++j) {
      if (status[j] && points_next[j].x >= 0 && points_next[j].x < gray.cols &&
          points_next[j].y >= 0 && points_next[j].y < gray.rows) {

        int bootstrap_idx = active_indices[j];
        bool too_close = false;
        if (inliers_init[bootstrap_idx] && have_prev_pose) {
          const cv::Point3f &wp = points3D[bootstrap_idx];
          cv::Mat pt_w = (cv::Mat_<double>(3, 1) << wp.x, wp.y, wp.z);
          cv::Mat pt_c = prev_R * pt_w + prev_t;
          if (pt_c.at<double>(2, 0) <= 0.40) {
            too_close = true;
          }
        }
        if (too_close) {
          continue; // discard from active tracking to prevent near-field
                    // corruption/drift
        }

        int good_next_idx = static_cast<int>(good_next.size());
        good_next.push_back(points_next[j]);
        good_indices.push_back(active_indices[j]);
        good_next_to_active_pnp_map.push_back(-1);

        if (inliers_init[bootstrap_idx]) {
          good_next_to_active_pnp_map[good_next_idx] =
              static_cast<int>(active_2d_points.size());
          active_3d_points.push_back(points3D[bootstrap_idx]);
          active_2d_points.push_back(points_next[j]);
        } else {
          ++new_untracked_count;
        }
      }
    }

    Mat R, t;
    vector<int> pnp_inliers;
    bool track_success = estimator.estimatePose(
        active_3d_points, active_2d_points, R, t, pnp_inliers);


    vector<bool> inlier_mask(good_next.size(), false);

    if (track_success) {
      Mat C = -R.t() * t;
      trajectory.push_back(C);
      std::cout << "[SLAM] Frame " << current_frame_id << " | Pose C: ("
                << C.at<double>(0) << ", " << C.at<double>(1) << ", "
                << C.at<double>(2) << ")"
                << " | Tracked: " << active_3d_points.size()
                << " | PnP Inliers: " << pnp_inliers.size() << std::endl;

      // Update last accepted pose for next-frame chirality pre-filtering
      prev_R = R.clone();
      prev_t = t.clone();
      have_prev_pose = true;

      vector<bool> is_pnp_inlier(active_2d_points.size(), false);
      for (int idx : pnp_inliers)
        is_pnp_inlier[idx] = true;

      vector<Point2f> filtered_good_next;
      vector<int> filtered_good_indices;
      vector<bool> filtered_inlier_mask;

      for (size_t k = 0; k < good_next.size(); ++k) {
        filtered_good_next.push_back(good_next[k]);
        filtered_good_indices.push_back(good_indices[k]);

        int pnp_idx = good_next_to_active_pnp_map[k];
        if (pnp_idx != -1 && is_pnp_inlier[pnp_idx]) {
          filtered_inlier_mask.push_back(true);
        } else {
          filtered_inlier_mask.push_back(false);
        }
      }

      good_next = filtered_good_next;
      good_indices = filtered_good_indices;
      inlier_mask = filtered_inlier_mask;

      // ── Task B: pass new_untracked_count & good_indices into
      // checkNewKeyframe ──────
      bool should_create_kf = map_manager.checkNewKeyframe(
          R, t, static_cast<int>(pnp_inliers.size()),
          static_cast<int>(active_3d_points.size()), new_untracked_count,
          good_indices);

      if (should_create_kf) {
        // Detect new features — start from good_next so existing IDs carry over
        // (critical: preserves Jaccard similarity between frames)
        std::vector<Point2f> points_pool = good_next;
        tracker.processFrame(display_frame, points_pool);

        // Gather set of map point indices currently tracked in the active pool
        std::unordered_set<int> tracked_map_point_ids;
        for (size_t k = 0; k < good_next.size(); ++k) {
          int original_idx = good_indices[k];
          if (inliers_init[original_idx]) {
            int pt_idx = initial_3d_indices[original_idx];
            if (pt_idx != -1) {
              tracked_map_point_ids.insert(pt_idx);
            }
          }
        }

        /* Commented out projection of all map points:
        // Project all existing 3D map points into the current camera frame
        double fx = K.at<double>(0, 0);
        double fy = K.at<double>(1, 1);
        double cx = K.at<double>(0, 2);
        double cy = K.at<double>(1, 2);

        std::vector<cv::Point2f> projected_map_pts;
        std::vector<int> projected_map_indices;
        const auto& map_pts = map_manager.getMapPoints();

        for (size_t m_idx = 0; m_idx < map_pts.size(); ++m_idx) {
            if (tracked_map_point_ids.count(static_cast<int>(m_idx)) > 0) {
                continue;
            }
            const auto& wp = map_pts[m_idx];
            cv::Mat pt_w = (cv::Mat_<double>(3, 1) << wp.x, wp.y, wp.z);
            cv::Mat pt_c = R * pt_w + t;
            double zc = pt_c.at<double>(2, 0);
            if (zc > 0.40) {
                double u = fx * (pt_c.at<double>(0, 0) / zc) + cx;
                double v = fy * (pt_c.at<double>(1, 0) / zc) + cy;
                if (u >= 0 && u < display_frame.cols && v >= 0 && v <
        display_frame.rows) {
                    projected_map_pts.push_back(cv::Point2f(static_cast<float>(u),
        static_cast<float>(v)));
                    projected_map_indices.push_back(static_cast<int>(m_idx));
                }
            }
        }
        */

        // Project 3D map points from the last keyframe only into the current
        // camera frame
        double fx = K.at<double>(0, 0);
        double fy = K.at<double>(1, 1);
        double cx = K.at<double>(0, 2);
        double cy = K.at<double>(1, 2);

        std::vector<cv::Point2f> projected_map_pts;
        std::vector<int> projected_map_indices;
        const auto &map_pts = map_manager.getMapPoints();
        const auto &last_kf = map_manager.getKeyframes().back();

        for (int m_idx : last_kf.point3D_indices) {
          if (m_idx == -1) {
            continue;
          }
          if (tracked_map_point_ids.count(m_idx) > 0) {
            continue;
          }
          const auto &wp = map_pts[m_idx];
          cv::Mat pt_w = (cv::Mat_<double>(3, 1) << wp.x, wp.y, wp.z);
          cv::Mat pt_c = R * pt_w + t;
          double zc = pt_c.at<double>(2, 0);
          if (zc > 0.40) {
            double u = fx * (pt_c.at<double>(0, 0) / zc) + cx;
            double v = fy * (pt_c.at<double>(1, 0) / zc) + cy;
            if (u >= 0 && u < display_frame.cols && v >= 0 &&
                v < display_frame.rows) {
              projected_map_pts.push_back(
                  cv::Point2f(static_cast<float>(u), static_cast<float>(v)));
              projected_map_indices.push_back(m_idx);
            }
          }
        }

        int matched_features_count = 0;
        size_t old_good_next_size = good_next.size();
        for (size_t i = old_good_next_size; i < points_pool.size(); ++i) {
          const auto &pt = points_pool[i];
          // Match new 2D feature to projected 3D landmarks
          int best_map_pt_idx = -1;
          double min_proj_dist = 6.0; // matching threshold in pixels
          for (size_t p_i = 0; p_i < projected_map_pts.size(); ++p_i) {
            int m_idx = projected_map_indices[p_i];
            if (tracked_map_point_ids.count(m_idx) > 0)
              continue;
            double dist = cv::norm(projected_map_pts[p_i] - pt);
            if (dist < min_proj_dist) {
              min_proj_dist = dist;
              best_map_pt_idx = m_idx;
            }
          }

          // Apply color descriptor validation for room video
          if (best_map_pt_idx != -1 &&
              video_path.find("room") != string::npos) {
            int px = static_cast<int>(std::round(pt.x));
            int py = static_cast<int>(std::round(pt.y));
            if (px >= 0 && px < display_frame.cols && py >= 0 &&
                py < display_frame.rows) {
              cv::Vec3b img_col = display_frame.at<cv::Vec3b>(py, px);
              cv::Scalar lm_col = getLandmarkColor(map_pts[best_map_pt_idx]);
              double color_diff = std::abs(img_col[0] - lm_col[0]) +
                                  std::abs(img_col[1] - lm_col[1]) +
                                  std::abs(img_col[2] - lm_col[2]);
              if (color_diff > 45.0) {
                best_map_pt_idx = -1; // mismatch
              }
            }
          }

          good_next.push_back(pt);
          int new_idx = static_cast<int>(points3D.size());
          good_indices.push_back(new_idx);
          inlier_mask.push_back(false);

          if (best_map_pt_idx != -1) {
            points3D.push_back(map_pts[best_map_pt_idx]);
            inliers_init.push_back(true);
            initial_3d_indices.push_back(best_map_pt_idx);
            tracked_map_point_ids.insert(best_map_pt_idx);
            matched_features_count++;
          } else {
            points3D.push_back(
                cv::Point3f(0, 0, 0)); // placeholder until triangulated
            inliers_init.push_back(false);
            initial_3d_indices.push_back(-1);
          }
        }
        if (matched_features_count > 0) {
          std::cout << "[SLAM] Associated " << matched_features_count
                    << " newly detected features with existing 3D map points."
                    << std::endl;
        }

        vector<int> current_3d_indices(good_next.size(), -1);
        for (size_t k = 0; k < good_next.size(); ++k) {
          int original_idx = good_indices[k];
          if (inliers_init[original_idx]) {
            current_3d_indices[k] = initial_3d_indices[original_idx];
          }
        }

        map_manager.addKeyframe(gray, R, t, good_next, current_3d_indices,
                                good_indices);

        int last_kf_idx =
            static_cast<int>(map_manager.getKeyframes().size()) - 1;
        map_manager.triangulateNewPoints(last_kf_idx - 1, last_kf_idx, K);

        // Sync newly triangulated points back to our local tracking arrays
        auto &kf_last = map_manager.getKeyframesRef().back();
        const auto &map_points = map_manager.getMapPoints();
        for (size_t k = 0; k < kf_last.point3D_indices.size(); ++k) {
          int pt_idx = kf_last.point3D_indices[k];
          if (pt_idx != -1 && k < good_indices.size()) {
            int original_idx = good_indices[k];
            points3D[original_idx] = map_points[pt_idx];
            inliers_init[original_idx] = true;
            initial_3d_indices[original_idx] = pt_idx;
          }
        }
      }
    } else {
      cout << "[SLAM] Warning: Camera localization lost on Frame "
           << current_frame_id << endl;
      tracking_failures++;
      have_prev_pose = false;
      // Re-detect features every 3rd failure to prevent starvation
      if (tracking_failures % 3 == 1) {
        std::vector<Point2f> rescue_pool = good_next;
        tracker.processFrame(display_frame, rescue_pool);
        double fx = K.at<double>(0,0), fy = K.at<double>(1,1);
        double cx = K.at<double>(0,2), cy = K.at<double>(1,2);
        const auto &map_pts = map_manager.getMapPoints();
        std::unordered_set<int> already_tracked;
        for (size_t k = 0; k < good_next.size(); ++k) {
          int oi = good_indices[k];
          if (oi < (int)inliers_init.size() && inliers_init[oi]) {
            int pt_idx = initial_3d_indices[oi];
            if (pt_idx != -1) already_tracked.insert(pt_idx);
          }
        }
        std::vector<cv::Point2f> proj_pts;
        std::vector<int> proj_indices;
        const auto &kfs = map_manager.getKeyframes();
        if (!kfs.empty()) {
          for (int m_idx : kfs.back().point3D_indices) {
            if (m_idx == -1 || already_tracked.count(m_idx)) continue;
            const auto &wp = map_pts[m_idx];
            cv::Mat pt_w = (cv::Mat_<double>(3,1) << wp.x, wp.y, wp.z);
            cv::Mat pt_c = prev_R * pt_w + prev_t;
            double zc = pt_c.at<double>(2,0);
            if (zc > 0.10) {
              double u = fx*(pt_c.at<double>(0,0)/zc)+cx;
              double v = fy*(pt_c.at<double>(1,0)/zc)+cy;
              if (u>=0&&u<gray.cols&&v>=0&&v<gray.rows) {
                proj_pts.push_back(cv::Point2f(u,v));
                proj_indices.push_back(m_idx);
              }
            }
          }
        }
        size_t old_sz = good_next.size();
        for (size_t i = old_sz; i < rescue_pool.size(); ++i) {
          const auto &pt = rescue_pool[i];
          int best=-1; double best_d=8.0;
          for (size_t p=0; p<proj_pts.size(); ++p) {
            double d=cv::norm(proj_pts[p]-pt);
            if (d<best_d&&!already_tracked.count(proj_indices[p])){best_d=d;best=(int)p;}
          }
          good_next.push_back(pt);
          good_indices.push_back((int)points3D.size());
          inlier_mask.push_back(false);
          if (best!=-1) {
            int m_idx=proj_indices[best];
            points3D.push_back(map_pts[m_idx]);
            inliers_init.push_back(true);
            initial_3d_indices.push_back(m_idx);
            already_tracked.insert(m_idx);
          } else {
            points3D.push_back(cv::Point3f(0,0,0));
            inliers_init.push_back(false);
            initial_3d_indices.push_back(-1);
          }
        }
      }
    }

    // Draw 2D & 3D real-time visualizers
    visualizer.draw2D(display_frame, good_next, inlier_mask, current_frame_id);
    if (current_frame_id % 5 == 0) {
      std::vector<cv::Point3f> dense_points = map_manager.generateDenseMap();
      // Extract keyframe centres for the 3D viewer markers
      std::vector<cv::Point3f> kf_centers_viz;
      std::vector<int> kf_ids_viz;
      for (const auto &kf : map_manager.getKeyframes()) {
        if (!kf.is_culled) {
          cv::Mat C = -kf.R.t() * kf.t;
          kf_centers_viz.push_back(cv::Point3f(
            (float)C.at<double>(0),(float)C.at<double>(1),(float)C.at<double>(2)));
          kf_ids_viz.push_back(kf.id);
        }
      }
      visualizer.draw3D(map_manager.getMapPoints(), trajectory, dense_points);
      perspective_visualizer.render3D(map_manager.getMapPoints(), trajectory,
                                      dense_points, R, t,
                                      kf_centers_viz, kf_ids_viz);
    }

    char key = static_cast<char>(waitKey(1));
    if (key == 27 || key == 'q' || key == 'Q') {
      cout << "SLAM execution terminated by user." << endl;
      break;
    }

    final_good_next = good_next; // save for post-run viewer
    // Prepare next iteration — swap avoids allocation
    points_prev = good_next;
    active_indices = good_indices;
    std::swap(prev_gray, gray);
  }

  cout << "=========================================================" << endl;
  cout << "            SLAM Run Completed Successfully!" << endl;
  cout << "=========================================================" << endl;
  cout << "Total Keyframes Stored: " << map_manager.getKeyframes().size()
       << endl;
  cout << "Total 3D Points Mapped: " << map_manager.getMapPoints().size()
       << endl;

  cout << endl << "All viewers open — drag to rotate, scroll to zoom." << endl;
  cout << "Press  Q / Esc  to exit." << endl;

  std::vector<cv::Point3f> final_dense = map_manager.generateDenseMap();
  std::vector<cv::Point3f> final_kf_centers;
  std::vector<int> final_kf_ids;
  for (const auto &kf : map_manager.getKeyframes()) {
    if (!kf.is_culled) {
      cv::Mat C = -kf.R.t() * kf.t;
      final_kf_centers.push_back(cv::Point3f(
        (float)C.at<double>(0),(float)C.at<double>(1),(float)C.at<double>(2)));
      final_kf_ids.push_back(kf.id);
    }
  }
  cv::Mat final_gray_display;
  cv::cvtColor(prev_gray, final_gray_display, cv::COLOR_GRAY2BGR);
  std::vector<bool> final_mask(final_good_next.size(), true);

  while (true) {
    perspective_visualizer.render3D(map_manager.getMapPoints(), trajectory,
                                    final_dense, prev_R, prev_t,
                                    final_kf_centers, final_kf_ids);
    visualizer.draw3D(map_manager.getMapPoints(), trajectory, final_dense);
    visualizer.draw2D(final_gray_display, final_good_next, final_mask, current_frame_id);
    int key = cv::waitKey(30);
    if (key == 27 || key == 'q' || key == 'Q') break;
  }

  cv::destroyAllWindows();
  return 0;
}