mirror of https://github.com/koide3/small_gicp.git
Docpy (#59)
* doc for py * doc for cpp * sphinx * fix sphinx * generate docs
This commit is contained in:
koide3
GitHub
parent
1183e3d39a
commit
84406aefda
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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).
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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']
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
# small_gicp
|
||||
|
||||
## Documentation
|
||||
|
||||
- [C++](doc_cpp/index.html)
|
||||
- [Python](doc_py/index.html)
|
||||
|
|
@ -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`
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
small\_gicp module
|
||||
==================
|
||||
|
||||
.. automodule:: small_gicp
|
||||
:members:
|
||||
:undoc-members:
|
||||
:show-inheritance:
|
||||
|
|
@ -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 <bool HasNormals = false, bool HasCovs = false>
|
||||
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 <typename PointCloud>
|
||||
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<Eigen::Vector4d> points;
|
||||
std::conditional_t<HasNormals, std::vector<Eigen::Vector4d>, Empty> normals;
|
||||
std::conditional_t<HasCovs, std::vector<Eigen::Matrix4d>, Empty> covs;
|
||||
std::vector<Eigen::Vector4d> points; ///< Points
|
||||
std::conditional_t<HasNormals, std::vector<Eigen::Vector4d>, Empty> normals; ///< Normals (Empty if HasNormals is false)
|
||||
std::conditional_t<HasCovs, std::vector<Eigen::Matrix4d>, Empty> covs; ///< Covariances (Empty if HasCovs is false)
|
||||
};
|
||||
|
||||
/// @brief FlatContainer that stores only points.
|
||||
using FlatContainerPoints = FlatContainer<false, false>;
|
||||
/// @brief FlatContainer with normals.
|
||||
using FlatContainerNormal = FlatContainer<true, false>;
|
||||
/// @brief FlatContainer with covariances.
|
||||
using FlatContainerCov = FlatContainer<false, true>;
|
||||
/// @brief FlatContainer with normals and covariances.
|
||||
using FlatContainerNormalCov = FlatContainer<true, true>;
|
||||
|
||||
namespace traits {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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 <typename VoxelContents>
|
||||
struct IncrementalVoxelMap {
|
||||
public:
|
||||
using Ptr = std::shared_ptr<IncrementalVoxelMap>;
|
||||
using ConstPtr = std::shared_ptr<const IncrementalVoxelMap>;
|
||||
|
||||
/// @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 <typename PointCloud>
|
||||
|
|
@ -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<std::shared_ptr<std::pair<VoxelInfo, VoxelContents>>> flat_voxels; ///< Voxel contents
|
||||
std::unordered_map<Eigen::Vector3i, size_t, XORVector3iHash> voxels; ///< Voxel index map
|
||||
typename VoxelContents::Setting voxel_setting; ///< Voxel setting.
|
||||
std::vector<std::shared_ptr<std::pair<VoxelInfo, VoxelContents>>> flat_voxels; ///< Voxel contents.
|
||||
std::unordered_map<Eigen::Vector3i, size_t, XORVector3iHash> voxels; ///< Voxel index map.
|
||||
};
|
||||
|
||||
namespace traits {
|
||||
|
|
|
|||
|
|
@ -9,24 +9,30 @@ namespace pcl {
|
|||
using Matrix4fMap = Eigen::Map<Eigen::Matrix4f, Eigen::Aligned>;
|
||||
using Matrix4fMapConst = const Eigen::Map<const Eigen::Matrix4f, Eigen::Aligned>;
|
||||
|
||||
/// @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
|
||||
|
|
|
|||
|
|
@ -7,12 +7,13 @@
|
|||
|
||||
namespace small_gicp {
|
||||
|
||||
/// @brief Proxy class to access PCL point cloud with external covariance matrices.
|
||||
template <typename PointT>
|
||||
struct PointCloudProxy {
|
||||
PointCloudProxy(const pcl::PointCloud<PointT>& cloud, std::vector<Eigen::Matrix4d>& covs) : cloud(cloud), covs(covs) {}
|
||||
|
||||
const pcl::PointCloud<PointT>& cloud;
|
||||
std::vector<Eigen::Matrix4d>& covs;
|
||||
const pcl::PointCloud<PointT>& cloud; ///< Point cloud
|
||||
std::vector<Eigen::Matrix4d>& covs; ///< Covariance matrices
|
||||
};
|
||||
|
||||
namespace traits {
|
||||
|
|
|
|||
|
|
@ -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<double, 6, 1>& a) {
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
|
|
|||
|
|
@ -7,10 +7,13 @@
|
|||
|
||||
namespace small_gicp {
|
||||
|
||||
/// @brief Computes point normals from eigenvectors and sets them to the point cloud.
|
||||
template <typename PointCloud>
|
||||
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 <typename PointCloud>
|
||||
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 <typename PointCloud>
|
||||
struct NormalCovarianceSetter {
|
||||
/// @brief Handle invalid case (too few points).
|
||||
static void set_invalid(PointCloud& cloud, size_t i) {
|
||||
NormalSetter<PointCloud>::set_invalid(cloud, i);
|
||||
CovarianceSetter<PointCloud>::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<PointCloud>::set(cloud, i, eigenvectors);
|
||||
CovarianceSetter<PointCloud>::set(cloud, i, eigenvectors);
|
||||
|
|
|
|||
|
|
@ -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 <typename RandomAccessIterator, typename Compare>
|
||||
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 <typename RandomAccessIterator, typename Compare>
|
||||
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 <typename RandomAccessIterator, typename Compare>
|
||||
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 <typename RandomAccessIterator, typename Compare>
|
||||
void quick_sort_omp(RandomAccessIterator first, RandomAccessIterator last, const Compare& comp, int num_threads) {
|
||||
#ifndef _MSC_VER
|
||||
|
|
|
|||
|
|
@ -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'
|
||||
|
|
@ -20,12 +20,12 @@ using namespace small_gicp;
|
|||
|
||||
void define_factors(py::module& m) {
|
||||
// DistanceRejector
|
||||
py::class_<DistanceRejector>(m, "DistanceRejector") //
|
||||
py::class_<DistanceRejector>(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_<ICPFactor>(m, "ICPFactor") //
|
||||
py::class_<ICPFactor>(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_<PointToPlaneICPFactor>(m, "PointToPlaneICPFactor") //
|
||||
py::class_<PointToPlaneICPFactor>(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_<GICPFactor>(m, "GICPFactor") //
|
||||
py::class_<GICPFactor>(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.");
|
||||
}
|
||||
|
|
@ -16,7 +16,7 @@ using namespace small_gicp;
|
|||
|
||||
void define_kdtree(py::module& m) {
|
||||
// KdTree
|
||||
py::class_<KdTree<PointCloud>, std::shared_ptr<KdTree<PointCloud>>>(m, "KdTree") //
|
||||
py::class_<KdTree<PointCloud>, std::shared_ptr<KdTree<PointCloud>>>(m, "KdTree", "KdTree") //
|
||||
.def(
|
||||
py::init([](const PointCloud::ConstPtr& points, int num_threads) { return std::make_shared<KdTree<PointCloud>>(points, KdTreeBuilderOMP(num_threads)); }),
|
||||
py::arg("points"),
|
||||
|
|
@ -28,11 +28,18 @@ void define_kdtree(py::module& m) {
|
|||
double k_sq_dist = std::numeric_limits<double>::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<PointCloud>& kdtree, const Eigen::Vector3d& pt, int k) {
|
||||
std::vector<size_t> k_indices(k, -1);
|
||||
std::vector<double> k_sq_dists(k, std::numeric_limits<double>::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<PointCloud>& kdtree, const Eigen::Vector3d& pt, int k) {
|
||||
std::vector<size_t> k_indices(k, -1);
|
||||
std::vector<double> k_sq_dists(k, std::numeric_limits<double>::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.");
|
||||
}
|
||||
|
|
@ -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<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(points.points[0].data(), points.size(), 4); })
|
||||
[](const PointCloud& points) -> Eigen::MatrixXd { return Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(points.points[0].data(), points.size(), 4); },
|
||||
"Get the points as a Nx4 matrix.")
|
||||
.def(
|
||||
"normals",
|
||||
[](const PointCloud& points) -> Eigen::MatrixXd { return Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(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<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(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.");
|
||||
}
|
||||
|
|
@ -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<Eigen::MatrixXd, PointCloud>(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");
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
}
|
||||
|
|
@ -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<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(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<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(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<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(normals[0].data(), normals.size(), 4);
|
||||
});
|
||||
vox.def(
|
||||
"voxel_normals",
|
||||
[](const VoxelMap& voxelmap) -> Eigen::MatrixXd {
|
||||
auto normals = traits::voxel_normals(voxelmap);
|
||||
return Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(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<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(covs[0].data(), covs.size(), 16);
|
||||
});
|
||||
vox.def(
|
||||
"voxel_covs",
|
||||
[](const VoxelMap& voxelmap) -> Eigen::MatrixXd {
|
||||
auto covs = traits::voxel_covs(voxelmap);
|
||||
return Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(covs[0].data(), covs.size(), 16);
|
||||
},
|
||||
"Get the voxel covariance matrices.");
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue