diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..e2c18d1
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,5 @@
+{
+    "files.associations": {
+        "ostream": "cpp"
+    }
+}
\ No newline at end of file
diff --git a/apps/endomapper.cc b/apps/endomapper.cc
index 610917f..30c257d 100644
--- a/apps/endomapper.cc
+++ b/apps/endomapper.cc
@@ -68,6 +68,7 @@ int main(int argc, char **argv) {
 
         SLAM.TrackImage(*image);
     }
+    SLAM.SaveTraj();
 
     return 0;
 }
\ No newline at end of file
diff --git a/apps/hamlyn.cc b/apps/hamlyn.cc
index ee200dc..11f206e 100644
--- a/apps/hamlyn.cc
+++ b/apps/hamlyn.cc
@@ -71,6 +71,7 @@ int main(int argc, char **argv) {
 
         SLAM.TrackImageWithStereo(*im_left, *im_right);
     }
+    SLAM.SaveTraj();
 
     return 0;
 }
diff --git a/apps/simulation.cc b/apps/simulation.cc
index 2c5e233..fd2ec67 100644
--- a/apps/simulation.cc
+++ b/apps/simulation.cc
@@ -72,6 +72,7 @@ int main(int argc, char **argv) {
 
         SLAM.TrackImageWithDepth((*image), (*depth_image));
     }
+    SLAM.SaveTraj();
 
     return 0;
 }
\ No newline at end of file
diff --git a/modules/SLAM/system.cc b/modules/SLAM/system.cc
index 1cb2dac..1cddef9 100644
--- a/modules/SLAM/system.cc
+++ b/modules/SLAM/system.cc
@@ -97,6 +97,8 @@ System::System(const string settings_file_path) {
     // Initialize frame evaluator.
     FrameEvaluator::Options frame_evaluator_options;
     frame_evaluator_options.results_file_path = settings_->GetEvaluationPath();
+    cout << "Evaluation path: " << settings_->GetEvaluationPath() << endl;
+
     frame_evaluator_options.precomputed_depth_ = true;
     frame_evaluator_ = make_unique<FrameEvaluator>(frame_evaluator_options, stereo_pattern_matcher_,
                                                    map_visualizer_.get());
@@ -110,6 +112,57 @@ System::~System() {
     map_visualizer_thread_->join();
 }
 
+vector<Eigen::Vector3f> System::GetTraj() {
+    auto latest_frames = map_->GetTemporalBuffer()->GetLatestCameraPoses();
+
+    vector<Eigen::Vector3f> trajectory;
+    for (auto& pose : latest_frames) {
+        trajectory.push_back(pose.inverse().translation());
+    }
+    return trajectory;
+}
+
+
+void System::SaveTraj() {
+    // Get the latest camera poses directly (not just translation)
+    // auto latest_frames = map_->GetTemporalBuffer()->GetLatestCameraPoses();
+    auto poses = tracker_->GetCameraPoses();
+
+    // Open file for writing
+    std::ofstream traj_file("trajectory.tum", std::ios::out);
+    if (!traj_file.is_open()) {
+        std::cerr << "Failed to open trajectory file!" << std::endl;
+        return;
+    }
+
+    // Set precision for floating-point output
+    traj_file << std::fixed << std::setprecision(6);
+
+    // Iterate over poses and write in TUM format
+    double time_step = 0.033; // Assume 30 FPS; adjust as needed
+    int frame_idx = 0;
+    for (const auto& pose : poses) {
+        // Assuming pose is Sophus::SE3f or Sophus::SE3d
+        Eigen::Vector3f t = pose.inverse().translation(); // Translation (x, y, z)
+        Eigen::Quaternionf q(pose.inverse().unit_quaternion()); // Quaternion (qx, qy, qz, qw)
+
+        // Synthetic timestamp (adjust if real timestamps are available)
+        double timestamp = frame_idx * time_step;
+
+        // Write to file: timestamp tx ty tz qx qy qz qw
+        traj_file << timestamp << " "
+                  << t.x() << " " << t.y() << " " << t.z() << " "
+                  << q.x() << " " << q.y() << " " << q.z() << " " << q.w() << "\n";
+
+        frame_idx++;
+    }
+
+    // Close the file
+    traj_file.close();
+    std::cout << "Trajectory saved to trajectory.tum" << std::endl;
+}
+
+
 void System::TrackImage(const cv::Mat &im) {
     // Preprocess image.
     cv::Mat im_gray;
diff --git a/modules/SLAM/system.h b/modules/SLAM/system.h
index 2ddbbcc..4af8fa4 100644
--- a/modules/SLAM/system.h
+++ b/modules/SLAM/system.h
@@ -24,6 +24,10 @@
 #include <memory>
 #include <thread>
 
+#include <fstream>
+#include <iomanip>
+#include <Eigen/Geometry>
+
 #include "map/map.h"
 #include "mapping/mapping.h"
 #include "SLAM/settings.h"
@@ -53,6 +57,10 @@ class System {
     // TODO: implement stereo options
     void TrackImageWithStereo(const cv::Mat& im_left, const cv::Mat& im_right);
 
+    vector<Eigen::Vector3f> GetTraj();
+
+    void SaveTraj();
+
     // Tracks the next image using a precomputed depth image.
     void TrackImageWithDepth(const cv::Mat& im_left, const cv::Mat& im_depth);
 
diff --git a/modules/matching/lucas_kanade_tracker.cc b/modules/matching/lucas_kanade_tracker.cc
index df5f37e..9983fa1 100644
--- a/modules/matching/lucas_kanade_tracker.cc
+++ b/modules/matching/lucas_kanade_tracker.cc
@@ -125,6 +125,10 @@ void LucasKanadeTracker::SetReferenceImage(const Mat &refIm, const vector<KeyPoi
                     int mx = (ipoint.x+x) * scaleFactor;
                     int my = (ipoint.y+y) * scaleFactor;
 
+                    if (!mask.empty()&(mx < 0 || my < 0 || mx >= mask.cols || my >= mask.rows)){
+                        continue;
+                    } 
+
                     if(!mask.empty() && mask.at<uchar>(my,mx) == 0){
                         valid = false;
                         break;
diff --git a/modules/tracking/monocular_map_initializer.cc b/modules/tracking/monocular_map_initializer.cc
index 08d8ce7..84c360c 100644
--- a/modules/tracking/monocular_map_initializer.cc
+++ b/modules/tracking/monocular_map_initializer.cc
@@ -49,14 +49,16 @@ MonocularMapInitializer::MonocularMapInitializer(Options& options,
     internal_status_ = NO_DATA;
 }
 
-absl::StatusOr<MonocularMapInitializer::InitializationResults>
+MonocularMapInitializer::InitializationResults
 MonocularMapInitializer::ProcessNewImage(const cv::Mat& im, const cv::Mat& im_clahe,
-                                              const cv::Mat& mask) {
+                                                const cv::Mat& mask) {
     // Track features and update the feature tracks.
     DataAssociation(im, im_clahe, mask);
 
     if (internal_status_ == RECENTLY_RESET) {
-        return absl::InternalError("Just reset");
+        MonocularMapInitializer::InitializationResults partial_results;
+        partial_results.status = absl::InternalError("Just reset");
+        return partial_results;
     }
 
     // Perform optical flow clustering.
@@ -64,13 +66,17 @@ MonocularMapInitializer::ProcessNewImage(const cv::Mat& im, const cv::Mat& im_cl
 
     // Try to perform a rigid initialization.
     auto initialization_results_status = RigidInitialization();
-
-    if (!initialization_results_status.ok()) {
-        LOG(INFO) << initialization_results_status.status().message();
-        return absl::InternalError("Rigid Initialization failed");
-    }
-
-    auto [camera_transform_world, landmarks_positions] = *initialization_results_status;
+    auto status = initialization_results_status.status;
+    auto camera_transform_world = initialization_results_status.camera_transform_world;
+    auto landmarks_positions = initialization_results_status.landmarks_position;
+
+        if (!status.ok()) {
+            LOG(INFO) << status.message();
+            MonocularMapInitializer::InitializationResults partial_results;
+            partial_results.camera_transform_world = camera_transform_world;
+            partial_results.status = absl::InternalError("Rigid Initialization failed");
+            return partial_results;
+        }
 
     // Perform a deformable Bundle Adjustment to refine the results.
     return InitializationRefinement(current_keypoints_, landmarks_positions, feature_labels, camera_transform_world);
@@ -224,12 +230,12 @@ MonocularMapInitializer::RigidInitializationResults MonocularMapInitializer::Rig
     auto status = rigid_initializer_->Initialize(current_keypoints_, current_keypoint_statuses_,
                                                 n_tracks_in_image_, camera_transform_world,
                                                 landmarks_position);
+    RigidInitializationResults result = {status, camera_transform_world, landmarks_position};
 
     if (!status.ok()) {
-        return absl::InternalError(status.message());
-    } else {
-        return make_tuple(camera_transform_world, landmarks_position);
+        result.landmarks_position.clear(); // Invalidate landmarks on failure
     }
+    return result;
 }
 
 MonocularMapInitializer::InitializationResults
diff --git a/modules/tracking/monocular_map_initializer.h b/modules/tracking/monocular_map_initializer.h
index 0a2a126..39f11e6 100644
--- a/modules/tracking/monocular_map_initializer.h
+++ b/modules/tracking/monocular_map_initializer.h
@@ -61,8 +61,8 @@ class MonocularMapInitializer {
                             std::shared_ptr<CameraModel> calibration,
                             std::shared_ptr<ImageVisualizer> image_visualizer);
 
-    absl::StatusOr<InitializationResults> ProcessNewImage(const cv::Mat& im, const cv::Mat& im_clahe,
-                         const cv::Mat& mask);
+    InitializationResults ProcessNewImage(const cv::Mat& im, const cv::Mat& im_clahe,
+        const cv::Mat& mask);
 
 private:
     void DataAssociation(const cv::Mat& im, const cv::Mat& im_clahe,
@@ -81,9 +81,11 @@ class MonocularMapInitializer {
     void ResetInitialization(const cv::Mat& im, const cv::Mat& im_clahe,
                              const cv::Mat& mask);
 
-    typedef absl::StatusOr<
-            std::tuple<Sophus::SE3f, std::vector<absl::StatusOr<Eigen::Vector3f>>>>
-            RigidInitializationResults;
+    struct RigidInitializationResults {
+    absl::Status status;
+    Sophus::SE3f camera_transform_world;
+    std::vector<absl::StatusOr<Eigen::Vector3f>> landmarks_position; // Empty if status not OK
+    };
 
     RigidInitializationResults RigidInitialization();
 
diff --git a/modules/tracking/tracking.cc b/modules/tracking/tracking.cc
index 078295e..da6e06b 100644
--- a/modules/tracking/tracking.cc
+++ b/modules/tracking/tracking.cc
@@ -95,12 +95,15 @@ void Tracking::TrackImage(const cv::Mat &im, const absl::flat_hash_map<std::stri
         PointReuse(im, cv::Mat(), lost_mappoint_ids);
 
         if (current_frame_->GetKeypointsWithStatus({TRACKED_WITH_3D}).size() < 10) {
+            cout<<"Not enough points to track."<<endl;
             exit(0);
         }
 
         // KeyFrame insertion.
         KeyFrameInsertion(im, masks);
 
+        poses.push_back(current_frame_->CameraTransformationWorld());
+        cout<<"Num poses: "<<poses.size()<<endl;
         // Insert frame to the temporal buffer.
         map_->SetLastFrame(current_frame_);
 
@@ -111,6 +114,10 @@ void Tracking::TrackImage(const cv::Mat &im, const absl::flat_hash_map<std::stri
     }
 }
 
+std::vector<Sophus::SE3f> Tracking::GetCameraPoses() {
+    return poses;
+}
+
 Tracking::TrackingStatus Tracking::GetTrackingStatus() const {
     return tracking_status_;
 }
@@ -137,12 +144,14 @@ void Tracking::MonocularMapInitialization(const cv::Mat& im_left,
                                        const cv::Mat& mask, const cv::Mat& im_clahe) {
     auto initialization_status = monocular_map_initializer_->ProcessNewImage(im_left, im_clahe, mask);
 
-    if(!initialization_status.ok()) {
-        LOG(INFO) << initialization_status.status().message();
+    poses.push_back(initialization_status.camera_transform_world);
+    cout<<"Num poses: "<<poses.size()<<endl;
+    
+    if(!initialization_status.status.ok()) {
+        LOG(INFO) << initialization_status.status.message();
         return;
     }
-
-    auto initialization_results = *initialization_status;
+    auto initialization_results = initialization_status;
 
     vector<float> depths;
     for (int idx = 0; idx < initialization_results.current_keypoints.size(); idx++) {