Add basic occupancy_grid_implementation

-- Added placeholder implementation for bresenham line algo and ray trace.
-- Added world to grid and grid to world and error utility functions
-- Added test code to check whether the basic occupancy grid with const log odds
working fine.

Author: saiga006

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 src/data_analyser.cpp)
+add_executable(occupancy_grid_main src/main.cpp src/occupancy_grid.cpp src/data_analyser.cpp)
 
 target_link_libraries(occupancy_grid_main PRIVATE dataloader visualizer)
 

src/main.cpp

@@ -2,7 +2,11 @@
 #include "occupancy_grid.hpp"
 #include "data_analyser.hpp"
 
+#define TEST
+
 int main(int argc, char* argv[]) {
+
+#ifndef TEST
     if (argc != 2) {
         std::cerr << "Usage: " << argv[0] << " <data_directory>" << std::endl;
         return -1;
@@ -85,6 +89,40 @@ int main(int argc, char* argv[]) {
     //OccupancyGrid grid(grid_dims, resolution, optimal_origin);
 
     std::cout << "\n=== STARTING MAPPING ===" << std::endl;
+ #else
+        // Test with small grid first
+    std::cout << "=== TESTING BASIC FUNCTIONALITY ===" << std::endl;
+    
+    std::array<size_t, 3> test_dims = {50, 50, 25}; // Small test grid
+    double test_resolution = 1.0; // 1m voxels for testing
+    Vector3d test_origin(-25.0, -25.0, 0.0);
+    
+    OccupancyGrid grid(test_dims, test_resolution, test_origin);
+    
+    // Test coordinate conversions
+    Vector3d test_world(5.5, -3.2, 2.8);
+    Vector3i test_grid = grid.world_to_grid(test_world);
+    Vector3d back_world = grid.grid_to_world(test_grid);
+    
+    std::cout << "Coordinate Conversion Test:" << std::endl;
+    std::cout << "  World: " << test_world.transpose() << std::endl;
+    std::cout << "  Grid: " << test_grid.transpose() << std::endl;
+    std::cout << "  Back to World: " << back_world.transpose() << std::endl;
+    std::cout << "  In Bounds: " << (grid.is_InBounds(test_grid) ? "YES" : "NO") << std::endl;
 
+    // Test occupancy updates
+    grid.update_occupied(test_grid);
+    std::cout << "  Occupancy after update: " << grid.getOccupancy(test_grid) << std::endl;
+    
+    // Test multiple updates
+    for (int i = 0; i < 5; ++i) {
+        grid.update_occupied(test_grid);
+    }
+    std::cout << "  Occupancy after 5 updates: " << grid.getOccupancy(test_grid) << std::endl;
+    
+    std::cout << "\n=== BASIC FUNCTIONALITY TEST COMPLETED ===" << std::endl;
+ #endif
+
+
     return 0;
 }

src/occupancy_grid.cpp

@@ -0,0 +1,107 @@
+#include "occupancy_grid.hpp"
+#include <algorithm>
+#include <iostream>
+#include <cmath>
+
+
+// Log-odds constants
+constexpr float LOG_ODDS_OCCUPIED = 0.85f;
+constexpr float LOG_ODDS_FREE = -0.4f;
+constexpr float LOG_ODDS_MIN = -5.0f;
+constexpr float LOG_ODDS_MAX = 5.0f;
+
+OccupancyGrid::OccupancyGrid(const std::array<size_t, 3>& dimensions, 
+    double resolution, const Vector3d& origin)
+    :grid_dimensions(dimensions), voxel_resolution(resolution), grid_origin(origin) {
+
+    size_t total_cells = grid_dimensions[0] * grid_dimensions[1] * grid_dimensions[2];
+    grid_data.resize(total_cells,0.0f);
+    
+    std::cout << "OccupancyGrid3D initialized:" << std::endl;
+    std::cout << "  Dimensions: " << grid_dimensions[0] << "×" << grid_dimensions[1] << "×" << grid_dimensions[2] << std::endl;
+    std::cout << "  Resolution: " << voxel_resolution << "m" << std::endl;
+    std::cout << "  Origin: " << grid_origin.transpose() << std::endl;
+    std::cout << "  Total voxels: " << total_cells << std::endl;
+
+}
+
+// converts world coordinates to grid coordinates, assuming grid origin starts in bottom left corner of the map.
+Vector3i OccupancyGrid::world_to_grid(const Vector3d& world_coord) const {
+    Vector3d diff = world_coord - grid_origin;
+    return Vector3i(
+        static_cast<int>(std::floor(diff.x()/voxel_resolution)),
+        static_cast<int>(std::floor(diff.y()/voxel_resolution)),
+        static_cast<int>(std::floor(diff.z()/voxel_resolution))
+    );
+}
+
+// converts grid coordinates to world coordinates 
+Vector3d OccupancyGrid::grid_to_world(const Vector3i& grid_index) const {
+    return grid_origin + Vector3d((grid_index.x() + 0.5) * voxel_resolution,
+                                (grid_index.y() + 0.5) * voxel_resolution,
+                                (grid_index.z() + 0.5) * voxel_resolution);
+}
+
+// check if the index is valid and within the grid dimensions
+bool OccupancyGrid::is_InBounds(const Vector3i& idx) const {
+    return idx.x() >= 0 && idx.x() < static_cast<int>(grid_dimensions[0]) &&
+           idx.y() >= 0 && idx.y() < static_cast<int>(grid_dimensions[1]) &&
+           idx.z() >= 0 && idx.z() < static_cast<int>(grid_dimensions[2]);
+}
+
+//converts 3d grid coordinates to 1d index.
+size_t OccupancyGrid::to_LinearIndex(const Vector3i& idx) const {
+    return static_cast<size_t>(idx.z()) * grid_dimensions[0] * grid_dimensions[1] +
+           static_cast<size_t>(idx.y()) * grid_dimensions[0] +
+           static_cast<size_t>(idx.x());
+}
+
+// updates occupancy
+void OccupancyGrid::update_occupied(const Vector3i& idx) {
+    if(!is_InBounds(idx)) return;
+    size_t index = to_LinearIndex(idx);
+    grid_data[index] = std::clamp(grid_data[index]+ LOG_ODDS_OCCUPIED,
+         LOG_ODDS_MIN, LOG_ODDS_MAX);
+}
+
+// updates occupancy
+void OccupancyGrid::update_free(const Vector3i& idx) {
+    if(!is_InBounds(idx)) return;
+    size_t index = to_LinearIndex(idx);
+    grid_data[index] = std::clamp(grid_data[index]+ LOG_ODDS_FREE,
+         LOG_ODDS_MIN, LOG_ODDS_MAX);
+}
+
+// Simple getOccupancy for testing
+float OccupancyGrid::getOccupancy(const Vector3i& grid_index) const {
+    if (!is_InBounds(grid_index)) return 0.0f;
+    return grid_data[to_LinearIndex(grid_index)];
+}
+
+// PLACEHOLDER FUNCTIONS - IMPLEMENT
+std::vector<Vector3i> OccupancyGrid::draw_bresenham3d_line(const Vector3i& start, const Vector3i& end) const {
+    // TODO: Implement 3D Bresenham algorithm
+    // PRIORITY: CRITICAL - Core mapping algorithm
+    std::vector<Vector3i> voxels;
+    voxels.push_back(start);
+    voxels.push_back(end);  // Temporary placeholder
+    return voxels;
+}
+
+void OccupancyGrid::rayTrace(const Vector3d& start, const Vector3d& end) {
+    // TODO: Implement ray tracing
+    // PRIORITY: CRITICAL - Main occupancy update method
+    Vector3i start_grid = world_to_grid(start);
+    Vector3i end_grid = world_to_grid(end);
+    
+    if (is_InBounds(start_grid) && is_InBounds(end_grid)) {
+        update_occupied(end_grid);  // Temporary: just mark endpoint
+    }
+}
+
+Vector3dVector OccupancyGrid::getOccupiedVoxels(float threshold) const {
+    // TODO: Implement visualization extraction
+    // PRIORITY: HIGH - Needed for final visualization
+    Vector3dVector occupied_points;
+    return occupied_points;
+}

src/occupancy_grid.hpp

@@ -11,18 +11,18 @@ using Vector3i = Eigen::Vector3i;
 //TODO: implement the member functions 
 class OccupancyGrid {
     private:
-        std::array<size_t, 3> voxel_dimensions;
+        std::array<size_t, 3> grid_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);
+        // updates the log odds in the 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;
+        //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:
@@ -31,13 +31,19 @@ class OccupancyGrid {
         // TODO: implement transformations
         Vector3i world_to_grid(const Vector3d& world_coord) const;
         Vector3d grid_to_world(const Vector3i& grid_index) const;
+        //for simple test
+        float getOccupancy(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;
-};
+        
+        bool is_InBounds(const Vector3i& idx) const;
+        void update_occupied(const Vector3i& idx);
+    };