commit 3b0f422bcb6ede61c4b3e623bf0f61c424f4ea16 Author: k.koide Date: Fri Mar 22 19:55:26 2024 +0900 init diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..4676fbc --- /dev/null +++ b/.clang-format @@ -0,0 +1,168 @@ +--- +Language: Cpp +# BasedOnStyle: Google +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +AlignConsecutiveMacros: false +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: true +AllowAllArgumentsOnNextLine: false +AllowAllConstructorInitializersOnNextLine: false +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortBlocksOnASingleLine: Never +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: Inline +AllowShortLambdasOnASingleLine: All +AllowShortIfStatementsOnASingleLine: WithoutElse +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: Yes +BinPackArguments: false +BinPackParameters: false +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakInheritanceList: BeforeColon +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 180 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +Cpp11BracedListStyle: true +DeriveLineEnding: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeBlocks: Regroup +IncludeCategories: + - Regex: '^' + Priority: 2 + SortPriority: 0 + - Regex: '^<.*\.h>' + Priority: 1 + SortPriority: 0 + - Regex: '^<.*' + Priority: 2 + SortPriority: 0 + - Regex: '.*' + Priority: 3 + SortPriority: 0 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IncludeIsMainSourceRegex: '' +IndentCaseLabels: true +IndentGotoLabels: true +IndentPPDirectives: None +IndentWidth: 2 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBinPackProtocolList: Never +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyBreakTemplateDeclaration: 10 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +RawStringFormats: + - Language: Cpp + Delimiters: + - cc + - CC + - cpp + - Cpp + - CPP + - 'c++' + - 'C++' + CanonicalDelimiter: '' + BasedOnStyle: google + - Language: TextProto + Delimiters: + - pb + - PB + - proto + - PROTO + EnclosingFunctions: + - EqualsProto + - EquivToProto + - PARSE_PARTIAL_TEXT_PROTO + - PARSE_TEST_PROTO + - PARSE_TEXT_PROTO + - ParseTextOrDie + - ParseTextProtoOrDie + CanonicalDelimiter: '' + BasedOnStyle: google +ReflowComments: true +SortIncludes: false +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeRangeBasedForLoopColon: true +SpaceInEmptyBlock: false +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInConditionalStatement: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +SpaceBeforeSquareBrackets: false +Standard: Auto +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 8 +UseCRLF: false +UseTab: Never +... + diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..1f40305 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +.vscode/* +build/* diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..3dc6f7b --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.5.2) +project(small_gicp) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") + +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Choose the type of build." FORCE) + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") +endif() + +option(BUILD_TESTS "Build test" OFF) +option(BUILD_WITH_MARCH_NATIVE "Build with -march=native" OFF) + +if(BUILD_WITH_MARCH_NATIVE) + add_compile_options(-march=native) + set(CMAKE_C_FLAGS "-march=native ${CMAKE_C_FLAGS}") + set(CMAKE_CXX_FLAGS "-march=native ${CMAKE_CXX_FLAGS}") +endif() + +find_package(PCL REQUIRED) +find_package(TBB REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(Iridescence REQUIRED) + +find_package(OpenMP) +if (OPENMP_FOUND) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +endif() + +########### +## Build ## +########### + +add_executable(small_gicp_test + src/small_gicp_test.cpp +) +target_include_directories(small_gicp_test PUBLIC + include + ${PCL_INCLUDE_DIRS} + ${TBB_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${Iridescence_INCLUDE_DIRS} +) +target_link_libraries(small_gicp_test + ${PCL_LIBRARIES} + ${TBB_LIBRARIES} + ${Iridescence_LIBRARIES} +) \ No newline at end of file diff --git a/cmake/FindGTSAM.cmake b/cmake/FindGTSAM.cmake new file mode 100644 index 0000000..0779a92 --- /dev/null +++ b/cmake/FindGTSAM.cmake @@ -0,0 +1,26 @@ +find_path(GTSAM_INCLUDE_DIRS gtsam/inference/FactorGraph.h + HINTS /usr/local/include /usr/include + DOC "GTSAM include directories") + +find_library(GTSAM_LIB NAMES gtsam + HINTS /usr/local/lib /usr/lib + DOC "GTSAM libraries") + +find_library(GTSAM_UNSTABLE_LIB NAMES gtsam_unstable + HINTS /usr/local/lib /usr/lib + DOC "GTSAM_UNSTABLE libraries") + +find_library(TBB_LIB NAMES tbb + HINTS /usr/local/lib /usr/lib + DOC "TBB libraries") + +find_library(TBB_MALLOC_LIB NAMES tbbmalloc + HINTS /usr/local/lib /usr/lib + DOC "TBB malloc libraries") + +if(GTSAM_LIB AND GTSAM_UNSTABLE_LIB AND TBB_LIB) + set(GTSAM_LIBRARIES ${GTSAM_LIB} ${GTSAM_UNSTABLE_LIB} ${TBB_LIB} ${TBB_MALLOC_LIB}) +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(GTSAM DEFAULT_MSG GTSAM_INCLUDE_DIRS GTSAM_LIBRARIES) diff --git a/cmake/FindIridescence.cmake b/cmake/FindIridescence.cmake new file mode 100644 index 0000000..ab9ff69 --- /dev/null +++ b/cmake/FindIridescence.cmake @@ -0,0 +1,16 @@ +find_path(Iridescence_INCLUDE_DIRS glk/drawable.hpp + HINTS /usr/local/include/iridescence /usr/include/iridescence + DOC "Iridescence include directories") + +find_library(Iridescence_LIBRARY NAMES iridescence + HINTS /usr/local/lib /usr/lib + DOC "Iridescence libraries") + + find_library(gl_imgui_LIBRARY NAMES gl_imgui + HINTS /usr/local/lib /usr/lib + DOC "Iridescence libraries") + +set(Iridescence_LIBRARIES ${Iridescence_LIBRARY} ${gl_imgui_LIBRARY}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Iridescence DEFAULT_MSG Iridescence_INCLUDE_DIRS Iridescence_LIBRARIES) diff --git a/cmake/FindTBB.cmake b/cmake/FindTBB.cmake new file mode 100644 index 0000000..cbcc73d --- /dev/null +++ b/cmake/FindTBB.cmake @@ -0,0 +1,15 @@ +find_path(TBB_INCLUDE_DIRS tbb/tbb.h + HINTS /usr/local/include /usr/include + DOC "oneTBB include directories") + +find_library(TBB_LIB NAMES tbb + HINTS /usr/local/lib /usr/lib /usr/lib/x86_64-linux-gnu + DOC "TBB libraries") + +# if(GTSAM_LIB AND GTSAM_UNSTABLE_LIB AND TBB_LIB) +if(TBB_LIB) + set(TBB_LIBRARIES ${TBB_LIB}) +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(TBB DEFAULT_MSG TBB_INCLUDE_DIRS TBB_LIBRARIES) diff --git a/data/source.ply b/data/source.ply new file mode 100644 index 0000000..7e6c0fc Binary files /dev/null and b/data/source.ply differ diff --git a/data/target.ply b/data/target.ply new file mode 100644 index 0000000..c3382d3 Binary files /dev/null and b/data/target.ply differ diff --git a/include/small_gicp/ann/cached_kdtree.hpp b/include/small_gicp/ann/cached_kdtree.hpp new file mode 100644 index 0000000..8049408 --- /dev/null +++ b/include/small_gicp/ann/cached_kdtree.hpp @@ -0,0 +1,62 @@ +#pragma once + +#include +#include +#include +#include + +namespace small_gicp { + +template +class CachedKdTree { +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + CachedKdTree(const PointCloud& points, double leaf_size) : inv_leaf_size(1.0 / leaf_size), kdtree(points) {} + + size_t knn_search(const Eigen::Vector4d& pt, size_t k, size_t* k_indices, double* k_sq_dists) const { + const Eigen::Vector3i coord = (pt * inv_leaf_size).array().floor().cast().head<3>(); + + CacheTable::const_accessor ca; + if (cache.find(ca, coord)) { + std::ranges::copy(ca->second.first, k_indices); + std::ranges::copy(ca->second.second, k_sq_dists); + return ca->second.first.size(); + } + + const size_t n = kdtree.knn_search(pt, k, k_indices, k_sq_dists); + std::vector indices(k_indices, k_indices + n); + std::vector sq_dists(k_sq_dists, k_sq_dists + n); + + CacheTable::accessor a; + if (cache.insert(a, coord)) { + a->second.first = std::move(indices); + a->second.second = std::move(sq_dists); + } + + return n; + } + +public: + const double inv_leaf_size; + + using KnnResult = std::pair, std::vector>; + using CacheTable = tbb::concurrent_hash_map; + mutable CacheTable cache; + + UnsafeKdTree kdtree; +}; + +namespace traits { + +template +struct Traits> { + static size_t knn_search(const CachedKdTree& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) { + return tree.knn_search(point, k, k_indices, k_sq_dists); + } +}; + +} // namespace traits + +} // namespace small_gicp diff --git a/include/small_gicp/ann/kdtree.hpp b/include/small_gicp/ann/kdtree.hpp new file mode 100644 index 0000000..c472fe7 --- /dev/null +++ b/include/small_gicp/ann/kdtree.hpp @@ -0,0 +1,69 @@ +#pragma once + +#include +#include +#include +#include + +namespace small_gicp { + +template +class UnsafeKdTree { +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + using Index = nanoflann::KDTreeSingleIndexAdaptor, double>, UnsafeKdTree, 3, size_t>; + + UnsafeKdTree(const PointCloud& points) : points(points), index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10)) { index.buildIndex(); } + ~UnsafeKdTree() {} + + size_t kdtree_get_point_count() const { return traits::size(points); } + double kdtree_get_pt(const size_t idx, const size_t dim) const { return traits::point(points, idx)[dim]; } + + template + bool kdtree_get_bbox(BBox&) const { + return false; + } + + size_t knn_search(const Eigen::Vector4d& pt, size_t k, size_t* k_indices, double* k_sq_dists) const { return index.knnSearch(pt.data(), k, k_indices, k_sq_dists); } + +private: + const PointCloud& points; + Index index; +}; + +template +class KdTree { +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + KdTree(const std::shared_ptr& points) : points(points), tree(*points) {} + ~KdTree() {} + + size_t knn_search(const Eigen::Vector4d& pt, size_t k, size_t* k_indices, double* k_sq_dists) const { return tree.knn_search(pt, k, k_indices, k_sq_dists); } + +private: + const std::shared_ptr points; + const UnsafeKdTree tree; +}; + +namespace traits { + +template +struct Traits> { + static size_t knn_search(const UnsafeKdTree& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) { + return tree.knn_search(point, k, k_indices, k_sq_dists); + } +}; + +template +struct Traits> { + static size_t knn_search(const KdTree& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) { + return tree.knn_search(point, k, k_indices, k_sq_dists); + } +}; + +} // namespace traits + +} // namespace small_gicp diff --git a/include/small_gicp/ann/nanoflann.hpp b/include/small_gicp/ann/nanoflann.hpp new file mode 100644 index 0000000..aefdaee --- /dev/null +++ b/include/small_gicp/ann/nanoflann.hpp @@ -0,0 +1,2048 @@ +/*********************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. + * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. + * Copyright 2011-2021 Jose Luis Blanco (joseluisblancoc@gmail.com). + * All rights reserved. + * + * THE BSD LICENSE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *************************************************************************/ + +/** \mainpage nanoflann C++ API documentation + * nanoflann is a C++ header-only library for building KD-Trees, mostly + * optimized for 2D or 3D point clouds. + * + * nanoflann does not require compiling or installing, just an + * #include in your code. + * + * See: + * - C++ API organized by modules + * - Online README + * - Doxygen + * documentation + */ + +#ifndef NANOFLANN_HPP_ +#define NANOFLANN_HPP_ + +#include +#include +#include +#include // for abs() +#include // for fwrite() +#include // for abs() +#include +#include // std::reference_wrapper +#include +#include + +/** Library version: 0xMmP (M=Major,m=minor,P=patch) */ +#define NANOFLANN_VERSION 0x132 + +// Avoid conflicting declaration of min/max macros in windows headers +#if !defined(NOMINMAX) && \ + (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64)) +#define NOMINMAX +#ifdef max +#undef max +#undef min +#endif +#endif + +namespace nanoflann { +/** @addtogroup nanoflann_grp nanoflann C++ library for ANN + * @{ */ + +/** the PI constant (required to avoid MSVC missing symbols) */ +template T pi_const() { + return static_cast(3.14159265358979323846); +} + +/** + * Traits if object is resizable and assignable (typically has a resize | assign + * method) + */ +template struct has_resize : std::false_type {}; + +template +struct has_resize().resize(1), 0)> + : std::true_type {}; + +template struct has_assign : std::false_type {}; + +template +struct has_assign().assign(1, 0), 0)> + : std::true_type {}; + +/** + * Free function to resize a resizable object + */ +template +inline typename std::enable_if::value, void>::type +resize(Container &c, const size_t nElements) { + c.resize(nElements); +} + +/** + * Free function that has no effects on non resizable containers (e.g. + * std::array) It raises an exception if the expected size does not match + */ +template +inline typename std::enable_if::value, void>::type +resize(Container &c, const size_t nElements) { + if (nElements != c.size()) + throw std::logic_error("Try to change the size of a std::array."); +} + +/** + * Free function to assign to a container + */ +template +inline typename std::enable_if::value, void>::type +assign(Container &c, const size_t nElements, const T &value) { + c.assign(nElements, value); +} + +/** + * Free function to assign to a std::array + */ +template +inline typename std::enable_if::value, void>::type +assign(Container &c, const size_t nElements, const T &value) { + for (size_t i = 0; i < nElements; i++) + c[i] = value; +} + +/** @addtogroup result_sets_grp Result set classes + * @{ */ +template +class KNNResultSet { +public: + typedef _DistanceType DistanceType; + typedef _IndexType IndexType; + typedef _CountType CountType; + +private: + IndexType *indices; + DistanceType *dists; + CountType capacity; + CountType count; + +public: + inline KNNResultSet(CountType capacity_) + : indices(0), dists(0), capacity(capacity_), count(0) {} + + inline void init(IndexType *indices_, DistanceType *dists_) { + indices = indices_; + dists = dists_; + count = 0; + if (capacity) + dists[capacity - 1] = (std::numeric_limits::max)(); + } + + inline CountType size() const { return count; } + + inline bool full() const { return count == capacity; } + + /** + * Called during search to add an element matching the criteria. + * @return true if the search should be continued, false if the results are + * sufficient + */ + inline bool addPoint(DistanceType dist, IndexType index) { + CountType i; + for (i = count; i > 0; --i) { +#ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same + // distance, the one with the lowest-index will be + // returned first. + if ((dists[i - 1] > dist) || + ((dist == dists[i - 1]) && (indices[i - 1] > index))) { +#else + if (dists[i - 1] > dist) { +#endif + if (i < capacity) { + dists[i] = dists[i - 1]; + indices[i] = indices[i - 1]; + } + } else + break; + } + if (i < capacity) { + dists[i] = dist; + indices[i] = index; + } + if (count < capacity) + count++; + + // tell caller that the search shall continue + return true; + } + + inline DistanceType worstDist() const { return dists[capacity - 1]; } +}; + +/** operator "<" for std::sort() */ +struct IndexDist_Sorter { + /** PairType will be typically: std::pair */ + template + inline bool operator()(const PairType &p1, const PairType &p2) const { + return p1.second < p2.second; + } +}; + +/** + * A result-set class used when performing a radius based search. + */ +template +class RadiusResultSet { +public: + typedef _DistanceType DistanceType; + typedef _IndexType IndexType; + +public: + const DistanceType radius; + + std::vector> &m_indices_dists; + + inline RadiusResultSet( + DistanceType radius_, + std::vector> &indices_dists) + : radius(radius_), m_indices_dists(indices_dists) { + init(); + } + + inline void init() { clear(); } + inline void clear() { m_indices_dists.clear(); } + + inline size_t size() const { return m_indices_dists.size(); } + + inline bool full() const { return true; } + + /** + * Called during search to add an element matching the criteria. + * @return true if the search should be continued, false if the results are + * sufficient + */ + inline bool addPoint(DistanceType dist, IndexType index) { + if (dist < radius) + m_indices_dists.push_back(std::make_pair(index, dist)); + return true; + } + + inline DistanceType worstDist() const { return radius; } + + /** + * Find the worst result (furtherest neighbor) without copying or sorting + * Pre-conditions: size() > 0 + */ + std::pair worst_item() const { + if (m_indices_dists.empty()) + throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on " + "an empty list of results."); + typedef + typename std::vector>::const_iterator + DistIt; + DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end(), + IndexDist_Sorter()); + return *it; + } +}; + +/** @} */ + +/** @addtogroup loadsave_grp Load/save auxiliary functions + * @{ */ +template +void save_value(FILE *stream, const T &value, size_t count = 1) { + fwrite(&value, sizeof(value), count, stream); +} + +template +void save_value(FILE *stream, const std::vector &value) { + size_t size = value.size(); + fwrite(&size, sizeof(size_t), 1, stream); + fwrite(&value[0], sizeof(T), size, stream); +} + +template +void load_value(FILE *stream, T &value, size_t count = 1) { + size_t read_cnt = fread(&value, sizeof(value), count, stream); + if (read_cnt != count) { + throw std::runtime_error("Cannot read from file"); + } +} + +template void load_value(FILE *stream, std::vector &value) { + size_t size; + size_t read_cnt = fread(&size, sizeof(size_t), 1, stream); + if (read_cnt != 1) { + throw std::runtime_error("Cannot read from file"); + } + value.resize(size); + read_cnt = fread(&value[0], sizeof(T), size, stream); + if (read_cnt != size) { + throw std::runtime_error("Cannot read from file"); + } +} +/** @} */ + +/** @addtogroup metric_grp Metric (distance) classes + * @{ */ + +struct Metric {}; + +/** Manhattan distance functor (generic version, optimized for + * high-dimensionality data sets). Corresponding distance traits: + * nanoflann::metric_L1 \tparam T Type of the elements (e.g. double, float, + * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) + * (e.g. float, double, int64_t) + */ +template +struct L1_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} + + inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size, + DistanceType worst_dist = -1) const { + DistanceType result = DistanceType(); + const T *last = a + size; + const T *lastgroup = last - 3; + size_t d = 0; + + /* Process 4 items with each loop for efficiency. */ + while (a < lastgroup) { + const DistanceType diff0 = + std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++)); + const DistanceType diff1 = + std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++)); + const DistanceType diff2 = + std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++)); + const DistanceType diff3 = + std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++)); + result += diff0 + diff1 + diff2 + diff3; + a += 4; + if ((worst_dist > 0) && (result > worst_dist)) { + return result; + } + } + /* Process last 0-3 components. Not needed for standard vector lengths. */ + while (a < last) { + result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++)); + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + return std::abs(a - b); + } +}; + +/** Squared Euclidean distance functor (generic version, optimized for + * high-dimensionality data sets). Corresponding distance traits: + * nanoflann::metric_L2 \tparam T Type of the elements (e.g. double, float, + * uint8_t) \tparam _DistanceType Type of distance variables (must be signed) + * (e.g. float, double, int64_t) + */ +template +struct L2_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} + + inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size, + DistanceType worst_dist = -1) const { + DistanceType result = DistanceType(); + const T *last = a + size; + const T *lastgroup = last - 3; + size_t d = 0; + + /* Process 4 items with each loop for efficiency. */ + while (a < lastgroup) { + const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++); + const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++); + const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++); + const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++); + result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3; + a += 4; + if ((worst_dist > 0) && (result > worst_dist)) { + return result; + } + } + /* Process last 0-3 components. Not needed for standard vector lengths. */ + while (a < last) { + const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++); + result += diff0 * diff0; + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + return (a - b) * (a - b); + } +}; + +/** Squared Euclidean (L2) distance functor (suitable for low-dimensionality + * datasets, like 2D or 3D point clouds) Corresponding distance traits: + * nanoflann::metric_L2_Simple \tparam T Type of the elements (e.g. double, + * float, uint8_t) \tparam _DistanceType Type of distance variables (must be + * signed) (e.g. float, double, int64_t) + */ +template +struct L2_Simple_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + L2_Simple_Adaptor(const DataSource &_data_source) + : data_source(_data_source) {} + + inline DistanceType evalMetric(const T *a, const size_t b_idx, + size_t size) const { + DistanceType result = DistanceType(); + for (size_t i = 0; i < size; ++i) { + const DistanceType diff = a[i] - data_source.kdtree_get_pt(b_idx, i); + result += diff * diff; + } + return result; + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + return (a - b) * (a - b); + } +}; + +/** SO2 distance functor + * Corresponding distance traits: nanoflann::metric_SO2 + * \tparam T Type of the elements (e.g. double, float) + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. + * float, double) orientation is constrained to be in [-pi, pi] + */ +template +struct SO2_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + const DataSource &data_source; + + SO2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {} + + inline DistanceType evalMetric(const T *a, const size_t b_idx, + size_t size) const { + return accum_dist(a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1), + size - 1); + } + + /** Note: this assumes that input angles are already in the range [-pi,pi] */ + template + inline DistanceType accum_dist(const U a, const V b, const size_t) const { + DistanceType result = DistanceType(); + DistanceType PI = pi_const(); + result = b - a; + if (result > PI) + result -= 2 * PI; + else if (result < -PI) + result += 2 * PI; + return result; + } +}; + +/** SO3 distance functor (Uses L2_Simple) + * Corresponding distance traits: nanoflann::metric_SO3 + * \tparam T Type of the elements (e.g. double, float) + * \tparam _DistanceType Type of distance variables (must be signed) (e.g. + * float, double) + */ +template +struct SO3_Adaptor { + typedef T ElementType; + typedef _DistanceType DistanceType; + + L2_Simple_Adaptor distance_L2_Simple; + + SO3_Adaptor(const DataSource &_data_source) + : distance_L2_Simple(_data_source) {} + + inline DistanceType evalMetric(const T *a, const size_t b_idx, + size_t size) const { + return distance_L2_Simple.evalMetric(a, b_idx, size); + } + + template + inline DistanceType accum_dist(const U a, const V b, const size_t idx) const { + return distance_L2_Simple.accum_dist(a, b, idx); + } +}; + +/** Metaprogramming helper traits class for the L1 (Manhattan) metric */ +struct metric_L1 : public Metric { + template struct traits { + typedef L1_Adaptor distance_t; + }; +}; +/** Metaprogramming helper traits class for the L2 (Euclidean) metric */ +struct metric_L2 : public Metric { + template struct traits { + typedef L2_Adaptor distance_t; + }; +}; +/** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */ +struct metric_L2_Simple : public Metric { + template struct traits { + typedef L2_Simple_Adaptor distance_t; + }; +}; +/** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ +struct metric_SO2 : public Metric { + template struct traits { + typedef SO2_Adaptor distance_t; + }; +}; +/** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */ +struct metric_SO3 : public Metric { + template struct traits { + typedef SO3_Adaptor distance_t; + }; +}; + +/** @} */ + +/** @addtogroup param_grp Parameter structs + * @{ */ + +/** Parameters (see README.md) */ +struct KDTreeSingleIndexAdaptorParams { + KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10) + : leaf_max_size(_leaf_max_size) {} + + size_t leaf_max_size; +}; + +/** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */ +struct SearchParams { + /** Note: The first argument (checks_IGNORED_) is ignored, but kept for + * compatibility with the FLANN interface */ + SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true) + : checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {} + + int checks; //!< Ignored parameter (Kept for compatibility with the FLANN + //!< interface). + float eps; //!< search for eps-approximate neighbours (default: 0) + bool sorted; //!< only for radius search, require neighbours sorted by + //!< distance (default: true) +}; +/** @} */ + +/** @addtogroup memalloc_grp Memory allocation + * @{ */ + +/** + * Allocates (using C's malloc) a generic type T. + * + * Params: + * count = number of instances to allocate. + * Returns: pointer (of type T*) to memory buffer + */ +template inline T *allocate(size_t count = 1) { + T *mem = static_cast(::malloc(sizeof(T) * count)); + return mem; +} + +/** + * Pooled storage allocator + * + * The following routines allow for the efficient allocation of storage in + * small chunks from a specified pool. Rather than allowing each structure + * to be freed individually, an entire pool of storage is freed at once. + * This method has two advantages over just using malloc() and free(). First, + * it is far more efficient for allocating small objects, as there is + * no overhead for remembering all the information needed to free each + * object or consolidating fragmented memory. Second, the decision about + * how long to keep an object is made at the time of allocation, and there + * is no need to track down all the objects to free them. + * + */ + +const size_t WORDSIZE = 16; +const size_t BLOCKSIZE = 8192; + +class PooledAllocator { + /* We maintain memory alignment to word boundaries by requiring that all + allocations be in multiples of the machine wordsize. */ + /* Size of machine word in bytes. Must be power of 2. */ + /* Minimum number of bytes requested at a time from the system. Must be + * multiple of WORDSIZE. */ + + size_t remaining; /* Number of bytes left in current block of storage. */ + void *base; /* Pointer to base of current block of storage. */ + void *loc; /* Current location in block to next allocate memory. */ + + void internal_init() { + remaining = 0; + base = NULL; + usedMemory = 0; + wastedMemory = 0; + } + +public: + size_t usedMemory; + size_t wastedMemory; + + /** + Default constructor. Initializes a new pool. + */ + PooledAllocator() { internal_init(); } + + /** + * Destructor. Frees all the memory allocated in this pool. + */ + ~PooledAllocator() { free_all(); } + + /** Frees all allocated memory chunks */ + void free_all() { + while (base != NULL) { + void *prev = + *(static_cast(base)); /* Get pointer to prev block. */ + ::free(base); + base = prev; + } + internal_init(); + } + + /** + * Returns a pointer to a piece of new memory of the given size in bytes + * allocated from the pool. + */ + void *malloc(const size_t req_size) { + /* Round size up to a multiple of wordsize. The following expression + only works for WORDSIZE that is a power of 2, by masking last bits of + incremented size to zero. + */ + const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1); + + /* Check whether a new block must be allocated. Note that the first word + of a block is reserved for a pointer to the previous block. + */ + if (size > remaining) { + + wastedMemory += remaining; + + /* Allocate new storage. */ + const size_t blocksize = + (size + sizeof(void *) + (WORDSIZE - 1) > BLOCKSIZE) + ? size + sizeof(void *) + (WORDSIZE - 1) + : BLOCKSIZE; + + // use the standard C malloc to allocate memory + void *m = ::malloc(blocksize); + if (!m) { + fprintf(stderr, "Failed to allocate memory.\n"); + throw std::bad_alloc(); + } + + /* Fill first word of new block with pointer to previous block. */ + static_cast(m)[0] = base; + base = m; + + size_t shift = 0; + // int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & + // (WORDSIZE-1))) & (WORDSIZE-1); + + remaining = blocksize - sizeof(void *) - shift; + loc = (static_cast(m) + sizeof(void *) + shift); + } + void *rloc = loc; + loc = static_cast(loc) + size; + remaining -= size; + + usedMemory += size; + + return rloc; + } + + /** + * Allocates (using this pool) a generic type T. + * + * Params: + * count = number of instances to allocate. + * Returns: pointer (of type T*) to memory buffer + */ + template T *allocate(const size_t count = 1) { + T *mem = static_cast(this->malloc(sizeof(T) * count)); + return mem; + } +}; +/** @} */ + +/** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff + * @{ */ + +/** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors + * when DIM=-1. Fixed size version for a generic DIM: + */ +template struct array_or_vector_selector { + typedef std::array container_t; +}; +/** Dynamic size version */ +template struct array_or_vector_selector<-1, T> { + typedef std::vector container_t; +}; + +/** @} */ + +/** kd-tree base-class + * + * Contains the member functions common to the classes KDTreeSingleIndexAdaptor + * and KDTreeSingleIndexDynamicAdaptor_. + * + * \tparam Derived The name of the class which inherits this class. + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use, these are all classes derived + * from nanoflann::Metric \tparam DIM Dimensionality of data points (e.g. 3 for + * 3D points) \tparam IndexType Will be typically size_t or int + */ + +template +class KDTreeBaseClass { + +public: + /** Frees the previously-built index. Automatically called within + * buildIndex(). */ + void freeIndex(Derived &obj) { + obj.pool.free_all(); + obj.root_node = NULL; + obj.m_size_at_index_build = 0; + } + + typedef typename Distance::ElementType ElementType; + typedef typename Distance::DistanceType DistanceType; + + /*--------------------- Internal Data Structures --------------------------*/ + struct Node { + /** Union used because a node can be either a LEAF node or a non-leaf node, + * so both data fields are never used simultaneously */ + union { + struct leaf { + IndexType left, right; //!< Indices of points in leaf node + } lr; + struct nonleaf { + int divfeat; //!< Dimension used for subdivision. + DistanceType divlow, divhigh; //!< The values used for subdivision. + } sub; + } node_type; + Node *child1, *child2; //!< Child nodes (both=NULL mean its a leaf node) + }; + + typedef Node *NodePtr; + + struct Interval { + ElementType low, high; + }; + + /** + * Array of indices to vectors in the dataset. + */ + std::vector vind; + + NodePtr root_node; + + size_t m_leaf_max_size; + + size_t m_size; //!< Number of current points in the dataset + size_t m_size_at_index_build; //!< Number of points in the dataset when the + //!< index was built + int dim; //!< Dimensionality of each data point + + /** Define "BoundingBox" as a fixed-size or variable-size container depending + * on "DIM" */ + typedef + typename array_or_vector_selector::container_t BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + typedef typename array_or_vector_selector::container_t + distance_vector_t; + + /** The KD-tree used to find neighbours */ + + BoundingBox root_bbox; + + /** + * Pooled memory allocator. + * + * Using a pooled memory allocator is more efficient + * than allocating memory directly when there is a large + * number small of memory allocations. + */ + PooledAllocator pool; + + /** Returns number of points in dataset */ + size_t size(const Derived &obj) const { return obj.m_size; } + + /** Returns the length of each point in the dataset */ + size_t veclen(const Derived &obj) { + return static_cast(DIM > 0 ? DIM : obj.dim); + } + + /// Helper accessor to the dataset points: + inline ElementType dataset_get(const Derived &obj, size_t idx, + int component) const { + return obj.dataset.kdtree_get_pt(idx, component); + } + + /** + * Computes the inde memory usage + * Returns: memory used by the index + */ + size_t usedMemory(Derived &obj) { + return obj.pool.usedMemory + obj.pool.wastedMemory + + obj.dataset.kdtree_get_point_count() * + sizeof(IndexType); // pool memory and vind array memory + } + + void computeMinMax(const Derived &obj, IndexType *ind, IndexType count, + int element, ElementType &min_elem, + ElementType &max_elem) { + min_elem = dataset_get(obj, ind[0], element); + max_elem = dataset_get(obj, ind[0], element); + for (IndexType i = 1; i < count; ++i) { + ElementType val = dataset_get(obj, ind[i], element); + if (val < min_elem) + min_elem = val; + if (val > max_elem) + max_elem = val; + } + } + + /** + * Create a tree node that subdivides the list of vecs from vind[first] + * to vind[last]. The routine is called recursively on each sublist. + * + * @param left index of the first vector + * @param right index of the last vector + */ + NodePtr divideTree(Derived &obj, const IndexType left, const IndexType right, + BoundingBox &bbox) { + NodePtr node = obj.pool.template allocate(); // allocate memory + + /* If too few exemplars remain, then make this a leaf node. */ + if ((right - left) <= static_cast(obj.m_leaf_max_size)) { + node->child1 = node->child2 = NULL; /* Mark as leaf node. */ + node->node_type.lr.left = left; + node->node_type.lr.right = right; + + // compute bounding-box of leaf points + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + bbox[i].low = dataset_get(obj, obj.vind[left], i); + bbox[i].high = dataset_get(obj, obj.vind[left], i); + } + for (IndexType k = left + 1; k < right; ++k) { + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + if (bbox[i].low > dataset_get(obj, obj.vind[k], i)) + bbox[i].low = dataset_get(obj, obj.vind[k], i); + if (bbox[i].high < dataset_get(obj, obj.vind[k], i)) + bbox[i].high = dataset_get(obj, obj.vind[k], i); + } + } + } else { + IndexType idx; + int cutfeat; + DistanceType cutval; + middleSplit_(obj, &obj.vind[0] + left, right - left, idx, cutfeat, cutval, + bbox); + + node->node_type.sub.divfeat = cutfeat; + + BoundingBox left_bbox(bbox); + left_bbox[cutfeat].high = cutval; + node->child1 = divideTree(obj, left, left + idx, left_bbox); + + BoundingBox right_bbox(bbox); + right_bbox[cutfeat].low = cutval; + node->child2 = divideTree(obj, left + idx, right, right_bbox); + + node->node_type.sub.divlow = left_bbox[cutfeat].high; + node->node_type.sub.divhigh = right_bbox[cutfeat].low; + + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low); + bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high); + } + } + + return node; + } + + void middleSplit_(Derived &obj, IndexType *ind, IndexType count, + IndexType &index, int &cutfeat, DistanceType &cutval, + const BoundingBox &bbox) { + const DistanceType EPS = static_cast(0.00001); + ElementType max_span = bbox[0].high - bbox[0].low; + for (int i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i) { + ElementType span = bbox[i].high - bbox[i].low; + if (span > max_span) { + max_span = span; + } + } + ElementType max_spread = -1; + cutfeat = 0; + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + ElementType span = bbox[i].high - bbox[i].low; + if (span > (1 - EPS) * max_span) { + ElementType min_elem, max_elem; + computeMinMax(obj, ind, count, i, min_elem, max_elem); + ElementType spread = max_elem - min_elem; + if (spread > max_spread) { + cutfeat = i; + max_spread = spread; + } + } + } + // split in the middle + DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2; + ElementType min_elem, max_elem; + computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem); + + if (split_val < min_elem) + cutval = min_elem; + else if (split_val > max_elem) + cutval = max_elem; + else + cutval = split_val; + + IndexType lim1, lim2; + planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2); + + if (lim1 > count / 2) + index = lim1; + else if (lim2 < count / 2) + index = lim2; + else + index = count / 2; + } + + /** + * Subdivide the list of points by a plane perpendicular on axe corresponding + * to the 'cutfeat' dimension at 'cutval' position. + * + * On return: + * dataset[ind[0..lim1-1]][cutfeat]cutval + */ + void planeSplit(Derived &obj, IndexType *ind, const IndexType count, + int cutfeat, DistanceType &cutval, IndexType &lim1, + IndexType &lim2) { + /* Move vector indices for left subtree to front of list. */ + IndexType left = 0; + IndexType right = count - 1; + for (;;) { + while (left <= right && dataset_get(obj, ind[left], cutfeat) < cutval) + ++left; + while (right && left <= right && + dataset_get(obj, ind[right], cutfeat) >= cutval) + --right; + if (left > right || !right) + break; // "!right" was added to support unsigned Index types + std::swap(ind[left], ind[right]); + ++left; + --right; + } + /* If either list is empty, it means that all remaining features + * are identical. Split in the middle to maintain a balanced tree. + */ + lim1 = left; + right = count - 1; + for (;;) { + while (left <= right && dataset_get(obj, ind[left], cutfeat) <= cutval) + ++left; + while (right && left <= right && + dataset_get(obj, ind[right], cutfeat) > cutval) + --right; + if (left > right || !right) + break; // "!right" was added to support unsigned Index types + std::swap(ind[left], ind[right]); + ++left; + --right; + } + lim2 = left; + } + + DistanceType computeInitialDistances(const Derived &obj, + const ElementType *vec, + distance_vector_t &dists) const { + assert(vec); + DistanceType distsq = DistanceType(); + + for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) { + if (vec[i] < obj.root_bbox[i].low) { + dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i); + distsq += dists[i]; + } + if (vec[i] > obj.root_bbox[i].high) { + dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i); + distsq += dists[i]; + } + } + return distsq; + } + + void save_tree(Derived &obj, FILE *stream, NodePtr tree) { + save_value(stream, *tree); + if (tree->child1 != NULL) { + save_tree(obj, stream, tree->child1); + } + if (tree->child2 != NULL) { + save_tree(obj, stream, tree->child2); + } + } + + void load_tree(Derived &obj, FILE *stream, NodePtr &tree) { + tree = obj.pool.template allocate(); + load_value(stream, *tree); + if (tree->child1 != NULL) { + load_tree(obj, stream, tree->child1); + } + if (tree->child2 != NULL) { + load_tree(obj, stream, tree->child2); + } + } + + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when + * loading the index object it must be constructed associated to the same + * source of data points used while building it. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void saveIndex_(Derived &obj, FILE *stream) { + save_value(stream, obj.m_size); + save_value(stream, obj.dim); + save_value(stream, obj.root_bbox); + save_value(stream, obj.m_leaf_max_size); + save_value(stream, obj.vind); + save_tree(obj, stream, obj.root_node); + } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the + * index object must be constructed associated to the same source of data + * points used while building the index. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void loadIndex_(Derived &obj, FILE *stream) { + load_value(stream, obj.m_size); + load_value(stream, obj.dim); + load_value(stream, obj.root_bbox); + load_value(stream, obj.m_leaf_max_size); + load_value(stream, obj.vind); + load_tree(obj, stream, obj.root_node); + } +}; + +/** @addtogroup kdtrees_grp KD-tree classes and adaptors + * @{ */ + +/** kd-tree static index + * + * Contains the k-d trees and other information for indexing a set of points + * for nearest-neighbor matching. + * + * The class "DatasetAdaptor" must provide the following interface (can be + * non-virtual, inlined methods): + * + * \code + * // Must return the number of data poins + * inline size_t kdtree_get_point_count() const { ... } + * + * + * // Must return the dim'th component of the idx'th point in the class: + * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } + * + * // Optional bounding-box computation: return false to default to a standard + * bbox computation loop. + * // Return true if the BBOX was already computed by the class and returned + * in "bb" so it can be avoided to redo it again. + * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 + * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const + * { + * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits + * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits + * ... + * return true; + * } + * + * \endcode + * + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM + * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will + * be typically size_t or int + */ +template +class KDTreeSingleIndexAdaptor + : public KDTreeBaseClass< + KDTreeSingleIndexAdaptor, + Distance, DatasetAdaptor, DIM, IndexType> { +public: + /** Deleted copy constructor*/ + KDTreeSingleIndexAdaptor( + const KDTreeSingleIndexAdaptor + &) = delete; + + /** + * The dataset used by this index + */ + const DatasetAdaptor &dataset; //!< The source of our data + + const KDTreeSingleIndexAdaptorParams index_params; + + Distance distance; + + typedef typename nanoflann::KDTreeBaseClass< + nanoflann::KDTreeSingleIndexAdaptor, + Distance, DatasetAdaptor, DIM, IndexType> + BaseClassRef; + + typedef typename BaseClassRef::ElementType ElementType; + typedef typename BaseClassRef::DistanceType DistanceType; + + typedef typename BaseClassRef::Node Node; + typedef Node *NodePtr; + + typedef typename BaseClassRef::Interval Interval; + /** Define "BoundingBox" as a fixed-size or variable-size container depending + * on "DIM" */ + typedef typename BaseClassRef::BoundingBox BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + typedef typename BaseClassRef::distance_vector_t distance_vector_t; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 + * for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexAdaptor(const int dimensionality, + const DatasetAdaptor &inputData, + const KDTreeSingleIndexAdaptorParams ¶ms = + KDTreeSingleIndexAdaptorParams()) + : dataset(inputData), index_params(params), distance(inputData) { + BaseClassRef::root_node = NULL; + BaseClassRef::m_size = dataset.kdtree_get_point_count(); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + BaseClassRef::dim = dimensionality; + if (DIM > 0) + BaseClassRef::dim = DIM; + BaseClassRef::m_leaf_max_size = params.leaf_max_size; + + // Create a permutable array of indices to the input vectors. + init_vind(); + } + + /** + * Builds the index + */ + void buildIndex() { + BaseClassRef::m_size = dataset.kdtree_get_point_count(); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + init_vind(); + this->freeIndex(*this); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + if (BaseClassRef::m_size == 0) + return; + computeBoundingBox(BaseClassRef::root_bbox); + BaseClassRef::root_node = + this->divideTree(*this, 0, BaseClassRef::m_size, + BaseClassRef::root_bbox); // construct the tree + } + + /** \name Query methods + * @{ */ + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * + * Params: + * result = the result object in which the indices of the + * nearest-neighbors are stored vec = the vector for which to search the + * nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + */ + template + bool findNeighbors(RESULTSET &result, const ElementType *vec, + const SearchParams &searchParams) const { + assert(vec); + if (this->size(*this) == 0) + return false; + if (!BaseClassRef::root_node) + throw std::runtime_error( + "[nanoflann] findNeighbors() called before building the index."); + float epsError = 1 + searchParams.eps; + + distance_vector_t + dists; // fixed or variable-sized container (depending on DIM) + auto zero = static_cast(0); + assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), + zero); // Fill it with zeros. + DistanceType distsq = this->computeInitialDistances(*this, vec, dists); + searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, + epsError); // "count_leaf" parameter removed since was neither + // used nor returned to the user. + return result.full(); + } + + /** + * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. + * Their indices are stored inside the result object. \sa radiusSearch, + * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility + * with the original FLANN interface. \return Number `N` of valid points in + * the result set. Only the first `N` entries in `out_indices` and + * `out_distances_sq` will be valid. Return may be less than `num_closest` + * only if the number of elements in the tree is less than `num_closest`. + */ + size_t knnSearch(const ElementType *query_point, const size_t num_closest, + IndexType *out_indices, DistanceType *out_distances_sq, + const int /* nChecks_IGNORED */ = 10) const { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + return resultSet.size(); + } + + /** + * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. + * The output is given as a vector of pairs, of which the first element is a + * point index and the second the corresponding distance. Previous contents of + * \a IndicesDists are cleared. + * + * If searchParams.sorted==true, the output list is sorted by ascending + * distances. + * + * For a better performance, it is advisable to do a .reserve() on the vector + * if you have any wild guess about the number of expected matches. + * + * \sa knnSearch, findNeighbors, radiusSearchCustomCallback + * \return The number of points within the given radius (i.e. indices.size() + * or dists.size() ) + */ + size_t + radiusSearch(const ElementType *query_point, const DistanceType &radius, + std::vector> &IndicesDists, + const SearchParams &searchParams) const { + RadiusResultSet resultSet(radius, IndicesDists); + const size_t nFound = + radiusSearchCustomCallback(query_point, resultSet, searchParams); + if (searchParams.sorted) + std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); + return nFound; + } + + /** + * Just like radiusSearch() but with a custom callback class for each point + * found in the radius of the query. See the source of RadiusResultSet<> as a + * start point for your own classes. \sa radiusSearch + */ + template + size_t radiusSearchCustomCallback( + const ElementType *query_point, SEARCH_CALLBACK &resultSet, + const SearchParams &searchParams = SearchParams()) const { + this->findNeighbors(resultSet, query_point, searchParams); + return resultSet.size(); + } + + /** @} */ + +public: + /** Make sure the auxiliary list \a vind has the same size than the current + * dataset, and re-generate if size has changed. */ + void init_vind() { + // Create a permutable array of indices to the input vectors. + BaseClassRef::m_size = dataset.kdtree_get_point_count(); + if (BaseClassRef::vind.size() != BaseClassRef::m_size) + BaseClassRef::vind.resize(BaseClassRef::m_size); + for (size_t i = 0; i < BaseClassRef::m_size; i++) + BaseClassRef::vind[i] = i; + } + + void computeBoundingBox(BoundingBox &bbox) { + resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); + if (dataset.kdtree_get_bbox(bbox)) { + // Done! It was implemented in derived class + } else { + const size_t N = dataset.kdtree_get_point_count(); + if (!N) + throw std::runtime_error("[nanoflann] computeBoundingBox() called but " + "no data points found."); + for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + bbox[i].low = bbox[i].high = this->dataset_get(*this, 0, i); + } + for (size_t k = 1; k < N; ++k) { + for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + if (this->dataset_get(*this, k, i) < bbox[i].low) + bbox[i].low = this->dataset_get(*this, k, i); + if (this->dataset_get(*this, k, i) > bbox[i].high) + bbox[i].high = this->dataset_get(*this, k, i); + } + } + } + } + + /** + * Performs an exact search in the tree starting from a node. + * \tparam RESULTSET Should be any ResultSet + * \return true if the search should be continued, false if the results are + * sufficient + */ + template + bool searchLevel(RESULTSET &result_set, const ElementType *vec, + const NodePtr node, DistanceType mindistsq, + distance_vector_t &dists, const float epsError) const { + /* If this is a leaf node, then do check and return. */ + if ((node->child1 == NULL) && (node->child2 == NULL)) { + // count_leaf += (node->lr.right-node->lr.left); // Removed since was + // neither used nor returned to the user. + DistanceType worst_dist = result_set.worstDist(); + for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; + ++i) { + const IndexType index = BaseClassRef::vind[i]; // reorder... : i; + DistanceType dist = distance.evalMetric( + vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); + if (dist < worst_dist) { + if (!result_set.addPoint(dist, BaseClassRef::vind[i])) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return false; + } + } + } + return true; + } + + /* Which child branch should be taken first? */ + int idx = node->node_type.sub.divfeat; + ElementType val = vec[idx]; + DistanceType diff1 = val - node->node_type.sub.divlow; + DistanceType diff2 = val - node->node_type.sub.divhigh; + + NodePtr bestChild; + NodePtr otherChild; + DistanceType cut_dist; + if ((diff1 + diff2) < 0) { + bestChild = node->child1; + otherChild = node->child2; + cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); + } else { + bestChild = node->child2; + otherChild = node->child1; + cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); + } + + /* Call recursively to search next level down. */ + if (!searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError)) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return false; + } + + DistanceType dst = dists[idx]; + mindistsq = mindistsq + cut_dist - dst; + dists[idx] = cut_dist; + if (mindistsq * epsError <= result_set.worstDist()) { + if (!searchLevel(result_set, vec, otherChild, mindistsq, dists, + epsError)) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return false; + } + } + dists[idx] = dst; + return true; + } + +public: + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when + * loading the index object it must be constructed associated to the same + * source of data points used while building it. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the + * index object must be constructed associated to the same source of data + * points used while building the index. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); } + +}; // class KDTree + +/** kd-tree dynamic index + * + * Contains the k-d trees and other information for indexing a set of points + * for nearest-neighbor matching. + * + * The class "DatasetAdaptor" must provide the following interface (can be + * non-virtual, inlined methods): + * + * \code + * // Must return the number of data poins + * inline size_t kdtree_get_point_count() const { ... } + * + * // Must return the dim'th component of the idx'th point in the class: + * inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... } + * + * // Optional bounding-box computation: return false to default to a standard + * bbox computation loop. + * // Return true if the BBOX was already computed by the class and returned + * in "bb" so it can be avoided to redo it again. + * // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 + * for point clouds) template bool kdtree_get_bbox(BBOX &bb) const + * { + * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits + * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits + * ... + * return true; + * } + * + * \endcode + * + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM + * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will + * be typically size_t or int + */ +template +class KDTreeSingleIndexDynamicAdaptor_ + : public KDTreeBaseClass, + Distance, DatasetAdaptor, DIM, IndexType> { +public: + /** + * The dataset used by this index + */ + const DatasetAdaptor &dataset; //!< The source of our data + + KDTreeSingleIndexAdaptorParams index_params; + + std::vector &treeIndex; + + Distance distance; + + typedef typename nanoflann::KDTreeBaseClass< + nanoflann::KDTreeSingleIndexDynamicAdaptor_, + Distance, DatasetAdaptor, DIM, IndexType> + BaseClassRef; + + typedef typename BaseClassRef::ElementType ElementType; + typedef typename BaseClassRef::DistanceType DistanceType; + + typedef typename BaseClassRef::Node Node; + typedef Node *NodePtr; + + typedef typename BaseClassRef::Interval Interval; + /** Define "BoundingBox" as a fixed-size or variable-size container depending + * on "DIM" */ + typedef typename BaseClassRef::BoundingBox BoundingBox; + + /** Define "distance_vector_t" as a fixed-size or variable-size container + * depending on "DIM" */ + typedef typename BaseClassRef::distance_vector_t distance_vector_t; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 + * for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexDynamicAdaptor_( + const int dimensionality, const DatasetAdaptor &inputData, + std::vector &treeIndex_, + const KDTreeSingleIndexAdaptorParams ¶ms = + KDTreeSingleIndexAdaptorParams()) + : dataset(inputData), index_params(params), treeIndex(treeIndex_), + distance(inputData) { + BaseClassRef::root_node = NULL; + BaseClassRef::m_size = 0; + BaseClassRef::m_size_at_index_build = 0; + BaseClassRef::dim = dimensionality; + if (DIM > 0) + BaseClassRef::dim = DIM; + BaseClassRef::m_leaf_max_size = params.leaf_max_size; + } + + /** Assignment operator definiton */ + KDTreeSingleIndexDynamicAdaptor_ + operator=(const KDTreeSingleIndexDynamicAdaptor_ &rhs) { + KDTreeSingleIndexDynamicAdaptor_ tmp(rhs); + std::swap(BaseClassRef::vind, tmp.BaseClassRef::vind); + std::swap(BaseClassRef::m_leaf_max_size, tmp.BaseClassRef::m_leaf_max_size); + std::swap(index_params, tmp.index_params); + std::swap(treeIndex, tmp.treeIndex); + std::swap(BaseClassRef::m_size, tmp.BaseClassRef::m_size); + std::swap(BaseClassRef::m_size_at_index_build, + tmp.BaseClassRef::m_size_at_index_build); + std::swap(BaseClassRef::root_node, tmp.BaseClassRef::root_node); + std::swap(BaseClassRef::root_bbox, tmp.BaseClassRef::root_bbox); + std::swap(BaseClassRef::pool, tmp.BaseClassRef::pool); + return *this; + } + + /** + * Builds the index + */ + void buildIndex() { + BaseClassRef::m_size = BaseClassRef::vind.size(); + this->freeIndex(*this); + BaseClassRef::m_size_at_index_build = BaseClassRef::m_size; + if (BaseClassRef::m_size == 0) + return; + computeBoundingBox(BaseClassRef::root_bbox); + BaseClassRef::root_node = + this->divideTree(*this, 0, BaseClassRef::m_size, + BaseClassRef::root_bbox); // construct the tree + } + + /** \name Query methods + * @{ */ + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * + * Params: + * result = the result object in which the indices of the + * nearest-neighbors are stored vec = the vector for which to search the + * nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + */ + template + bool findNeighbors(RESULTSET &result, const ElementType *vec, + const SearchParams &searchParams) const { + assert(vec); + if (this->size(*this) == 0) + return false; + if (!BaseClassRef::root_node) + return false; + float epsError = 1 + searchParams.eps; + + // fixed or variable-sized container (depending on DIM) + distance_vector_t dists; + // Fill it with zeros. + assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim), + static_cast(0)); + DistanceType distsq = this->computeInitialDistances(*this, vec, dists); + searchLevel(result, vec, BaseClassRef::root_node, distsq, dists, + epsError); // "count_leaf" parameter removed since was neither + // used nor returned to the user. + return result.full(); + } + + /** + * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1]. + * Their indices are stored inside the result object. \sa radiusSearch, + * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility + * with the original FLANN interface. \return Number `N` of valid points in + * the result set. Only the first `N` entries in `out_indices` and + * `out_distances_sq` will be valid. Return may be less than `num_closest` + * only if the number of elements in the tree is less than `num_closest`. + */ + size_t knnSearch(const ElementType *query_point, const size_t num_closest, + IndexType *out_indices, DistanceType *out_distances_sq, + const int /* nChecks_IGNORED */ = 10) const { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + this->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + return resultSet.size(); + } + + /** + * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius. + * The output is given as a vector of pairs, of which the first element is a + * point index and the second the corresponding distance. Previous contents of + * \a IndicesDists are cleared. + * + * If searchParams.sorted==true, the output list is sorted by ascending + * distances. + * + * For a better performance, it is advisable to do a .reserve() on the vector + * if you have any wild guess about the number of expected matches. + * + * \sa knnSearch, findNeighbors, radiusSearchCustomCallback + * \return The number of points within the given radius (i.e. indices.size() + * or dists.size() ) + */ + size_t + radiusSearch(const ElementType *query_point, const DistanceType &radius, + std::vector> &IndicesDists, + const SearchParams &searchParams) const { + RadiusResultSet resultSet(radius, IndicesDists); + const size_t nFound = + radiusSearchCustomCallback(query_point, resultSet, searchParams); + if (searchParams.sorted) + std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter()); + return nFound; + } + + /** + * Just like radiusSearch() but with a custom callback class for each point + * found in the radius of the query. See the source of RadiusResultSet<> as a + * start point for your own classes. \sa radiusSearch + */ + template + size_t radiusSearchCustomCallback( + const ElementType *query_point, SEARCH_CALLBACK &resultSet, + const SearchParams &searchParams = SearchParams()) const { + this->findNeighbors(resultSet, query_point, searchParams); + return resultSet.size(); + } + + /** @} */ + +public: + void computeBoundingBox(BoundingBox &bbox) { + resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim)); + + if (dataset.kdtree_get_bbox(bbox)) { + // Done! It was implemented in derived class + } else { + const size_t N = BaseClassRef::m_size; + if (!N) + throw std::runtime_error("[nanoflann] computeBoundingBox() called but " + "no data points found."); + for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + bbox[i].low = bbox[i].high = + this->dataset_get(*this, BaseClassRef::vind[0], i); + } + for (size_t k = 1; k < N; ++k) { + for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) { + if (this->dataset_get(*this, BaseClassRef::vind[k], i) < bbox[i].low) + bbox[i].low = this->dataset_get(*this, BaseClassRef::vind[k], i); + if (this->dataset_get(*this, BaseClassRef::vind[k], i) > bbox[i].high) + bbox[i].high = this->dataset_get(*this, BaseClassRef::vind[k], i); + } + } + } + } + + /** + * Performs an exact search in the tree starting from a node. + * \tparam RESULTSET Should be any ResultSet + */ + template + void searchLevel(RESULTSET &result_set, const ElementType *vec, + const NodePtr node, DistanceType mindistsq, + distance_vector_t &dists, const float epsError) const { + /* If this is a leaf node, then do check and return. */ + if ((node->child1 == NULL) && (node->child2 == NULL)) { + // count_leaf += (node->lr.right-node->lr.left); // Removed since was + // neither used nor returned to the user. + DistanceType worst_dist = result_set.worstDist(); + for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right; + ++i) { + const IndexType index = BaseClassRef::vind[i]; // reorder... : i; + if (treeIndex[index] == -1) + continue; + DistanceType dist = distance.evalMetric( + vec, index, (DIM > 0 ? DIM : BaseClassRef::dim)); + if (dist < worst_dist) { + if (!result_set.addPoint( + static_cast(dist), + static_cast( + BaseClassRef::vind[i]))) { + // the resultset doesn't want to receive any more points, we're done + // searching! + return; // false; + } + } + } + return; + } + + /* Which child branch should be taken first? */ + int idx = node->node_type.sub.divfeat; + ElementType val = vec[idx]; + DistanceType diff1 = val - node->node_type.sub.divlow; + DistanceType diff2 = val - node->node_type.sub.divhigh; + + NodePtr bestChild; + NodePtr otherChild; + DistanceType cut_dist; + if ((diff1 + diff2) < 0) { + bestChild = node->child1; + otherChild = node->child2; + cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx); + } else { + bestChild = node->child2; + otherChild = node->child1; + cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx); + } + + /* Call recursively to search next level down. */ + searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError); + + DistanceType dst = dists[idx]; + mindistsq = mindistsq + cut_dist - dst; + dists[idx] = cut_dist; + if (mindistsq * epsError <= result_set.worstDist()) { + searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError); + } + dists[idx] = dst; + } + +public: + /** Stores the index in a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so when + * loading the index object it must be constructed associated to the same + * source of data points used while building it. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); } + + /** Loads a previous index from a binary file. + * IMPORTANT NOTE: The set of data points is NOT stored in the file, so the + * index object must be constructed associated to the same source of data + * points used while building the index. See the example: + * examples/saveload_example.cpp \sa loadIndex */ + void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); } +}; + +/** kd-tree dynaimic index + * + * class to create multiple static index and merge their results to behave as + * single dynamic index as proposed in Logarithmic Approach. + * + * Example of usage: + * examples/dynamic_pointcloud_example.cpp + * + * \tparam DatasetAdaptor The user-provided adaptor (see comments above). + * \tparam Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM + * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will + * be typically size_t or int + */ +template +class KDTreeSingleIndexDynamicAdaptor { +public: + typedef typename Distance::ElementType ElementType; + typedef typename Distance::DistanceType DistanceType; + +protected: + size_t m_leaf_max_size; + size_t treeCount; + size_t pointCount; + + /** + * The dataset used by this index + */ + const DatasetAdaptor &dataset; //!< The source of our data + + std::vector treeIndex; //!< treeIndex[idx] is the index of tree in which + //!< point at idx is stored. treeIndex[idx]=-1 + //!< means that point has been removed. + + KDTreeSingleIndexAdaptorParams index_params; + + int dim; //!< Dimensionality of each data point + + typedef KDTreeSingleIndexDynamicAdaptor_ + index_container_t; + std::vector index; + +public: + /** Get a const ref to the internal list of indices; the number of indices is + * adapted dynamically as the dataset grows in size. */ + const std::vector &getAllIndices() const { return index; } + +private: + /** finds position of least significant unset bit */ + int First0Bit(IndexType num) { + int pos = 0; + while (num & 1) { + num = num >> 1; + pos++; + } + return pos; + } + + /** Creates multiple empty trees to handle dynamic support */ + void init() { + typedef KDTreeSingleIndexDynamicAdaptor_ + my_kd_tree_t; + std::vector index_( + treeCount, my_kd_tree_t(dim /*dim*/, dataset, treeIndex, index_params)); + index = index_; + } + +public: + Distance distance; + + /** + * KDTree constructor + * + * Refer to docs in README.md or online in + * https://github.com/jlblancoc/nanoflann + * + * The KD-Tree point dimension (the length of each point in the datase, e.g. 3 + * for 3D points) is determined by means of: + * - The \a DIM template parameter if >0 (highest priority) + * - Otherwise, the \a dimensionality parameter of this constructor. + * + * @param inputData Dataset with the input features + * @param params Basically, the maximum leaf node size + */ + KDTreeSingleIndexDynamicAdaptor(const int dimensionality, + const DatasetAdaptor &inputData, + const KDTreeSingleIndexAdaptorParams ¶ms = + KDTreeSingleIndexAdaptorParams(), + const size_t maximumPointCount = 1000000000U) + : dataset(inputData), index_params(params), distance(inputData) { + treeCount = static_cast(std::log2(maximumPointCount)); + pointCount = 0U; + dim = dimensionality; + treeIndex.clear(); + if (DIM > 0) + dim = DIM; + m_leaf_max_size = params.leaf_max_size; + init(); + const size_t num_initial_points = dataset.kdtree_get_point_count(); + if (num_initial_points > 0) { + addPoints(0, num_initial_points - 1); + } + } + + /** Deleted copy constructor*/ + KDTreeSingleIndexDynamicAdaptor( + const KDTreeSingleIndexDynamicAdaptor &) = delete; + + /** Add points to the set, Inserts all points from [start, end] */ + void addPoints(IndexType start, IndexType end) { + size_t count = end - start + 1; + treeIndex.resize(treeIndex.size() + count); + for (IndexType idx = start; idx <= end; idx++) { + int pos = First0Bit(pointCount); + index[pos].vind.clear(); + treeIndex[pointCount] = pos; + for (int i = 0; i < pos; i++) { + for (int j = 0; j < static_cast(index[i].vind.size()); j++) { + index[pos].vind.push_back(index[i].vind[j]); + if (treeIndex[index[i].vind[j]] != -1) + treeIndex[index[i].vind[j]] = pos; + } + index[i].vind.clear(); + index[i].freeIndex(index[i]); + } + index[pos].vind.push_back(idx); + index[pos].buildIndex(); + pointCount++; + } + } + + /** Remove a point from the set (Lazy Deletion) */ + void removePoint(size_t idx) { + if (idx >= pointCount) + return; + treeIndex[idx] = -1; + } + + /** + * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored + * inside the result object. + * + * Params: + * result = the result object in which the indices of the + * nearest-neighbors are stored vec = the vector for which to search the + * nearest neighbors + * + * \tparam RESULTSET Should be any ResultSet + * \return True if the requested neighbors could be found. + * \sa knnSearch, radiusSearch + */ + template + bool findNeighbors(RESULTSET &result, const ElementType *vec, + const SearchParams &searchParams) const { + for (size_t i = 0; i < treeCount; i++) { + index[i].findNeighbors(result, &vec[0], searchParams); + } + return result.full(); + } +}; + +/** An L2-metric KD-tree adaptor for working with data directly stored in an + * Eigen Matrix, without duplicating the data storage. You can select whether a + * row or column in the matrix represents a point in the state space. + * + * Example of usage: + * \code + * Eigen::Matrix mat; + * // Fill out "mat"... + * + * typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix > + * my_kd_tree_t; const int max_leaf = 10; my_kd_tree_t mat_index(mat, max_leaf + * ); mat_index.index->buildIndex(); mat_index.index->... \endcode + * + * \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality + * for the points in the data set, allowing more compiler optimizations. \tparam + * Distance The distance metric to use: nanoflann::metric_L1, + * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam row_major + * If set to true the rows of the matrix are used as the points, if set to false + * the columns of the matrix are used as the points. + */ +template +struct KDTreeEigenMatrixAdaptor { + typedef KDTreeEigenMatrixAdaptor self_t; + typedef typename MatrixType::Scalar num_t; + typedef typename MatrixType::Index IndexType; + typedef + typename Distance::template traits::distance_t metric_t; + typedef KDTreeSingleIndexAdaptor + index_t; + + index_t *index; //! The kd-tree index for the user to call its methods as + //! usual with any other FLANN index. + + /// Constructor: takes a const ref to the matrix object with the data points + KDTreeEigenMatrixAdaptor(const size_t dimensionality, + const std::reference_wrapper &mat, + const int leaf_max_size = 10) + : m_data_matrix(mat) { + const auto dims = row_major ? mat.get().cols() : mat.get().rows(); + if (size_t(dims) != dimensionality) + throw std::runtime_error( + "Error: 'dimensionality' must match column count in data matrix"); + if (DIM > 0 && int(dims) != DIM) + throw std::runtime_error( + "Data set dimensionality does not match the 'DIM' template argument"); + index = + new index_t(static_cast(dims), *this /* adaptor */, + nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size)); + index->buildIndex(); + } + +public: + /** Deleted copy constructor */ + KDTreeEigenMatrixAdaptor(const self_t &) = delete; + + ~KDTreeEigenMatrixAdaptor() { delete index; } + + const std::reference_wrapper m_data_matrix; + + /** Query for the \a num_closest closest points to a given point (entered as + * query_point[0:dim-1]). Note that this is a short-cut method for + * index->findNeighbors(). The user can also call index->... methods as + * desired. \note nChecks_IGNORED is ignored but kept for compatibility with + * the original FLANN interface. + */ + inline void query(const num_t *query_point, const size_t num_closest, + IndexType *out_indices, num_t *out_distances_sq, + const int /* nChecks_IGNORED */ = 10) const { + nanoflann::KNNResultSet resultSet(num_closest); + resultSet.init(out_indices, out_distances_sq); + index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); + } + + /** @name Interface expected by KDTreeSingleIndexAdaptor + * @{ */ + + const self_t &derived() const { return *this; } + self_t &derived() { return *this; } + + // Must return the number of data points + inline size_t kdtree_get_point_count() const { + if(row_major) + return m_data_matrix.get().rows(); + else + return m_data_matrix.get().cols(); + } + + // Returns the dim'th component of the idx'th point in the class: + inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const { + if(row_major) + return m_data_matrix.get().coeff(idx, IndexType(dim)); + else + return m_data_matrix.get().coeff(IndexType(dim), idx); + } + + // Optional bounding-box computation: return false to default to a standard + // bbox computation loop. + // Return true if the BBOX was already computed by the class and returned in + // "bb" so it can be avoided to redo it again. Look at bb.size() to find out + // the expected dimensionality (e.g. 2 or 3 for point clouds) + template bool kdtree_get_bbox(BBOX & /*bb*/) const { + return false; + } + + /** @} */ + +}; // end of KDTreeEigenMatrixAdaptor + /** @} */ + +/** @} */ // end of grouping +} // namespace nanoflann + +#endif /* NANOFLANN_HPP_ */ diff --git a/include/small_gicp/ann/traits.hpp b/include/small_gicp/ann/traits.hpp new file mode 100644 index 0000000..04c5fd3 --- /dev/null +++ b/include/small_gicp/ann/traits.hpp @@ -0,0 +1,19 @@ +#pragma once + +#include + +namespace small_gicp { + +namespace traits { + +template +struct Traits; + +template +size_t knn_search(const T& tree, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) { + return Traits::knn_search(tree, point, k, k_indices, k_sq_dists); +} + +} // namespace traits + +} // namespace small_gicp diff --git a/include/small_gicp/factors/gicp_factor.hpp b/include/small_gicp/factors/gicp_factor.hpp new file mode 100644 index 0000000..4897f0a --- /dev/null +++ b/include/small_gicp/factors/gicp_factor.hpp @@ -0,0 +1,72 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace small_gicp { + +struct GICPFactor { + GICPFactor() : target_index(std::numeric_limits::max()), source_index(std::numeric_limits::max()), mahalanobis(Eigen::Matrix4d::Zero()) {} + + template + bool linearize( + const TargetPointCloud& target, + const SourcePointCloud& source, + const TargetTree& target_tree, + const Eigen::Isometry3d& T, + size_t source_index, + const CorrespondenceRejector& rejector, + Eigen::Matrix* H, + Eigen::Matrix* b, + double* e) { + // + this->source_index = source_index; + this->target_index = std::numeric_limits::max(); + + const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index); + + size_t k_index; + double k_sq_dist; + if (!traits::knn_search(target_tree, transed_source_pt, 1, &k_index, &k_sq_dist) || rejector(T, k_index, source_index, k_sq_dist)) { + return false; + } + + target_index = k_index; + + const Eigen::Matrix4d RCR = traits::cov(target, target_index) + T.matrix() * traits::cov(source, source_index) * T.matrix().transpose(); + mahalanobis.block<3, 3>(0, 0) = RCR.block<3, 3>(0, 0).inverse(); + + const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt; + + Eigen::Matrix J = Eigen::Matrix::Zero(); + J.block<3, 3>(0, 0) = T.linear() * skew(traits::point(source, source_index).template head<3>()); + J.block<3, 3>(0, 3) = -T.linear(); + + *H = J.transpose() * mahalanobis * J; + *b = J.transpose() * mahalanobis * residual; + *e = 0.5 * residual.dot(mahalanobis * residual); + + return true; + } + + template + double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T) const { + if (target_index == std::numeric_limits::max()) { + return 0.0; + } + + const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index); + const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt; + return 0.5 * residual.dot(mahalanobis * residual); + } + + bool inlier() const { return target_index != std::numeric_limits::max(); } + + size_t target_index; + size_t source_index; + Eigen::Matrix4d mahalanobis; +}; +} // namespace small_gicp diff --git a/include/small_gicp/factors/icp_factor.hpp b/include/small_gicp/factors/icp_factor.hpp new file mode 100644 index 0000000..fcc42b8 --- /dev/null +++ b/include/small_gicp/factors/icp_factor.hpp @@ -0,0 +1,66 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace small_gicp { + +struct ICPFactor { + ICPFactor() : target_index(std::numeric_limits::max()), source_index(std::numeric_limits::max()) {} + + template + bool linearize( + const TargetPointCloud& target, + const SourcePointCloud& source, + const TargetTree& target_tree, + const Eigen::Isometry3d& T, + size_t source_index, + const CorrespondenceRejector& rejector, + Eigen::Matrix* H, + Eigen::Matrix* b, + double* e) { + // + this->source_index = source_index; + this->target_index = std::numeric_limits::max(); + + const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index); + + size_t k_index; + double k_sq_dist; + if (!traits::knn_search(target_tree, transed_source_pt, 1, &k_index, &k_sq_dist) || rejector(T, k_index, source_index, k_sq_dist)) { + return false; + } + + target_index = k_index; + const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt; + + Eigen::Matrix J = Eigen::Matrix::Zero(); + J.block<3, 3>(0, 0) = T.linear() * skew(traits::point(source, source_index).template head<3>()); + J.block<3, 3>(0, 3) = -T.linear(); + + *H = J.transpose() * J; + *b = J.transpose() * residual; + *e = 0.5 * residual.squaredNorm(); + + return true; + } + + template + double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T) const { + if (target_index == std::numeric_limits::max()) { + return 0.0; + } + + const Eigen::Vector4d residual = traits::point(target, target_index) - T * traits::point(source, source_index); + return 0.5 * residual.squaredNorm(); + } + + bool inlier() const { return target_index != std::numeric_limits::max(); } + + size_t target_index; + size_t source_index; +}; +} // namespace small_gicp diff --git a/include/small_gicp/factors/plane_icp_factor.hpp b/include/small_gicp/factors/plane_icp_factor.hpp new file mode 100644 index 0000000..28ff8c8 --- /dev/null +++ b/include/small_gicp/factors/plane_icp_factor.hpp @@ -0,0 +1,71 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace small_gicp { + +struct PointToPlaneICPFactor { + PointToPlaneICPFactor() : target_index(std::numeric_limits::max()), source_index(std::numeric_limits::max()) {} + + template + bool linearize( + const TargetPointCloud& target, + const SourcePointCloud& source, + const TargetTree& target_tree, + const Eigen::Isometry3d& T, + size_t source_index, + const CorrespondenceRejector& rejector, + Eigen::Matrix* H, + Eigen::Matrix* b, + double* e) { + // + this->source_index = source_index; + this->target_index = std::numeric_limits::max(); + + const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index); + + size_t k_index; + double k_sq_dist; + if (!traits::knn_search(target_tree, transed_source_pt, 1, &k_index, &k_sq_dist) || rejector(T, k_index, source_index, k_sq_dist)) { + return false; + } + + target_index = k_index; + const auto& target_normal = traits::normal(target, target_index); + + const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt; + const Eigen::Vector4d error = target_normal.array() * residual.array(); + + Eigen::Matrix J = Eigen::Matrix::Zero(); + J.block<3, 3>(0, 0) = target_normal.template head<3>().asDiagonal() * T.linear() * skew(traits::point(source, source_index).template head<3>()); + J.block<3, 3>(0, 3) = target_normal.template head<3>().asDiagonal() * (-T.linear()); + + *H = J.transpose() * J; + *b = J.transpose() * error; + *e = 0.5 * error.squaredNorm(); + + return true; + } + + template + double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T) const { + if (target_index == std::numeric_limits::max()) { + return 0.0; + } + + const Eigen::Vector4d transed_source_pt = T * traits::point(source, source_index); + const Eigen::Vector4d residual = traits::point(target, target_index) - transed_source_pt; + const Eigen::Vector4d error = traits::normal(target, target_index).array() * residual.array(); + return 0.5 * error.squaredNorm(); + } + + bool inlier() const { return target_index != std::numeric_limits::max(); } + + size_t target_index; + size_t source_index; +}; +} // namespace small_gicp diff --git a/include/small_gicp/points/gaussian_voxelmap.hpp b/include/small_gicp/points/gaussian_voxelmap.hpp new file mode 100644 index 0000000..61e0258 --- /dev/null +++ b/include/small_gicp/points/gaussian_voxelmap.hpp @@ -0,0 +1,135 @@ +#pragma once + +#include + +#include +#include +#include + +namespace small_gicp { + +struct GaussianVoxel { +public: + GaussianVoxel(const Eigen::Vector3i& coord) : finalized(false), lru(0), num_points(0), coord(coord), mean(Eigen::Vector4d::Zero()), cov(Eigen::Matrix4d::Zero()) {} + ~GaussianVoxel() {} + + void add(const Eigen::Vector4d& mean, const Eigen::Matrix4d& cov, size_t lru) { + if (finalized) { + this->finalized = false; + this->mean *= num_points; + this->cov *= num_points; + } + + num_points++; + this->mean += mean; + this->cov += cov; + this->lru = lru; + } + + void finalize() { + if (finalized) { + return; + } + + mean /= num_points; + cov /= num_points; + finalized = true; + } + +public: + bool finalized; + size_t lru; + size_t num_points; + Eigen::Vector3i coord; + + Eigen::Vector4d mean; + Eigen::Matrix4d cov; +}; + +struct GaussianVoxelMap { +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + GaussianVoxelMap(double leaf_size) : inv_leaf_size(1.0 / leaf_size), lru_horizon(100), lru_clear_cycle(10), lru_counter(0) {} + ~GaussianVoxelMap() {} + + size_t size() const { return flat_voxels.size(); } + + template + void insert(const PointCloud& points, const Eigen::Isometry3d& T = Eigen::Isometry3d::Identity()) { + for (size_t i = 0; i < traits::size(points); i++) { + const Eigen::Vector4d pt = T * traits::point(points, i); + const Eigen::Vector3i coord = (pt * inv_leaf_size).array().floor().cast().head<3>(); + + auto found = voxels.find(coord); + if (found == voxels.end()) { + found = voxels.emplace_hint(found, coord, flat_voxels.size()); + flat_voxels.emplace_back(coord); + } + + auto& voxel = flat_voxels[found->second]; + const Eigen::Matrix4d cov = T.matrix() * traits::cov(points, i) * T.matrix().transpose(); + voxel.add(pt, cov, lru_counter); + } + + if ((lru_counter++) % lru_clear_cycle == 0) { + std::erase_if(flat_voxels, [&](const GaussianVoxel& voxel) { return voxel.lru + lru_horizon < lru_counter; }); + voxels.clear(); + for (size_t i = 0; i < flat_voxels.size(); i++) { + voxels[flat_voxels[i].coord] = i; + } + } + + for (auto& voxel : flat_voxels) { + voxel.finalize(); + } + } + + size_t knn_search(const Eigen::Vector4d& pt, size_t k, size_t* k_indices, double* k_sq_dists) const { + if (k != 1) { + std::cerr << "warning:!!" << std::endl; + } + + const Eigen::Vector3i coord = (pt * inv_leaf_size).array().floor().cast().head<3>(); + const auto found = voxels.find(coord); + if (found == voxels.end()) { + return 0; + } + + const GaussianVoxel& voxel = flat_voxels[found->second]; + k_indices[0] = found->second; + k_sq_dists[0] = (voxel.mean - pt).squaredNorm(); + return 1; + } + +public: + const double inv_leaf_size; + size_t lru_horizon; + size_t lru_clear_cycle; + size_t lru_counter; + + std::vector flat_voxels; + std::unordered_map voxels; +}; + +namespace traits { + +template <> +struct Traits { + static size_t size(const GaussianVoxelMap& voxelmap) { return voxelmap.size(); } + + static bool has_points(const GaussianVoxelMap& voxelmap) { return !voxelmap.flat_voxels.empty(); } + static bool has_covs(const GaussianVoxelMap& voxelmap) { return !voxelmap.flat_voxels.empty(); } + + static const Eigen::Vector4d& point(const GaussianVoxelMap& voxelmap, size_t i) { return voxelmap.flat_voxels[i].mean; } + static const Eigen::Matrix4d& cov(const GaussianVoxelMap& voxelmap, size_t i) { return voxelmap.flat_voxels[i].cov; } + + static size_t knn_search(const GaussianVoxelMap& voxelmap, const Eigen::Vector4d& point, size_t k, size_t* k_indices, double* k_sq_dists) { + return voxelmap.knn_search(point, k, k_indices, k_sq_dists); + } +}; + +} // namespace traits + +} // namespace small_gicp diff --git a/include/small_gicp/points/pcl_point.hpp b/include/small_gicp/points/pcl_point.hpp new file mode 100644 index 0000000..08545d0 --- /dev/null +++ b/include/small_gicp/points/pcl_point.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include + +namespace pcl { + +using Matrix4fMap = Eigen::Map; +using Matrix4fMapConst = const Eigen::Map; + +struct EIGEN_ALIGN16 PointCovariance { + PCL_ADD_POINT4D; + Eigen::Matrix4f cov; + + Matrix4fMap getCovariance4fMap() { return Matrix4fMap(cov.data()); } + Matrix4fMapConst getCovariance4fMap() const { return Matrix4fMapConst(cov.data()); } + + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct EIGEN_ALIGN16 PointNormalCovariance { + PCL_ADD_POINT4D; + PCL_ADD_NORMAL4D + Eigen::Matrix4f cov; + + Matrix4fMap getCovariance4fMap() { return Matrix4fMap(cov.data()); } + Matrix4fMapConst getCovariance4fMap() const { return Matrix4fMapConst(cov.data()); } + + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace pcl diff --git a/include/small_gicp/points/pcl_point_traits.hpp b/include/small_gicp/points/pcl_point_traits.hpp new file mode 100644 index 0000000..4e76c6b --- /dev/null +++ b/include/small_gicp/points/pcl_point_traits.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include +#include + +#include + +namespace small_gicp { + +namespace traits { + +template +struct Traits> { + using Points = pcl::PointCloud; + + static size_t size(const Points& points) { return points.size(); } + static void resize(Points& points, size_t n) { points.resize(n); } + + static bool has_points(const Points& points) { return !points.empty(); } + static bool has_normals(const Points& points) { return !points.empty(); } + static bool has_covs(const Points& points) { return !points.empty(); } + + static void set_point(Points& points, size_t i, const Eigen::Vector4d& pt) { points.at(i).getVector4fMap() = pt.template cast(); } + static void set_normal(Points& points, size_t i, const Eigen::Vector4d& pt) { points.at(i).getNormalVector4fMap() = pt.template cast(); } + static void set_cov(Points& points, size_t i, const Eigen::Matrix4d& cov) { points.at(i).getCovariance4fMap() = cov.template cast(); } + + static Eigen::Vector4d point(const Points& points, size_t i) { return points.at(i).getVector4fMap().template cast(); } + static Eigen::Vector4d normal(const Points& points, size_t i) { return points.at(i).getNormalVector4fMap().template cast(); } + static Eigen::Matrix4d cov(const Points& points, size_t i) { return points.at(i).getCovariance4fMap().template cast(); } +}; + +} // namespace traits + +} // namespace small_gicp diff --git a/include/small_gicp/points/point_cloud.hpp b/include/small_gicp/points/point_cloud.hpp new file mode 100644 index 0000000..3ee98fb --- /dev/null +++ b/include/small_gicp/points/point_cloud.hpp @@ -0,0 +1,69 @@ +#pragma once + +#include +#include + +namespace small_gicp { + +struct PointCloud { +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + PointCloud() {} + ~PointCloud() {} + + template + PointCloud(const std::vector, Allocator>& points) { + this->resize(points.size()); + for (size_t i = 0; i < points.size(); i++) { + this->point(i) << points[i].template cast().template head<3>(), 1.0; + } + } + + size_t size() const { return points.size(); } + + void resize(size_t n) { + points.resize(n); + normals.resize(n); + covs.resize(n); + } + + Eigen::Vector4d& point(size_t i) { return points[i]; } + Eigen::Vector4d& normal(size_t i) { return normals[i]; } + Eigen::Matrix4d& cov(size_t i) { return covs[i]; } + const Eigen::Vector4d& point(size_t i) const { return points[i]; } + const Eigen::Vector4d& normal(size_t i) const { return normals[i]; } + const Eigen::Matrix4d& cov(size_t i) const { return covs[i]; } + +public: + std::vector points; + std::vector normals; + std::vector covs; +}; + +namespace traits { + +template <> +struct Traits { + using Points = PointCloud; + + static size_t size(const Points& points) { return points.size(); } + + static bool has_points(const Points& points) { return !points.points.empty(); } + static bool has_normals(const Points& points) { return !points.normals.empty(); } + static bool has_covs(const Points& points) { return !points.covs.empty(); } + + static const Eigen::Vector4d& point(const Points& points, size_t i) { return points.point(i); } + static const Eigen::Vector4d& normal(const Points& points, size_t i) { return points.normal(i); } + static const Eigen::Matrix4d& cov(const Points& points, size_t i) { return points.cov(i); } + + static void resize(Points& points, size_t n) { points.resize(n); } + static void set_point(Points& points, size_t i, const Eigen::Vector4d& pt) { points.point(i) = pt; } + static void set_normal(Points& points, size_t i, const Eigen::Vector4d& n) { points.normal(i) = n; } + static void set_cov(Points& points, size_t i, const Eigen::Matrix4d& cov) { points.cov(i) = cov; } +}; + +} // namespace traits + +} // namespace small_gicp diff --git a/include/small_gicp/points/traits.hpp b/include/small_gicp/points/traits.hpp new file mode 100644 index 0000000..70b0d5b --- /dev/null +++ b/include/small_gicp/points/traits.hpp @@ -0,0 +1,68 @@ +#pragma once + +#include + +namespace small_gicp { + +namespace traits { + +template +struct Traits; + +template +size_t size(const T& points) { + return Traits::size(points); +} + +template +bool has_points(const T& points) { + return Traits::has_points(points); +} + +template +bool has_normals(const T& points) { + return Traits::has_normals(points); +} + +template +bool has_covs(const T& points) { + return Traits::has_covs(points); +} + +template +auto point(const T& points, size_t i) { + return Traits::point(points, i); +} + +template +auto normal(const T& points, size_t i) { + return Traits::normal(points, i); +} + +template +auto cov(const T& points, size_t i) { + return Traits::cov(points, i); +} + +template +void resize(T& points, size_t n) { + Traits::resize(points, n); +} + +template +void set_point(T& points, size_t i, const Eigen::Vector4d& pt) { + Traits::set_point(points, i, pt); +} + +template +void set_normal(T& points, size_t i, const Eigen::Vector4d& pt) { + Traits::set_normal(points, i, pt); +} + +template +void set_cov(T& points, size_t i, const Eigen::Matrix4d& cov) { + Traits::set_cov(points, i, cov); +} + +} // namespace traits +} // namespace small_gicp diff --git a/include/small_gicp/registration/optimizer.hpp b/include/small_gicp/registration/optimizer.hpp new file mode 100644 index 0000000..ab01f52 --- /dev/null +++ b/include/small_gicp/registration/optimizer.hpp @@ -0,0 +1,131 @@ +#pragma once + +#include +#include + +namespace small_gicp { + +struct GaussNewtonOptimizer { + GaussNewtonOptimizer() : verbose(false), max_iterations(20), lambda(1e-6) {} + + template < + typename TargetPointCloud, + typename SourcePointCloud, + typename TargetTree, + typename CorrespondenceRejector, + typename TerminationCriteria, + typename Reduction, + typename Factor> + RegistrationResult optimize( + const TargetPointCloud& target, + const SourcePointCloud& source, + const TargetTree& target_tree, + const CorrespondenceRejector& rejector, + const TerminationCriteria& criteria, + Reduction& reduction, + const Eigen::Isometry3d& init_T, + std::vector& factors) { + // + if (verbose) { + std::cout << "--- GN optimization ---" << std::endl; + } + + RegistrationResult result(init_T); + for (int i = 0; i < max_iterations && !result.converged; i++) { + const auto [H, b, e] = reduction.linearize(target, source, target_tree, rejector, result.T_target_source, factors); + const Eigen::Matrix delta = (H + lambda * Eigen ::Matrix::Identity()).ldlt().solve(-b); + + if (verbose) { + std::cout << "iter=" << i << " e=" << e << " lambda=" << lambda << " dt=" << delta.tail<3>().norm() << " dr=" << delta.head<3>().norm() << std::endl; + } + + result.converged = criteria.converged(delta); + result.T_target_source = result.T_target_source * se3_exp(delta); + result.iterations = i; + result.H = H; + result.b = b; + result.error = e; + } + + result.num_inliers = std::ranges::count_if(factors, [](const auto& factor) { return factor.inlier(); }); + + return result; + } + + bool verbose; + int max_iterations; + double lambda; +}; + +struct LevenbergMarquardtOptimizer { + LevenbergMarquardtOptimizer() : verbose(false), max_iterations(20), max_inner_iterations(10), init_lambda(1e-3), lambda_factor(10.0) {} + + template < + typename TargetPointCloud, + typename SourcePointCloud, + typename TargetTree, + typename CorrespondenceRejector, + typename TerminationCriteria, + typename Reduction, + typename Factor> + RegistrationResult optimize( + const TargetPointCloud& target, + const SourcePointCloud& source, + const TargetTree& target_tree, + const CorrespondenceRejector& rejector, + const TerminationCriteria& criteria, + Reduction& reduction, + const Eigen::Isometry3d& init_T, + std::vector& factors) { + // + if (verbose) { + std::cout << "--- LM optimization ---" << std::endl; + } + + double lambda = init_lambda; + RegistrationResult result(init_T); + for (int i = 0; i < max_iterations && !result.converged; i++) { + const auto [H, b, e] = reduction.linearize(target, source, target_tree, rejector, result.T_target_source, factors); + + bool success = false; + for (int j = 0; j < max_inner_iterations; j++) { + const Eigen::Matrix delta = (H + lambda * Eigen ::Matrix::Identity()).ldlt().solve(-b); + const Eigen::Isometry3d new_T = result.T_target_source * se3_exp(delta); + const double new_e = reduction.error(target, source, new_T, factors); + + if (verbose) { + std::cout << "iter=" << i << " inner=" << j << " e=" << e << " new_e=" << new_e << " lambda=" << lambda << " dt=" << delta.tail<3>().norm() + << " dr=" << delta.head<3>().norm() << std::endl; + } + + if (new_e < e) { + result.converged = criteria.converged(delta); + result.T_target_source = new_T; + lambda /= lambda_factor; + success = true; + + break; + } else { + lambda *= lambda_factor; + } + } + + result.iterations = i; + result.H = H; + result.b = b; + result.error = e; + } + + result.num_inliers = std::ranges::count_if(factors, [](const auto& factor) { return factor.inlier(); }); + + return result; + } + + bool verbose; + int max_iterations; + int max_inner_iterations; + double init_lambda; + double lambda_factor; +}; + +} // namespace small_gicp diff --git a/include/small_gicp/registration/reduction.hpp b/include/small_gicp/registration/reduction.hpp new file mode 100644 index 0000000..ff18231 --- /dev/null +++ b/include/small_gicp/registration/reduction.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include + +namespace small_gicp { + +struct SerialReduction { + template + std::tuple, Eigen::Matrix, double> linearize( + const TargetPointCloud& target, + const SourcePointCloud& source, + const TargetTree& target_tree, + const CorrespondenceRejector& rejector, + const Eigen::Isometry3d& T, + std::vector& factors) { + Eigen::Matrix sum_H = Eigen::Matrix::Zero(); + Eigen::Matrix sum_b = Eigen::Matrix::Zero(); + double sum_e = 0.0; + + for (size_t i = 0; i < factors.size(); i++) { + Eigen::Matrix H; + Eigen::Matrix b; + double e; + + if (!factors[i].linearize(target, source, target_tree, T, i, rejector, &H, &b, &e)) { + continue; + } + + sum_H += H; + sum_b += b; + sum_e += e; + } + + return {sum_H, sum_b, sum_e}; + } + + template + double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector& factors) { + double sum_e = 0.0; + for (size_t i = 0; i < factors.size(); i++) { + sum_e += factors[i].error(target, source, T); + } + return sum_e; + } +}; + +} // namespace small_gicp diff --git a/include/small_gicp/registration/reduction_omp.hpp b/include/small_gicp/registration/reduction_omp.hpp new file mode 100644 index 0000000..23d013f --- /dev/null +++ b/include/small_gicp/registration/reduction_omp.hpp @@ -0,0 +1,61 @@ +#pragma once + +#include + +namespace small_gicp { + +struct ParallelReductionOMP { + ParallelReductionOMP() : num_threads(8) {} + + template + std::tuple, Eigen::Matrix, double> linearize( + const TargetPointCloud& target, + const SourcePointCloud& source, + const TargetTree& target_tree, + const CorrespondenceRejector& rejector, + const Eigen::Isometry3d& T, + std::vector& factors) { + std::vector> Hs(num_threads, Eigen::Matrix::Zero()); + std::vector> bs(num_threads, Eigen::Matrix::Zero()); + std::vector es(num_threads, 0.0); + +#pragma omp parallel for num_threads(num_threads) schedule(guided, 8) + for (size_t i = 0; i < factors.size(); i++) { + Eigen::Matrix H; + Eigen::Matrix b; + double e; + + if (!factors[i].linearize(target, source, target_tree, T, i, rejector, &H, &b, &e)) { + continue; + } + + const int thread_id = omp_get_thread_num(); + Hs[thread_id] += H; + bs[thread_id] += b; + es[thread_id] += e; + } + + for (int i = 1; i < num_threads; i++) { + Hs[0] += Hs[i]; + bs[0] += bs[i]; + es[0] += es[i]; + } + + return {Hs[0], bs[0], es[0]}; + } + + template + double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector& factors) { + double sum_e = 0.0; + +#pragma omp parallel for num_threads(num_threads) schedule(guided, 8) reduction(+ : sum_e) + for (size_t i = 0; i < factors.size(); i++) { + sum_e += factors[i].error(target, source, T); + } + return sum_e; + } + + int num_threads; +}; + +} // namespace small_gicp diff --git a/include/small_gicp/registration/reduction_tbb.hpp b/include/small_gicp/registration/reduction_tbb.hpp new file mode 100644 index 0000000..93c7a02 --- /dev/null +++ b/include/small_gicp/registration/reduction_tbb.hpp @@ -0,0 +1,136 @@ +#pragma once + +#include +#include + +namespace small_gicp { + +template +struct LinearizeSum { + LinearizeSum( + const TargetPointCloud& target, + const SourcePointCloud& source, + const TargetTree& target_tree, + const CorrespondenceRejector& rejector, + const Eigen::Isometry3d& T, + std::vector& factors) + : target(target), + source(source), + target_tree(target_tree), + rejector(rejector), + T(T), + factors(factors), + H(Eigen::Matrix::Zero()), + b(Eigen::Matrix::Zero()), + e(0.0) {} + + LinearizeSum(LinearizeSum& x, tbb::split) + : target(x.target), + source(x.source), + target_tree(x.target_tree), + rejector(x.rejector), + T(x.T), + factors(x.factors), + H(Eigen::Matrix::Zero()), + b(Eigen::Matrix::Zero()), + e(0.0) {} + + void operator()(const tbb::blocked_range& r) { + Eigen::Matrix Ht = H; + Eigen::Matrix bt = b; + double et = e; + + for (size_t i = r.begin(); i != r.end(); i++) { + Eigen::Matrix Hi; + Eigen::Matrix bi; + double ei; + + if (!factors[i].linearize(target, source, target_tree, T, i, rejector, &Hi, &bi, &ei)) { + continue; + } + + Ht += Hi; + bt += bi; + et += ei; + } + + H = Ht; + b = bt; + e = et; + } + + void join(const LinearizeSum& y) { + H += y.H; + b += y.b; + e += y.e; + } + + const TargetPointCloud& target; + const SourcePointCloud& source; + const TargetTree& target_tree; + const CorrespondenceRejector& rejector; + const Eigen::Isometry3d& T; + std::vector& factors; + + Eigen::Matrix H; + Eigen::Matrix b; + double e; +}; + +template +struct ErrorSum { + ErrorSum(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector& factors) + : target(target), + source(source), + T(T), + factors(factors), + e(0.0) {} + + ErrorSum(ErrorSum& x, tbb::split) : target(x.target), source(x.source), T(x.T), factors(x.factors), e(0.0) {} + + void operator()(const tbb::blocked_range& r) { + double et = e; + for (size_t i = r.begin(); i != r.end(); i++) { + et += factors[i].error(target, source, T); + } + e = et; + } + + void join(const ErrorSum& y) { e += y.e; } + + const TargetPointCloud& target; + const SourcePointCloud& source; + const Eigen::Isometry3d& T; + std::vector& factors; + + double e; +}; + +struct ParallelReductionTBB { + ParallelReductionTBB() {} + + template + std::tuple, Eigen::Matrix, double> linearize( + const TargetPointCloud& target, + const SourcePointCloud& source, + const TargetTree& target_tree, + const CorrespondenceRejector& rejector, + const Eigen::Isometry3d& T, + std::vector& factors) { + // + LinearizeSum sum(target, source, target_tree, rejector, T, factors); + + tbb::parallel_reduce(tbb::blocked_range(0, factors.size()), sum); + + return {sum.H, sum.b, sum.e}; + } + + template + double error(const TargetPointCloud& target, const SourcePointCloud& source, const Eigen::Isometry3d& T, std::vector& factors) { + ErrorSum sum(target, source, T, factors); + tbb::parallel_reduce(tbb::blocked_range(0, factors.size()), sum); + return sum.e; + } +}; + +} // namespace small_gicp diff --git a/include/small_gicp/registration/registration.hpp b/include/small_gicp/registration/registration.hpp new file mode 100644 index 0000000..288250f --- /dev/null +++ b/include/small_gicp/registration/registration.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +namespace small_gicp { + +struct TerminationCriteria { + TerminationCriteria() : translation_eps(1e-3), rotation_eps(0.1 * M_PI / 180.0) {} + + bool converged(const Eigen::Matrix& delta) const { return delta.template head<3>().norm() < rotation_eps && delta.template tail<3>().norm() < translation_eps; } + + double translation_eps; + double rotation_eps; +}; + +template +struct Registration { +public: + RegistrationResult align(const TargetPointCloud& target, const SourcePointCloud& source, const TargetTree& target_tree, const Eigen::Isometry3d& init_T) { + std::vector factors(traits::size(source)); + return optimizer.optimize(target, source, target_tree, rejector, criteria, reduction, init_T, factors); + } + +public: + TerminationCriteria criteria; + CorrespondenceRejector rejector; + Reduction reduction; + Optimizer optimizer; +}; + +} // namespace small_gicp diff --git a/include/small_gicp/registration/registration_result.hpp b/include/small_gicp/registration/registration_result.hpp new file mode 100644 index 0000000..3fd105f --- /dev/null +++ b/include/small_gicp/registration/registration_result.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include +#include + +namespace small_gicp { + +struct RegistrationResult { + RegistrationResult(const Eigen::Isometry3d& T) + : T_target_source(T), + converged(false), + iterations(0), + num_inliers(0), + H(Eigen::Matrix::Zero()), + b(Eigen::Matrix::Zero()), + error(0.0) {} + + Eigen::Isometry3d T_target_source; + + bool converged; + size_t iterations; + size_t num_inliers; + + Eigen::Matrix H; + Eigen::Matrix b; + double error; +}; + +} // namespace small_gicp diff --git a/include/small_gicp/registration/rejector.hpp b/include/small_gicp/registration/rejector.hpp new file mode 100644 index 0000000..ddd6ac7 --- /dev/null +++ b/include/small_gicp/registration/rejector.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include +#include + +namespace small_gicp { + +struct NullRejector { + bool operator()(const Eigen::Isometry3d& T, size_t target_index, size_t source_index, double sq_dist) const { return false; } +}; + +struct DistanceRejector { + DistanceRejector() : max_dist_sq(1.0) {} + DistanceRejector(double max_dist) : max_dist_sq(max_dist * max_dist) {} + + bool operator()(const Eigen::Isometry3d& T, size_t target_index, size_t source_index, double sq_dist) const { return sq_dist > max_dist_sq; } + + double max_dist_sq; +}; + +} // namespace small_gicp diff --git a/include/small_gicp/util/downsampling.hpp b/include/small_gicp/util/downsampling.hpp new file mode 100644 index 0000000..16c361e --- /dev/null +++ b/include/small_gicp/util/downsampling.hpp @@ -0,0 +1,89 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace small_gicp { + +template +std::shared_ptr voxelgrid_sampling(const InputPointCloud& points, double leaf_size) { + const double inv_leaf_size = 1.0 / leaf_size; + + std::unordered_map voxels; + for (size_t i = 0; i < traits::size(points); i++) { + const auto& pt = traits::point(points, i); + + const Eigen::Vector3i coord = (pt * inv_leaf_size).array().floor().template cast().template head<3>(); + auto found = voxels.find(coord); + if (found == voxels.end()) { + found = voxels.emplace_hint(found, coord, Eigen::Vector4d::Zero()); + } + found->second += pt; + } + + auto downsampled = std::make_shared(); + traits::resize(*downsampled, voxels.size()); + size_t i = 0; + for (const auto& v : voxels) { + traits::set_point(*downsampled, i++, v.second / v.second.w()); + } + + return downsampled; +} + +template +std::shared_ptr randomgrid_sampling(const InputPointCloud& points, double leaf_size, size_t target_num_points, std::mt19937& rng) { + if (traits::size(points) <= target_num_points) { + auto downsampled = std::make_shared(); + traits::resize(*downsampled, traits::size(points)); + for (size_t i = 0; i < traits::size(points); i++) { + traits::set_point(*downsampled, i, traits::point(points, i)); + } + return downsampled; + } + + const double inv_leaf_size = 1.0 / leaf_size; + + using Indices = std::shared_ptr>; + std::unordered_map voxels; + for (size_t i = 0; i < traits::size(points); i++) { + const auto& pt = traits::point(points, i); + + const Eigen::Vector3i coord = (pt * inv_leaf_size).array().floor().template cast().template head<3>(); + auto found = voxels.find(coord); + if (found == voxels.end()) { + found = voxels.emplace_hint(found, coord, std::make_shared>()); + found->second->reserve(20); + } + + found->second->emplace_back(i); + } + + const size_t points_per_voxel = std::ceil(static_cast(target_num_points) / voxels.size()); + + std::vector indices; + indices.reserve(points_per_voxel * voxels.size()); + + for (const auto& voxel : voxels) { + const auto& voxel_indices = *voxel.second; + if (voxel_indices.size() <= points_per_voxel) { + indices.insert(indices.end(), voxel_indices.begin(), voxel_indices.end()); + } else { + std::ranges::sample(voxel_indices, std::back_inserter(indices), points_per_voxel, rng); + } + } + std::ranges::sort(indices); + + auto downsampled = std::make_shared(); + traits::resize(*downsampled, indices.size()); + for (size_t i = 0; i < indices.size(); i++) { + traits::set_point(*downsampled, i, traits::point(points, indices[i])); + } + + return downsampled; +} + +} // namespace small_gicp diff --git a/include/small_gicp/util/downsampling_tbb.hpp b/include/small_gicp/util/downsampling_tbb.hpp new file mode 100644 index 0000000..4e96b3d --- /dev/null +++ b/include/small_gicp/util/downsampling_tbb.hpp @@ -0,0 +1,68 @@ +#pragma once + +#include + +#include +#include +#include + +namespace small_gicp { + +/** + * @brief Voxel grid downsampling using TBB. + * @note This TBB version brings only a minor speedup compared to the single-thread version (e.g., 32-threads -> 1.4x speedup), and is not worth using usually. + */ +template +std::shared_ptr voxelgrid_sampling_tbb(const InputPointCloud& points, double leaf_size) { + const double inv_leaf_size = 1.0 / leaf_size; + + typedef tbb::concurrent_hash_map VoxelMap; + + std::atomic_uint64_t num_voxels = 0; + VoxelMap voxels; + std::vector voxel_values(traits::size(points), Eigen::Vector4d::Zero()); + + const int chunk_size = 8; + tbb::parallel_for(tbb::blocked_range(0, traits::size(points), chunk_size), [&](const tbb::blocked_range& range) { + std::vector coords; + std::vector values; + coords.reserve(range.size()); + values.reserve(range.size()); + + for (size_t i = range.begin(); i < range.end(); i++) { + const Eigen::Vector4d pt = traits::point(points, i); + const Eigen::Vector3i coord = (pt * inv_leaf_size).array().floor().template cast().template head<3>(); + + auto found = std::ranges::find(coords, coord); + if (found == coords.end()) { + coords.emplace_back(coord); + values.emplace_back(pt); + } else { + values[std::distance(coords.begin(), found)] += pt; + } + } + + for (size_t i = 0; i < coords.size(); i++) { + VoxelMap::accessor a; + if (voxels.insert(a, coords[i])) { + a->second = num_voxels++; + voxel_values[a->second] = values[i]; + } else { + voxel_values[a->second] += values[i]; + } + } + }); + + const int N = num_voxels; + auto downsampled = std::make_shared(); + traits::resize(*downsampled, N); + + for (size_t i = 0; i < N; i++) { + const Eigen::Vector4d pt = voxel_values[i]; + traits::set_point(*downsampled, i, pt / pt.w()); + } + + return downsampled; +} + +} // namespace small_gicp diff --git a/include/small_gicp/util/lie.hpp b/include/small_gicp/util/lie.hpp new file mode 100644 index 0000000..216629b --- /dev/null +++ b/include/small_gicp/util/lie.hpp @@ -0,0 +1,93 @@ +#pragma once + +#include +#include + +namespace small_gicp { + +inline Eigen::Matrix3d skew(const Eigen::Vector3d& x) { + Eigen::Matrix3d skew = Eigen::Matrix3d::Zero(); + skew(0, 1) = -x[2]; + skew(0, 2) = x[1]; + skew(1, 0) = x[2]; + skew(1, 2) = -x[0]; + skew(2, 0) = -x[1]; + skew(2, 1) = x[0]; + + return skew; +} + +/* + * SO3 expmap code taken from Sophus + * https://github.com/strasdat/Sophus/blob/593db47500ea1a2de5f0e6579c86147991509c59/sophus/so3.hpp#L585 + * + * Copyright 2011-2017 Hauke Strasdat + * 2012-2017 Steven Lovegrove + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to + * deal in the Software without restriction, including without limitation the + * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or + * sell copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS + * IN THE SOFTWARE. + */ +inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) { + double theta_sq = omega.dot(omega); + + double theta; + double imag_factor; + double real_factor; + if (theta_sq < 1e-10) { + theta = 0; + double theta_quad = theta_sq * theta_sq; + imag_factor = 0.5 - 1.0 / 48.0 * theta_sq + 1.0 / 3840.0 * theta_quad; + real_factor = 1.0 - 1.0 / 8.0 * theta_sq + 1.0 / 384.0 * theta_quad; + } else { + theta = std::sqrt(theta_sq); + double half_theta = 0.5 * theta; + imag_factor = std::sin(half_theta) / theta; + real_factor = std::cos(half_theta); + } + + return Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z()); +} + +// Rotation-first +inline Eigen::Isometry3d se3_exp(const Eigen::Matrix& a) { + using std::cos; + using std::sin; + const Eigen::Vector3d omega = a.head<3>(); + + double theta = std::sqrt(omega.dot(omega)); + const Eigen::Quaterniond so3 = so3_exp(omega); + const Eigen::Matrix3d Omega = skew(omega); + const Eigen::Matrix3d Omega_sq = Omega * Omega; + Eigen::Matrix3d V; + + if (theta < 1e-10) { + V = so3.matrix(); + /// Note: That is an accurate expansion! + } else { + const double theta_sq = theta * theta; + V = (Eigen::Matrix3d::Identity() + (1.0 - cos(theta)) / (theta_sq)*Omega + (theta - sin(theta)) / (theta_sq * theta) * Omega_sq); + } + + Eigen::Isometry3d se3 = Eigen::Isometry3d::Identity(); + se3.linear() = so3.toRotationMatrix(); + se3.translation() = V * a.tail<3>(); + + return se3; +} + +} // namespace small_gicp diff --git a/include/small_gicp/util/normal_estimation.hpp b/include/small_gicp/util/normal_estimation.hpp new file mode 100644 index 0000000..d11f913 --- /dev/null +++ b/include/small_gicp/util/normal_estimation.hpp @@ -0,0 +1,105 @@ +#pragma once + +#include +#include + +namespace small_gicp { + +template +struct NormalSetter { + static void set_invalid(PointCloud& cloud, size_t i) { traits::set_normal(cloud, i, Eigen::Vector4d::Zero()); } + + 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) { + traits::set_normal(cloud, i, -normal); + } else { + traits::set_normal(cloud, i, normal); + } + } +}; + +template +struct CovarianceSetter { + 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); + } + + 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(); + cov.block<3, 3>(0, 0) = eigenvectors * values.asDiagonal() * eigenvectors.transpose(); + traits::set_cov(cloud, i, cov); + } +}; + +template +struct NormalCovarianceSetter { + static void set_invalid(PointCloud& cloud, size_t i) { + NormalSetter::set_invalid(cloud, i); + CovarianceSetter::set_invalid(cloud, i); + } + + static void set(PointCloud& cloud, size_t i, const Eigen::Matrix3d& eigenvectors) { + NormalSetter::set(cloud, i, eigenvectors); + CovarianceSetter::set(cloud, i, eigenvectors); + } +}; + +template +void estimate_local_features(PointCloud& cloud, Tree& kdtree, int num_neighbors, size_t point_index) { + std::vector k_indices(num_neighbors); + std::vector k_sq_dists(num_neighbors); + const size_t n = kdtree.knn_search(traits::point(cloud, point_index), num_neighbors, k_indices.data(), k_sq_dists.data()); + + if (n < 5) { + // Insufficient number of neighbors + Setter::set_invalid(cloud, point_index); + return; + } + + Eigen::Vector4d sum_points = Eigen::Vector4d::Zero(); + Eigen::Matrix4d sum_cross = Eigen::Matrix4d::Zero(); + for (size_t i = 0; i < n; i++) { + const auto& pt = traits::point(cloud, k_indices[i]); + sum_points += pt; + sum_cross += pt * pt.transpose(); + } + + const Eigen::Vector4d mean = sum_points / n; + const Eigen::Matrix4d cov = (sum_cross - mean * sum_points.transpose()) / n; + + Eigen::SelfAdjointEigenSolver eig; + eig.computeDirect(cov.block<3, 3>(0, 0)); + + Setter::set(cloud, point_index, eig.eigenvectors()); +} + +template +void estimate_local_features(PointCloud& cloud, int num_neighbors) { + traits::resize(cloud, traits::size(cloud)); + + KdTree kdtree(cloud); + for (size_t i = 0; i < traits::size(cloud); i++) { + estimate_local_features, Setter>(cloud, kdtree, num_neighbors, i); + } +} + +template +void estimate_normals(PointCloud& cloud, int num_neighbors = 20) { + estimate_local_features>(cloud, num_neighbors); +} + +template +void estimate_covariances(PointCloud& cloud, int num_neighbors = 20) { + estimate_local_features>(cloud, num_neighbors); +} + +template +void estimate_normals_covariances(PointCloud& cloud, int num_neighbors = 20) { + estimate_local_features>(cloud, num_neighbors); +} + +} // namespace small_gicp diff --git a/include/small_gicp/util/normal_estimation_omp.hpp b/include/small_gicp/util/normal_estimation_omp.hpp new file mode 100644 index 0000000..94b115d --- /dev/null +++ b/include/small_gicp/util/normal_estimation_omp.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include + +namespace small_gicp { + +template +void estimate_local_features_omp(PointCloud& cloud, int num_neighbors, int num_threads) { + traits::resize(cloud, traits::size(cloud)); + + UnsafeKdTree kdtree(cloud); + +#pragma omp parallel for num_threads(num_threads) + for (size_t i = 0; i < traits::size(cloud); i++) { + estimate_local_features, Setter>(cloud, kdtree, num_neighbors, i); + } +} + +template +void estimate_normals_omp(PointCloud& cloud, int num_neighbors = 20, int num_threads = 4) { + estimate_local_features_omp>(cloud, num_neighbors, num_threads); +} + +template +void estimate_covariances_omp(PointCloud& cloud, int num_neighbors = 20, int num_threads = 4) { + estimate_local_features_omp>(cloud, num_neighbors, num_threads); +} + +template +void estimate_normals_covariances_omp(PointCloud& cloud, int num_neighbors = 20, int num_threads = 4) { + estimate_local_features_omp>(cloud, num_neighbors, num_threads); +} + +} // namespace small_gicp diff --git a/include/small_gicp/util/normal_estimation_tbb.hpp b/include/small_gicp/util/normal_estimation_tbb.hpp new file mode 100644 index 0000000..2591342 --- /dev/null +++ b/include/small_gicp/util/normal_estimation_tbb.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include +#include + +namespace small_gicp { + +template +void estimate_local_features_tbb(PointCloud& cloud, int num_neighbors) { + traits::resize(cloud, traits::size(cloud)); + + UnsafeKdTree kdtree(cloud); + + tbb::parallel_for(tbb::blocked_range(0, traits::size(cloud)), [&](const tbb::blocked_range& range) { + for (size_t i = range.begin(); i < range.end(); i++) { + estimate_local_features, Setter>(cloud, kdtree, num_neighbors, i); + } + }); +} + +template +void estimate_normals_tbb(PointCloud& cloud, int num_neighbors = 20) { + estimate_local_features_tbb>(cloud, num_neighbors); +} + +template +void estimate_covariances_tbb(PointCloud& cloud, int num_neighbors = 20) { + estimate_local_features_tbb>(cloud, num_neighbors); +} + +template +void estimate_normals_covariances_tbb(PointCloud& cloud, int num_neighbors = 20) { + estimate_local_features_tbb>(cloud, num_neighbors); +} + +} // namespace small_gicp diff --git a/include/small_gicp/util/read_points.hpp b/include/small_gicp/util/read_points.hpp new file mode 100644 index 0000000..d2b85d4 --- /dev/null +++ b/include/small_gicp/util/read_points.hpp @@ -0,0 +1,25 @@ +#pragma once + +#include +#include + +namespace small_gicp { + +static std::vector read_points(const std::string& filename) { + std::ifstream ifs(filename, std::ios::binary | std::ios::ate); + if (!ifs) { + std::cerr << "error: failed to open " << filename << std::endl; + return {}; + } + + std::streamsize points_bytes = ifs.tellg(); + size_t num_points = points_bytes / (sizeof(Eigen::Vector4f)); + + ifs.seekg(0, std::ios::beg); + std::vector points(num_points); + ifs.read(reinterpret_cast(points.data()), sizeof(Eigen::Vector4f) * num_points); + + return points; +} + +} // namespace small_gicp diff --git a/include/small_gicp/util/vector3i_hash.hpp b/include/small_gicp/util/vector3i_hash.hpp new file mode 100644 index 0000000..4345ed3 --- /dev/null +++ b/include/small_gicp/util/vector3i_hash.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include + +namespace small_gicp { + +/** + * @brief Spatial hashing function + * Teschner et al., "Optimized Spatial Hashing for Collision Detection of Deformable Objects", VMV2003 + */ +struct XORVector3iHash { +public: + size_t operator()(const Eigen::Vector3i& x) const { + const size_t p1 = 73856093; + const size_t p2 = 19349669; // 19349663 was not a prime number + const size_t p3 = 83492791; + return static_cast((x[0] * p1) ^ (x[1] * p2) ^ (x[2] * p3)); + } + + static size_t hash(const Eigen::Vector3i& x) { return XORVector3iHash()(x); } + static bool equal(const Eigen::Vector3i& x1, const Eigen::Vector3i& x2) { return x1 == x2; } +}; + +} // namespace small_gicp diff --git a/src/small_gicp_test.cpp b/src/small_gicp_test.cpp new file mode 100644 index 0000000..9f0e761 --- /dev/null +++ b/src/small_gicp_test.cpp @@ -0,0 +1,100 @@ +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +int main(int argc, char** argv) { + using namespace small_gicp; + + const int num_threads = 32; + tbb::task_scheduler_init init(num_threads); + + const std::string dataset_path = "/home/koide/datasets/kitti/velodyne_filtered"; + std::vector filenames; + for (const auto& path : std::filesystem::directory_iterator(dataset_path)) { + if (path.path().extension() != ".bin") { + continue; + } + + filenames.emplace_back(path.path().string()); + } + std::ranges::sort(filenames); + + auto viewer = guik::viewer(); + viewer->disable_vsync(); + + GaussianVoxelMap::Ptr voxelmap; + + Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); + for (const auto& filename : filenames) { + glim::EasyProfiler prof("prof"); + prof.push("read_points"); + const auto raw_points = read_points(filename); + + prof.push("copy"); + auto points = std::make_shared(raw_points); + prof.push("downsample"); + points = voxelgrid_sampling_tbb(*points, 0.1); + prof.push("estimate covs"); + estimate_covariances_tbb(*points, 10); + std::cout << raw_points.size() << " => " << points->size() << std::endl; + + if (voxelmap == nullptr) { + voxelmap = std::make_shared(1.0); + voxelmap->insert(*points, T); + continue; + } + + // + prof.push("create_tbb"); + Registration registration_tbb; + registration_tbb.optimizer.verbose = true; + prof.push("registration_tbb"); + auto result = registration_tbb.align(*voxelmap, *points, *voxelmap, T); + + prof.push("update"); + T = result.T_target_source; + voxelmap->insert(*points, T); + + prof.push("show"); + viewer->update_points("current", points->points, guik::FlatOrange(T).set_point_scale(2.0f)); + + std::vector means; + std::vector covs; + for (const auto& voxel : voxelmap->flat_voxels) { + means.emplace_back(voxel.mean); + covs.emplace_back(voxel.cov); + } + viewer->update_normal_dists("target", means, covs, 0.5, guik::Rainbow()); + + if (!viewer->spin_once()) { + break; + } + } + + return 0; +} \ No newline at end of file