Mapping Integration

.gitignore

@@ -20,6 +20,10 @@ models/*
 
 images/*
 !images/.gitkeep
+!images/mapping/
+
+images/mapping/*
+!images/mapping/.gitkeep
 
 tests/integration/images/*
 !tests/integration/images/.gitkeep
@@ -38,9 +42,15 @@ tests/integration/mapping/batch2/*
 tests/integration/mapping/images/*
 !tests/integration/mapping/images/.gitkeep
 
+tests/integration/mapping/direct_test_images/*
+!tests/integration/mapping/direct_test_images/.gitkeep
+
 tests/integration/mapping/output/*
 !tests/integration/mapping/output/.gitkeep
 
+tests/integration/mapping/direct_output/*
+!tests/integration/mapping/direct_output/.gitkeep
+
 tests/integration/aggregator/*
 !tests/integration/aggregator/.gitkeep
 

include/core/mission_state.hpp

@@ -104,6 +104,10 @@ class MissionState {
     std::shared_ptr<CameraInterface> getCamera();
     void setCamera(std::shared_ptr<CameraInterface> camera);
 
+    // Getters and setters for mapping status.
+    bool getMappingIsDone();
+    void setMappingIsDone(bool isDone);
+
     MissionParameters mission_params;  // has its own mutex
 
     OBCConfig config;
@@ -139,6 +143,8 @@ class MissionState {
     // with the detected_target specified by the index
     std::array<size_t, NUM_AIRDROPS> cv_matches;
 
+    bool mappingIsDone;
+
     void _setTick(Tick* newTick);  // does not acquire the tick_mut
 };
 

include/cv/mapping.hpp

@@ -46,6 +46,11 @@ class Mapping {
     // This clears the counters but does NOT delete your saved chunk images on disk.
     void reset();
 
+    // Direct stitch: loads all images from input_path and stitches them into a single panorama
+    // Saves the result to output_path. This bypasses the two-pass chunking system.
+    void directStitch(const std::string& input_path, const std::string& output_path,
+                      cv::Stitcher::Mode mode, int max_dim, bool preprocess = true);
+
  private:
     // Helper: read & optionally downscale an image so its largest dimension <= max_dim
     static cv::Mat readAndResize(const cv::Mat& input, int max_dim);

src/core/mission_state.cpp

@@ -140,3 +140,11 @@ std::shared_ptr<CameraInterface> MissionState::getCamera() {
 void MissionState::setCamera(std::shared_ptr<CameraInterface> camera) {
     this->camera = camera;
 }
+
+bool MissionState::getMappingIsDone() {
+    return this->mappingIsDone;
+}
+
+void MissionState::setMappingIsDone(bool isDone) {
+    this->mappingIsDone = isDone;
+}

src/cv/mapping.cpp

@@ -11,9 +11,11 @@
 #include <string>
 #include <vector>
 
+
 #include "cv/preprocess.hpp"  // include the Preprocess header
 #include "opencv2/opencv.hpp"
 #include "opencv2/stitching.hpp"
+#include "utilities/logging.hpp"
 
 namespace fs = std::filesystem;
 
@@ -101,8 +103,8 @@ void Mapping::processChunk(const std::vector<cv::Mat>& chunk_images, const std::
     if (chunk_images.empty()) return;
 
     chunk_counter++;
-    std::cout << "\nProcessing chunk #" << chunk_counter << " with " << chunk_images.size()
-              << " images.\n";
+    LOG_S(INFO) << "Processing chunk #" << chunk_counter << " with " << chunk_images.size()
+                            << "images.\n";
 
     // Stitch the chunk using the (now ignored) preprocess flag
     auto [status, stitched] = globalStitch(chunk_images, mode, max_dim, preprocess);
@@ -113,9 +115,9 @@ void Mapping::processChunk(const std::vector<cv::Mat>& chunk_images, const std::
 
     if (status == cv::Stitcher::OK && !stitched.empty()) {
         if (cv::imwrite(chunk_filename, stitched)) {
-            std::cout << "Saved chunk result to: " << chunk_filename << "\n";
+            LOG_S(INFO) << "Saved chunk result to: " << chunk_filename << "\n";
         } else {
-            std::cerr << "Failed to save chunk result to disk. This chunk will be lost.\n";
+            LOG_F(ERROR, "Failed to save chunk result to disk. This chunk will be lost.\n");
         }
     } else {
         std::cerr << "Chunk stitching failed, status = " << static_cast<int>(status)
@@ -134,9 +136,9 @@ void Mapping::firstPass(const std::string& input_path, const std::string& run_su
         current_run_folder = run_subdir + "/" + currentDateTimeStr();
         try {
             fs::create_directories(current_run_folder);
-            std::cout << "Created run folder: " << current_run_folder << "\n";
+            LOG_S(INFO) << "Created run folder: " << current_run_folder << "\n";
         } catch (const fs::filesystem_error& e) {
-            std::cerr << "Error creating run folder: " << e.what() << "\n";
+            LOG_S(ERROR) << "Error creating run folder: " << e.what() << "\n";
             throw;
         }
     }
@@ -157,7 +159,7 @@ void Mapping::firstPass(const std::string& input_path, const std::string& run_su
 
     // 3. Skip already processed images
     if (static_cast<int>(all_filenames.size()) <= processed_image_count) {
-        std::cout << "No new images found to process.\n";
+        LOG_F(INFO, "No new images found to process.\n");
         return;
     }
 
@@ -178,10 +180,10 @@ void Mapping::firstPass(const std::string& input_path, const std::string& run_su
             }
             images.push_back(img);
         } else {
-            std::cerr << "Warning: Failed to load image: " << filename << "\n";
+            LOG_S(WARNING) << "Warning: Failed to load image: " << filename << "\n";
         }
     }
-    std::cout << "Loaded and preprocessed " << images.size() << " new images.\n";
+    LOG_S(INFO) << "Loaded and preprocessed " << images.size() << " new images.\n";
 
     // 5. Chunk the preprocessed images
     auto chunks = chunkListWithOverlap(static_cast<int>(images.size()), chunk_size, overlap);
@@ -221,7 +223,8 @@ void Mapping::secondPass(const std::string& run_subdir, cv::Stitcher::Mode mode,
     }
 
     if (chunk_files.empty()) {
-        std::cerr << "No chunk images found in " << folder_to_scan << ". Nothing to stitch.\n";
+        LOG_S(WARNING) << "No chunk images found in " << folder_to_scan
+            << ". Nothing to stitch.\n";
         return;
     }
 
@@ -233,11 +236,11 @@ void Mapping::secondPass(const std::string& run_subdir, cv::Stitcher::Mode mode,
         if (!cimg.empty()) {
             chunk_images.push_back(cimg);
         } else {
-            std::cerr << "Warning: could not load chunk image: " << cfile << "\n";
+            LOG_S(WARNING) << "Warning: could not load chunk image: " << cfile << "\n";
         }
     }
 
-    std::cout << "Loaded " << chunk_images.size() << " chunk images from " << folder_to_scan
+    LOG_S(INFO) << "Loaded " << chunk_images.size() << " chunk images from " << folder_to_scan
               << ". Attempting final stitch...\n";
 
     // Stitch without additional preprocessing (images are already preprocessed)
@@ -246,17 +249,103 @@ void Mapping::secondPass(const std::string& run_subdir, cv::Stitcher::Mode mode,
     if (status == cv::Stitcher::OK && !final_stitched.empty()) {
         std::string final_name = folder_to_scan + "/final_" + currentDateTimeStr() + ".jpg";
         if (cv::imwrite(final_name, final_stitched)) {
-            std::cout << "Final image saved to: " << final_name << "\n";
+            LOG_S(INFO) << "Final image saved to: " << final_name << "\n";
         } else {
-            std::cerr << "Failed to save final image to " << final_name << "\n";
+            LOG_S(WARNING) << "Failed to save final image to " << final_name << "\n";
         }
     } else {
-        std::cerr << "Final stitching failed. Status = " << static_cast<int>(status) << "\n";
+        LOG_S(WARNING) << "Final stitching failed. Status = " << static_cast<int>(status) << "\n";
     }
 
     chunk_images.clear();
 }
 
+// -----------------------------------------------------------------------------
+// directStitch()
+// -----------------------------------------------------------------------------
+void Mapping::directStitch(const std::string& input_path, const std::string& output_path,
+                           cv::Stitcher::Mode mode, int max_dim, bool preprocess) {
+    // 1. Scan directory for image filenames
+    std::vector<std::string> all_filenames;
+    for (const auto& entry : fs::directory_iterator(input_path)) {
+        if (!entry.is_regular_file()) continue;
+        auto ext = entry.path().extension().string();
+        std::transform(ext.begin(), ext.end(), ext.begin(), ::tolower);
+        if (ext == ".jpg" || ext == ".jpeg" || ext == ".png") {
+            all_filenames.push_back(entry.path().string());
+        }
+    }
+
+    // 2. Sort them for consistent processing (assuming image filenames are timestamped)
+    // Mapping works better when the images are sequential
+    std::sort(all_filenames.begin(), all_filenames.end());
+
+    if (all_filenames.empty()) {
+        LOG_S(WARNING) << "No images found in " << input_path << ". Nothing to stitch.\n";
+        return;
+    }
+
+    LOG_S(INFO) << "Found " << all_filenames.size() << " images to stitch.\n";
+
+    // 3. Load and preprocess all images
+    std::vector<cv::Mat> all_images;
+    all_images.reserve(all_filenames.size());
+    Preprocess preprocessor;
+
+    for (const auto& filename : all_filenames) {
+        cv::Mat img = cv::imread(filename);
+        if (!img.empty()) {
+            if (preprocess) {
+                // Apply custom preprocessing before further processing
+                // Removes the 20px green bar from Daniel's camera processing
+                img = preprocessor.cropRight(img);
+            }
+            img = readAndResize(img, max_dim);
+            all_images.push_back(img);
+        } else {
+            LOG_S(WARNING) << "Warning: Failed to load image: " << filename << "\n";
+        }
+    }
+
+    if (all_images.empty()) {
+        LOG_S(WARNING) << "No valid images loaded. Cannot stitch.\n";
+        return;
+    }
+
+    LOG_S(INFO) << "Loaded " << all_images.size() << " images. Starting stitching...\n";
+
+    // 4. Stitch all images at once
+    auto [status, stitched] =
+        globalStitch(all_images, mode, max_dim, false);  // preprocess=false since already done
+
+    // 5. Save result
+    if (status == cv::Stitcher::OK && !stitched.empty()) {
+        // Create output directory if it doesn't exist
+        fs::path output_file_path(output_path);
+        fs::path output_dir = output_file_path.parent_path();
+        if (!output_dir.empty() && !fs::exists(output_dir)) {
+            fs::create_directories(output_dir);
+        }
+
+        // If output_path is a directory, create a filename
+        std::string final_output_path = output_path;
+        if (fs::is_directory(output_path) || output_path.back() == '/') {
+            final_output_path = output_path + "/direct_stitch_" + currentDateTimeStr() + ".jpg";
+        }
+
+        if (cv::imwrite(final_output_path, stitched)) {
+            LOG_S(INFO) << "Direct stitch result saved to: " << final_output_path << "\n";
+        } else {
+            LOG_S(WARNING) << "Failed to save stitched image to " << final_output_path << "\n";
+        }
+    } else {
+        LOG_S(ERROR) << "Direct stitching failed. Status = " << static_cast<int>(status) << "\n";
+    }
+
+    // 6. Free memory
+    all_images.clear();
+}
+
 // -----------------------------------------------------------------------------
 // reset()
 // -----------------------------------------------------------------------------
@@ -266,5 +355,5 @@ void Mapping::reset() {
     processed_image_count = 0;
     chunk_counter = 0;
     current_run_folder.clear();
-    std::cout << "Mapping state has been reset.\n";
+    LOG_F(INFO, "Mapping state has been reset.\n");
 }

src/ticks/manual_landing.cpp

@@ -2,20 +2,56 @@
 
 #include <memory>
 
+#include "cv/mapping.hpp"
 #include "ticks/ids.hpp"
 #include "utilities/constants.hpp"
 
+namespace fs = std::filesystem;
+
 ManualLandingTick::ManualLandingTick(std::shared_ptr<MissionState> state, Tick* next_tick)
     :Tick(state, TickID::ManualLanding), next_tick(next_tick) {}
 
-std::chrono::milliseconds ManualLandingTick::getWait() const {
-    return MANUAL_LANDING_TICK_WAIT;
-}
+std::chrono::milliseconds ManualLandingTick::getWait() const { return MANUAL_LANDING_TICK_WAIT; }
 
 Tick* ManualLandingTick::tick() {
-    // TODO: Test this in mock testflight
-    if (!state.get()->getMav()->isArmed()) {
-        return next_tick;
+    if (state->getDroppedAirdrops().size() >= NUM_AIRDROPS) {
+        if (state->getMappingIsDone() == false) {
+            // Currently does a two pass at the end
+            // Probably should update for next year to optimize it.
+
+            cv::Stitcher::Mode scan_mode = cv::Stitcher::SCANS;
+            // Direct stitching of all images in the mapping folder
+
+            fs::path base_dir = "../images/mapping";
+            fs::path output_dir = base_dir / "output";
+
+            // Make sure the output directory exists (create if it doesn't)
+            if (!fs::exists(output_dir)) {
+                fs::create_directories(output_dir);
+            }
+
+            // TODO: Change this to be a config setting later
+            Mapping mapper;
+            const int chunk_size = 5;
+            const int chunk_overlap = 2;
+            const int max_dim = 3000;
+
+            LOG_F(INFO, "First pass stitching...");
+            mapper.firstPass(base_dir.string(), output_dir.string(), chunk_size,
+            chunk_overlap, scan_mode, max_dim, true);
+
+            LOG_F(INFO, "Second pass stitching...");
+            mapper.secondPass(output_dir.string(), scan_mode, max_dim, true);
+            state->setMappingIsDone(true);
+
+            LOG_F(INFO, "Mapping complete.");
+        }
+    } else {
+        // TODO: Test this in mock testflight
+        if (!state.get()->getMav()->isArmed()) {
+            return next_tick;
+        }
     }
+
     return nullptr;
 }