🎉 open source code

.gitignore

@@ -0,0 +1,6 @@
+.vscode/
+
+build/
+
+examples/cpp/demo_visualize
+examples/cpp/demo_sequential

CMakeLists.txt

@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 3.11)
+project(PATCHWORK VERSION 1.0.0)
+
+set(CMAKE_CXX_STANDARD 14)
+set(PYTHON_EXECUTABLE python3)
+set(CMAKE_BUILD_TYPE Release)
+
+
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${Open3D_C_FLAGS}")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${Open3D_CXX_FLAGS}")
+set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${Open3D_EXE_LINKER_FLAGS}")
+
+
+if(CMAKE_VERSION VERSION_LESS "3.15")
+  include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/ListPrepend.cmake")
+  list_prepend(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
+else()
+  list(PREPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
+endif()
+
+include(FetchContent)
+include(pybind11)
+
+find_package(Eigen3 REQUIRED QUIET)
+find_package(Open3D HINTS ${CMAKE_INSTALL_PREFIX}/lib/CMake)
+list(APPEND Open3D_LIBRARIES dl)
+
+message(STATUS "Found Open3D ${Open3D_VERSION}")
+
+link_directories(${Open3D_LIBRARY_DIRS})
+
+add_subdirectory(patchworkpp)
+add_subdirectory(python_wrapper)
+
+add_executable(demo_visualize examples/cpp/demo_visualize.cpp)
+target_link_libraries(demo_visualize PRIVATE PATCHWORK::patchworkpp ${Open3D_LIBRARIES})
+target_include_directories(demo_visualize PUBLIC ${Open3D_INCLUDE_DIRS})
+set_target_properties(demo_visualize PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/examples/cpp")
+
+add_executable(demo_sequential examples/cpp/demo_sequential.cpp)
+target_link_libraries(demo_sequential PRIVATE PATCHWORK::patchworkpp ${Open3D_LIBRARIES})
+target_include_directories(demo_sequential PUBLIC ${Open3D_INCLUDE_DIRS})
+set_target_properties(demo_sequential PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/examples/cpp")
+

README.md

@@ -1,11 +1,87 @@
-# patchwork-plusplus
-Patchwork++: Fast and Robust Ground Segmentation Solving Partial Under-segmentation Using 3D Point Cloud
+# Patchwork++
 
-# Notice
+## :bookmark_tabs: About Patchwork++ (IROS'22)
 
-(April, 2022) The code will be updated soon :)
+* A fast and robust **ground segmentation algorithm** on 3D point cloud.
 
-# Brief Explanation and Short Demo
+<p align="center"><img src=pictures/patchwork++.gif alt="animated" /></p>
 
-https://user-images.githubusercontent.com/67184306/161928900-e3aafbb6-2918-4e96-96df-f11b78161b1e.mp4
+* An extension of [Patchwork][patchworklink] (RA-L'21 with IROS'21).
+* Validated on [SemanticKITTI][SemanticKITTIlink] dataset. Please refer the paper for detailed results!
+    * The ground segmentation benchmark source code is available on [here][benchmarklink].
 
+[patchworklink]: https://github.com/LimHyungTae/patchwork
+[SemanticKITTIlink]: http://www.semantic-kitti.org/
+[benchmarklink]: https://github.com/url-kaist/Ground-Segmentation-Benchmark
+
+## :open_file_folder: What's in this repo
+
+* C++ source code of Patchwork++ ([patchworkpp][sourcecodelink])
+* Python binding of Patchwork++ using pybind11 ([python_wrapper][wraplink])
+* Examples codes, which visualizes a ground segmentation result by Patchwork++ ([examples][examplelink]) :thumbsup:
+
+[sourcecodelink]: https://github.com/seungjae24/pypatchwork/tree/master/patchworkpp
+[pybind11link]: https://github.com/pybind/pybind11
+[wraplink]: https://github.com/seungjae24/pypatchwork/tree/master/python_wrapper
+[examplelink]: https://github.com/seungjae24/pypatchwork/tree/master/examples
+
+## :package: Prerequisite packages
+> You may need to install Eigen, numpy, and Open3D. Open3D is used for point cloud visualization.
+
+```bash
+# To install Eigen and numpy
+$ sudo apt-get install libeigen3-dev
+$ pip install numpy
+
+# To install Open3D Python packages
+$ pip install open3d
+
+# To install Open3D C++ packages
+$ git clone https://github.com/isl-org/Open3D
+$ cd Open3D
+$ util/install_deps_ubuntu.sh # Only needed for Ubuntu
+$ mkdir build && cd build
+$ cmake ..
+$ make
+$ sudo make install
+```
+
+## :gear: How to build
+> Please follow below codes to build Patchwork++.
+
+```bash
+$ git clone https://github.com/url-kaist/patchwork-plusplus
+$ cd patchwork-plusplus
+$ mkdir build && cd build
+$ cmake ..
+$ make
+```
+
+## :runner: To run the demo codes
+> There are some example codes for your convenience!
+> Please try using Patchwork++ to segment ground points in a 3D point cloud :smiley:
+
+### Python
+```bash
+# Run patchwork++ and visualize ground points(green) and nonground points(red)
+$ python examples/python/demo_visualize.py
+
+# Run patchwork++ with sequential point cloud inputs 
+$ python examples/python/demo_sequential.py
+```
+
+### C++
+```bash
+# Run patchwork++ and visualize ground points(green) and nonground points(red)
+$ ./examples/cpp/demo_visualize
+
+# Run patchwork++ with sequential point cloud inputs 
+$ ./examples/cpp/demo_sequential
+```
+
+### Demo Result
+If you execute Patchwork++ with given demo codes well, you can get the following result!
+
+It is a ground segmentation result of data/000000.bin file using Open3D visualization. (Ground : Green, Nonground : Red)
+
+![Open3D Visualization of "data/000000.bin"](pictures/demo_000000.png)

cmake/ListPrepend.cmake

@@ -0,0 +1,8 @@
+FUNCTION(list_prepend var value)
+  message("${${var}}")
+  if("${${var}}" STREQUAL "")
+    SET(${var} "${value}" PARENT_SCOPE)
+  else()
+    SET(${var} "${value}" "${${var}}" PARENT_SCOPE)
+  endif()
+ENDFUNCTION(list_prepend)

cmake/pybind11.cmake

@@ -0,0 +1,13 @@
+include(FetchContent)
+
+FetchContent_Declare(
+    pybind11
+    GIT_REPOSITORY https://github.com/pybind/pybind11
+    GIT_TAG        v2.2.3
+)
+
+FetchContent_GetProperties(pybind11)
+if(NOT pybind11_POPULATED)
+    FetchContent_Populate(pybind11)
+    add_subdirectory(${pybind11_SOURCE_DIR} ${pybind11_BINARY_DIR} EXCLUDE_FROM_ALL)
+endif()

examples/cpp/demo_sequential.cpp

@@ -0,0 +1,106 @@
+#include <patchworkpp.h>
+
+#include <iostream>
+#include <fstream>
+#include <open3d/Open3D.h>
+
+using namespace open3d;
+
+int filename_length = std::string("demo_sequential.cpp").length();
+std::string file_dir = std::string(__FILE__);
+std::string data_dir = file_dir.substr(0, file_dir.size()-filename_length) + "../../data/";
+
+void read_bin(std::string bin_path, Eigen::MatrixX3f &cloud)
+{
+  FILE *file = fopen(bin_path.c_str(), "rb");
+  if (!file) {
+    std::cerr << "error: failed to load " << bin_path << std::endl;
+    return;
+  }
+
+  std::vector<float> buffer(1000000);
+  size_t num_points = fread(reinterpret_cast<char *>(buffer.data()), sizeof(float), buffer.size(), file) / 4;
+
+  cloud.resize(num_points, 3);
+  for (int i=0; i<num_points; i++)
+  {
+      cloud.row(i) << buffer[i*4], buffer[i*4+1], buffer[i*4+2];
+  }
+}
+
+void eigen2geo(Eigen::MatrixX3f add, std::shared_ptr<geometry::PointCloud> geo)
+{
+  for ( int i=0; i<add.rows(); i++ ) {
+    geo->points_.push_back(Eigen::Vector3d(add.row(i)(0), add.row(i)(1), add.row(i)(2)));
+  }
+}
+
+void addNormals(Eigen::MatrixX3f normals, std::shared_ptr<geometry::PointCloud> geo)
+{
+  for (int i=0; i<normals.rows(); i++) {
+    geo->normals_.push_back(Eigen::Vector3d(normals.row(i)(0), normals.row(i)(1), normals.row(i)(2)));
+  }
+}
+
+
+int main() {
+
+  cout << "Execute" << __FILE__ << endl;
+
+  // Patchwork++ initialization
+  patchwork::Params patchwork_parameters;
+  patchwork_parameters.verbose = true;
+
+  patchwork::PatchWorkpp Patchworkpp(patchwork_parameters);
+
+  for (int i=0; i<6; i++)
+  {
+    // Load point cloud
+    std::string file = data_dir + "00000" + std::to_string(i) + ".bin";
+    Eigen::MatrixX3f cloud;
+    read_bin(file, cloud);
+
+    // Estimate Ground
+    Patchworkpp.estimateGround(cloud);
+
+    // Get Ground and Nonground
+    Eigen::MatrixX3f ground     = Patchworkpp.getGround();
+    Eigen::MatrixX3f nonground  = Patchworkpp.getNonground();
+    double time_taken = Patchworkpp.getTimeTaken();
+
+    // Get centers and normals for patches
+    Eigen::MatrixX3f centers    = Patchworkpp.getCenters();
+    Eigen::MatrixX3f normals    = Patchworkpp.getNormals();
+
+    cout << "Origianl Points  #: " << cloud.rows() << endl;
+    cout << "Ground Points    #: " << ground.rows() << endl;
+    cout << "Nonground Points #: " << nonground.rows() << endl;
+    cout << "Time Taken : "<< time_taken / 1000000 << "(sec)" << endl;
+    cout << "Press ... \n" << endl;
+    cout << "\t H  : help" << endl;
+    cout << "\t N  : visualize the surface normals" << endl;
+    cout << "\tESC : close the Open3D window" << endl;
+
+    // Visualize
+    std::shared_ptr<geometry::PointCloud> geo_ground(new geometry::PointCloud);
+    std::shared_ptr<geometry::PointCloud> geo_nonground(new geometry::PointCloud);
+    std::shared_ptr<geometry::PointCloud> geo_centers(new geometry::PointCloud);
+
+    eigen2geo(ground,     geo_ground);
+    eigen2geo(nonground,  geo_nonground);
+    eigen2geo(centers,    geo_centers);
+    addNormals(normals,   geo_centers);
+
+    geo_ground->PaintUniformColor(Eigen::Vector3d(0.0, 1.0, 0.0));
+    geo_nonground->PaintUniformColor(Eigen::Vector3d(1.0, 0.0, 0.0));
+    geo_centers->PaintUniformColor(Eigen::Vector3d(1.0, 1.0, 0.0));  
+
+    visualization::Visualizer visualizer;
+    visualizer.CreateVisualizerWindow("Open3D", 1600, 900);
+    visualizer.AddGeometry(geo_ground);
+    visualizer.AddGeometry(geo_nonground);
+    visualizer.AddGeometry(geo_centers);
+    visualizer.Run();
+    visualizer.DestroyVisualizerWindow();
+  }
+}

examples/cpp/demo_visualize.cpp

@@ -0,0 +1,103 @@
+#include <patchworkpp.h>
+
+#include <iostream>
+#include <fstream>
+#include <open3d/Open3D.h>
+
+using namespace open3d;
+
+int filename_length = std::string("demo_visaulize.cpp").length();
+std::string file_dir = std::string(__FILE__);
+std::string data_dir = file_dir.substr(0, file_dir.size()-filename_length) + "../../data/";
+std::string input_cloud_filepath = data_dir + "000000.bin";
+
+void read_bin(std::string bin_path, Eigen::MatrixX3f &cloud)
+{
+  FILE *file = fopen(bin_path.c_str(), "rb");
+  if (!file) {
+    std::cerr << "error: failed to load " << bin_path << std::endl;
+    return;
+  }
+
+  std::vector<float> buffer(1000000);
+  size_t num_points = fread(reinterpret_cast<char *>(buffer.data()), sizeof(float), buffer.size(), file) / 4;
+
+  cloud.resize(num_points, 3);
+  for (int i=0; i<num_points; i++)
+  {
+      cloud.row(i) << buffer[i*4], buffer[i*4+1], buffer[i*4+2];
+  }
+}
+
+void eigen2geo(Eigen::MatrixX3f add, std::shared_ptr<geometry::PointCloud> geo)
+{
+  for ( int i=0; i<add.rows(); i++ ) {
+    geo->points_.push_back(Eigen::Vector3d(add.row(i)(0), add.row(i)(1), add.row(i)(2)));
+  }
+}
+
+void addNormals(Eigen::MatrixX3f normals, std::shared_ptr<geometry::PointCloud> geo)
+{
+  for (int i=0; i<normals.rows(); i++) {
+    geo->normals_.push_back(Eigen::Vector3d(normals.row(i)(0), normals.row(i)(1), normals.row(i)(2)));
+  }
+}
+
+
+int main() {
+
+  cout << "Execute" << __FILE__ << endl;
+
+  // Patchwork++ initialization
+  patchwork::Params patchwork_parameters;
+  patchwork_parameters.verbose = true;
+
+  patchwork::PatchWorkpp Patchworkpp(patchwork_parameters);
+
+  // Load point cloud
+  Eigen::MatrixX3f cloud;
+  read_bin(input_cloud_filepath, cloud);
+
+  // Estimate Ground
+  Patchworkpp.estimateGround(cloud);
+
+  // Get Ground and Nonground
+  Eigen::MatrixX3f ground     = Patchworkpp.getGround();
+  Eigen::MatrixX3f nonground  = Patchworkpp.getNonground();
+  double time_taken = Patchworkpp.getTimeTaken();
+
+  // Get centers and normals for patches
+  Eigen::MatrixX3f centers    = Patchworkpp.getCenters();
+  Eigen::MatrixX3f normals    = Patchworkpp.getNormals();
+
+  cout << "Origianl Points  #: " << cloud.rows() << endl;
+  cout << "Ground Points    #: " << ground.rows() << endl;
+  cout << "Nonground Points #: " << nonground.rows() << endl;
+  cout << "Time Taken : "<< time_taken / 1000000 << "(sec)" << endl;
+  cout << "Press ... \n" << endl;
+  cout << "\t H  : help" << endl;
+  cout << "\t N  : visualize the surface normals" << endl;
+  cout << "\tESC : close the Open3D window" << endl;
+
+  // Visualize
+  std::shared_ptr<geometry::PointCloud> geo_ground(new geometry::PointCloud);
+  std::shared_ptr<geometry::PointCloud> geo_nonground(new geometry::PointCloud);
+  std::shared_ptr<geometry::PointCloud> geo_centers(new geometry::PointCloud);
+
+  eigen2geo(ground,     geo_ground);
+  eigen2geo(nonground,  geo_nonground);
+  eigen2geo(centers,    geo_centers);
+  addNormals(normals,   geo_centers);
+
+  geo_ground->PaintUniformColor(Eigen::Vector3d(0.0, 1.0, 0.0));
+  geo_nonground->PaintUniformColor(Eigen::Vector3d(1.0, 0.0, 0.0));
+  geo_centers->PaintUniformColor(Eigen::Vector3d(1.0, 1.0, 0.0));  
+
+  visualization::Visualizer visualizer;
+  visualizer.CreateVisualizerWindow("Open3D", 1600, 900);
+  visualizer.AddGeometry(geo_ground);
+  visualizer.AddGeometry(geo_nonground);
+  visualizer.AddGeometry(geo_centers);
+  visualizer.Run();
+  visualizer.DestroyVisualizerWindow();
+}