perf(speed): memcpy directly for np.array to std::vector<Eigen::Vector3d>

Author: Kin-Zhang

README.md

@@ -10,6 +10,9 @@ Running on macOS, Windows and Linux, with Python Version >= 3.8.
 Available in: <a href="https://github.com/Kin-Zhang/linefit"><img src="https://img.shields.io/badge/Windows-0078D6?st&logo=windows&logoColor=white" /> <a href="https://github.com/Kin-Zhang/linefit"><img src="https://img.shields.io/badge/Linux-FCC624?logo=linux&logoColor=black" />  <a href="https://github.com/Kin-Zhang/linefit"><img src="https://img.shields.io/badge/mac%20os-000000?&logo=apple&logoColor=white" /> </a>
 
 <!--  -->
+📜 Change Log:
+- 2024-07-03: Speed up nanobind `np.array` <-> `std::vector<Eigen:: Vector3d>` conversion and also `NOMINSIZE` in make. Speed difference: 0.1s -> 0.01s. Based on [discussion here](https://github.com/wjakob/nanobind/discussions/426).
+- 2024-02-15: Initial version.
 
 ## 0. Setup
 

cpp/linefit/ground_segmentation.cpp

@@ -63,12 +63,7 @@ GroundSegmentation::GroundSegmentation(const std::string &toml_file) {
 
 }
 
-std::vector<bool> GroundSegmentation::segment(const std::vector<std::vector<float>> points) {
-  // TODO: Maybe there is a better way to convert the points to Eigen::Vector3d
-  PointCloud cloud;
-  for (auto point : points) {
-    cloud.push_back(Eigen::Vector3d(point[0], point[1], point[2]));
-  }
+std::vector<bool> GroundSegmentation::segment(const PointCloud &cloud) {
   if (verbose_)
     std::cout << "Segmenting cloud with " << cloud.size() << " points...\n";
 

cpp/linefit/ground_segmentation.h

@@ -101,6 +101,6 @@ class GroundSegmentation {
   GroundSegmentation(const std::string& toml_file);
   // virtual ~GroundSegmentation() = default;
 
-  std::vector<bool> segment(const std::vector<std::vector<float>> points);
+  std::vector<bool> segment(const PointCloud &cloud);
 
 };

example.py

@@ -26,7 +26,7 @@
         groundseg = ground_seg()
     else:
         groundseg = ground_seg(config_path)
-    label = np.array(groundseg.run(pc_data[:,:3].tolist()))
+    label = np.array(groundseg.run(pc_data[:,:3]))
     print(f"point cloud shape: {pc_data[:, :3].shape}, label shape: {label.shape}, ground points: {np.sum(label)}, time: {time.time() - start_time:.3f} s")
 
     from python.utils.o3d_view import MyVisualizer

pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "scikit_build_core.build"
 
 [project]
 name = "linefit"
-version = "0.2.4"
+version = "1.0.0"
 description = "linefit ground segmentation algorithm Python bindings"
 readme = "README.md"
 requires-python = ">=3.8"

python/CMakeLists.txt

@@ -1,3 +1,3 @@
-nanobind_add_module(linefit MODULE linefit_ext.cpp)
+nanobind_add_module(linefit MODULE NOMINSIZE linefit_ext.cpp)
 target_link_libraries(linefit PRIVATE gm_lib)
 install(TARGETS linefit LIBRARY DESTINATION .)

python/linefit_ext.cpp

@@ -39,23 +39,28 @@
  */
 
 #include <Eigen/Core>
-#include "nanobind/nanobind.h"
+
+#include <nanobind/nanobind.h>
 #include <nanobind/stl/string.h>
 #include <nanobind/stl/vector.h>
 #include <nanobind/ndarray.h>
-// #include <nanobind/stl/bind_vector.h>
 
 #include "../cpp/linefit/ground_segmentation.h"
 #include "../cpp/linefit/mics.h"
-// #include "stl_vector_eigen.h"
 
 namespace nb = nanobind;
 using namespace nb::literals;
 
 NB_MODULE(linefit, m) {
     nb::class_<GroundSegmentation>(m, "ground_seg")
-        .def(nb::init<>(), "linefit ground segmentation constructor, param: TODO")
+        .def(nb::init<>(), "linefit ground segmentation constructor, param: check default config to know more.")
         .def(nb::init<const std::string &>(), "linefit ground segmentation constructor, with toml file as param file input.")
-        // .def("run", nb::overload_cast<std::vector<Eigen::Vector3d> &>(&GroundSegmentation::segment), "points"_a);
-        .def("run", &GroundSegmentation::segment, "points"_a, nanobind::rv_policy::reference);
-}
+        .def("run", [](GroundSegmentation& self, const nb::ndarray<double>& array) -> std::vector<bool> {
+            if (array.ndim() != 2 || array.shape(1) != 3) {
+                throw std::runtime_error("Input array must have shape (N, 3)");
+            }
+            std::vector<Eigen::Vector3d> points_vec(array.shape(0));
+            std::memcpy(points_vec.data(), array.data(), array.size() * sizeof(double));
+            return self.segment(points_vec);
+        }, "points"_a);
+}

python/utils/o3d_view.py

@@ -1,36 +1,40 @@
 '''
 # Created: 2023-1-26 16:38
-# Copyright (C) 2022-now, RPL, KTH Royal Institute of Technology
-# Author: Kin ZHANG  (https://kin-zhang.github.io/)
-
-# This work is licensed under the terms of the MIT license.
-# For a copy, see <https://opensource.org/licenses/MIT>.
+# Updated: 2024-04-15 12:06
+# Copyright (C) 2023-now, RPL, KTH Royal Institute of Technology
+# Author: Qingwen ZHANG  (https://kin-zhang.github.io/)
+# 
+# code gits: https://gist.github.com/Kin-Zhang/77e8aa77a998f1a4f7495357843f24ef
+# 
+# Description as follows:
 
 This file is for open3d view control set from view_file, which should be json
 1. use normal way to open any geometry and set view by mouse you want
 2. `CTRL+C` it will copy the view detail at this moment.
 3. `CTRL+V` to json file, you can create new one
 4. give the json file path
-Check this part: http://www.open3d.org/docs/release/tutorial/visualization/visualization.html#Store-view-point
 
-Test if you want by run this script
+Check this part: http://www.open3d.org/docs/release/tutorial/visualization/visualization.html#Store-view-point
 
-Then press 'V' on keyboard, will set from json
+Test if you want by run this script: by press 'V' on keyboard, will set from json
 
+# CHANGELOG:
+# 2024-04-15 12:06(Qingwen): show a example json text. add hex_to_rgb, color_map_hex, color_map (for color points if needed)
+# 2024-01-27 0:41(Qingwen): update MyVisualizer class, reference from kiss-icp 
+[python/kiss-icp/tools/visualizer.py](https://github.com/PRBonn/kiss-icp/blob/main/python/kiss_icp/tools/visualizer.py)
 '''
 
 import open3d as o3d
 import json
 import os, sys
-from typing import List
-BASE_DIR = os.path.abspath(os.path.join( os.path.dirname( __file__ ), '..' ))
-sys.path.append(BASE_DIR)
+from typing import List, Callable
+from functools import partial
 
 def hex_to_rgb(hex_color):
     hex_color = hex_color.lstrip("#")
     return tuple(int(hex_color[i:i + 2], 16) / 255.0 for i in (0, 2, 4))
 
-color_map_hex = ['#a6cee3','#1f78b4','#b2df8a','#33a02c','#fb9a99','#e31a1c','#fdbf6f','#ff7f00','#cab2d6','#6a3d9a','#ffff99','#b15928',\
+color_map_hex = ['#a6cee3', '#de2d26', '#1f78b4','#b2df8a','#33a02c','#fb9a99','#e31a1c','#fdbf6f','#ff7f00','#cab2d6','#6a3d9a','#ffff99','#b15928',\
                  '#8dd3c7','#ffffb3','#bebada','#fb8072','#80b1d3','#fdb462','#b3de69','#fccde5','#d9d9d9','#bc80bd','#ccebc5','#ffed6f']
 color_map = [hex_to_rgb(color) for color in color_map_hex]
 
@@ -40,16 +44,24 @@ def __init__(self, vctrl: o3d.visualization.ViewControl, view_file=None):
         self.params = None
         if view_file is not None:
             print(f"Init with view_file from: {view_file}")
-            self.parase_file(view_file)
+            self.parse_file(view_file)
             self.set_param()
         else:
             print("Init without view_file")
+
     def read_viewTfile(self, view_file):
-        self.parase_file(view_file)
+        if view_file is None:
+            return
+        self.parse_file(view_file)
         self.set_param()
+
     def save_viewTfile(self, view_file):
         return
-    def parase_file(self, view_file):
+    
+    def parse_file(self, view_file):
+        if view_file is None:
+            print(f"\033[91mNo specific view file. Skip to setup viewpoint in open3d. \033[0m")
+            return
         if(os.path.exists(view_file)):
             with open((view_file)) as user_file:
                 file_contents = user_file.read()
@@ -69,26 +81,94 @@ def set_param(self):
 class MyVisualizer:
     def __init__(self, view_file=None, window_title="Default"):
         self.params = None
-        self.viz = o3d.visualization.VisualizerWithKeyCallback()
-        self.viz.create_window(window_name=window_title)
-        self.o3d_vctrl = ViewControl(self.viz.get_view_control(), view_file=view_file)
+        self.vis = o3d.visualization.VisualizerWithKeyCallback()
+        self.vis.create_window(window_name=window_title)
+        self.o3d_vctrl = ViewControl(self.vis.get_view_control(), view_file=view_file)
         self.view_file = view_file
-        
+
+        self.block_vis = True
+        self.play_crun = False
+        self.reset_bounding_box = True
+        print(
+            f"\n{window_title.capitalize()} initialized. Press:\n"
+            "\t[SPACE] to pause/start\n"
+            "\t  [ESC] to exit\n"
+            "\t    [N] to step\n"
+        )
+        self._register_key_callback(["Ā", "Q", "\x1b"], self._quit)
+        self._register_key_callback([" "], self._start_stop)
+        self._register_key_callback(["N"], self._next_frame)
+
     def show(self, assets: List):
-        self.viz.clear_geometries()
+        self.vis.clear_geometries()
 
         for asset in assets:
-            self.viz.add_geometry(asset)
-        if self.view_file is not None:
+            self.vis.add_geometry(asset)
             self.o3d_vctrl.read_viewTfile(self.view_file)
 
-        self.viz.update_renderer()
-        self.viz.poll_events()
-        self.viz.run()
-        self.viz.destroy_window()
+        self.vis.update_renderer()
+        self.vis.poll_events()
+        self.vis.run()
+        self.vis.destroy_window()
+
+    def update(self, assets: List, clear: bool = True):
+        if clear:
+            self.vis.clear_geometries()
+
+        for asset in assets:
+            self.vis.add_geometry(asset, reset_bounding_box=False)
+            self.vis.update_geometry(asset)
+
+        if self.reset_bounding_box:
+            self.vis.reset_view_point(True)
+            if self.view_file is not None:
+                self.o3d_vctrl.read_viewTfile(self.view_file)
+            self.reset_bounding_box = False
+
+        self.vis.update_renderer()
+        while self.block_vis:
+            self.vis.poll_events()
+            if self.play_crun:
+                break
+        self.block_vis = not self.block_vis
+
+    def _register_key_callback(self, keys: List, callback: Callable):
+        for key in keys:
+            self.vis.register_key_callback(ord(str(key)), partial(callback))
+    def _next_frame(self, vis):
+        self.block_vis = not self.block_vis
+    def _start_stop(self, vis):
+        self.play_crun = not self.play_crun
+    def _quit(self, vis):
+        print("Destroying Visualizer. Thanks for using ^v^.")
+        vis.destroy_window()
+        os._exit(0)
 
 if __name__ == "__main__":
-    view_json_file = f"{BASE_DIR}/data/o3d_view/default_test.json"
+    json_content = """{
+	"class_name" : "ViewTrajectory",
+	"interval" : 29,
+	"is_loop" : false,
+	"trajectory" : 
+	[
+		{
+			"boundingbox_max" : [ 3.9660897254943848, 2.427476167678833, 2.55859375 ],
+			"boundingbox_min" : [ 0.55859375, 0.83203125, 0.56663715839385986 ],
+			"field_of_view" : 60.0,
+			"front" : [ 0.27236083595988803, -0.25567329763523589, -0.92760484038816615 ],
+			"lookat" : [ 2.4114965637897101, 1.8070288935660688, 1.5662280268112718 ],
+			"up" : [ -0.072779625398507866, -0.96676294585190281, 0.24509698622097265 ],
+			"zoom" : 0.47999999999999976
+		}
+	],
+	"version_major" : 1,
+	"version_minor" : 0
+}
+"""
+    # write to json file
+    view_json_file = "view.json"
+    with open(view_json_file, 'w') as f:
+        f.write(json_content)
     sample_ply_data = o3d.data.PLYPointCloud()
     pcd = o3d.io.read_point_cloud(sample_ply_data.path)
     # 1. define