[FEAT][V1.0] Added occupancy mapping pipeline code with optimisations

-- Added basic mapping pipeline code linking bresenham algo with ray trace function
-- Added function to process lidar scan points - converts from sensor frame to world frame
and then performs ray trace
-- Added compiler optimisations for faster floating point additions.
-- Optimised emplace back logic in bresenham algo
-- Increased unordered set size to avoid hash collisions
-- Added timing across entire pipeline
-- Limited mapping and processing pipeline to first 10% of entire lidar point cloud dataset.
-- Updated occupancy probability logic code
-- Reduced the average scan time from 450ms to 350ms

Author: saiga006

CMakeLists.txt

@@ -24,10 +24,21 @@ target_include_directories(visualizer INTERFACE src/visualizer)
 target_link_libraries(visualizer INTERFACE Open3D::Open3D Eigen3::Eigen)
 
 #will have occupancy grid logic
-add_executable(occupancy_grid_main src/main.cpp src/occupancy_grid.cpp src/data_analyser.cpp)
+add_executable(occupancy_grid_main src/main.cpp src/occupancy_grid.cpp src/data_analyser.cpp src/scan_processor.cpp)
 
 target_link_libraries(occupancy_grid_main PRIVATE dataloader visualizer)
 
 target_compile_options(occupancy_grid_main PRIVATE -Wall -Wextra)
 
-target_compile_features(occupancy_grid_main PRIVATE cxx_std_17)
+target_compile_features(occupancy_grid_main PRIVATE cxx_std_17)
+
+# Release build optimizations
+if(CMAKE_BUILD_TYPE STREQUAL "Release")
+    target_compile_options(occupancy_grid_main PRIVATE 
+        -O3                 # Maximum optimization
+        -DNDEBUG           # Disable assertions  
+        -march=native      # CPU-specific optimization
+        -ffast-math        # Faster floating-point math
+        -funroll-loops     # Loop unrolling
+    )
+endif()

analyser.md

@@ -32,6 +32,7 @@ Robot trajectory bounds:
   Range: 276.928 343.819 47.8035
 Total poses: 6770
 Total points processed: 432690151
+Dataset Analysis time: 443.096 seconds 
 
 === OPTIMAL GRID CONFIGURATION ===
 Origin: -163.1 -254.1  -11.9
@@ -41,7 +42,7 @@ Total voxels: 7872308976
 Estimated memory: 30030.5 MB
 Grid covers: [-163.1 -254.1  -11.9] to [169.3 158.5  45.5]
 WARNING: Large memory usage! Consider:
-  - Increasing resolution (e.g., 0.5m instead of 0.2m)
+  - Increasing resolution (e.g., 0.2m instead of 0.1m)
   - Reducing safety margin
   - Processing subset of data first
 
@@ -56,6 +57,12 @@ Resolution: 0.2m (20cm per voxels)
 Total voxels: 758336304
 Estimated memory: 2892.82 MB
 Grid covers: [-149.2 -236.9   -9.5] to [155.6 141.5  43.1]
+OccupancyGrid3D initialized:
+  Dimensions: 1524×1892×263
+  Resolution: 0.2m
+  Origin: -149.2 -236.9   -9.5
+  Total voxels: 758336304
+Grid creation time: 1.56853 seconds 
 
 === STARTING MAPPING ===
 saiga@sai-Ideapad:~/Downloads/ModernCppProject2025/build$ cd ..
@@ -81,3 +88,109 @@ Coordinate Conversion Test:
 
 === BASIC FUNCTIONALITY TEST COMPLETED ===
 ```
+
+### Run with Timing Stats
+
+```bash
+saiga@sai-Ideapad:~/Downloads/ModernCppProject2025/build$ ./occupancy_grid_main ../src/Data
+Loading dataset...
+Found 6770 scans
+
+=== ANALYZING WORKSPACE ===
+Analyzing 6770 poses...
+Processed 0/6770 scans...
+Processed 338/6770 scans...
+Processed 676/6770 scans...
+Processed 1014/6770 scans...
+Processed 1352/6770 scans...
+Processed 1690/6770 scans...
+Processed 2028/6770 scans...
+Processed 2366/6770 scans...
+Processed 2704/6770 scans...
+Processed 3042/6770 scans...
+Processed 3380/6770 scans...
+Processed 3718/6770 scans...
+Processed 4056/6770 scans...
+Processed 4394/6770 scans...
+Processed 4732/6770 scans...
+Processed 5070/6770 scans...
+Processed 5408/6770 scans...
+Processed 5746/6770 scans...
+Processed 6084/6770 scans...
+Processed 6422/6770 scans...
+Processed 6760/6770 scans...
+=== WORKSPACE ANALYSIS ===
+Robot trajectory bounds:
+  Min: -135.352 -219.638 -7.10028
+  Max: 141.576  124.18 40.7033
+  Range: 276.928 343.819 47.8035
+Total poses: 6770
+Total points processed: 432690151
+Dataset Analysis time: 22.9543 seconds 
+
+=== OPTIMAL GRID CONFIGURATION ===
+Origin: -163.1 -254.1  -11.9
+Dimensions: 3324×4126×574
+Resolution: 0.1m (10cm voxels)
+Total voxels: 7872308976
+Estimated memory: 30030.5 MB
+Grid covers: [-163.1 -254.1  -11.9] to [169.3 158.5  45.5]
+WARNING: Large memory usage! Consider:
+  - Increasing resolution (e.g., 0.2m instead of 0.1m)
+  - Reducing safety margin
+  - Processing subset of data first
+
+=== APPLYING OPTIMIZATIONS ===
+Auto-adjusted resolution to: 0.5m
+Optimized memory usage: 148.276 MB
+
+=== OPTIMIZED GRID CONFIGURATION ===
+Origin: -138.2 -223.1   -7.6
+Dimensions: 565×702×98
+Resolution: 0.5m (50cm per voxels)
+Total voxels: 38869740
+Estimated memory: 148.276 MB
+Grid covers: [-138.2 -223.1   -7.6] to [144.3 127.9  41.4]
+
+=== STARTING MAPPING ===
+OccupancyGrid3D initialized:
+  Dimensions: 565×702×98
+  Resolution: 0.5m
+  Origin: -138.2 -223.1   -7.6
+  Total voxels: 38869740
+Grid creation time: 0.0459019 seconds 
+
+=== PROCESSING LIDAR SCANS ===
+Scan completed. Best rayTrace: 0.088 us, Worst rayTrace: 11.628 us
+Mapped :: 0/6770 scans...
+Scan completed. Best rayTrace: 0.099 us, Worst rayTrace: 13.404 us
+Scan completed. Best rayTrace: 0.033 us, Worst rayTrace: 13.938 us
+Scan completed. Best rayTrace: 0.034 us, Worst rayTrace: 13.828 us
+Scan completed. Best rayTrace: 0.033 us, Worst rayTrace: 11.217 us
+Scan completed. Best rayTrace: 0.035 us, Worst rayTrace: 13.52 us
+Scan completed. Best rayTrace: 0.031 us, Worst rayTrace: 52.029 us
+Scan completed. Best rayTrace: 0.031 us, Worst rayTrace: 16.557 us
+Scan completed. Best rayTrace: 0.033 us, Worst rayTrace: 22.091 us
+Scan completed. Best rayTrace: 0.055 us, Worst rayTrace: 137.093 us
+Scan completed. Best rayTrace: 0.033 us, Worst rayTrace: 160.792 us
+Scan completed. Best rayTrace: 0.033 us, Worst rayTrace: 173.393 us
+Scan completed. Best rayTrace: 0.034 us, Worst rayTrace: 217.114 us
+Scan completed. Best rayTrace: 0.034 us, Worst rayTrace: 405.679 us
+Scan completed. Best rayTrace: 0.034 us, Worst rayTrace: 115.71 us
+Scan completed. Best rayTrace: 0.034 us, Worst rayTrace: 155.032 us
+Scan completed. Best rayTrace: 0.034 us, Worst rayTrace: 167.785 us
+Mapped :: 677/6770 scans...
+Mapping completed!
+=== SCAN STATISTICS ===
+Total scans processed: 678
+Total execution time: 240.746 seconds
+Best scan time: 0.0372507 seconds Worst scan time: 0.615129 seconds 
+Average time per scan: 354.904 ms
+Scan rate: 2.81624 scans/second
+=== EXTRACTING OCCUPIED VOXELS ===
+Extracted 40985 occupied voxels from 40985 tracked voxels (vs 38869740 total voxels)
+Voxel extraction time: 0.00129689 seconds.
+Visualising Voxels using open3D ===
+Visualizing 40985 occupied voxels...
+Visualization time: 54.764 seconds.
+```

src/data_analyser.cpp

@@ -22,17 +22,10 @@ WorkspaceBounds DataAnalyzer::analyzeDataset(const dataloader::Dataset& dataset,
         max_coords = max_coords.cwiseMax(robot_pos);
 
         if (include_sensor_points) {
+            const Eigen::Matrix3d rotation = pose.block<3,3>(0,0);
             // Transform sensor points to world frame and include in bounds
             for (const auto& sensor_point : sensor_points) {
-                // Manual transformation
-                Eigen::Vector4d point_homogeneous;
-                point_homogeneous(0) = sensor_point(0);
-                point_homogeneous(1) = sensor_point(1);
-                point_homogeneous(2) = sensor_point(2);
-                point_homogeneous(3) = 1.0;
-                
-                Eigen::Vector4d world_point_h = pose * point_homogeneous;
-                Eigen::Vector3d world_point = world_point_h.head<3>();
+                Eigen::Vector3d world_point = rotation * sensor_point + robot_pos;
 
                 // Update bounds
                 min_coords = min_coords.cwiseMin(world_point);
@@ -42,7 +35,7 @@ WorkspaceBounds DataAnalyzer::analyzeDataset(const dataloader::Dataset& dataset,
         }
 
         // Progress indicator
-        if (idx % (dataset.size() / 10) == 0) {
+        if (idx % (dataset.size() / 20) == 0) {
             std::cout << "Processed " << idx << "/" << dataset.size() << " scans..." << std::endl;
         }
     }
@@ -85,3 +78,5 @@ std::array<size_t, 3> DataAnalyzer::estimateGridDimensions(const WorkspaceBounds
 }
 
 
+
+

src/data_analyser.hpp

@@ -30,4 +30,5 @@ class DataAnalyzer {
     static std::array<size_t, 3> estimateGridDimensions(const WorkspaceBounds& bounds,
                                                        double resolution,
                                                        double safety_margin = 0.15);
+
 };

src/main.cpp

@@ -1,8 +1,36 @@
 #include <iostream>
+#include <chrono>
+#include <algorithm>
+#include <vector>
+#include <numeric>
+#include <functional>
 #include "occupancy_grid.hpp"
 #include "data_analyser.hpp"
+#include "scan_processor.hpp"
+#include "visualizer.hpp"
+//#define TEST
+
+constexpr float OCCUPANCY_THRESHOLD = 0.5f;
+
+void printTimingStats(const std::vector<double>& scan_times, double total_time) {
+    std::cout << "=== SCAN STATISTICS ===" << std::endl;
+    if (scan_times.empty()) {
+
+        std::cout << "No scan data available!" << std::endl;
+        return;
+    }
+    
+    double avg_scan_time = std::reduce(scan_times.begin(), scan_times.end(), 0.0,std::plus<double>()) / scan_times.size();
+    const auto [min,max] = std::minmax_element(begin(scan_times), end(scan_times));
+    std::cout << "Total scans processed: " << scan_times.size() << std::endl;
+    std::cout << "Total execution time: " << total_time << " seconds" << std::endl;
+    std::cout << "Best scan time: " << *min << " seconds " << "Worst scan time: " << *max << " seconds " << std::endl;
+    std::cout << "Average time per scan: " << avg_scan_time * 1000 << " ms" << std::endl;
+    std::cout << "Scan rate: " << scan_times.size() / total_time << " scans/second" << std::endl;
+}
+
+
 
-#define TEST
 
 int main(int argc, char* argv[]) {
 
@@ -13,7 +41,7 @@ int main(int argc, char* argv[]) {
     }
 
     const std::string data_dir = argv[1];
-    
+    auto start_analys_time = std::chrono::high_resolution_clock::now();
     // Load dataset
     std::cout << "Loading dataset..." << std::endl;
     dataloader::Dataset dataset(data_dir);
@@ -23,7 +51,9 @@ int main(int argc, char* argv[]) {
     std::cout << "\n=== ANALYZING WORKSPACE ===" << std::endl;
     WorkspaceBounds bounds = DataAnalyzer::analyzeDataset(dataset, true);
     bounds.printSummary();
-    
+    auto end_analys_time = std::chrono::high_resolution_clock::now();
+    double total_analysis_time = std::chrono::duration<double>(end_analys_time - start_analys_time).count();
+    std::cout << "Dataset Analysis time: " << total_analysis_time << " seconds " << std::endl;
     // Determine optimal parameters
     double resolution = 0.1;  // 10cm voxels - adjust as needed
     double safety_margin = 0.10;  // 10% margin around workspace
@@ -58,8 +88,8 @@ int main(int argc, char* argv[]) {
         double target_voxels = (target_memory_gb * 1024 * 1024 * 1024) / sizeof(float);
         double optimized_resolution = std::pow(volume / target_voxels, 1.0/3.0);
 
-        resolution = std::max(optimized_resolution, 0.2);  // Minimum 20cm
-        safety_margin = 0.05;  // Reduced safety margin
+        resolution = std::max(optimized_resolution, 0.5);  // Minimum 50cm
+        safety_margin = 0.01;  // Reduced safety margin
 
         std::cout << "Auto-adjusted resolution to: " << resolution << "m" << std::endl;
 
@@ -83,12 +113,75 @@ int main(int argc, char* argv[]) {
                 << (optimal_origin + Eigen::Vector3d(grid_dims[0]*resolution, grid_dims[1]*resolution, grid_dims[2]*resolution)).transpose() << "]" << std::endl;
     }
 
-
-    
-    // Create optimally sized grid
-    //OccupancyGrid grid(grid_dims, resolution, optimal_origin);
-    
     std::cout << "\n=== STARTING MAPPING ===" << std::endl;
+    auto start_time = std::chrono::high_resolution_clock::now();
+    // Create optimally sized grid
+    OccupancyGrid grid(grid_dims, resolution, optimal_origin);
+    auto end_time = std::chrono::high_resolution_clock::now();
+    double total_time = std::chrono::duration<double>(end_time - start_time).count();
+    std::cout << "Grid creation time: " << total_time << " seconds " << std::endl;
+    
+    std::cout << "\n=== PROCESSING LIDAR SCANS ===" << std::endl;
+    std::vector<double> scan_times;
+    scan_times.reserve(dataset.size());
+
+    auto mapping_start = std::chrono::high_resolution_clock::now();
+
+    // test var
+    int scan_count = 0;
+    // Process each scan
+    for (size_t i = 0; i < dataset.size(); ++i) {
+        auto scan_start = std::chrono::high_resolution_clock::now();
+        
+        // Load pose and pointcloud from the dataset
+        const auto& [pose, pointcloud] = dataset[i];
+        
+        // Process the scan using your function
+        ScanProcessor::processLidarScan(grid, pose, pointcloud);
+        
+        auto scan_end = std::chrono::high_resolution_clock::now();
+        double scan_time = std::chrono::duration<double>(scan_end - scan_start).count();
+        scan_times.emplace_back(scan_time);
+        
+        // Progress indicator
+        if (i % (dataset.size() / 10) == 0) {
+            std::cout << "Mapped :: " << i << "/" << dataset.size() << " scans..." << std::endl;
+            scan_count++;
+            if (scan_count > 1) {
+                // TODO: fix this issue: for now: restrict the scan processing to 10% of lidar dataset, as the scan time reaches 250 seconds.
+                break;
+            }
+        }
+    }
+
+    auto mapping_end = std::chrono::high_resolution_clock::now();
+    double total_mapping_time = std::chrono::duration<double>(mapping_end - mapping_start).count();
+
+    std::cout << "Mapping completed!" << std::endl;
+    printTimingStats(scan_times, total_mapping_time);
+
+    std::cout << "=== EXTRACTING OCCUPIED VOXELS ===" << std::endl;
+    auto extraction_start = std::chrono::high_resolution_clock::now();
+    Vector3dVector occupied_voxels = grid.getOccupiedVoxels(OCCUPANCY_THRESHOLD);
+    auto extraction_end = std::chrono::high_resolution_clock::now();
+    double extraction_time = std::chrono::duration<double>(extraction_end - extraction_start).count();
+    std::cout << "Voxel extraction time: " << extraction_time << " seconds." << std::endl;
+
+    if(!occupied_voxels.empty()) {
+
+        std::cout << "Visualising Voxels using open3D ===" << std::endl;
+        std::cout << "Visualizing " << occupied_voxels.size() << " occupied voxels..." << std::endl;
+        auto viz_start = std::chrono::high_resolution_clock::now();
+        visualize(occupied_voxels);  // Calls open3d API
+        auto viz_end = std::chrono::high_resolution_clock::now();
+        double viz_time = std::chrono::duration<double>(viz_end - viz_start).count();
+        std::cout << "Visualization time: " << viz_time << " seconds." << std::endl;
+    } else {
+        std::cout << "No occupied voxels found! Check mapping pipeline." <<  std::endl;
+    }
+
+
+
  #else
         // Test with small grid first
     std::cout << "=== TESTING BASIC FUNCTIONALITY ===" << std::endl;