diff --git a/README.md b/README.md index c9e072f..3e2dcc9 100644 --- a/README.md +++ b/README.md @@ -61,6 +61,12 @@ cd ~/.local/lib/python3.10/site-packages pybind11-stubgen -o . --ignore-invalid=all small_gicp ``` +## Documentation + +- Auto-generated docs: [C++](https://koide3.github.io/small_gicp/doc_cpp/index.html) [Python](https://koide3.github.io/small_gicp/doc_py/index.html) + + + ## Usage (C++) The following examples assume `using namespace small_gicp` is placed somewhere. diff --git a/docs/Doxyfile b/docs/Doxyfile index eead51a..6e41898 100644 --- a/docs/Doxyfile +++ b/docs/Doxyfile @@ -829,7 +829,7 @@ WARN_LOGFILE = # spaces. See also FILE_PATTERNS and EXTENSION_MAPPING # Note: If this tag is empty the current directory is searched. -INPUT = include README.md +INPUT = ../include ../README.md # This tag can be used to specify the character encoding of the source files # that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses @@ -1184,7 +1184,7 @@ GENERATE_HTML = YES # The default directory is: html. # This tag requires that the tag GENERATE_HTML is set to YES. -HTML_OUTPUT = html +HTML_OUTPUT = doc_cpp # The HTML_FILE_EXTENSION tag can be used to specify the file extension for each # generated HTML page (for example: .htm, .php, .asp). diff --git a/docs/Makefile b/docs/Makefile new file mode 100644 index 0000000..0097174 --- /dev/null +++ b/docs/Makefile @@ -0,0 +1,28 @@ +.PHONY: help +help: + @echo "make cpp|py|all" + +.PHONY: cpp +cpp: + @echo "Building C++ documentation..." + doxygen Doxyfile doc_cpp + +.PHONY: py +py: + @echo "Building Python documentation..." + mkdir -p build && cd build && cmake ../../ -DBUILD_PYTHON_BINDINGS=ON && make -j + sphinx-build -b singlehtml . ./doc_py/ + +.PHONY: mkdocs +mkdocs: + @echo "Building MkDocs documentation..." + cd .. && mkdocs build + +.PHONY: all +all: cpp py mkdocs + @echo "All documentation built." + +.PHONY: deploy +deploy: + @echo "Deploying documentation..." + cd .. && mkdocs gh-deploy \ No newline at end of file diff --git a/docs/conf.py b/docs/conf.py new file mode 100644 index 0000000..33a1a30 --- /dev/null +++ b/docs/conf.py @@ -0,0 +1,30 @@ +# Configuration file for the Sphinx documentation builder. +# +# For the full list of built-in configuration values, see the documentation: +# https://www.sphinx-doc.org/en/master/usage/configuration.html + +# -- Project information ----------------------------------------------------- +# https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information + +project = 'small_gicp' +copyright = '2024, k.koide' +author = 'k.koide' + +import os +import sys +sys.path.insert(0, os.path.abspath('./build/')) + +# -- General configuration --------------------------------------------------- +# https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration + +extensions = ['sphinx.ext.autodoc', 'sphinx.ext.napoleon'] + +templates_path = ['_templates'] +exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] + + +# -- Options for HTML output ------------------------------------------------- +# https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output + +html_theme = 'sphinx_rtd_theme' +html_static_path = ['_static'] diff --git a/docs/index.md b/docs/index.md new file mode 100644 index 0000000..46c6d97 --- /dev/null +++ b/docs/index.md @@ -0,0 +1,6 @@ +# small_gicp + +## Documentation + +- [C++](doc_cpp/index.html) +- [Python](doc_py/index.html) \ No newline at end of file diff --git a/docs/index.rst b/docs/index.rst new file mode 100644 index 0000000..481dbda --- /dev/null +++ b/docs/index.rst @@ -0,0 +1,22 @@ +.. small_gicp documentation master file, created by + sphinx-quickstart on Wed Jun 5 11:07:43 2024. + You can adapt this file completely to your liking, but it should at least + contain the root `toctree` directive. + +Welcome to small_gicp's documentation! +====================================== + +.. toctree:: + :maxdepth: 2 + :caption: Contents: + + small_gicp + + + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`modindex` +* :ref:`search` diff --git a/docs/small_gicp.rst b/docs/small_gicp.rst new file mode 100644 index 0000000..78ff7e6 --- /dev/null +++ b/docs/small_gicp.rst @@ -0,0 +1,7 @@ +small\_gicp module +================== + +.. automodule:: small_gicp + :members: + :undoc-members: + :show-inheritance: diff --git a/include/small_gicp/ann/flat_container.hpp b/include/small_gicp/ann/flat_container.hpp index 71fb40a..4f0b4bd 100644 --- a/include/small_gicp/ann/flat_container.hpp +++ b/include/small_gicp/ann/flat_container.hpp @@ -8,26 +8,32 @@ namespace small_gicp { -/// @brief Flat point container -/// @note IncrementalVoxelMap combined with FlastContainer is mostly the same as iVox. +/// @brief Point container with a flat vector. +/// @note IncrementalVoxelMap combined with FlastContainer is mostly the same as linear iVox. /// Bai et al., "Faster-LIO: Lightweight Tightly Coupled Lidar-Inertial Odometry Using Parallel Sparse Incremental Voxels", IEEE RA-L, 2022 -/// @tparam HasNormals If true, normals are stored -/// @tparam HasCovs If true, covariances are stored +/// @tparam HasNormals If true, store normals. +/// @tparam HasCovs If true, store covariances. template struct FlatContainer { public: + /// @brief FlatContainer setting. struct Setting { - double min_sq_dist_in_cell = 0.1 * 0.1; ///< Minimum squared distance between points in a cell - size_t max_num_points_in_cell = 10; ///< Maximum number of points in a cell + double min_sq_dist_in_cell = 0.1 * 0.1; ///< Minimum squared distance between points in a cell. + size_t max_num_points_in_cell = 10; ///< Maximum number of points in a cell. }; - /// @brief Constructor + /// @brief Constructor. FlatContainer() { points.reserve(5); } - /// @brief Number of points + /// @brief Number of points. size_t size() const { return points.size(); } - /// @brief Add a point to the container + /// @brief Add a point to the container. + /// @param setting Setting + /// @param transformed_pt Transformed point (== T * points[i]) + /// @param points Point cloud + /// @param i Index of the point + /// @param T Transformation matrix template void add(const Setting& setting, const Eigen::Vector4d& transformed_pt, const PointCloud& points, size_t i, const Eigen::Isometry3d& T) { if ( @@ -46,10 +52,10 @@ public: } } - /// @brief Finalize the container (Nothing to do for FlatContainer) + /// @brief Finalize the container (Nothing to do for FlatContainer). void finalize() {} - /// @brief Find the nearest neighbor + /// @brief Find the nearest neighbor. /// @param pt Query point /// @param k_index Index of the nearest neighbor /// @param k_sq_dist Squared distance to the nearest neighbor @@ -76,7 +82,7 @@ public: return 1; } - /// @brief Find k nearest neighbors + /// @brief Find k nearest neighbors. /// @param pt Query point /// @param k Number of neighbors /// @param k_index Indices of nearest neighbors @@ -109,14 +115,18 @@ public: public: struct Empty {}; - std::vector points; - std::conditional_t, Empty> normals; - std::conditional_t, Empty> covs; + std::vector points; ///< Points + std::conditional_t, Empty> normals; ///< Normals (Empty if HasNormals is false) + std::conditional_t, Empty> covs; ///< Covariances (Empty if HasCovs is false) }; +/// @brief FlatContainer that stores only points. using FlatContainerPoints = FlatContainer; +/// @brief FlatContainer with normals. using FlatContainerNormal = FlatContainer; +/// @brief FlatContainer with covariances. using FlatContainerCov = FlatContainer; +/// @brief FlatContainer with normals and covariances. using FlatContainerNormalCov = FlatContainer; namespace traits { diff --git a/include/small_gicp/ann/gaussian_voxelmap.hpp b/include/small_gicp/ann/gaussian_voxelmap.hpp index c520048..7f7fc38 100644 --- a/include/small_gicp/ann/gaussian_voxelmap.hpp +++ b/include/small_gicp/ann/gaussian_voxelmap.hpp @@ -11,18 +11,18 @@ namespace small_gicp { -/// @brief Gaussian voxel. +/// @brief Gaussian voxel that computes and stores voxel mean and covariance. struct GaussianVoxel { public: struct Setting {}; - /// @brief Constructor + /// @brief Constructor. GaussianVoxel() : finalized(false), num_points(0), mean(Eigen::Vector4d::Zero()), cov(Eigen::Matrix4d::Zero()) {} - /// @brief Number of points in the voxel (Always 1 for GaussianVoxel) + /// @brief Number of points in the voxel (Always 1 for GaussianVoxel). size_t size() const { return 1; } - /// @brief Add a point to the voxel + /// @brief Add a point to the voxel. /// @param setting Setting /// @param transformed_pt Transformed point mean /// @param points Point cloud @@ -41,7 +41,7 @@ public: this->cov += T.matrix() * traits::cov(points, i) * T.matrix().transpose(); } - /// @brief Finalize the voxel mean and covariance + /// @brief Finalize the voxel mean and covariance. void finalize() { if (finalized) { return; diff --git a/include/small_gicp/ann/incremental_voxelmap.hpp b/include/small_gicp/ann/incremental_voxelmap.hpp index 162f87d..db6a3d5 100644 --- a/include/small_gicp/ann/incremental_voxelmap.hpp +++ b/include/small_gicp/ann/incremental_voxelmap.hpp @@ -16,11 +16,11 @@ namespace small_gicp { -/// @brief Voxel meta information +/// @brief Voxel meta information. struct VoxelInfo { public: - /// @brief Constructor - /// @param coord Voxel coordinate + /// @brief Constructor. + /// @param coord Integer voxel coordinates /// @param lru LRU counter for caching VoxelInfo(const Eigen::Vector3i& coord, size_t lru) : lru(lru), coord(coord) {} @@ -31,21 +31,21 @@ public: /// @brief Incremental voxelmap. /// This class supports incremental point cloud insertion and LRU-based voxel deletion. -/// @note This class can be used as a point cloud as well as a neighbor search. +/// @note This class can be used as a point cloud as well as a neighbor search structure. template struct IncrementalVoxelMap { public: using Ptr = std::shared_ptr; using ConstPtr = std::shared_ptr; - /// @brief Constructor + /// @brief Constructor. /// @param leaf_size Voxel size explicit IncrementalVoxelMap(double leaf_size) : inv_leaf_size(1.0 / leaf_size), lru_horizon(100), lru_clear_cycle(10), lru_counter(0) {} - /// @brief Number of points in the voxelmap + /// @brief Number of points in the voxelmap. size_t size() const { return flat_voxels.size(); } - /// @brief Insert points to the voxelmap + /// @brief Insert points to the voxelmap. /// @param points Point cloud /// @param T Transformation matrix template @@ -88,7 +88,7 @@ public: } } - /// @brief Find the nearest neighbor + /// @brief Find the nearest neighbor. /// @param pt Query point /// @param index Index of the nearest neighbor /// @param sq_dist Squared distance to the nearest neighbor @@ -139,9 +139,10 @@ public: return num_found; } + /// @brief Calculate the global point index from the voxel index and the point index. inline size_t calc_index(const size_t voxel_id, const size_t point_id) const { return (voxel_id << point_id_bits) | point_id; } - inline size_t voxel_id(const size_t i) const { return i >> point_id_bits; } ///< Extract the point ID from an index - inline size_t point_id(const size_t i) const { return i & ((1ull << point_id_bits) - 1); } ///< Extract the voxel ID from an index + inline size_t voxel_id(const size_t i) const { return i >> point_id_bits; } ///< Extract the point ID from a global index. + inline size_t point_id(const size_t i) const { return i & ((1ull << point_id_bits) - 1); } ///< Extract the voxel ID from a global index. public: static_assert(sizeof(size_t) == 8, "size_t must be 64-bit"); @@ -149,13 +150,13 @@ public: static constexpr int voxel_id_bits = 64 - point_id_bits; ///< Use the remaining bits for voxel id const double inv_leaf_size; ///< Inverse of the voxel size - size_t lru_horizon; ///< LRU horizon size - size_t lru_clear_cycle; ///< LRU clear cycle - size_t lru_counter; ///< LRU counter + size_t lru_horizon; ///< LRU horizon size. Voxels that have not been accessed for lru_horizon steps are deleted. + size_t lru_clear_cycle; ///< LRU clear cycle. Voxel deletion is performed every lru_clear_cycle steps. + size_t lru_counter; ///< LRU counter. Incremented every step. - typename VoxelContents::Setting voxel_setting; ///< Voxel setting - std::vector>> flat_voxels; ///< Voxel contents - std::unordered_map voxels; ///< Voxel index map + typename VoxelContents::Setting voxel_setting; ///< Voxel setting. + std::vector>> flat_voxels; ///< Voxel contents. + std::unordered_map voxels; ///< Voxel index map. }; namespace traits { diff --git a/include/small_gicp/pcl/pcl_point.hpp b/include/small_gicp/pcl/pcl_point.hpp index 63f3c12..2ba8ad7 100644 --- a/include/small_gicp/pcl/pcl_point.hpp +++ b/include/small_gicp/pcl/pcl_point.hpp @@ -9,24 +9,30 @@ namespace pcl { using Matrix4fMap = Eigen::Map; using Matrix4fMapConst = const Eigen::Map; -/// @brief Point with covariance +/// @brief Point with covariance for PCL struct PointCovariance { - PCL_ADD_POINT4D; - Eigen::Matrix4f cov; + PCL_ADD_POINT4D; ///< Point coordinates + Eigen::Matrix4f cov; ///< 4x4 covariance matrix + /// @brief Get covariance matrix as Matrix4fMap Matrix4fMap getCovariance4fMap() { return Matrix4fMap(cov.data()); } + + /// @brief Get covariance matrix as Matrix4fMapConst Matrix4fMapConst getCovariance4fMap() const { return Matrix4fMapConst(cov.data()); } PCL_MAKE_ALIGNED_OPERATOR_NEW }; -/// @brief Point with normal and covariance +/// @brief Point with normal and covariance for PCL struct PointNormalCovariance { - PCL_ADD_POINT4D; - PCL_ADD_NORMAL4D - Eigen::Matrix4f cov; + PCL_ADD_POINT4D; ///< Point coordinates + PCL_ADD_NORMAL4D ///< Point normal + Eigen::Matrix4f cov; ///< 4x4 covariance matrix + /// @brief Get covariance matrix as Matrix4fMap Matrix4fMap getCovariance4fMap() { return Matrix4fMap(cov.data()); } + + /// @brief Get covariance matrix as Matrix4fMapConst Matrix4fMapConst getCovariance4fMap() const { return Matrix4fMapConst(cov.data()); } PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/include/small_gicp/pcl/pcl_proxy.hpp b/include/small_gicp/pcl/pcl_proxy.hpp index 108fd6b..ff92bbf 100644 --- a/include/small_gicp/pcl/pcl_proxy.hpp +++ b/include/small_gicp/pcl/pcl_proxy.hpp @@ -7,12 +7,13 @@ namespace small_gicp { +/// @brief Proxy class to access PCL point cloud with external covariance matrices. template struct PointCloudProxy { PointCloudProxy(const pcl::PointCloud& cloud, std::vector& covs) : cloud(cloud), covs(covs) {} - const pcl::PointCloud& cloud; - std::vector& covs; + const pcl::PointCloud& cloud; ///< Point cloud + std::vector& covs; ///< Covariance matrices }; namespace traits { diff --git a/include/small_gicp/util/lie.hpp b/include/small_gicp/util/lie.hpp index 2f4aa8f..d53a55a 100644 --- a/include/small_gicp/util/lie.hpp +++ b/include/small_gicp/util/lie.hpp @@ -47,6 +47,10 @@ inline Eigen::Matrix3d skew(const Eigen::Vector3d& x) { * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS * IN THE SOFTWARE. */ + +/// @brief SO3 expmap. +/// @param omega [rx, ry, rz] +/// @return Quaternion inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) { double theta_sq = omega.dot(omega); @@ -67,6 +71,9 @@ inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) { } // Rotation-first +/// @brief SE3 expmap (Rotation-first). +/// @param a Twist vector [rx, ry, rz, tx, ty, tz] +/// @return SE3 matrix inline Eigen::Isometry3d se3_exp(const Eigen::Matrix& a) { using std::cos; using std::sin; diff --git a/include/small_gicp/util/normal_estimation.hpp b/include/small_gicp/util/normal_estimation.hpp index de59bbb..d9cc661 100644 --- a/include/small_gicp/util/normal_estimation.hpp +++ b/include/small_gicp/util/normal_estimation.hpp @@ -7,10 +7,13 @@ namespace small_gicp { +/// @brief Computes point normals from eigenvectors and sets them to the point cloud. template struct NormalSetter { + /// @brief Handle invalid case (too few points). static void set_invalid(PointCloud& cloud, size_t i) { traits::set_normal(cloud, i, Eigen::Vector4d::Zero()); } + /// @brief Compute and set the normal to the point cloud. static void set(PointCloud& cloud, size_t i, const Eigen::Matrix3d& eigenvectors) { const Eigen::Vector4d normal = (Eigen::Vector4d() << eigenvectors.col(0).normalized(), 0.0).finished(); if (traits::point(cloud, i).dot(normal) > 0) { @@ -21,14 +24,17 @@ struct NormalSetter { } }; +/// @brief Computes point covariances from eigenvectors and sets them to the point cloud. template struct CovarianceSetter { + /// @brief Handle invalid case (too few points). static void set_invalid(PointCloud& cloud, size_t i) { Eigen::Matrix4d cov = Eigen::Matrix4d::Identity(); cov(3, 3) = 0.0; traits::set_cov(cloud, i, cov); } + /// @brief Compute and set the covariance to the point cloud. static void set(PointCloud& cloud, size_t i, const Eigen::Matrix3d& eigenvectors) { const Eigen::Vector3d values(1e-3, 1.0, 1.0); Eigen::Matrix4d cov = Eigen::Matrix4d::Zero(); @@ -37,13 +43,16 @@ struct CovarianceSetter { } }; +/// @brief Computes point normals and covariances from eigenvectors and sets them to the point cloud. template struct NormalCovarianceSetter { + /// @brief Handle invalid case (too few points). static void set_invalid(PointCloud& cloud, size_t i) { NormalSetter::set_invalid(cloud, i); CovarianceSetter::set_invalid(cloud, i); } + /// @brief Compute and set the normal and covariance to the point cloud. static void set(PointCloud& cloud, size_t i, const Eigen::Matrix3d& eigenvectors) { NormalSetter::set(cloud, i, eigenvectors); CovarianceSetter::set(cloud, i, eigenvectors); diff --git a/include/small_gicp/util/sort_omp.hpp b/include/small_gicp/util/sort_omp.hpp index 5e39d51..351cf2d 100644 --- a/include/small_gicp/util/sort_omp.hpp +++ b/include/small_gicp/util/sort_omp.hpp @@ -11,6 +11,10 @@ namespace small_gicp { +/// @brief Implementation of merge sort with OpenMP parallelism. Do not call this directly. Use merge_sort_omp instead. +/// @param first First iterator +/// @param last Last iterator +/// @param comp Comparison function template void merge_sort_omp_impl(RandomAccessIterator first, RandomAccessIterator last, const Compare& comp) { const size_t n = std::distance(first, last); @@ -31,6 +35,12 @@ void merge_sort_omp_impl(RandomAccessIterator first, RandomAccessIterator last, std::inplace_merge(first, center, last, comp); } +/// @brief Merge sort with OpenMP parallelism. +/// @note This tends to be slower than quick_sort_omp. +/// @param first First iterator +/// @param last Last iterator +/// @param comp Comparison function +/// @param num_threads Number of threads template void merge_sort_omp(RandomAccessIterator first, RandomAccessIterator last, const Compare& comp, int num_threads) { #ifndef _MSC_VER @@ -44,6 +54,10 @@ void merge_sort_omp(RandomAccessIterator first, RandomAccessIterator last, const #endif } +/// @brief Implementation of quick sort with OpenMP parallelism. Do not call this directly. Use quick_sort_omp instead. +/// @param first First iterator +/// @param last Last iterator +/// @param comp Comparison function template void quick_sort_omp_impl(RandomAccessIterator first, RandomAccessIterator last, const Compare& comp) { const std::ptrdiff_t n = std::distance(first, last); @@ -72,6 +86,11 @@ void quick_sort_omp_impl(RandomAccessIterator first, RandomAccessIterator last, quick_sort_omp_impl(middle2, last, comp); } +/// @brief Quick sort with OpenMP parallelism. +/// @param first First iterator +/// @param last Last iterator +/// @param comp Comparison function +/// @param num_threads Number of threads template void quick_sort_omp(RandomAccessIterator first, RandomAccessIterator last, const Compare& comp, int num_threads) { #ifndef _MSC_VER diff --git a/mkdocs.yml b/mkdocs.yml new file mode 100644 index 0000000..63a5cf2 --- /dev/null +++ b/mkdocs.yml @@ -0,0 +1,45 @@ +site_name: "small_gicp" +site_author: k.koide +repo_url: https://github.com/koide3/small_gicp +site_url: https://github.com/koide3/small_gicp +site_description: "small_gicp: Efficient and parallel algorithms for point cloud registration" + +theme: + name: material + palette: + primary: indigo + front: + text: Roboto + +use_directory_urls: false + +markdown_extensions: + - meta + - attr_list + - admonition + - pymdownx.highlight: + anchor_linenums: true + - pymdownx.inlinehilite + - pymdownx.snippets + - pymdownx.superfences + - pymdownx.emoji: + emoji_index: !!python/name:materialx.emoji.twemoji + emoji_generator: !!python/name:materialx.emoji.to_svg + - fontawesome_markdown + +copyright: Copyright © 2024 Kenji Koide +extra: + social: + - icon: material/home + link: https://staff.aist.go.jp/k.koide/ + - icon: fontawesome/brands/github + link: https://github.com/koide3/small_gicp + - icon: fontawesome/brands/twitter + link: https://twitter.com/k_koide3 + +extra_css: + - "css/custom.css" + - "https://maxcdn.bootstrapcdn.com/font-awesome/4.6.1/css/font-awesome.min.css" + +nav: + - 'index.md' diff --git a/src/python/factors.cpp b/src/python/factors.cpp index 5f0e672..7b6d288 100644 --- a/src/python/factors.cpp +++ b/src/python/factors.cpp @@ -20,12 +20,12 @@ using namespace small_gicp; void define_factors(py::module& m) { // DistanceRejector - py::class_(m, "DistanceRejector") // + py::class_(m, "DistanceRejector", "Correspondence rejection based on the distance between points.") .def(py::init<>()) - .def("set_max_distance", [](DistanceRejector& rejector, double dist) { rejector.max_dist_sq = dist * dist; }); + .def("set_max_distance", [](DistanceRejector& rejector, double dist) { rejector.max_dist_sq = dist * dist; }, py::arg("dist"), "Set the maximum distance."); // ICPFactor - py::class_(m, "ICPFactor") // + py::class_(m, "ICPFactor", "ICP per-point factor") .def(py::init<>()) .def( "linearize", @@ -42,10 +42,17 @@ void define_factors(py::module& m) { double e = 0.0; bool succ = factor.linearize(target, source, kdtree, Eigen::Isometry3d(T), source_index, rejector, &H, &b, &e); return std::make_tuple(succ, H, b, e); - }); + }, + py::arg("target"), + py::arg("source"), + py::arg("kdtree"), + py::arg("T"), + py::arg("source_index"), + py::arg("rejector"), + "Linearize the factor. Returns a tuple of success, Hessian, gradient, and error."); // PointToPlaneICPFactor - py::class_(m, "PointToPlaneICPFactor") // + py::class_(m, "PointToPlaneICPFactor", "Point-to-plane ICP per-point factor") .def(py::init<>()) .def( "linearize", @@ -62,10 +69,17 @@ void define_factors(py::module& m) { double e = 0.0; bool succ = factor.linearize(target, source, kdtree, Eigen::Isometry3d(T), source_index, rejector, &H, &b, &e); return std::make_tuple(succ, H, b, e); - }); + }, + py::arg("target"), + py::arg("source"), + py::arg("kdtree"), + py::arg("T"), + py::arg("source_index"), + py::arg("rejector"), + "Linearize the factor. Returns a tuple of success, Hessian, gradient, and error."); // GICPFactor - py::class_(m, "GICPFactor") // + py::class_(m, "GICPFactor", "Generalized ICP per-point factor") // .def(py::init<>()) .def( "linearize", @@ -82,5 +96,12 @@ void define_factors(py::module& m) { double e = 0.0; bool succ = factor.linearize(target, source, kdtree, Eigen::Isometry3d(T), source_index, rejector, &H, &b, &e); return std::make_tuple(succ, H, b, e); - }); + }, + py::arg("target"), + py::arg("source"), + py::arg("kdtree"), + py::arg("T"), + py::arg("source_index"), + py::arg("rejector"), + "Linearize the factor. Returns a tuple of success, Hessian, gradient, and error."); } \ No newline at end of file diff --git a/src/python/kdtree.cpp b/src/python/kdtree.cpp index 70bfc24..cd05a8d 100644 --- a/src/python/kdtree.cpp +++ b/src/python/kdtree.cpp @@ -16,7 +16,7 @@ using namespace small_gicp; void define_kdtree(py::module& m) { // KdTree - py::class_, std::shared_ptr>>(m, "KdTree") // + py::class_, std::shared_ptr>>(m, "KdTree", "KdTree") // .def( py::init([](const PointCloud::ConstPtr& points, int num_threads) { return std::make_shared>(points, KdTreeBuilderOMP(num_threads)); }), py::arg("points"), @@ -28,11 +28,18 @@ void define_kdtree(py::module& m) { double k_sq_dist = std::numeric_limits::max(); const size_t found = traits::nearest_neighbor_search(kdtree, Eigen::Vector4d(pt.x(), pt.y(), pt.z(), 1.0), &k_index, &k_sq_dist); return std::make_tuple(found, k_index, k_sq_dist); - }) - .def("knn_search", [](const KdTree& kdtree, const Eigen::Vector3d& pt, int k) { - std::vector k_indices(k, -1); - std::vector k_sq_dists(k, std::numeric_limits::max()); - const size_t found = traits::knn_search(kdtree, Eigen::Vector4d(pt.x(), pt.y(), pt.z(), 1.0), k, k_indices.data(), k_sq_dists.data()); - return std::make_pair(k_indices, k_sq_dists); - }); + }, + py::arg("pt"), + "Search the nearest neighbor. Returns a tuple of found flag, index, and squared distance.") + .def( + "knn_search", + [](const KdTree& kdtree, const Eigen::Vector3d& pt, int k) { + std::vector k_indices(k, -1); + std::vector k_sq_dists(k, std::numeric_limits::max()); + const size_t found = traits::knn_search(kdtree, Eigen::Vector4d(pt.x(), pt.y(), pt.z(), 1.0), k, k_indices.data(), k_sq_dists.data()); + return std::make_pair(k_indices, k_sq_dists); + }, + py::arg("pt"), + py::arg("k"), + "Search the k-nearest neighbors. Returns a pair of indices and squared distances."); } \ No newline at end of file diff --git a/src/python/pointcloud.cpp b/src/python/pointcloud.cpp index 36764da..cc9ce69 100644 --- a/src/python/pointcloud.cpp +++ b/src/python/pointcloud.cpp @@ -38,16 +38,29 @@ void define_pointcloud(py::module& m) { })) // .def("__repr__", [](const PointCloud& points) { return "small_gicp.PointCloud (" + std::to_string(points.size()) + " points)"; }) .def("__len__", [](const PointCloud& points) { return points.size(); }) - .def("empty", &PointCloud::empty) - .def("size", &PointCloud::size) + .def("empty", &PointCloud::empty, "Check if the point cloud is empty.") + .def("size", &PointCloud::size, "Get the number of points.") .def( "points", - [](const PointCloud& points) -> Eigen::MatrixXd { return Eigen::Map>(points.points[0].data(), points.size(), 4); }) + [](const PointCloud& points) -> Eigen::MatrixXd { return Eigen::Map>(points.points[0].data(), points.size(), 4); }, + "Get the points as a Nx4 matrix.") .def( "normals", - [](const PointCloud& points) -> Eigen::MatrixXd { return Eigen::Map>(points.normals[0].data(), points.size(), 4); }) - .def("covs", [](const PointCloud& points) { return points.covs; }) - .def("point", [](const PointCloud& points, size_t i) -> Eigen::Vector4d { return points.point(i); }) - .def("normal", [](const PointCloud& points, size_t i) -> Eigen::Vector4d { return points.normal(i); }) - .def("cov", [](const PointCloud& points, size_t i) -> Eigen::Matrix4d { return points.cov(i); }); + [](const PointCloud& points) -> Eigen::MatrixXd { return Eigen::Map>(points.normals[0].data(), points.size(), 4); }, + "Get the normals as a Nx4 matrix.") + .def( + "covs", + [](const PointCloud& points) { return points.covs; }, + "Get the covariance matrices.") + .def( + "point", + [](const PointCloud& points, size_t i) -> Eigen::Vector4d { return points.point(i); }, + py::arg("i"), + "Get the i-th point.") + .def( + "normal", + [](const PointCloud& points, size_t i) -> Eigen::Vector4d { return points.normal(i); }, + py::arg("i"), + "Get the i-th normal.") + .def("cov", [](const PointCloud& points, size_t i) -> Eigen::Matrix4d { return points.cov(i); }, py::arg("i"), "Get the i-th covariance matrix."); } \ No newline at end of file diff --git a/src/python/preprocess.cpp b/src/python/preprocess.cpp index 41d3550..d1d843c 100644 --- a/src/python/preprocess.cpp +++ b/src/python/preprocess.cpp @@ -29,10 +29,26 @@ void define_preprocess(py::module& m) { } return voxelgrid_sampling_omp(points, resolution, num_threads); }, - "Voxelgrid sampling", py::arg("points"), py::arg("downsampling_resolution"), - py::arg("num_threads") = 1); + py::arg("num_threads") = 1, + R"pbdoc( + Voxelgrid downsampling. + + Parameters + ---------- + points : PointCloud + Input point cloud. + resolution : float + Voxel size. + num_threads : int, optional + Number of threads. (default: 1) + + Returns + ------- + PointCloud + Downsampled point cloud. + )pbdoc"); // voxelgrid_sampling (numpy) m.def( @@ -49,10 +65,26 @@ void define_preprocess(py::module& m) { return voxelgrid_sampling_omp(points, resolution, num_threads); } }, - "Voxelgrid sampling", py::arg("points"), py::arg("downsampling_resolution"), - py::arg("num_threads") = 1); + py::arg("num_threads") = 1, + R"pbdoc( + Voxelgrid downsampling. + + Parameters + ---------- + points : [np.float64] + Input point cloud. Nx3 or Nx4. + resolution : float + Voxel size. + num_threads : int, optional + Number of threads. (default: 1) + + Returns + ------- + PointCloud + Downsampled point cloud. + )pbdoc"); // estimate_normals m.def( @@ -71,7 +103,21 @@ void define_preprocess(py::module& m) { py::arg("points"), py::arg("tree") = nullptr, py::arg("num_neighbors") = 20, - py::arg("num_threads") = 1); + py::arg("num_threads") = 1, + R"pbdoc( + Estimate point normals. + + Parameters + ---------- + points : PointCloud + Input point cloud. Normals will be estimated in-place. (in/out) + tree : KdTree, optional + Nearest neighbor search. If None, create a new KdTree (default: None) + num_neighbors : int, optional + Number of neighbors. (default: 20) + num_threads : int, optional + Number of threads. (default: 1) + )pbdoc"); // estimate_covariances m.def( @@ -90,7 +136,21 @@ void define_preprocess(py::module& m) { py::arg("points"), py::arg("tree") = nullptr, py::arg("num_neighbors") = 20, - py::arg("num_threads") = 1); + py::arg("num_threads") = 1, + R"pbdoc( + Estimate point covariances. + + Parameters + ---------- + points : PointCloud + Input point cloud. Covariances will be estimated in-place. (in/out) + tree : KdTree, optional + Nearest neighbor search. If None, create a new KdTree (default: None) + num_neighbors : int, optional + Number of neighbors. (default: 20) + num_threads : int, optional + Number of threads. (default: 1) + )pbdoc"); // estimate_normals_covariances m.def( @@ -109,7 +169,21 @@ void define_preprocess(py::module& m) { py::arg("points"), py::arg("tree") = nullptr, py::arg("num_neighbors") = 20, - py::arg("num_threads") = 1); + py::arg("num_threads") = 1, + R"pbdoc( + Estimate point normals and covariances. + + Parameters + ---------- + points : PointCloud + Input point cloud. Normals and covariances will be estimated in-place. (in/out) + tree : KdTree, optional + Nearest neighbor search. If None, create a new KdTree (default: None) + num_neighbors : int, optional + Number of neighbors. (default: 20) + num_threads : int, optional + Number of threads. (default: 1) + )pbdoc"); // preprocess_points (numpy) m.def( @@ -138,7 +212,28 @@ void define_preprocess(py::module& m) { py::arg("points"), py::arg("downsampling_resolution") = 0.25, py::arg("num_neighbors") = 10, - py::arg("num_threads") = 1); + py::arg("num_threads") = 1, + R"pbdoc( + Preprocess point cloud (Downsampling and normal/covariance estimation). + + Parameters + ---------- + points : [np.float64] + Input point cloud. Nx3 or Nx4. + downsampling_resolution : float, optional + Resolution for downsampling the point clouds. (default: 0.25) + num_neighbors : int, optional + Number of neighbors used for attribute estimation. (default: 10) + num_threads : int, optional + Number of threads. (default: 1) + + Returns + ------- + PointCloud + Downsampled point cloud. + KdTree + KdTree for the downsampled point cloud. + )pbdoc"); // preprocess_points m.def( @@ -157,5 +252,26 @@ void define_preprocess(py::module& m) { py::arg("points"), py::arg("downsampling_resolution") = 0.25, py::arg("num_neighbors") = 10, - py::arg("num_threads") = 1); + py::arg("num_threads") = 1, + R"pbdoc( + Preprocess point cloud (Downsampling and normal/covariance estimation). + + Parameters + ---------- + points : PointCloud + Input point cloud. + downsampling_resolution : float, optional + Resolution for downsampling the point clouds. (default: 0.25) + num_neighbors : int, optional + Number of neighbors used for attribute estimation. (default: 10) + num_threads : int, optional + Number of threads. (default: 1) + + Returns + ------- + PointCloud + Downsampled point cloud. + KdTree + KdTree for the downsampled point cloud. + )pbdoc"); } \ No newline at end of file diff --git a/src/python/python.cpp b/src/python/python.cpp index 5878740..e26bc6a 100644 --- a/src/python/python.cpp +++ b/src/python/python.cpp @@ -14,7 +14,7 @@ void define_factors(py::module& m); void define_misc(py::module& m); PYBIND11_MODULE(small_gicp, m) { - m.doc() = "Small GICP"; + m.doc() = "Efficient and parallel algorithms for point cloud registration"; define_pointcloud(m); define_kdtree(m); diff --git a/src/python/result.cpp b/src/python/result.cpp index 614078b..a1cc47b 100644 --- a/src/python/result.cpp +++ b/src/python/result.cpp @@ -30,11 +30,14 @@ void define_result(py::module& m) { sst << "error= " << result.error << "\n"; return sst.str(); }) - .def_property_readonly("T_target_source", [](const RegistrationResult& result) -> Eigen::Matrix4d { return result.T_target_source.matrix(); }) - .def_readonly("converged", &RegistrationResult::converged) - .def_readonly("iterations", &RegistrationResult::iterations) - .def_readonly("num_inliers", &RegistrationResult::num_inliers) - .def_readonly("H", &RegistrationResult::H) - .def_readonly("b", &RegistrationResult::b) - .def_readonly("error", &RegistrationResult::error); + .def_property_readonly( + "T_target_source", + [](const RegistrationResult& result) -> Eigen::Matrix4d { return result.T_target_source.matrix(); }, + "Final transformation matrix") + .def_readonly("converged", &RegistrationResult::converged, "Convergence flag") + .def_readonly("iterations", &RegistrationResult::iterations, "Number of iterations") + .def_readonly("num_inliers", &RegistrationResult::num_inliers, "Number of inliers") + .def_readonly("H", &RegistrationResult::H, "Final Hessian matrix") + .def_readonly("b", &RegistrationResult::b, "Final information vector") + .def_readonly("error", &RegistrationResult::error, "Final error"); } \ No newline at end of file diff --git a/src/python/voxelmap.cpp b/src/python/voxelmap.cpp index 15306c8..ed617c8 100644 --- a/src/python/voxelmap.cpp +++ b/src/python/voxelmap.cpp @@ -27,12 +27,13 @@ auto define_class(py::module& m, const std::string& name) { return sst.str(); }) .def("__len__", [](const VoxelMap& voxelmap) { return voxelmap.size(); }) - .def("size", &VoxelMap::size) + .def("size", &VoxelMap::size, "Get the number of voxels.") .def( "insert", [](VoxelMap& voxelmap, const PointCloud& points, const Eigen::Matrix4d& T) { voxelmap.insert(points, Eigen::Isometry3d(T)); }, py::arg("points"), - py::arg("T") = Eigen::Matrix4d::Identity()) + py::arg("T") = Eigen::Matrix4d::Identity(), + "Insert a point cloud.") .def( "set_lru", [](VoxelMap& voxelmap, size_t horizon, size_t clear_cycle) { @@ -40,24 +41,34 @@ auto define_class(py::module& m, const std::string& name) { voxelmap.lru_clear_cycle = clear_cycle; }, py::arg("horizon") = 100, - py::arg("clear_cycle") = 10) - .def("voxel_points", [](const VoxelMap& voxelmap) -> Eigen::MatrixXd { - auto points = traits::voxel_points(voxelmap); - return Eigen::Map>(points[0].data(), points.size(), 4); - }); + py::arg("clear_cycle") = 10, + "Set the LRU cache parameters.") + .def( + "voxel_points", + [](const VoxelMap& voxelmap) -> Eigen::MatrixXd { + auto points = traits::voxel_points(voxelmap); + return Eigen::Map>(points[0].data(), points.size(), 4); + }, + "Get the voxel points."); if constexpr (has_normals) { - vox.def("voxel_normals", [](const VoxelMap& voxelmap) -> Eigen::MatrixXd { - auto normals = traits::voxel_normals(voxelmap); - return Eigen::Map>(normals[0].data(), normals.size(), 4); - }); + vox.def( + "voxel_normals", + [](const VoxelMap& voxelmap) -> Eigen::MatrixXd { + auto normals = traits::voxel_normals(voxelmap); + return Eigen::Map>(normals[0].data(), normals.size(), 4); + }, + "Get the voxel normals."); } if constexpr (has_covs) { - vox.def("voxel_covs", [](const VoxelMap& voxelmap) -> Eigen::MatrixXd { - auto covs = traits::voxel_covs(voxelmap); - return Eigen::Map>(covs[0].data(), covs.size(), 16); - }); + vox.def( + "voxel_covs", + [](const VoxelMap& voxelmap) -> Eigen::MatrixXd { + auto covs = traits::voxel_covs(voxelmap); + return Eigen::Map>(covs[0].data(), covs.size(), 16); + }, + "Get the voxel covariance matrices."); } };