removed PCL dependency and combined scan and map loader

Author: ga58lar

Dockerfile

@@ -38,8 +38,8 @@ RUN apt-get update && apt-get install -y -q --no-install-recommends \
     libgflags-dev \
     libatlas-base-dev \
     libsuitesparse-dev \
-    libpcl-dev \
-    python3-pip
+    python3-pip \
+    liblzf-dev
 
 RUN pip install numpy ninja
 

Dockerfile.dev

@@ -38,8 +38,8 @@ RUN apt-get update && apt-get install -y -q --no-install-recommends \
     libgflags-dev \
     libatlas-base-dev \
     libsuitesparse-dev \
-    libpcl-dev \
-    python3-pip
+    python3-pip \
+    liblzf-dev
 
 RUN pip install numpy ninja
 

cpp/CMakeLists.txt

@@ -14,9 +14,11 @@ set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3")
 
 find_package(Eigen3 REQUIRED)
 find_package(Ceres REQUIRED)
-find_package(PCL REQUIRED)
 find_package(Iridescence REQUIRED)
 
+find_path(LIBLZF_INCLUDE_DIR lzf.h PATH_SUFFIXES liblzf)
+find_library(LIBLZF_LIBRARY NAMES lzf liblzf)
+
 # Fetch external dependencies
 include(FetchContent)
 FetchContent_Declare(small_gicp GIT_REPOSITORY https://github.com/ga58lar/small_gicp)
@@ -34,17 +36,15 @@ add_library(
   io/format/pcd.cpp
   io/format/ply.cpp
   io/loader_factory.cpp
-  io/map_loader.cpp
   pipeline/openlidarmap.cpp
   utils/file_utils.cpp
   utils/pose_utils.cpp
   utils/helpers.cpp)
 
-target_include_directories(openlidarmap_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} PRIVATE ${PCL_INCLUDE_DIRS}
-                                                                                           ${small_gicp_INCLUDE_DIRS})
+target_include_directories(openlidarmap_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR} PRIVATE ${small_gicp_INCLUDE_DIRS} ${LIBLZF_INCLUDE_DIR})
 
-target_link_libraries(openlidarmap_lib PUBLIC Eigen3::Eigen small_gicp ${PCL_LIBRARIES} Ceres::ceres
-                      PRIVATE Iridescence::Iridescence stdc++fs)
+target_link_libraries(openlidarmap_lib PUBLIC Eigen3::Eigen small_gicp Ceres::ceres
+                      PRIVATE Iridescence::Iridescence stdc++fs ${LIBLZF_LIBRARY})
 
 # Add executable
 add_executable(openlidarmap_cpp apps/main.cpp)

cpp/config/types.hpp

@@ -4,14 +4,8 @@
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
-// PCL
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-
 namespace openlidarmap {
 
-using PointT = pcl::PointXYZ;
-using PointCloudT = pcl::PointCloud<PointT>;
 using Vector7d = Eigen::Matrix<double, 7, 1>;
 
 }  // namespace openlidarmap

cpp/io/format/pcd.cpp

@@ -4,7 +4,7 @@
 #include <vector>
 #include <cstring>
 #include <iomanip>
-
+#include <liblzf/lzf.h>
 
 namespace openlidarmap::io {
 
@@ -78,42 +78,6 @@ bool ParsePCDHeader(std::ifstream& file, PCDHeader& header) {
     return true;
 }
 
-double UnpackBinary(const char* data_ptr, char type, int size) {
-    switch (type) {
-        case 'F': {
-            if (size == 4) {
-                float value;
-                std::memcpy(&value, data_ptr, sizeof(float));
-                return static_cast<double>(value);
-            } else if (size == 8) {
-                double value;
-                std::memcpy(&value, data_ptr, sizeof(double));
-                return value;
-            }
-            break;
-        }
-        case 'I': {
-            switch (size) {
-                case 1: return static_cast<double>(*reinterpret_cast<const int8_t*>(data_ptr));
-                case 2: return static_cast<double>(*reinterpret_cast<const int16_t*>(data_ptr));
-                case 4: return static_cast<double>(*reinterpret_cast<const int32_t*>(data_ptr));
-                case 8: return static_cast<double>(*reinterpret_cast<const int64_t*>(data_ptr));
-            }
-            break;
-        }
-        case 'U': {
-            switch (size) {
-                case 1: return static_cast<double>(*reinterpret_cast<const uint8_t*>(data_ptr));
-                case 2: return static_cast<double>(*reinterpret_cast<const uint16_t*>(data_ptr));
-                case 4: return static_cast<double>(*reinterpret_cast<const uint32_t*>(data_ptr));
-                case 8: return static_cast<double>(*reinterpret_cast<const uint64_t*>(data_ptr));
-            }
-            break;
-        }
-    }
-    throw std::runtime_error("Unsupported PCD data type or size");
-}
-
 small_gicp::PointCloud::Ptr PCDLoader::load(const std::string& file_path) {
     std::ifstream file(file_path, std::ios::binary);
     if (!file) {
@@ -140,7 +104,6 @@ small_gicp::PointCloud::Ptr PCDLoader::load(const std::string& file_path) {
             throw std::runtime_error("Missing XYZ fields in PCD file");
         }
 
-        // Reset file position and search for DATA line
         file.clear();
         file.seekg(0);
 
@@ -182,6 +145,79 @@ small_gicp::PointCloud::Ptr PCDLoader::load(const std::string& file_path) {
             }
             points_read++;
         }
+    } else if (header.is_compressed) {
+        file.clear();
+        file.seekg(0);
+        
+        std::string line;
+        while (std::getline(file, line)) {
+            if (line.find("DATA binary_compressed") != std::string::npos) {
+                break;
+            }
+        }
+
+        uint32_t compressed_size = 0, uncompressed_size = 0;
+        if (!file.read(reinterpret_cast<char*>(&compressed_size), sizeof(compressed_size)) ||
+            !file.read(reinterpret_cast<char*>(&uncompressed_size), sizeof(uncompressed_size))) {
+            throw std::runtime_error("Failed to read size info");
+        }
+
+        // Allocate buffers
+        std::vector<char> compressed_data(compressed_size);
+        std::vector<char> uncompressed_data(uncompressed_size);
+
+        // Read compressed data
+        if (!file.read(compressed_data.data(), compressed_size)) {
+            throw std::runtime_error("Failed to read compressed data");
+        }
+
+        // Decompress using LZF
+        int result = lzf_decompress(
+            compressed_data.data(), compressed_size,
+            uncompressed_data.data(), uncompressed_size
+        );
+
+        if (result != uncompressed_size) {
+            throw std::runtime_error("LZF decompression failed");
+        }
+
+        // Points are stored field by field
+        int strip_size = header.points;
+        points.resize(header.points);
+
+        // Process each coordinate field
+        for (const auto& field : header.fields) {
+            const char* base_ptr = uncompressed_data.data() + field.offset * strip_size;
+            
+            if (field.name == "x") {
+                for (int i = 0; i < header.points; i++) {
+                    float value;
+                    std::memcpy(&value, base_ptr + i * field.size, sizeof(float));
+                    points[i][0] = value;
+                }
+            } else if (field.name == "y") {
+                for (int i = 0; i < header.points; i++) {
+                    float value;
+                    std::memcpy(&value, base_ptr + i * field.size, sizeof(float));
+                    points[i][1] = value;
+                }
+            } else if (field.name == "z") {
+                for (int i = 0; i < header.points; i++) {
+                    float value;
+                    std::memcpy(&value, base_ptr + i * field.size, sizeof(float));
+                    points[i][2] = value;
+                    points[i][3] = 1.0;
+                }
+            }
+        }
+
+        // Apply range filtering
+        auto it = std::remove_if(points.begin(), points.end(),
+            [this](const Eigen::Vector4d& p) {
+                const double norm = p.head<3>().norm();
+                return norm < config_.preprocess_.min_range || norm > config_.preprocess_.max_range;
+            });
+        points.erase(it, points.end());
     } else {
         std::string line;
         while (std::getline(file, line) && points.size() < header.points) {

cpp/io/format/ply.cpp

@@ -80,10 +80,6 @@ small_gicp::PointCloud::Ptr PLYLoader::load(const std::string& file_path) {
         throw std::runtime_error("Missing XYZ coordinates in PLY file");
     }
 
-    // Add debug output for first few points
-    const int debug_points = 5;
-    int points_output = 0;
-
     if (header.is_binary) {
         // Read all properties per point
         std::vector<char> point_data(header.properties.size() * sizeof(float));

cpp/io/map_loader.cpp

@@ -1,7 +1,6 @@
 #include "pipeline/openlidarmap.hpp"
 #include "utils/helpers.hpp"
 #include "io/loader_factory.hpp"
-#include "io/map_loader.hpp"
 
 #include <guik/viewer/async_light_viewer.hpp>
 #include <guik/viewer/light_viewer.hpp>
@@ -14,6 +13,8 @@ Pipeline::Pipeline(const config::Config &config)
     preprocess_ = std::make_unique<Preprocess>(config_);
     scan2map_config_.registration_.removal_horizon = 1e9;
     scan2map_config_.registration_.max_num_points_in_cell = 100;
+    scan2map_config_.preprocess_.min_range = -1e9;
+    scan2map_config_.preprocess_.max_range = 1e9;
     scan2map_registration_ = std::make_unique<Registration>(scan2map_config_);
     scan2scan_registration_ = std::make_unique<Registration>(scan2scan_config_);
     pose_graph_ = std::make_unique<PoseGraph>(config_, poses_);
@@ -36,7 +37,7 @@ bool Pipeline::initialize(const std::string &map_path,
     output_path_ = output_path;
 
     // Load and initialize map
-    auto map_cloud = io::loadPCD_map(map_path);
+    auto map_cloud = io::LoaderFactory::loadPointCloud(scan2map_config_, map_path);
     if (!scan2map_registration_->initialize(map_cloud, Eigen::Isometry3d::Identity())) {
         return false;
     }