Segmentation fault

[Nasrudin78]

Hi all, I have the following simplified main code that reads a collection of .ply files resulting from a custom scene: point_clouds is std::vector<std::shared_ptr>

    kiss_icp::pipeline::KISSConfig config;
    config.max_range = 100.;
    config.min_range = 5.;
    config.deskew = true;
    config.voxel_size = 10.;
    config.max_points_per_voxel = 50;
    config.initial_threshold = 1.;
    config.min_motion_th = 0.1;
    config.max_num_iterations = 200;
    config.convergence_criterion = 0.0001;
    config.max_num_threads = 0;

    kiss_icp::pipeline::KissICP kiss_icp_pipeline(config);

    for (size_t i = 1; i < point_clouds.size(); ++i) {
        auto transformation = ComputeOdometryKISSICP(kiss_icp_pipeline, point_clouds[i]);
        transformations[i] = transformations[i - 1] * transformation;
    }

where :

std::vector<Eigen::Vector3d> ConvertToKissICPPointCloud(const geometry::PointCloud& pcd) {
    std::vector<Eigen::Vector3d> points;
    points.reserve(pcd.points_.size()); 
    for (const auto& point : pcd.points_) {
        if (!point.hasNaN()) { 
            points.emplace_back(point(0), point(1), point(2));
        } else {
            std::cerr << "Warning: Point with NaN values encountered and ignored." << std::endl;
        }
    }
    return points;
}

Eigen::Matrix4d ComputeOdometryKISSICP(kiss_icp::pipeline::KissICP& kiss_icp_pipeline,
                                       const std::shared_ptr<geometry::PointCloud>& source) {
    const std::vector<Eigen::Vector3d> source_pc = ConvertToKissICPPointCloud(*source);

    if (source_pc.empty()) {
        std::cerr << "Error: Converted point cloud is empty!" << std::endl;
        return Eigen::Matrix4d::Identity();
    }

    std::cout << "Calling RegisterFrame with " << source_pc.size() << " points." << std::endl;
    auto [deskew_source, registered_source] = kiss_icp_pipeline.RegisterFrame(source_pc);
    std::cout << "RegisterFrame returned." << std::endl;
    return kiss_icp_pipeline.pose().matrix();
}

With the parent CMakelists.txt adding the kiss_icp project:

cmake_minimum_required(VERSION 3.10)
project(OdometryKissICP VERSION 0.4.0 LANGUAGES CXX)

# Set the C++ standard
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED True)

add_definitions(-DNOMINMAX)

# Add the kiss_icp subdirectory
add_subdirectory(kiss_icp)
find_package(Open3D REQUIRED)
set(CMAKE_BUILD_TYPE Debug)  # Set build type to Debug

# Include directories
include_directories(${Open3D_INCLUDE_DIRS})
include_directories(${CMAKE_SOURCE_DIR})

# Add the executable
add_executable(odometry_kiss_icp src/main.cpp)

# Link libraries
target_link_libraries(odometry_kiss_icp ${Open3D_LIBRARIES} Sophus::Sophus TBB::tbb kiss_icp_core kiss_icp_pipeline)

The problem is when I execute the binary it yields a "segmentation fault error" and std::cout << "RegisterFrame returned." << std::endl; is never displayed

Gdb code:

Breakpoint 1, ComputeOdometryKISSICP (kiss_icp_pipeline=..., source=std::shared_ptr<open3d::geometry::PointCloud> (use count 1, weak count 0) = {...})
    at src/main.cpp:51
51          auto [deskew_source, registered_source] = kiss_icp_pipeline.RegisterFrame(source_pc);
(gdb) step
[New Thread 0x7fffd9f39700 (LWP 42437)]
[New Thread 0x7fffd9b38700 (LWP 42438)]
[New Thread 0x7fffd9737700 (LWP 42439)]
getPoints 
[New Thread 0x7fffd9336700 (LWP 42440)]
[New Thread 0x7fffd6f92700 (LWP 42442)]
[New Thread 0x7fffd8f35700 (LWP 42441)]
[New Thread 0x7fffd6790700 (LWP 42443)]
[New Thread 0x7fffd6b91700 (LWP 42444)]

Thread 1 "odometry_kiss_i" received signal SIGSEGV, Segmentation fault.
0x00007ffff7fb6f97 in kiss_icp::VoxelHashMap::GetPoints(std::vector<Eigen::Matrix<int, 3, 1, 0, 3, 1>, std::allocator<Eigen::Matrix<int, 3, 1, 0, 3, 1> > > const&) const () from kiss_icp/build/core/libkiss_icp_core.so

Any idea? Thank you very much

[tizianoGuadagnino]

Hi, I do not understand why you are converting the internal points of an Open3D point cloud to a std::vector<Eigen::Vector3d>. You can just access the attribute .points_ and get what you want. have you try that?

[Nasrudin78]

Same:

Full code:

#include <iostream>
#include <vector>
#include <string>
#include <filesystem>
#include <open3d/Open3D.h>
#include <kiss_icp/pipeline/KissICP.hpp>

namespace fs = std::filesystem;
using namespace open3d;

std::vector<std::shared_ptr<geometry::PointCloud>> point_clouds;
std::vector<Eigen::Matrix4d> transformations;

// Function to load a point cloud from a file
std::shared_ptr<geometry::PointCloud> LoadPointCloud(const std::string& filename) {
    auto pcd = std::make_shared<geometry::PointCloud>();
    if (io::ReadPointCloud(filename, *pcd)) {
        std::cout << "Successfully read " << filename << std::endl;
    } else {
        std::cerr << "Failed to read " << filename << std::endl;
        return nullptr;
    }
    return pcd;
}

// Function to compute odometry using KISS-ICP
Eigen::Matrix4d ComputeOdometryKISSICP(std::shared_ptr<kiss_icp::pipeline::KissICP>& kiss_icp_pipeline,
                                       const std::shared_ptr<geometry::PointCloud>& source) {
    const std::vector<Eigen::Vector3d> source_pc = source->points_;
    std::cout << "Calling RegisterFrame\n";
    auto [deskew_source, registered_source] = kiss_icp_pipeline->RegisterFrame(source_pc);
    std::cout << "RegisterFrame returned\n";
    return kiss_icp_pipeline->poses()[0].matrix();
}

int main(int argc, char** argv) {
    if (argc < 2) {
        std::cerr << "Usage: " << argv[0] << " <directory_path>" << std::endl;
        return 1;
    }

    std::string directory_path = argv[1];

    // Iterate through the directory and load PLY files
    for (const auto& entry : fs::directory_iterator(directory_path)) {
        if (entry.path().extension() == ".ply") {
            auto pcd = LoadPointCloud(entry.path().string());
            if (pcd != nullptr) {
                point_clouds.push_back(pcd);
                transformations.push_back(Eigen::Matrix4d::Identity());
            }
        }
    }

    if (point_clouds.size() < 2) {
        std::cerr << "At least two point clouds are required." << std::endl;
        return 1;
    }

    // Initialize KISS-ICP pipeline
    kiss_icp::pipeline::KISSConfig config;

    auto kiss_icp_pipeline = std::make_shared<kiss_icp::pipeline::KissICP>(config);

    for (size_t i = 1; i < point_clouds.size(); ++i) {
        auto transformation = ComputeOdometryKISSICP(kiss_icp_pipeline, point_clouds[i]);
        transformations[i] = transformations[i - 1] * transformation;
    }

    return 0;
}

And output:

Successfully read ../data2/point_cloud_stitched_0500.ply Successfully read ../data2/point_cloud_stitched_0501.ply Successfully read ../data2/point_cloud_stitched_0502.ply Successfully read ../data2/point_cloud_stitched_0503.ply Successfully read ../data2/point_cloud_stitched_0504.ply Successfully read ../data2/point_cloud_stitched_0505.ply Successfully read ../data2/point_cloud_stitched_0506.ply Successfully read ../data2/point_cloud_stitched_0507.ply Successfully read ../data2/point_cloud_stitched_0508.ply Successfully read ../data2/point_cloud_stitched_0509.ply Calling RegisterFrame Segmentation fault (core dumped) `

[Nasrudin78]

I found the error: I linked kiss_cip_core and pipeline with my main program:

target_link_libraries(odometry_kiss_icp ${Open3D_LIBRARIES} Sophus::Sophus TBB::tbb kiss_icp_core kiss_icp_pipeline) Since kiss_icp_pipeline already comes linked with kiss_icp_core , is not necessary to link it again So I removed:

target_link_libraries(odometry_kiss_icp
    kiss_icp_pipeline
    ${Open3D_LIBRARIES}
)