Added OccupancyGrid class definition and Dataset Analyser code

saiga006 · saiga006 · commit 3cc435184619 · 2025-08-17T04:27:57.000+02:00
-- Added occupancygrid class with member function prototypes
-- Added dataset analyser to read the pose and determine
the range of the occupancy grid from the laser scan data.
-- Dataset Analyser gives the optimal grid origin, voxel resolution
and grid dimensions needed for the mapping.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -24,7 +24,7 @@ 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)
+add_executable(occupancy_grid_main src/main.cpp src/data_analyser.cpp)
 
 target_link_libraries(occupancy_grid_main PRIVATE dataloader visualizer)
 
diff --git a/analyser.md b/analyser.md
@@ -0,0 +1,62 @@
+## Build and Execution Output
+
+```bash
+saiga@sai-Ideapad:~/Downloads/ModernCppProject2025$ cmake --build build
+Consolidate compiler generated dependencies of target dataloader
+[ 40%] Built target dataloader
+Consolidate compiler generated dependencies of target occupancy_grid_main
+[ 60%] Building CXX object CMakeFiles/occupancy_grid_main.dir/src/main.cpp.o
+[ 80%] Linking CXX executable occupancy_grid_main
+[100%] Built target occupancy_grid_main
+saiga@sai-Ideapad:~/Downloads/ModernCppProject2025$ cd build/
+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 677/6770 scans...
+Processed 1354/6770 scans...
+Processed 2031/6770 scans...
+Processed 2708/6770 scans...
+Processed 3385/6770 scans...
+Processed 4062/6770 scans...
+Processed 4739/6770 scans...
+Processed 5416/6770 scans...
+Processed 6093/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
+
+=== 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.5m instead of 0.2m)
+  - Reducing safety margin
+  - Processing subset of data first
+
+=== APPLYING OPTIMIZATIONS ===
+Auto-adjusted resolution to: 0.2m
+Optimized memory usage: 2892.82 MB
+
+=== OPTIMIZED GRID CONFIGURATION ===
+Origin: -149.2 -236.9   -9.5
+Dimensions: 1524×1892×263
+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]
+
+=== STARTING MAPPING ===
+saiga@sai-Ideapad:~/Downloads/ModernCppProject2025/build$ cd ..
+```
diff --git a/src/data_analyser.cpp b/src/data_analyser.cpp
@@ -0,0 +1,87 @@
+#include "data_analyser.hpp"
+#include <iostream>
+#include <algorithm>
+
+WorkspaceBounds DataAnalyzer::analyzeDataset(const dataloader::Dataset& dataset, 
+                                                 bool include_sensor_points) {
+    // Initialize bounds with extreme values
+    Eigen::Vector3d min_coords(1e9, 1e9, 1e9);
+    Eigen::Vector3d max_coords(-1e9, -1e9, -1e9);
+    size_t total_points = 0;
+    
+    std::cout << "Analyzing " << dataset.size() << " poses..." << std::endl;
+    
+    for (size_t idx = 0; idx < dataset.size(); ++idx) {
+        const auto& [pose, sensor_points] = dataset[idx];
+        
+        // Extract robot position from pose matrix (translation part)
+        Eigen::Vector3d robot_pos(pose(0,3), pose(1,3), pose(2,3));
+        
+        // Update bounds with robot position
+        min_coords = min_coords.cwiseMin(robot_pos);
+        max_coords = max_coords.cwiseMax(robot_pos);
+        
+        if (include_sensor_points) {
+            // 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>();
+                
+                // Update bounds
+                min_coords = min_coords.cwiseMin(world_point);
+                max_coords = max_coords.cwiseMax(world_point);
+                total_points++;
+            }
+        }
+        
+        // Progress indicator
+        if (idx % (dataset.size() / 10) == 0) {
+            std::cout << "Processed " << idx << "/" << dataset.size() << " scans..." << std::endl;
+        }
+    }
+    
+    Eigen::Vector3d range = max_coords - min_coords;
+    
+    return {min_coords, max_coords, range, dataset.size(), total_points};
+}
+
+Eigen::Vector3d DataAnalyzer::estimateOptimalOrigin(const WorkspaceBounds& bounds, 
+                                                        double safety_margin) {
+    // Add safety margin around the workspace
+    Eigen::Vector3d margin = bounds.range * safety_margin;
+    Eigen::Vector3d optimal_origin = bounds.min_coords - margin;
+    
+    // Optional: Round to nice numbers for easier debugging
+    for (int i = 0; i < 3; ++i) {
+        optimal_origin(i) = std::floor(optimal_origin(i) * 10.0) / 10.0; // Round to 0.1m
+    }
+    
+    return optimal_origin;
+}
+
+std::array<size_t, 3> DataAnalyzer::estimateGridDimensions(const WorkspaceBounds& bounds,
+                                                              double resolution,
+                                                              double safety_margin) {
+    // Calculate total range including margins
+    Eigen::Vector3d margin = bounds.range * safety_margin;
+    Eigen::Vector3d total_range = bounds.range + 2.0 * margin; // margin on both sides
+    
+    std::array<size_t, 3> dimensions;
+    for (int i = 0; i < 3; ++i) {
+        dimensions[i] = static_cast<size_t>(std::ceil(total_range(i) / resolution));
+        
+        // Ensure minimum dimensions
+        dimensions[i] = std::max(dimensions[i], static_cast<size_t>(10));
+    }
+    
+    return dimensions;
+}
+
+
diff --git a/src/data_analyser.hpp b/src/data_analyser.hpp
@@ -0,0 +1,33 @@
+#pragma once
+#include <Eigen/Core>
+#include "dataloader.hpp"
+#include <iostream>
+
+struct WorkspaceBounds {
+    Eigen::Vector3d min_coords;
+    Eigen::Vector3d max_coords;
+    Eigen::Vector3d range;
+    size_t total_poses;
+    size_t total_points;
+    
+    void printSummary() const {
+        std::cout << "=== WORKSPACE ANALYSIS ===" << std::endl;
+        std::cout << "Robot trajectory bounds:" << std::endl;
+        std::cout << "  Min: " << min_coords.transpose() << std::endl;
+        std::cout << "  Max: " << max_coords.transpose() << std::endl;
+        std::cout << "  Range: " << range.transpose() << std::endl;
+        std::cout << "Total poses: " << total_poses << std::endl;
+        std::cout << "Total points processed: " << total_points << std::endl;
+    }
+};
+
+class DataAnalyzer {
+public:
+    static WorkspaceBounds analyzeDataset(const dataloader::Dataset& dataset, 
+                                        bool include_sensor_points = true);
+    static Eigen::Vector3d estimateOptimalOrigin(const WorkspaceBounds& bounds, 
+                                               double safety_margin = 0.15);
+    static std::array<size_t, 3> estimateGridDimensions(const WorkspaceBounds& bounds,
+                                                       double resolution,
+                                                       double safety_margin = 0.15);
+};
diff --git a/src/main.cpp b/src/main.cpp
@@ -1,7 +1,90 @@
-#include<iostream>
-using std::cout;
-using std::endl;
+#include <iostream>
+#include "occupancy_grid.hpp"
+#include "data_analyser.hpp"
 
-int main() {
-    cout << "Hello World!" << endl;
-}
+int main(int argc, char* argv[]) {
+    if (argc != 2) {
+        std::cerr << "Usage: " << argv[0] << " <data_directory>" << std::endl;
+        return -1;
+    }
+    
+    const std::string data_dir = argv[1];
+    
+    // Load dataset
+    std::cout << "Loading dataset..." << std::endl;
+    dataloader::Dataset dataset(data_dir);
+    std::cout << "Found " << dataset.size() << " scans" << std::endl;
+    
+    // Analyze workspace to determine optimal grid parameters
+    std::cout << "\n=== ANALYZING WORKSPACE ===" << std::endl;
+    WorkspaceBounds bounds = DataAnalyzer::analyzeDataset(dataset, true);
+    bounds.printSummary();
+    
+    // Determine optimal parameters
+    double resolution = 0.1;  // 10cm voxels - adjust as needed
+    double safety_margin = 0.10;  // 10% margin around workspace
+    
+    Eigen::Vector3d optimal_origin = DataAnalyzer::estimateOptimalOrigin(bounds, safety_margin);
+    std::array<size_t, 3> grid_dims = DataAnalyzer::estimateGridDimensions(bounds, resolution, safety_margin);
+    
+    // Calculate memory usage
+    size_t total_voxels = grid_dims[0] * grid_dims[1] * grid_dims[2];
+    double memory_mb = (total_voxels * sizeof(float)) / (1024.0 * 1024.0);
+    
+    std::cout << "\n=== OPTIMAL GRID CONFIGURATION ===" << std::endl;
+    std::cout << "Origin: " << optimal_origin.transpose() << std::endl;
+    std::cout << "Dimensions: " << grid_dims[0] << "×" << grid_dims[1] << "×" << grid_dims[2] << std::endl;
+    std::cout << "Resolution: " << resolution << "m (" << (resolution*100) << "cm voxels)" << std::endl;
+    std::cout << "Total voxels: " << total_voxels << std::endl;
+    std::cout << "Estimated memory: " << memory_mb << " MB" << std::endl;
+    std::cout << "Grid covers: [" << optimal_origin.transpose() << "] to [" 
+              << (optimal_origin + Eigen::Vector3d(grid_dims[0]*resolution, grid_dims[1]*resolution, grid_dims[2]*resolution)).transpose() << "]" << std::endl;
+    
+    // Warn if memory usage is too high
+    if (memory_mb > 5000) {  // > 5GB
+        std::cout << "WARNING: Large memory usage! Consider:" << std::endl;
+        std::cout << "  - Increasing resolution (e.g., 0.2m instead of 0.1m)" << std::endl;
+        std::cout << "  - Reducing safety margin" << std::endl;
+        std::cout << "  - Processing subset of data first" << std::endl;
+        std::cout << "\n=== APPLYING OPTIMIZATIONS ===" << std::endl;
+        
+        // Auto-adjust resolution
+        double target_memory_gb = 5.0;  // Target 2GB
+        double volume = bounds.range.x() * bounds.range.y() * bounds.range.z();
+        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
+        
+        std::cout << "Auto-adjusted resolution to: " << resolution << "m" << std::endl;
+        
+        // Recalculate with optimized parameters
+        optimal_origin = DataAnalyzer::estimateOptimalOrigin(bounds, safety_margin);
+        grid_dims = DataAnalyzer::estimateGridDimensions(bounds, resolution, safety_margin);
+        
+        // Recalculate memory
+        total_voxels = grid_dims[0] * grid_dims[1] * grid_dims[2];
+        memory_mb = (total_voxels * sizeof(float)) / (1024.0 * 1024.0);
+        
+        std::cout << "Optimized memory usage: " << memory_mb << " MB" << std::endl;
+    
+        std::cout << "\n=== OPTIMIZED GRID CONFIGURATION ===" << std::endl;
+        std::cout << "Origin: " << optimal_origin.transpose() << std::endl;
+        std::cout << "Dimensions: " << grid_dims[0] << "×" << grid_dims[1] << "×" << grid_dims[2] << std::endl;
+        std::cout << "Resolution: " << resolution << "m (" << (resolution*100) << "cm per voxels)" << std::endl;
+        std::cout << "Total voxels: " << total_voxels << std::endl;
+        std::cout << "Estimated memory: " << memory_mb << " MB" << std::endl;
+        std::cout << "Grid covers: [" << optimal_origin.transpose() << "] to [" 
+                << (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;
+    
+    return 0;
+}
diff --git a/src/occupancy_grid.hpp b/src/occupancy_grid.hpp
@@ -0,0 +1,57 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <array>
+#include <vector>
+
+using Vector3dVector = std::vector<Eigen::Vector3d>;
+using Vector3d = Eigen::Vector3d;
+using Vector3i = Eigen::Vector3i;
+
+//TODO: implement the member functions 
+class OccupancyGrid {
+    private:
+        std::array<size_t, 3> voxel_dimensions;
+        double voxel_resolution;
+        // links grid coordinates with world coordinates
+        Vector3d grid_origin;
+        std::vector<float> grid_data;
+        // connects the start and end point of lidar ray in voxel map
+        std::vector<Vector3i> draw_bresenham3d_line(const Vector3i& start, const Vector3i& end) const;
+        // updates the log odds aka grid data for the voxel
+        void update_occupied(const Vector3i& idx);
+        void update_free(const Vector3i& idx);
+        // check if the index within the grid boundary
+        bool is_InBounds(const Vector3i& idx) const;
+        // converts voxel index 3D to 1D index to fetch grid data when required.
+        size_t to_LinearIndex(const Vector3i& idx) const;
+    public:
+        OccupancyGrid(const std::array<size_t, 3>& dimensions, double resolution, const Vector3d& origin);
+        
+        // TODO: implement transformations
+        Vector3i world_to_grid(const Vector3d& world_coord) const;
+        Vector3d grid_to_world(const Vector3i& grid_index) const;
+        // to update occupancy probability along the lidar scan
+        void rayTrace(const Vector3d& start, const Vector3d& end);
+        // to return the voxels which are occupied either by obstacles or grid boundary
+        Vector3dVector getOccupiedVoxels(float threshold = 0.0f) const;
+        //to detect if the lidar ray is crossing the grid boundary
+        Vector3i findGridBoundaryIntersection(const Vector3i& start, const Vector3i& end) const;
+};
+
+
+
+// namespace dataloader
+// {
+//     class Dataset
+//     {
+//     public:
+//         Dataset(const std::string &data_dir);
+//         std::size_t size() const { return pointcloud_files_.size(); }
+//         PoseAndCloud operator[](const int idx) const;
+
+//     private:
+//         std::vector<std::string> pointcloud_files_;
+//         std::vector<Eigen::Matrix4d> poses_;
+//     };
+// } // namespace dataloader
