├── .gitignore
├── CMakeLists.txt
├── README.md
├── Scancontext
    ├── KDTreeVectorOfVectorsAdaptor.h
    ├── Scancontext.cpp
    ├── Scancontext.h
    ├── nanoflann.hpp
    └── tictoc.h
├── config
    └── sc_config.yaml
├── img
    ├── loop.png
    └── origin.png
├── launch
    └── sc_optimization.launch
├── package.xml
└── src
    └── SC_Optimization_node.cpp


/.gitignore:
--------------------------------------------------------------------------------
 1 | # Prerequisites
 2 | *.d
 3 | 
 4 | # Compiled Object files
 5 | *.slo
 6 | *.lo
 7 | *.o
 8 | *.obj
 9 | 
10 | # Precompiled Headers
11 | *.gch
12 | *.pch
13 | 
14 | # Compiled Dynamic libraries
15 | *.so
16 | *.dylib
17 | *.dll
18 | 
19 | # Fortran module files
20 | *.mod
21 | *.smod
22 | 
23 | # Compiled Static libraries
24 | *.lai
25 | *.la
26 | *.a
27 | *.lib
28 | 
29 | # Executables
30 | *.exe
31 | *.out
32 | *.app
33 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.1)
 2 | 
 3 | project(sc_aloam)
 4 | 
 5 | set(CMAKE_CXX_STANDARD 14)# for the problem of error: ‘make_unique’ is not a member of ‘std’
 6 | set(CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build." FORCE)
 7 | set(CMAKE_CXX_FLAGS "-std=c++11")
 8 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
 9 | 
10 | find_package(Eigen3 REQUIRED)
11 | find_package(GTSAM REQUIRED)
12 | find_package(PCL REQUIRED)
13 | include_directories(
14 |                     ${EIGEN3_INCLUDE_DIR}
15 |                     ${PCL_INCLUDE_DIRS}
16 |                     ${GTSAM_INCLUDE_DIRS}
17 |                     ${PROJECT_SOURCE_DIR}
18 |                     ${PROJECT_SOURCE_DIR}/include   
19 |                     ${PROJECT_SOURCE_DIR}/Scancontext
20 | )           
21 | 
22 | find_package(
23 |     catkin REQUIRED COMPONENTS
24 |         roscpp
25 |         tf
26 |         sensor_msgs
27 |         pcl_conversions
28 |         nav_msgs
29 |         geometry_msgs
30 |         cv_bridge
31 | )
32 | include_directories(${catkin_INCLUDE_DIRS})
33 | 
34 | catkin_package()
35 | 
36 | add_executable(loopclosure_aloam src/SC_Optimization_node.cpp
37 |                                  Scancontext/Scancontext.cpp)
38 | target_link_libraries(loopclosure_aloam ${catkin_LIBRARIES} ${PCL_LIBRARIES} gtsam)
39 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Loopclosure for A-LOAM
 2 | 
 3 | This is a basic implementation of loopclosure detection for A-LOAM without optimization.
 4 | 
 5 | ## 1. Dependency
 6 | - GTSAM
 7 | - ROS
 8 | - PCL
 9 | - [Scancontext](https://github.com/irapkaist/scancontext)
10 | 
11 | ## 2. Results
12 | 
13 | - Before performing loop closure
14 | <img src="./img/origin.png">
15 | 
16 | - After performing loop closure
17 | <img src="./img/loop.png">
18 | 
19 | ## 3. Acknowledgements
20 | Thanks for [A-LOAM](https://github.com/HKUST-Aerial-Robotics/A-LOAM) and LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time) and [Scancontext](https://github.com/irapkaist/scancontext).
21 | 


--------------------------------------------------------------------------------
/Scancontext/KDTreeVectorOfVectorsAdaptor.h:
--------------------------------------------------------------------------------
  1 | /***********************************************************************
  2 |  * Software License Agreement (BSD License)
  3 |  *
  4 |  * Copyright 2011-16 Jose Luis Blanco (joseluisblancoc@gmail.com).
  5 |  *   All rights reserved.
  6 |  *
  7 |  * Redistribution and use in source and binary forms, with or without
  8 |  * modification, are permitted provided that the following conditions
  9 |  * are met:
 10 |  *
 11 |  * 1. Redistributions of source code must retain the above copyright
 12 |  *    notice, this list of conditions and the following disclaimer.
 13 |  * 2. Redistributions in binary form must reproduce the above copyright
 14 |  *    notice, this list of conditions and the following disclaimer in the
 15 |  *    documentation and/or other materials provided with the distribution.
 16 |  *
 17 |  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 18 |  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 19 |  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 20 |  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 21 |  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 22 |  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 23 |  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 24 |  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 25 |  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 26 |  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 27 |  *************************************************************************/
 28 | 
 29 | #pragma once
 30 | 
 31 | #include <nanoflann.hpp>
 32 | 
 33 | #include <vector>
 34 | 
 35 | // ===== This example shows how to use nanoflann with these types of containers: =======
 36 | //typedef std::vector<std::vector<double> > my_vector_of_vectors_t;
 37 | //typedef std::vector<Eigen::VectorXd> my_vector_of_vectors_t;   // This requires #include <Eigen/Dense>
 38 | // =====================================================================================
 39 | 
 40 | 
 41 | /** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage.
 42 |   *  The i'th vector represents a point in the state space.
 43 |   *
 44 |   *  \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations.
 45 |   *  \tparam num_t The type of the point coordinates (typically, double or float).
 46 |   *  \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
 47 |   *  \tparam IndexType The type for indices in the KD-tree index (typically, size_t of int)
 48 |   */
 49 | template <class VectorOfVectorsType, typename num_t = double, int DIM = -1, class Distance = nanoflann::metric_L2, typename IndexType = size_t>
 50 | struct KDTreeVectorOfVectorsAdaptor
 51 | {
 52 | 	typedef KDTreeVectorOfVectorsAdaptor<VectorOfVectorsType,num_t,DIM,Distance> self_t;
 53 | 	typedef typename Distance::template traits<num_t,self_t>::distance_t metric_t;
 54 | 	typedef nanoflann::KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType>  index_t;
 55 | 
 56 | 	index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index.
 57 | 
 58 | 	/// Constructor: takes a const ref to the vector of vectors object with the data points
 59 | 	KDTreeVectorOfVectorsAdaptor(const size_t /* dimensionality */, const VectorOfVectorsType &mat, const int leaf_max_size = 10) : m_data(mat)
 60 | 	{
 61 | 		assert(mat.size() != 0 && mat[0].size() != 0);
 62 | 		const size_t dims = mat[0].size();
 63 | 		if (DIM>0 && static_cast<int>(dims) != DIM)
 64 | 			throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
 65 | 		index = new index_t( static_cast<int>(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size ) );
 66 | 		index->buildIndex();
 67 | 	}
 68 | 
 69 | 	~KDTreeVectorOfVectorsAdaptor() {
 70 | 		delete index;
 71 | 	}
 72 | 
 73 | 	const VectorOfVectorsType &m_data;
 74 | 
 75 | 	/** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]).
 76 | 	  *  Note that this is a short-cut method for index->findNeighbors().
 77 | 	  *  The user can also call index->... methods as desired.
 78 | 	  * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
 79 | 	  */
 80 | 	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
 81 | 	{
 82 | 		nanoflann::KNNResultSet<num_t,IndexType> resultSet(num_closest);
 83 | 		resultSet.init(out_indices, out_distances_sq);
 84 | 		index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
 85 | 	}
 86 | 
 87 | 	/** @name Interface expected by KDTreeSingleIndexAdaptor
 88 | 	  * @{ */
 89 | 
 90 | 	const self_t & derived() const {
 91 | 		return *this;
 92 | 	}
 93 | 	self_t & derived()       {
 94 | 		return *this;
 95 | 	}
 96 | 
 97 | 	// Must return the number of data points
 98 | 	inline size_t kdtree_get_point_count() const {
 99 | 		return m_data.size();
100 | 	}
101 | 
102 | 	// Returns the dim'th component of the idx'th point in the class:
103 | 	inline num_t kdtree_get_pt(const size_t idx, const size_t dim) const {
104 | 		return m_data[idx][dim];
105 | 	}
106 | 
107 | 	// Optional bounding-box computation: return false to default to a standard bbox computation loop.
108 | 	//   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
109 | 	//   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
110 | 	template <class BBOX>
111 | 	bool kdtree_get_bbox(BBOX & /*bb*/) const {
112 | 		return false;
113 | 	}
114 | 
115 | 	/** @} */
116 | 
117 | }; // end of KDTreeVectorOfVectorsAdaptor
118 | 


--------------------------------------------------------------------------------
/Scancontext/Scancontext.cpp:
--------------------------------------------------------------------------------
  1 | #include "Scancontext.h"
  2 | 
  3 | // namespace SC2
  4 | // {
  5 | 
  6 | void coreImportTest (void)
  7 | {
  8 |     cout << "scancontext lib is successfully imported." << endl;
  9 | } // coreImportTest
 10 | 
 11 | 
 12 | float rad2deg(float radians)
 13 | {
 14 |     return radians * 180.0 / M_PI;
 15 | }
 16 | 
 17 | float deg2rad(float degrees)
 18 | {
 19 |     return degrees * M_PI / 180.0;
 20 | }
 21 | 
 22 | 
 23 | float xy2theta( const float & _x, const float & _y )
 24 | {
 25 |     if ( _x >= 0 & _y >= 0) 
 26 |         return (180/M_PI) * atan(_y / _x);
 27 | 
 28 |     if ( _x < 0 & _y >= 0) 
 29 |         return 180 - ( (180/M_PI) * atan(_y / (-_x)) );
 30 | 
 31 |     if ( _x < 0 & _y < 0) 
 32 |         return 180 + ( (180/M_PI) * atan(_y / _x) );
 33 | 
 34 |     if ( _x >= 0 & _y < 0)
 35 |         return 360 - ( (180/M_PI) * atan((-_y) / _x) );
 36 | } // xy2theta
 37 | 
 38 | 
 39 | MatrixXd circshift( MatrixXd &_mat, int _num_shift )
 40 | {
 41 |     // shift columns to right direction 
 42 |     assert(_num_shift >= 0);
 43 | 
 44 |     if( _num_shift == 0 )
 45 |     {
 46 |         MatrixXd shifted_mat( _mat );
 47 |         return shifted_mat; // Early return 
 48 |     }
 49 | 
 50 |     MatrixXd shifted_mat = MatrixXd::Zero( _mat.rows(), _mat.cols() );
 51 |     for ( int col_idx = 0; col_idx < _mat.cols(); col_idx++ )
 52 |     {
 53 |         int new_location = (col_idx + _num_shift) % _mat.cols();
 54 |         shifted_mat.col(new_location) = _mat.col(col_idx);
 55 |     }
 56 | 
 57 |     return shifted_mat;
 58 | 
 59 | } // circshift
 60 | 
 61 | 
 62 | std::vector<float> eig2stdvec( MatrixXd _eigmat )
 63 | {
 64 |     std::vector<float> vec( _eigmat.data(), _eigmat.data() + _eigmat.size() );
 65 |     return vec;
 66 | } // eig2stdvec
 67 | 
 68 | 
 69 | double SCManager::distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 )
 70 | {
 71 |     int num_eff_cols = 0; // i.e., to exclude all-nonzero sector
 72 |     double sum_sector_similarity = 0;
 73 |     for ( int col_idx = 0; col_idx < _sc1.cols(); col_idx++ )
 74 |     {
 75 |         VectorXd col_sc1 = _sc1.col(col_idx);
 76 |         VectorXd col_sc2 = _sc2.col(col_idx);
 77 |         
 78 |         if( col_sc1.norm() == 0 | col_sc2.norm() == 0 )
 79 |             continue; // don't count this sector pair. 
 80 | 
 81 |         double sector_similarity = col_sc1.dot(col_sc2) / (col_sc1.norm() * col_sc2.norm());
 82 | 
 83 |         sum_sector_similarity = sum_sector_similarity + sector_similarity;
 84 |         num_eff_cols = num_eff_cols + 1;
 85 |     }
 86 |     
 87 |     double sc_sim = sum_sector_similarity / num_eff_cols;
 88 |     return 1.0 - sc_sim;
 89 | 
 90 | } // distDirectSC
 91 | 
 92 | 
 93 | int SCManager::fastAlignUsingVkey( MatrixXd & _vkey1, MatrixXd & _vkey2)
 94 | {
 95 |     int argmin_vkey_shift = 0;
 96 |     double min_veky_diff_norm = 10000000;
 97 |     for ( int shift_idx = 0; shift_idx < _vkey1.cols(); shift_idx++ )
 98 |     {
 99 |         MatrixXd vkey2_shifted = circshift(_vkey2, shift_idx);
100 | 
101 |         MatrixXd vkey_diff = _vkey1 - vkey2_shifted;
102 | 
103 |         double cur_diff_norm = vkey_diff.norm();
104 |         if( cur_diff_norm < min_veky_diff_norm )
105 |         {
106 |             argmin_vkey_shift = shift_idx;
107 |             min_veky_diff_norm = cur_diff_norm;
108 |         }
109 |     }
110 | 
111 |     return argmin_vkey_shift;
112 | 
113 | } // fastAlignUsingVkey
114 | 
115 | 
116 | std::pair<double, int> SCManager::distanceBtnScanContext( MatrixXd &_sc1, MatrixXd &_sc2 )
117 | {
118 |     // 1. fast align using variant key (not in original IROS18)
119 |     MatrixXd vkey_sc1 = makeSectorkeyFromScancontext( _sc1 );
120 |     MatrixXd vkey_sc2 = makeSectorkeyFromScancontext( _sc2 );
121 |     int argmin_vkey_shift = fastAlignUsingVkey( vkey_sc1, vkey_sc2 );
122 | 
123 |     const int SEARCH_RADIUS = round( 0.5 * SEARCH_RATIO * _sc1.cols() ); // a half of search range 
124 |     std::vector<int> shift_idx_search_space { argmin_vkey_shift };
125 |     for ( int ii = 1; ii < SEARCH_RADIUS + 1; ii++ )
126 |     {
127 |         shift_idx_search_space.push_back( (argmin_vkey_shift + ii + _sc1.cols()) % _sc1.cols() );
128 |         shift_idx_search_space.push_back( (argmin_vkey_shift - ii + _sc1.cols()) % _sc1.cols() );
129 |     }
130 |     std::sort(shift_idx_search_space.begin(), shift_idx_search_space.end());
131 | 
132 |     // 2. fast columnwise diff 
133 |     int argmin_shift = 0;
134 |     double min_sc_dist = 10000000;
135 |     for ( int num_shift: shift_idx_search_space )
136 |     {
137 |         MatrixXd sc2_shifted = circshift(_sc2, num_shift);
138 |         double cur_sc_dist = distDirectSC( _sc1, sc2_shifted );
139 |         if( cur_sc_dist < min_sc_dist )
140 |         {
141 |             argmin_shift = num_shift;
142 |             min_sc_dist = cur_sc_dist;
143 |         }
144 |     }
145 | 
146 |     return make_pair(min_sc_dist, argmin_shift);
147 | 
148 | } // distanceBtnScanContext
149 | 
150 | 
151 | MatrixXd SCManager::makeScancontext( pcl::PointCloud<SCPointType> & _scan_down )
152 | {
153 |     TicToc t_making_desc;
154 | 
155 |     int num_pts_scan_down = _scan_down.points.size();
156 | 
157 |     // main
158 |     const int NO_POINT = -1000;
159 |     MatrixXd desc = NO_POINT * MatrixXd::Ones(PC_NUM_RING, PC_NUM_SECTOR);
160 | 
161 |     SCPointType pt;
162 |     float azim_angle, azim_range; // wihtin 2d plane
163 |     int ring_idx, sctor_idx;
164 |     for (int pt_idx = 0; pt_idx < num_pts_scan_down; pt_idx++)
165 |     {
166 |         pt.x = _scan_down.points[pt_idx].x; 
167 |         pt.y = _scan_down.points[pt_idx].y;
168 |         pt.z = _scan_down.points[pt_idx].z + LIDAR_HEIGHT; // naive adding is ok (all points should be > 0).
169 | 
170 |         // xyz to ring, sector
171 |         azim_range = sqrt(pt.x * pt.x + pt.y * pt.y);
172 |         azim_angle = xy2theta(pt.x, pt.y);
173 | 
174 |         // if range is out of roi, pass
175 |         if( azim_range > PC_MAX_RADIUS )
176 |             continue;
177 | 
178 |         ring_idx = std::max( std::min( PC_NUM_RING, int(ceil( (azim_range / PC_MAX_RADIUS) * PC_NUM_RING )) ), 1 );
179 |         sctor_idx = std::max( std::min( PC_NUM_SECTOR, int(ceil( (azim_angle / 360.0) * PC_NUM_SECTOR )) ), 1 );
180 | 
181 |         // taking maximum z 
182 |         if ( desc(ring_idx-1, sctor_idx-1) < pt.z ) // -1 means cpp starts from 0
183 |             desc(ring_idx-1, sctor_idx-1) = pt.z; // update for taking maximum value at that bin
184 |     }
185 | 
186 |     // reset no points to zero (for cosine dist later)
187 |     for ( int row_idx = 0; row_idx < desc.rows(); row_idx++ )
188 |         for ( int col_idx = 0; col_idx < desc.cols(); col_idx++ )
189 |             if( desc(row_idx, col_idx) == NO_POINT )
190 |                 desc(row_idx, col_idx) = 0;
191 | 
192 |     t_making_desc.toc("PolarContext making");
193 | 
194 |     return desc;
195 | } // SCManager::makeScancontext
196 | 
197 | 
198 | MatrixXd SCManager::makeRingkeyFromScancontext( Eigen::MatrixXd &_desc )
199 | {
200 |     /* 
201 |      * summary: rowwise mean vector
202 |     */
203 |     Eigen::MatrixXd invariant_key(_desc.rows(), 1);
204 |     for ( int row_idx = 0; row_idx < _desc.rows(); row_idx++ )
205 |     {
206 |         Eigen::MatrixXd curr_row = _desc.row(row_idx);
207 |         invariant_key(row_idx, 0) = curr_row.mean();
208 |     }
209 | 
210 |     return invariant_key;
211 | } // SCManager::makeRingkeyFromScancontext
212 | 
213 | 
214 | MatrixXd SCManager::makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc )
215 | {
216 |     /* 
217 |      * summary: columnwise mean vector
218 |     */
219 |     Eigen::MatrixXd variant_key(1, _desc.cols());
220 |     for ( int col_idx = 0; col_idx < _desc.cols(); col_idx++ )
221 |     {
222 |         Eigen::MatrixXd curr_col = _desc.col(col_idx);
223 |         variant_key(0, col_idx) = curr_col.mean();
224 |     }
225 | 
226 |     return variant_key;
227 | } // SCManager::makeSectorkeyFromScancontext
228 | 
229 | 
230 | void SCManager::makeAndSaveScancontextAndKeys( pcl::PointCloud<SCPointType> & _scan_down )
231 | {
232 |     Eigen::MatrixXd sc = makeScancontext(_scan_down); // v1 
233 |     Eigen::MatrixXd ringkey = makeRingkeyFromScancontext( sc );
234 |     Eigen::MatrixXd sectorkey = makeSectorkeyFromScancontext( sc );
235 |     std::vector<float> polarcontext_invkey_vec = eig2stdvec( ringkey );
236 | 
237 |     polarcontexts_.push_back( sc ); 
238 |     polarcontext_invkeys_.push_back( ringkey );
239 |     polarcontext_vkeys_.push_back( sectorkey );
240 |     polarcontext_invkeys_mat_.push_back( polarcontext_invkey_vec );
241 | 
242 |     // cout <<polarcontext_vkeys_.size() << endl;
243 | 
244 | } // SCManager::makeAndSaveScancontextAndKeys
245 | 
246 | 
247 | std::pair<int, float> SCManager::detectLoopClosureID ( void )
248 | {
249 |     int loop_id { -1 }; // init with -1, -1 means no loop (== LeGO-LOAM's variable "closestHistoryFrameID")
250 | 
251 |     auto curr_key = polarcontext_invkeys_mat_.back(); // current observation (query)
252 |     auto curr_desc = polarcontexts_.back(); // current observation (query)
253 | 
254 |     /* 
255 |      * step 1: candidates from ringkey tree_
256 |      */
257 |     if( polarcontext_invkeys_mat_.size() < NUM_EXCLUDE_RECENT + 1)
258 |     {
259 |         std::pair<int, float> result {loop_id, 0.0};
260 |         return result; // Early return 
261 |     }
262 | 
263 |     // tree_ reconstruction (not mandatory to make everytime)
264 |     if( tree_making_period_conter % TREE_MAKING_PERIOD_ == 0) // to save computation cost
265 |     {
266 |         TicToc t_tree_construction;
267 | 
268 |         polarcontext_invkeys_to_search_.clear();
269 |         polarcontext_invkeys_to_search_.assign( polarcontext_invkeys_mat_.begin(), polarcontext_invkeys_mat_.end() - NUM_EXCLUDE_RECENT ) ;
270 | 
271 |         polarcontext_tree_.reset(); 
272 |         polarcontext_tree_ = std::make_unique<InvKeyTree>(PC_NUM_RING /* dim */, polarcontext_invkeys_to_search_, 10 /* max leaf */ );
273 |         // tree_ptr_->index->buildIndex(); // inernally called in the constructor of InvKeyTree (for detail, refer the nanoflann and KDtreeVectorOfVectorsAdaptor)
274 |         t_tree_construction.toc("Tree construction");
275 |     }
276 |     tree_making_period_conter = tree_making_period_conter + 1;
277 |         
278 |     double min_dist = 10000000; // init with somthing large
279 |     int nn_align = 0;
280 |     int nn_idx = 0;
281 | 
282 |     // knn search
283 |     std::vector<size_t> candidate_indexes( NUM_CANDIDATES_FROM_TREE ); 
284 |     std::vector<float> out_dists_sqr( NUM_CANDIDATES_FROM_TREE );
285 | 
286 |     TicToc t_tree_search;
287 |     nanoflann::KNNResultSet<float> knnsearch_result( NUM_CANDIDATES_FROM_TREE );
288 |     knnsearch_result.init( &candidate_indexes[0], &out_dists_sqr[0] );
289 |     polarcontext_tree_->index->findNeighbors( knnsearch_result, &curr_key[0] /* query */, nanoflann::SearchParams(10) ); 
290 |     t_tree_search.toc("Tree search");
291 | 
292 |     /* 
293 |      *  step 2: pairwise distance (find optimal columnwise best-fit using cosine distance)
294 |      */
295 |     TicToc t_calc_dist;   
296 |     for ( int candidate_iter_idx = 0; candidate_iter_idx < NUM_CANDIDATES_FROM_TREE; candidate_iter_idx++ )
297 |     {
298 |         MatrixXd polarcontext_candidate = polarcontexts_[ candidate_indexes[candidate_iter_idx] ];
299 |         std::pair<double, int> sc_dist_result = distanceBtnScanContext( curr_desc, polarcontext_candidate ); 
300 |         
301 |         double candidate_dist = sc_dist_result.first;
302 |         int candidate_align = sc_dist_result.second;
303 | 
304 |         if( candidate_dist < min_dist )
305 |         {
306 |             min_dist = candidate_dist;
307 |             nn_align = candidate_align;
308 | 
309 |             nn_idx = candidate_indexes[candidate_iter_idx];
310 |         }
311 |     }
312 |     t_calc_dist.toc("Distance calc");
313 | 
314 |     /* 
315 |      * loop threshold check
316 |      */
317 |     if( min_dist < SC_DIST_THRES )
318 |     {
319 |         loop_id = nn_idx; 
320 |     
321 |         //std::cout.precision(3); 
322 |         //cout << "[Loop found] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl;
323 |         //cout << "[Loop found] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl;
324 |     }
325 |     else
326 |     {
327 |         //std::cout.precision(3); 
328 |         //cout << "[Not loop] Nearest distance: " << min_dist << " btn " << polarcontexts_.size()-1 << " and " << nn_idx << "." << endl;
329 |         //cout << "[Not loop] yaw diff: " << nn_align * PC_UNIT_SECTORANGLE << " deg." << endl;
330 |     }
331 | 
332 |     // To do: return also nn_align (i.e., yaw diff)
333 |     float yaw_diff_rad = deg2rad(nn_align * PC_UNIT_SECTORANGLE);
334 |     std::pair<int, float> result {loop_id, yaw_diff_rad};
335 | 
336 |     return result;
337 | 
338 | } // SCManager::detectLoopClosureID
339 | 
340 | // } // namespace SC2


--------------------------------------------------------------------------------
/Scancontext/Scancontext.h:
--------------------------------------------------------------------------------
  1 | #pragma once
  2 | 
  3 | #include <ctime>
  4 | #include <cassert>
  5 | #include <cmath>
  6 | #include <utility>
  7 | #include <vector>
  8 | #include <algorithm> 
  9 | #include <cstdlib>
 10 | #include <memory>
 11 | #include <iostream>
 12 | 
 13 | #include <Eigen/Dense>
 14 | 
 15 | #include <opencv2/opencv.hpp>
 16 | #include <opencv2/core/eigen.hpp>
 17 | #include <opencv2/highgui/highgui.hpp>
 18 | #include <cv_bridge/cv_bridge.h>
 19 | 
 20 | #include <pcl/point_cloud.h>
 21 | #include <pcl/point_types.h>
 22 | #include <pcl/filters/voxel_grid.h>
 23 | #include <pcl_conversions/pcl_conversions.h>
 24 | 
 25 | #include "nanoflann.hpp"
 26 | #include "KDTreeVectorOfVectorsAdaptor.h"
 27 | 
 28 | #include "tictoc.h"
 29 | 
 30 | using namespace Eigen;
 31 | using namespace nanoflann;
 32 | 
 33 | using std::cout;
 34 | using std::endl;
 35 | using std::make_pair;
 36 | 
 37 | using std::atan2;
 38 | using std::cos;
 39 | using std::sin;
 40 | 
 41 | using SCPointType = pcl::PointXYZI; // using xyz only. but a user can exchange the original bin encoding function (i.e., max hegiht) to max intensity (for detail, refer 20 ICRA Intensity Scan Context)
 42 | using KeyMat = std::vector<std::vector<float> >;
 43 | using InvKeyTree = KDTreeVectorOfVectorsAdaptor< KeyMat, float >;
 44 | 
 45 | 
 46 | // namespace SC2
 47 | // {
 48 | 
 49 | void coreImportTest ( void );
 50 | 
 51 | 
 52 | // sc param-independent helper functions 
 53 | float xy2theta( const float & _x, const float & _y );
 54 | MatrixXd circshift( MatrixXd &_mat, int _num_shift );
 55 | std::vector<float> eig2stdvec( MatrixXd _eigmat );
 56 | 
 57 | 
 58 | class SCManager
 59 | {
 60 | public: 
 61 |     SCManager( ) = default; // reserving data space (of std::vector) could be considered. but the descriptor is lightweight so don't care.
 62 | 
 63 |     Eigen::MatrixXd makeScancontext( pcl::PointCloud<SCPointType> & _scan_down );
 64 |     Eigen::MatrixXd makeRingkeyFromScancontext( Eigen::MatrixXd &_desc );
 65 |     Eigen::MatrixXd makeSectorkeyFromScancontext( Eigen::MatrixXd &_desc );
 66 | 
 67 |     int fastAlignUsingVkey ( MatrixXd & _vkey1, MatrixXd & _vkey2 ); 
 68 |     double distDirectSC ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "d" (eq 5) in the original paper (IROS 18)
 69 |     std::pair<double, int> distanceBtnScanContext ( MatrixXd &_sc1, MatrixXd &_sc2 ); // "D" (eq 6) in the original paper (IROS 18)
 70 | 
 71 |     // User-side API
 72 |     void makeAndSaveScancontextAndKeys( pcl::PointCloud<SCPointType> & _scan_down );
 73 |     std::pair<int, float> detectLoopClosureID( void ); // int: nearest node index, float: relative yaw  
 74 | 
 75 | public:
 76 |     // hyper parameters ()
 77 |     const double LIDAR_HEIGHT = 2.0; // lidar height : add this for simply directly using lidar scan in the lidar local coord (not robot base coord) / if you use robot-coord-transformed lidar scans, just set this as 0.
 78 | 
 79 |     const int    PC_NUM_RING = 20; // 20 in the original paper (IROS 18)
 80 |     const int    PC_NUM_SECTOR = 60; // 60 in the original paper (IROS 18)
 81 |     const double PC_MAX_RADIUS = 80.0; // 80 meter max in the original paper (IROS 18)
 82 |     const double PC_UNIT_SECTORANGLE = 360.0 / double(PC_NUM_SECTOR);
 83 |     const double PC_UNIT_RINGGAP = PC_MAX_RADIUS / double(PC_NUM_RING);
 84 | 
 85 |     // tree
 86 |     const int    NUM_EXCLUDE_RECENT = 500; // simply just keyframe gap, but node position distance-based exclusion is ok. 
 87 |     const int    NUM_CANDIDATES_FROM_TREE = 10; // 10 is enough. (refer the IROS 18 paper)
 88 | 
 89 |     // loop thres
 90 |     const double SEARCH_RATIO = 0.1; // for fast comparison, no Brute-force, but search 10 % is okay. // but may well work for same-direction-revisits, not for reverse-revisits
 91 |     // const double SC_DIST_THRES = 0.13; // empirically 0.1-0.2 is fine (rare false-alarms) for 20x60 polar context (but for 0.15 <, DCS or ICP fit score check (e.g., in LeGO-LOAM) should be required for robustness)
 92 |     const double SC_DIST_THRES = 0.11; // 0.4-0.6 is good choice for using with robust kernel (e.g., Cauchy, DCS) + icp fitness threshold
 93 | 
 94 |     // config 
 95 |     const int    TREE_MAKING_PERIOD_ = 50; // i.e., remaking tree frequency, to avoid non-mandatory every remaking, to save time cost
 96 |     int          tree_making_period_conter = 0;
 97 | 
 98 |     // data 
 99 |     std::vector<double> polarcontexts_timestamp_; // optional.
100 |     std::vector<Eigen::MatrixXd> polarcontexts_;
101 |     std::vector<Eigen::MatrixXd> polarcontext_invkeys_;
102 |     std::vector<Eigen::MatrixXd> polarcontext_vkeys_;
103 | 
104 |     KeyMat polarcontext_invkeys_mat_;
105 |     KeyMat polarcontext_invkeys_to_search_;
106 |     std::unique_ptr<InvKeyTree> polarcontext_tree_;
107 | 
108 | }; // SCManager
109 | 
110 | // } // namespace SC2
111 | 


--------------------------------------------------------------------------------
/Scancontext/nanoflann.hpp:
--------------------------------------------------------------------------------
   1 | /***********************************************************************
   2 |  * Software License Agreement (BSD License)
   3 |  *
   4 |  * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
   5 |  * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
   6 |  * Copyright 2011-2016  Jose Luis Blanco (joseluisblancoc@gmail.com).
   7 |  *   All rights reserved.
   8 |  *
   9 |  * THE BSD LICENSE
  10 |  *
  11 |  * Redistribution and use in source and binary forms, with or without
  12 |  * modification, are permitted provided that the following conditions
  13 |  * are met:
  14 |  *
  15 |  * 1. Redistributions of source code must retain the above copyright
  16 |  *    notice, this list of conditions and the following disclaimer.
  17 |  * 2. Redistributions in binary form must reproduce the above copyright
  18 |  *    notice, this list of conditions and the following disclaimer in the
  19 |  *    documentation and/or other materials provided with the distribution.
  20 |  *
  21 |  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
  22 |  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
  23 |  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
  24 |  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
  25 |  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
  26 |  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  27 |  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  28 |  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  29 |  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  30 |  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  31 |  *************************************************************************/
  32 | 
  33 | /** \mainpage nanoflann C++ API documentation
  34 |  *  nanoflann is a C++ header-only library for building KD-Trees, mostly
  35 |  *  optimized for 2D or 3D point clouds.
  36 |  *
  37 |  *  nanoflann does not require compiling or installing, just an
  38 |  *  #include <nanoflann.hpp> in your code.
  39 |  *
  40 |  *  See:
  41 |  *   - <a href="modules.html" >C++ API organized by modules</a>
  42 |  *   - <a href="https://github.com/jlblancoc/nanoflann" >Online README</a>
  43 |  *   - <a href="http://jlblancoc.github.io/nanoflann/" >Doxygen
  44 |  * documentation</a>
  45 |  */
  46 | 
  47 | #ifndef NANOFLANN_HPP_
  48 | #define NANOFLANN_HPP_
  49 | 
  50 | #include <algorithm>
  51 | #include <array>
  52 | #include <cassert>
  53 | #include <cmath>   // for abs()
  54 | #include <cstdio>  // for fwrite()
  55 | #include <cstdlib> // for abs()
  56 | #include <functional>
  57 | #include <limits> // std::reference_wrapper
  58 | #include <stdexcept>
  59 | #include <vector>
  60 | 
  61 | /** Library version: 0xMmP (M=Major,m=minor,P=patch) */
  62 | #define NANOFLANN_VERSION 0x132
  63 | 
  64 | // Avoid conflicting declaration of min/max macros in windows headers
  65 | #if !defined(NOMINMAX) &&                                                      \
  66 |     (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64))
  67 | #define NOMINMAX
  68 | #ifdef max
  69 | #undef max
  70 | #undef min
  71 | #endif
  72 | #endif
  73 | 
  74 | namespace nanoflann {
  75 | /** @addtogroup nanoflann_grp nanoflann C++ library for ANN
  76 |  *  @{ */
  77 | 
  78 | /** the PI constant (required to avoid MSVC missing symbols) */
  79 | template <typename T> T pi_const() {
  80 |   return static_cast<T>(3.14159265358979323846);
  81 | }
  82 | 
  83 | /**
  84 |  * Traits if object is resizable and assignable (typically has a resize | assign
  85 |  * method)
  86 |  */
  87 | template <typename T, typename = int> struct has_resize : std::false_type {};
  88 | 
  89 | template <typename T>
  90 | struct has_resize<T, decltype((void)std::declval<T>().resize(1), 0)>
  91 |     : std::true_type {};
  92 | 
  93 | template <typename T, typename = int> struct has_assign : std::false_type {};
  94 | 
  95 | template <typename T>
  96 | struct has_assign<T, decltype((void)std::declval<T>().assign(1, 0), 0)>
  97 |     : std::true_type {};
  98 | 
  99 | /**
 100 |  * Free function to resize a resizable object
 101 |  */
 102 | template <typename Container>
 103 | inline typename std::enable_if<has_resize<Container>::value, void>::type
 104 | resize(Container &c, const size_t nElements) {
 105 |   c.resize(nElements);
 106 | }
 107 | 
 108 | /**
 109 |  * Free function that has no effects on non resizable containers (e.g.
 110 |  * std::array) It raises an exception if the expected size does not match
 111 |  */
 112 | template <typename Container>
 113 | inline typename std::enable_if<!has_resize<Container>::value, void>::type
 114 | resize(Container &c, const size_t nElements) {
 115 |   if (nElements != c.size())
 116 |     throw std::logic_error("Try to change the size of a std::array.");
 117 | }
 118 | 
 119 | /**
 120 |  * Free function to assign to a container
 121 |  */
 122 | template <typename Container, typename T>
 123 | inline typename std::enable_if<has_assign<Container>::value, void>::type
 124 | assign(Container &c, const size_t nElements, const T &value) {
 125 |   c.assign(nElements, value);
 126 | }
 127 | 
 128 | /**
 129 |  * Free function to assign to a std::array
 130 |  */
 131 | template <typename Container, typename T>
 132 | inline typename std::enable_if<!has_assign<Container>::value, void>::type
 133 | assign(Container &c, const size_t nElements, const T &value) {
 134 |   for (size_t i = 0; i < nElements; i++)
 135 |     c[i] = value;
 136 | }
 137 | 
 138 | /** @addtogroup result_sets_grp Result set classes
 139 |  *  @{ */
 140 | template <typename _DistanceType, typename _IndexType = size_t,
 141 |           typename _CountType = size_t>
 142 | class KNNResultSet {
 143 | public:
 144 |   typedef _DistanceType DistanceType;
 145 |   typedef _IndexType IndexType;
 146 |   typedef _CountType CountType;
 147 | 
 148 | private:
 149 |   IndexType *indices;
 150 |   DistanceType *dists;
 151 |   CountType capacity;
 152 |   CountType count;
 153 | 
 154 | public:
 155 |   inline KNNResultSet(CountType capacity_)
 156 |       : indices(0), dists(0), capacity(capacity_), count(0) {}
 157 | 
 158 |   inline void init(IndexType *indices_, DistanceType *dists_) {
 159 |     indices = indices_;
 160 |     dists = dists_;
 161 |     count = 0;
 162 |     if (capacity)
 163 |       dists[capacity - 1] = (std::numeric_limits<DistanceType>::max)();
 164 |   }
 165 | 
 166 |   inline CountType size() const { return count; }
 167 | 
 168 |   inline bool full() const { return count == capacity; }
 169 | 
 170 |   /**
 171 |    * Called during search to add an element matching the criteria.
 172 |    * @return true if the search should be continued, false if the results are
 173 |    * sufficient
 174 |    */
 175 |   inline bool addPoint(DistanceType dist, IndexType index) {
 176 |     CountType i;
 177 |     for (i = count; i > 0; --i) {
 178 | #ifdef NANOFLANN_FIRST_MATCH // If defined and two points have the same
 179 |                              // distance, the one with the lowest-index will be
 180 |                              // returned first.
 181 |       if ((dists[i - 1] > dist) ||
 182 |           ((dist == dists[i - 1]) && (indices[i - 1] > index))) {
 183 | #else
 184 |       if (dists[i - 1] > dist) {
 185 | #endif
 186 |         if (i < capacity) {
 187 |           dists[i] = dists[i - 1];
 188 |           indices[i] = indices[i - 1];
 189 |         }
 190 |       } else
 191 |         break;
 192 |     }
 193 |     if (i < capacity) {
 194 |       dists[i] = dist;
 195 |       indices[i] = index;
 196 |     }
 197 |     if (count < capacity)
 198 |       count++;
 199 | 
 200 |     // tell caller that the search shall continue
 201 |     return true;
 202 |   }
 203 | 
 204 |   inline DistanceType worstDist() const { return dists[capacity - 1]; }
 205 | };
 206 | 
 207 | /** operator "<" for std::sort() */
 208 | struct IndexDist_Sorter {
 209 |   /** PairType will be typically: std::pair<IndexType,DistanceType> */
 210 |   template <typename PairType>
 211 |   inline bool operator()(const PairType &p1, const PairType &p2) const {
 212 |     return p1.second < p2.second;
 213 |   }
 214 | };
 215 | 
 216 | /**
 217 |  * A result-set class used when performing a radius based search.
 218 |  */
 219 | template <typename _DistanceType, typename _IndexType = size_t>
 220 | class RadiusResultSet {
 221 | public:
 222 |   typedef _DistanceType DistanceType;
 223 |   typedef _IndexType IndexType;
 224 | 
 225 | public:
 226 |   const DistanceType radius;
 227 | 
 228 |   std::vector<std::pair<IndexType, DistanceType>> &m_indices_dists;
 229 | 
 230 |   inline RadiusResultSet(
 231 |       DistanceType radius_,
 232 |       std::vector<std::pair<IndexType, DistanceType>> &indices_dists)
 233 |       : radius(radius_), m_indices_dists(indices_dists) {
 234 |     init();
 235 |   }
 236 | 
 237 |   inline void init() { clear(); }
 238 |   inline void clear() { m_indices_dists.clear(); }
 239 | 
 240 |   inline size_t size() const { return m_indices_dists.size(); }
 241 | 
 242 |   inline bool full() const { return true; }
 243 | 
 244 |   /**
 245 |    * Called during search to add an element matching the criteria.
 246 |    * @return true if the search should be continued, false if the results are
 247 |    * sufficient
 248 |    */
 249 |   inline bool addPoint(DistanceType dist, IndexType index) {
 250 |     if (dist < radius)
 251 |       m_indices_dists.push_back(std::make_pair(index, dist));
 252 |     return true;
 253 |   }
 254 | 
 255 |   inline DistanceType worstDist() const { return radius; }
 256 | 
 257 |   /**
 258 |    * Find the worst result (furtherest neighbor) without copying or sorting
 259 |    * Pre-conditions: size() > 0
 260 |    */
 261 |   std::pair<IndexType, DistanceType> worst_item() const {
 262 |     if (m_indices_dists.empty())
 263 |       throw std::runtime_error("Cannot invoke RadiusResultSet::worst_item() on "
 264 |                                "an empty list of results.");
 265 |     typedef
 266 |         typename std::vector<std::pair<IndexType, DistanceType>>::const_iterator
 267 |             DistIt;
 268 |     DistIt it = std::max_element(m_indices_dists.begin(), m_indices_dists.end(),
 269 |                                  IndexDist_Sorter());
 270 |     return *it;
 271 |   }
 272 | };
 273 | 
 274 | /** @} */
 275 | 
 276 | /** @addtogroup loadsave_grp Load/save auxiliary functions
 277 |  * @{ */
 278 | template <typename T>
 279 | void save_value(FILE *stream, const T &value, size_t count = 1) {
 280 |   fwrite(&value, sizeof(value), count, stream);
 281 | }
 282 | 
 283 | template <typename T>
 284 | void save_value(FILE *stream, const std::vector<T> &value) {
 285 |   size_t size = value.size();
 286 |   fwrite(&size, sizeof(size_t), 1, stream);
 287 |   fwrite(&value[0], sizeof(T), size, stream);
 288 | }
 289 | 
 290 | template <typename T>
 291 | void load_value(FILE *stream, T &value, size_t count = 1) {
 292 |   size_t read_cnt = fread(&value, sizeof(value), count, stream);
 293 |   if (read_cnt != count) {
 294 |     throw std::runtime_error("Cannot read from file");
 295 |   }
 296 | }
 297 | 
 298 | template <typename T> void load_value(FILE *stream, std::vector<T> &value) {
 299 |   size_t size;
 300 |   size_t read_cnt = fread(&size, sizeof(size_t), 1, stream);
 301 |   if (read_cnt != 1) {
 302 |     throw std::runtime_error("Cannot read from file");
 303 |   }
 304 |   value.resize(size);
 305 |   read_cnt = fread(&value[0], sizeof(T), size, stream);
 306 |   if (read_cnt != size) {
 307 |     throw std::runtime_error("Cannot read from file");
 308 |   }
 309 | }
 310 | /** @} */
 311 | 
 312 | /** @addtogroup metric_grp Metric (distance) classes
 313 |  * @{ */
 314 | 
 315 | struct Metric {};
 316 | 
 317 | /** Manhattan distance functor (generic version, optimized for
 318 |  * high-dimensionality data sets). Corresponding distance traits:
 319 |  * nanoflann::metric_L1 \tparam T Type of the elements (e.g. double, float,
 320 |  * uint8_t) \tparam _DistanceType Type of distance variables (must be signed)
 321 |  * (e.g. float, double, int64_t)
 322 |  */
 323 | template <class T, class DataSource, typename _DistanceType = T>
 324 | struct L1_Adaptor {
 325 |   typedef T ElementType;
 326 |   typedef _DistanceType DistanceType;
 327 | 
 328 |   const DataSource &data_source;
 329 | 
 330 |   L1_Adaptor(const DataSource &_data_source) : data_source(_data_source) {}
 331 | 
 332 |   inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size,
 333 |                                  DistanceType worst_dist = -1) const {
 334 |     DistanceType result = DistanceType();
 335 |     const T *last = a + size;
 336 |     const T *lastgroup = last - 3;
 337 |     size_t d = 0;
 338 | 
 339 |     /* Process 4 items with each loop for efficiency. */
 340 |     while (a < lastgroup) {
 341 |       const DistanceType diff0 =
 342 |           std::abs(a[0] - data_source.kdtree_get_pt(b_idx, d++));
 343 |       const DistanceType diff1 =
 344 |           std::abs(a[1] - data_source.kdtree_get_pt(b_idx, d++));
 345 |       const DistanceType diff2 =
 346 |           std::abs(a[2] - data_source.kdtree_get_pt(b_idx, d++));
 347 |       const DistanceType diff3 =
 348 |           std::abs(a[3] - data_source.kdtree_get_pt(b_idx, d++));
 349 |       result += diff0 + diff1 + diff2 + diff3;
 350 |       a += 4;
 351 |       if ((worst_dist > 0) && (result > worst_dist)) {
 352 |         return result;
 353 |       }
 354 |     }
 355 |     /* Process last 0-3 components.  Not needed for standard vector lengths. */
 356 |     while (a < last) {
 357 |       result += std::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++));
 358 |     }
 359 |     return result;
 360 |   }
 361 | 
 362 |   template <typename U, typename V>
 363 |   inline DistanceType accum_dist(const U a, const V b, const size_t) const {
 364 |     return std::abs(a - b);
 365 |   }
 366 | };
 367 | 
 368 | /** Squared Euclidean distance functor (generic version, optimized for
 369 |  * high-dimensionality data sets). Corresponding distance traits:
 370 |  * nanoflann::metric_L2 \tparam T Type of the elements (e.g. double, float,
 371 |  * uint8_t) \tparam _DistanceType Type of distance variables (must be signed)
 372 |  * (e.g. float, double, int64_t)
 373 |  */
 374 | template <class T, class DataSource, typename _DistanceType = T>
 375 | struct L2_Adaptor {
 376 |   typedef T ElementType;
 377 |   typedef _DistanceType DistanceType;
 378 | 
 379 |   const DataSource &data_source;
 380 | 
 381 |   L2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {}
 382 | 
 383 |   inline DistanceType evalMetric(const T *a, const size_t b_idx, size_t size,
 384 |                                  DistanceType worst_dist = -1) const {
 385 |     DistanceType result = DistanceType();
 386 |     const T *last = a + size;
 387 |     const T *lastgroup = last - 3;
 388 |     size_t d = 0;
 389 | 
 390 |     /* Process 4 items with each loop for efficiency. */
 391 |     while (a < lastgroup) {
 392 |       const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++);
 393 |       const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++);
 394 |       const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++);
 395 |       const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++);
 396 |       result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
 397 |       a += 4;
 398 |       if ((worst_dist > 0) && (result > worst_dist)) {
 399 |         return result;
 400 |       }
 401 |     }
 402 |     /* Process last 0-3 components.  Not needed for standard vector lengths. */
 403 |     while (a < last) {
 404 |       const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++);
 405 |       result += diff0 * diff0;
 406 |     }
 407 |     return result;
 408 |   }
 409 | 
 410 |   template <typename U, typename V>
 411 |   inline DistanceType accum_dist(const U a, const V b, const size_t) const {
 412 |     return (a - b) * (a - b);
 413 |   }
 414 | };
 415 | 
 416 | /** Squared Euclidean (L2) distance functor (suitable for low-dimensionality
 417 |  * datasets, like 2D or 3D point clouds) Corresponding distance traits:
 418 |  * nanoflann::metric_L2_Simple \tparam T Type of the elements (e.g. double,
 419 |  * float, uint8_t) \tparam _DistanceType Type of distance variables (must be
 420 |  * signed) (e.g. float, double, int64_t)
 421 |  */
 422 | template <class T, class DataSource, typename _DistanceType = T>
 423 | struct L2_Simple_Adaptor {
 424 |   typedef T ElementType;
 425 |   typedef _DistanceType DistanceType;
 426 | 
 427 |   const DataSource &data_source;
 428 | 
 429 |   L2_Simple_Adaptor(const DataSource &_data_source)
 430 |       : data_source(_data_source) {}
 431 | 
 432 |   inline DistanceType evalMetric(const T *a, const size_t b_idx,
 433 |                                  size_t size) const {
 434 |     DistanceType result = DistanceType();
 435 |     for (size_t i = 0; i < size; ++i) {
 436 |       const DistanceType diff = a[i] - data_source.kdtree_get_pt(b_idx, i);
 437 |       result += diff * diff;
 438 |     }
 439 |     return result;
 440 |   }
 441 | 
 442 |   template <typename U, typename V>
 443 |   inline DistanceType accum_dist(const U a, const V b, const size_t) const {
 444 |     return (a - b) * (a - b);
 445 |   }
 446 | };
 447 | 
 448 | /** SO2 distance functor
 449 |  *  Corresponding distance traits: nanoflann::metric_SO2
 450 |  * \tparam T Type of the elements (e.g. double, float)
 451 |  * \tparam _DistanceType Type of distance variables (must be signed) (e.g.
 452 |  * float, double) orientation is constrained to be in [-pi, pi]
 453 |  */
 454 | template <class T, class DataSource, typename _DistanceType = T>
 455 | struct SO2_Adaptor {
 456 |   typedef T ElementType;
 457 |   typedef _DistanceType DistanceType;
 458 | 
 459 |   const DataSource &data_source;
 460 | 
 461 |   SO2_Adaptor(const DataSource &_data_source) : data_source(_data_source) {}
 462 | 
 463 |   inline DistanceType evalMetric(const T *a, const size_t b_idx,
 464 |                                  size_t size) const {
 465 |     return accum_dist(a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1),
 466 |                       size - 1);
 467 |   }
 468 | 
 469 |   /** Note: this assumes that input angles are already in the range [-pi,pi] */
 470 |   template <typename U, typename V>
 471 |   inline DistanceType accum_dist(const U a, const V b, const size_t) const {
 472 |     DistanceType result = DistanceType();
 473 |     DistanceType PI = pi_const<DistanceType>();
 474 |     result = b - a;
 475 |     if (result > PI)
 476 |       result -= 2 * PI;
 477 |     else if (result < -PI)
 478 |       result += 2 * PI;
 479 |     return result;
 480 |   }
 481 | };
 482 | 
 483 | /** SO3 distance functor (Uses L2_Simple)
 484 |  *  Corresponding distance traits: nanoflann::metric_SO3
 485 |  * \tparam T Type of the elements (e.g. double, float)
 486 |  * \tparam _DistanceType Type of distance variables (must be signed) (e.g.
 487 |  * float, double)
 488 |  */
 489 | template <class T, class DataSource, typename _DistanceType = T>
 490 | struct SO3_Adaptor {
 491 |   typedef T ElementType;
 492 |   typedef _DistanceType DistanceType;
 493 | 
 494 |   L2_Simple_Adaptor<T, DataSource> distance_L2_Simple;
 495 | 
 496 |   SO3_Adaptor(const DataSource &_data_source)
 497 |       : distance_L2_Simple(_data_source) {}
 498 | 
 499 |   inline DistanceType evalMetric(const T *a, const size_t b_idx,
 500 |                                  size_t size) const {
 501 |     return distance_L2_Simple.evalMetric(a, b_idx, size);
 502 |   }
 503 | 
 504 |   template <typename U, typename V>
 505 |   inline DistanceType accum_dist(const U a, const V b, const size_t idx) const {
 506 |     return distance_L2_Simple.accum_dist(a, b, idx);
 507 |   }
 508 | };
 509 | 
 510 | /** Metaprogramming helper traits class for the L1 (Manhattan) metric */
 511 | struct metric_L1 : public Metric {
 512 |   template <class T, class DataSource> struct traits {
 513 |     typedef L1_Adaptor<T, DataSource> distance_t;
 514 |   };
 515 | };
 516 | /** Metaprogramming helper traits class for the L2 (Euclidean) metric */
 517 | struct metric_L2 : public Metric {
 518 |   template <class T, class DataSource> struct traits {
 519 |     typedef L2_Adaptor<T, DataSource> distance_t;
 520 |   };
 521 | };
 522 | /** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */
 523 | struct metric_L2_Simple : public Metric {
 524 |   template <class T, class DataSource> struct traits {
 525 |     typedef L2_Simple_Adaptor<T, DataSource> distance_t;
 526 |   };
 527 | };
 528 | /** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */
 529 | struct metric_SO2 : public Metric {
 530 |   template <class T, class DataSource> struct traits {
 531 |     typedef SO2_Adaptor<T, DataSource> distance_t;
 532 |   };
 533 | };
 534 | /** Metaprogramming helper traits class for the SO3_InnerProdQuat metric */
 535 | struct metric_SO3 : public Metric {
 536 |   template <class T, class DataSource> struct traits {
 537 |     typedef SO3_Adaptor<T, DataSource> distance_t;
 538 |   };
 539 | };
 540 | 
 541 | /** @} */
 542 | 
 543 | /** @addtogroup param_grp Parameter structs
 544 |  * @{ */
 545 | 
 546 | /**  Parameters (see README.md) */
 547 | struct KDTreeSingleIndexAdaptorParams {
 548 |   KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10)
 549 |       : leaf_max_size(_leaf_max_size) {}
 550 | 
 551 |   size_t leaf_max_size;
 552 | };
 553 | 
 554 | /** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */
 555 | struct SearchParams {
 556 |   /** Note: The first argument (checks_IGNORED_) is ignored, but kept for
 557 |    * compatibility with the FLANN interface */
 558 |   SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true)
 559 |       : checks(checks_IGNORED_), eps(eps_), sorted(sorted_) {}
 560 | 
 561 |   int checks;  //!< Ignored parameter (Kept for compatibility with the FLANN
 562 |                //!< interface).
 563 |   float eps;   //!< search for eps-approximate neighbours (default: 0)
 564 |   bool sorted; //!< only for radius search, require neighbours sorted by
 565 |                //!< distance (default: true)
 566 | };
 567 | /** @} */
 568 | 
 569 | /** @addtogroup memalloc_grp Memory allocation
 570 |  * @{ */
 571 | 
 572 | /**
 573 |  * Allocates (using C's malloc) a generic type T.
 574 |  *
 575 |  * Params:
 576 |  *     count = number of instances to allocate.
 577 |  * Returns: pointer (of type T*) to memory buffer
 578 |  */
 579 | template <typename T> inline T *allocate(size_t count = 1) {
 580 |   T *mem = static_cast<T *>(::malloc(sizeof(T) * count));
 581 |   return mem;
 582 | }
 583 | 
 584 | /**
 585 |  * Pooled storage allocator
 586 |  *
 587 |  * The following routines allow for the efficient allocation of storage in
 588 |  * small chunks from a specified pool.  Rather than allowing each structure
 589 |  * to be freed individually, an entire pool of storage is freed at once.
 590 |  * This method has two advantages over just using malloc() and free().  First,
 591 |  * it is far more efficient for allocating small objects, as there is
 592 |  * no overhead for remembering all the information needed to free each
 593 |  * object or consolidating fragmented memory.  Second, the decision about
 594 |  * how long to keep an object is made at the time of allocation, and there
 595 |  * is no need to track down all the objects to free them.
 596 |  *
 597 |  */
 598 | 
 599 | const size_t WORDSIZE = 16;
 600 | const size_t BLOCKSIZE = 8192;
 601 | 
 602 | class PooledAllocator {
 603 |   /* We maintain memory alignment to word boundaries by requiring that all
 604 |       allocations be in multiples of the machine wordsize.  */
 605 |   /* Size of machine word in bytes.  Must be power of 2. */
 606 |   /* Minimum number of bytes requested at a time from	the system.  Must be
 607 |    * multiple of WORDSIZE. */
 608 | 
 609 |   size_t remaining; /* Number of bytes left in current block of storage. */
 610 |   void *base;       /* Pointer to base of current block of storage. */
 611 |   void *loc;        /* Current location in block to next allocate memory. */
 612 | 
 613 |   void internal_init() {
 614 |     remaining = 0;
 615 |     base = NULL;
 616 |     usedMemory = 0;
 617 |     wastedMemory = 0;
 618 |   }
 619 | 
 620 | public:
 621 |   size_t usedMemory;
 622 |   size_t wastedMemory;
 623 | 
 624 |   /**
 625 |       Default constructor. Initializes a new pool.
 626 |    */
 627 |   PooledAllocator() { internal_init(); }
 628 | 
 629 |   /**
 630 |    * Destructor. Frees all the memory allocated in this pool.
 631 |    */
 632 |   ~PooledAllocator() { free_all(); }
 633 | 
 634 |   /** Frees all allocated memory chunks */
 635 |   void free_all() {
 636 |     while (base != NULL) {
 637 |       void *prev =
 638 |           *(static_cast<void **>(base)); /* Get pointer to prev block. */
 639 |       ::free(base);
 640 |       base = prev;
 641 |     }
 642 |     internal_init();
 643 |   }
 644 | 
 645 |   /**
 646 |    * Returns a pointer to a piece of new memory of the given size in bytes
 647 |    * allocated from the pool.
 648 |    */
 649 |   void *malloc(const size_t req_size) {
 650 |     /* Round size up to a multiple of wordsize.  The following expression
 651 |         only works for WORDSIZE that is a power of 2, by masking last bits of
 652 |         incremented size to zero.
 653 |      */
 654 |     const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);
 655 | 
 656 |     /* Check whether a new block must be allocated.  Note that the first word
 657 |         of a block is reserved for a pointer to the previous block.
 658 |      */
 659 |     if (size > remaining) {
 660 | 
 661 |       wastedMemory += remaining;
 662 | 
 663 |       /* Allocate new storage. */
 664 |       const size_t blocksize =
 665 |           (size + sizeof(void *) + (WORDSIZE - 1) > BLOCKSIZE)
 666 |               ? size + sizeof(void *) + (WORDSIZE - 1)
 667 |               : BLOCKSIZE;
 668 | 
 669 |       // use the standard C malloc to allocate memory
 670 |       void *m = ::malloc(blocksize);
 671 |       if (!m) {
 672 |         fprintf(stderr, "Failed to allocate memory.\n");
 673 |         return NULL;
 674 |       }
 675 | 
 676 |       /* Fill first word of new block with pointer to previous block. */
 677 |       static_cast<void **>(m)[0] = base;
 678 |       base = m;
 679 | 
 680 |       size_t shift = 0;
 681 |       // int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) &
 682 |       // (WORDSIZE-1))) & (WORDSIZE-1);
 683 | 
 684 |       remaining = blocksize - sizeof(void *) - shift;
 685 |       loc = (static_cast<char *>(m) + sizeof(void *) + shift);
 686 |     }
 687 |     void *rloc = loc;
 688 |     loc = static_cast<char *>(loc) + size;
 689 |     remaining -= size;
 690 | 
 691 |     usedMemory += size;
 692 | 
 693 |     return rloc;
 694 |   }
 695 | 
 696 |   /**
 697 |    * Allocates (using this pool) a generic type T.
 698 |    *
 699 |    * Params:
 700 |    *     count = number of instances to allocate.
 701 |    * Returns: pointer (of type T*) to memory buffer
 702 |    */
 703 |   template <typename T> T *allocate(const size_t count = 1) {
 704 |     T *mem = static_cast<T *>(this->malloc(sizeof(T) * count));
 705 |     return mem;
 706 |   }
 707 | };
 708 | /** @} */
 709 | 
 710 | /** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff
 711 |  * @{ */
 712 | 
 713 | /** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors
 714 |  * when DIM=-1. Fixed size version for a generic DIM:
 715 |  */
 716 | template <int DIM, typename T> struct array_or_vector_selector {
 717 |   typedef std::array<T, DIM> container_t;
 718 | };
 719 | /** Dynamic size version */
 720 | template <typename T> struct array_or_vector_selector<-1, T> {
 721 |   typedef std::vector<T> container_t;
 722 | };
 723 | 
 724 | /** @} */
 725 | 
 726 | /** kd-tree base-class
 727 |  *
 728 |  * Contains the member functions common to the classes KDTreeSingleIndexAdaptor
 729 |  * and KDTreeSingleIndexDynamicAdaptor_.
 730 |  *
 731 |  * \tparam Derived The name of the class which inherits this class.
 732 |  * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
 733 |  * \tparam Distance The distance metric to use, these are all classes derived
 734 |  * from nanoflann::Metric \tparam DIM Dimensionality of data points (e.g. 3 for
 735 |  * 3D points) \tparam IndexType Will be typically size_t or int
 736 |  */
 737 | 
 738 | template <class Derived, typename Distance, class DatasetAdaptor, int DIM = -1,
 739 |           typename IndexType = size_t>
 740 | class KDTreeBaseClass {
 741 | 
 742 | public:
 743 |   /** Frees the previously-built index. Automatically called within
 744 |    * buildIndex(). */
 745 |   void freeIndex(Derived &obj) {
 746 |     obj.pool.free_all();
 747 |     obj.root_node = NULL;
 748 |     obj.m_size_at_index_build = 0;
 749 |   }
 750 | 
 751 |   typedef typename Distance::ElementType ElementType;
 752 |   typedef typename Distance::DistanceType DistanceType;
 753 | 
 754 |   /*--------------------- Internal Data Structures --------------------------*/
 755 |   struct Node {
 756 |     /** Union used because a node can be either a LEAF node or a non-leaf node,
 757 |      * so both data fields are never used simultaneously */
 758 |     union {
 759 |       struct leaf {
 760 |         IndexType left, right; //!< Indices of points in leaf node
 761 |       } lr;
 762 |       struct nonleaf {
 763 |         int divfeat;                  //!< Dimension used for subdivision.
 764 |         DistanceType divlow, divhigh; //!< The values used for subdivision.
 765 |       } sub;
 766 |     } node_type;
 767 |     Node *child1, *child2; //!< Child nodes (both=NULL mean its a leaf node)
 768 |   };
 769 | 
 770 |   typedef Node *NodePtr;
 771 | 
 772 |   struct Interval {
 773 |     ElementType low, high;
 774 |   };
 775 | 
 776 |   /**
 777 |    *  Array of indices to vectors in the dataset.
 778 |    */
 779 |   std::vector<IndexType> vind;
 780 | 
 781 |   NodePtr root_node;
 782 | 
 783 |   size_t m_leaf_max_size;
 784 | 
 785 |   size_t m_size;                //!< Number of current points in the dataset
 786 |   size_t m_size_at_index_build; //!< Number of points in the dataset when the
 787 |                                 //!< index was built
 788 |   int dim;                      //!< Dimensionality of each data point
 789 | 
 790 |   /** Define "BoundingBox" as a fixed-size or variable-size container depending
 791 |    * on "DIM" */
 792 |   typedef
 793 |       typename array_or_vector_selector<DIM, Interval>::container_t BoundingBox;
 794 | 
 795 |   /** Define "distance_vector_t" as a fixed-size or variable-size container
 796 |    * depending on "DIM" */
 797 |   typedef typename array_or_vector_selector<DIM, DistanceType>::container_t
 798 |       distance_vector_t;
 799 | 
 800 |   /** The KD-tree used to find neighbours */
 801 | 
 802 |   BoundingBox root_bbox;
 803 | 
 804 |   /**
 805 |    * Pooled memory allocator.
 806 |    *
 807 |    * Using a pooled memory allocator is more efficient
 808 |    * than allocating memory directly when there is a large
 809 |    * number small of memory allocations.
 810 |    */
 811 |   PooledAllocator pool;
 812 | 
 813 |   /** Returns number of points in dataset  */
 814 |   size_t size(const Derived &obj) const { return obj.m_size; }
 815 | 
 816 |   /** Returns the length of each point in the dataset */
 817 |   size_t veclen(const Derived &obj) {
 818 |     return static_cast<size_t>(DIM > 0 ? DIM : obj.dim);
 819 |   }
 820 | 
 821 |   /// Helper accessor to the dataset points:
 822 |   inline ElementType dataset_get(const Derived &obj, size_t idx,
 823 |                                  int component) const {
 824 |     return obj.dataset.kdtree_get_pt(idx, component);
 825 |   }
 826 | 
 827 |   /**
 828 |    * Computes the inde memory usage
 829 |    * Returns: memory used by the index
 830 |    */
 831 |   size_t usedMemory(Derived &obj) {
 832 |     return obj.pool.usedMemory + obj.pool.wastedMemory +
 833 |            obj.dataset.kdtree_get_point_count() *
 834 |                sizeof(IndexType); // pool memory and vind array memory
 835 |   }
 836 | 
 837 |   void computeMinMax(const Derived &obj, IndexType *ind, IndexType count,
 838 |                      int element, ElementType &min_elem,
 839 |                      ElementType &max_elem) {
 840 |     min_elem = dataset_get(obj, ind[0], element);
 841 |     max_elem = dataset_get(obj, ind[0], element);
 842 |     for (IndexType i = 1; i < count; ++i) {
 843 |       ElementType val = dataset_get(obj, ind[i], element);
 844 |       if (val < min_elem)
 845 |         min_elem = val;
 846 |       if (val > max_elem)
 847 |         max_elem = val;
 848 |     }
 849 |   }
 850 | 
 851 |   /**
 852 |    * Create a tree node that subdivides the list of vecs from vind[first]
 853 |    * to vind[last].  The routine is called recursively on each sublist.
 854 |    *
 855 |    * @param left index of the first vector
 856 |    * @param right index of the last vector
 857 |    */
 858 |   NodePtr divideTree(Derived &obj, const IndexType left, const IndexType right,
 859 |                      BoundingBox &bbox) {
 860 |     NodePtr node = obj.pool.template allocate<Node>(); // allocate memory
 861 | 
 862 |     /* If too few exemplars remain, then make this a leaf node. */
 863 |     if ((right - left) <= static_cast<IndexType>(obj.m_leaf_max_size)) {
 864 |       node->child1 = node->child2 = NULL; /* Mark as leaf node. */
 865 |       node->node_type.lr.left = left;
 866 |       node->node_type.lr.right = right;
 867 | 
 868 |       // compute bounding-box of leaf points
 869 |       for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
 870 |         bbox[i].low = dataset_get(obj, obj.vind[left], i);
 871 |         bbox[i].high = dataset_get(obj, obj.vind[left], i);
 872 |       }
 873 |       for (IndexType k = left + 1; k < right; ++k) {
 874 |         for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
 875 |           if (bbox[i].low > dataset_get(obj, obj.vind[k], i))
 876 |             bbox[i].low = dataset_get(obj, obj.vind[k], i);
 877 |           if (bbox[i].high < dataset_get(obj, obj.vind[k], i))
 878 |             bbox[i].high = dataset_get(obj, obj.vind[k], i);
 879 |         }
 880 |       }
 881 |     } else {
 882 |       IndexType idx;
 883 |       int cutfeat;
 884 |       DistanceType cutval;
 885 |       middleSplit_(obj, &obj.vind[0] + left, right - left, idx, cutfeat, cutval,
 886 |                    bbox);
 887 | 
 888 |       node->node_type.sub.divfeat = cutfeat;
 889 | 
 890 |       BoundingBox left_bbox(bbox);
 891 |       left_bbox[cutfeat].high = cutval;
 892 |       node->child1 = divideTree(obj, left, left + idx, left_bbox);
 893 | 
 894 |       BoundingBox right_bbox(bbox);
 895 |       right_bbox[cutfeat].low = cutval;
 896 |       node->child2 = divideTree(obj, left + idx, right, right_bbox);
 897 | 
 898 |       node->node_type.sub.divlow = left_bbox[cutfeat].high;
 899 |       node->node_type.sub.divhigh = right_bbox[cutfeat].low;
 900 | 
 901 |       for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
 902 |         bbox[i].low = std::min(left_bbox[i].low, right_bbox[i].low);
 903 |         bbox[i].high = std::max(left_bbox[i].high, right_bbox[i].high);
 904 |       }
 905 |     }
 906 | 
 907 |     return node;
 908 |   }
 909 | 
 910 |   void middleSplit_(Derived &obj, IndexType *ind, IndexType count,
 911 |                     IndexType &index, int &cutfeat, DistanceType &cutval,
 912 |                     const BoundingBox &bbox) {
 913 |     const DistanceType EPS = static_cast<DistanceType>(0.00001);
 914 |     ElementType max_span = bbox[0].high - bbox[0].low;
 915 |     for (int i = 1; i < (DIM > 0 ? DIM : obj.dim); ++i) {
 916 |       ElementType span = bbox[i].high - bbox[i].low;
 917 |       if (span > max_span) {
 918 |         max_span = span;
 919 |       }
 920 |     }
 921 |     ElementType max_spread = -1;
 922 |     cutfeat = 0;
 923 |     for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
 924 |       ElementType span = bbox[i].high - bbox[i].low;
 925 |       if (span > (1 - EPS) * max_span) {
 926 |         ElementType min_elem, max_elem;
 927 |         computeMinMax(obj, ind, count, i, min_elem, max_elem);
 928 |         ElementType spread = max_elem - min_elem;
 929 |         ;
 930 |         if (spread > max_spread) {
 931 |           cutfeat = i;
 932 |           max_spread = spread;
 933 |         }
 934 |       }
 935 |     }
 936 |     // split in the middle
 937 |     DistanceType split_val = (bbox[cutfeat].low + bbox[cutfeat].high) / 2;
 938 |     ElementType min_elem, max_elem;
 939 |     computeMinMax(obj, ind, count, cutfeat, min_elem, max_elem);
 940 | 
 941 |     if (split_val < min_elem)
 942 |       cutval = min_elem;
 943 |     else if (split_val > max_elem)
 944 |       cutval = max_elem;
 945 |     else
 946 |       cutval = split_val;
 947 | 
 948 |     IndexType lim1, lim2;
 949 |     planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2);
 950 | 
 951 |     if (lim1 > count / 2)
 952 |       index = lim1;
 953 |     else if (lim2 < count / 2)
 954 |       index = lim2;
 955 |     else
 956 |       index = count / 2;
 957 |   }
 958 | 
 959 |   /**
 960 |    *  Subdivide the list of points by a plane perpendicular on axe corresponding
 961 |    *  to the 'cutfeat' dimension at 'cutval' position.
 962 |    *
 963 |    *  On return:
 964 |    *  dataset[ind[0..lim1-1]][cutfeat]<cutval
 965 |    *  dataset[ind[lim1..lim2-1]][cutfeat]==cutval
 966 |    *  dataset[ind[lim2..count]][cutfeat]>cutval
 967 |    */
 968 |   void planeSplit(Derived &obj, IndexType *ind, const IndexType count,
 969 |                   int cutfeat, DistanceType &cutval, IndexType &lim1,
 970 |                   IndexType &lim2) {
 971 |     /* Move vector indices for left subtree to front of list. */
 972 |     IndexType left = 0;
 973 |     IndexType right = count - 1;
 974 |     for (;;) {
 975 |       while (left <= right && dataset_get(obj, ind[left], cutfeat) < cutval)
 976 |         ++left;
 977 |       while (right && left <= right &&
 978 |              dataset_get(obj, ind[right], cutfeat) >= cutval)
 979 |         --right;
 980 |       if (left > right || !right)
 981 |         break; // "!right" was added to support unsigned Index types
 982 |       std::swap(ind[left], ind[right]);
 983 |       ++left;
 984 |       --right;
 985 |     }
 986 |     /* If either list is empty, it means that all remaining features
 987 |      * are identical. Split in the middle to maintain a balanced tree.
 988 |      */
 989 |     lim1 = left;
 990 |     right = count - 1;
 991 |     for (;;) {
 992 |       while (left <= right && dataset_get(obj, ind[left], cutfeat) <= cutval)
 993 |         ++left;
 994 |       while (right && left <= right &&
 995 |              dataset_get(obj, ind[right], cutfeat) > cutval)
 996 |         --right;
 997 |       if (left > right || !right)
 998 |         break; // "!right" was added to support unsigned Index types
 999 |       std::swap(ind[left], ind[right]);
1000 |       ++left;
1001 |       --right;
1002 |     }
1003 |     lim2 = left;
1004 |   }
1005 | 
1006 |   DistanceType computeInitialDistances(const Derived &obj,
1007 |                                        const ElementType *vec,
1008 |                                        distance_vector_t &dists) const {
1009 |     assert(vec);
1010 |     DistanceType distsq = DistanceType();
1011 | 
1012 |     for (int i = 0; i < (DIM > 0 ? DIM : obj.dim); ++i) {
1013 |       if (vec[i] < obj.root_bbox[i].low) {
1014 |         dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].low, i);
1015 |         distsq += dists[i];
1016 |       }
1017 |       if (vec[i] > obj.root_bbox[i].high) {
1018 |         dists[i] = obj.distance.accum_dist(vec[i], obj.root_bbox[i].high, i);
1019 |         distsq += dists[i];
1020 |       }
1021 |     }
1022 |     return distsq;
1023 |   }
1024 | 
1025 |   void save_tree(Derived &obj, FILE *stream, NodePtr tree) {
1026 |     save_value(stream, *tree);
1027 |     if (tree->child1 != NULL) {
1028 |       save_tree(obj, stream, tree->child1);
1029 |     }
1030 |     if (tree->child2 != NULL) {
1031 |       save_tree(obj, stream, tree->child2);
1032 |     }
1033 |   }
1034 | 
1035 |   void load_tree(Derived &obj, FILE *stream, NodePtr &tree) {
1036 |     tree = obj.pool.template allocate<Node>();
1037 |     load_value(stream, *tree);
1038 |     if (tree->child1 != NULL) {
1039 |       load_tree(obj, stream, tree->child1);
1040 |     }
1041 |     if (tree->child2 != NULL) {
1042 |       load_tree(obj, stream, tree->child2);
1043 |     }
1044 |   }
1045 | 
1046 |   /**  Stores the index in a binary file.
1047 |    *   IMPORTANT NOTE: The set of data points is NOT stored in the file, so when
1048 |    * loading the index object it must be constructed associated to the same
1049 |    * source of data points used while building it. See the example:
1050 |    * examples/saveload_example.cpp \sa loadIndex  */
1051 |   void saveIndex_(Derived &obj, FILE *stream) {
1052 |     save_value(stream, obj.m_size);
1053 |     save_value(stream, obj.dim);
1054 |     save_value(stream, obj.root_bbox);
1055 |     save_value(stream, obj.m_leaf_max_size);
1056 |     save_value(stream, obj.vind);
1057 |     save_tree(obj, stream, obj.root_node);
1058 |   }
1059 | 
1060 |   /**  Loads a previous index from a binary file.
1061 |    *   IMPORTANT NOTE: The set of data points is NOT stored in the file, so the
1062 |    * index object must be constructed associated to the same source of data
1063 |    * points used while building the index. See the example:
1064 |    * examples/saveload_example.cpp \sa loadIndex  */
1065 |   void loadIndex_(Derived &obj, FILE *stream) {
1066 |     load_value(stream, obj.m_size);
1067 |     load_value(stream, obj.dim);
1068 |     load_value(stream, obj.root_bbox);
1069 |     load_value(stream, obj.m_leaf_max_size);
1070 |     load_value(stream, obj.vind);
1071 |     load_tree(obj, stream, obj.root_node);
1072 |   }
1073 | };
1074 | 
1075 | /** @addtogroup kdtrees_grp KD-tree classes and adaptors
1076 |  * @{ */
1077 | 
1078 | /** kd-tree static index
1079 |  *
1080 |  * Contains the k-d trees and other information for indexing a set of points
1081 |  * for nearest-neighbor matching.
1082 |  *
1083 |  *  The class "DatasetAdaptor" must provide the following interface (can be
1084 |  * non-virtual, inlined methods):
1085 |  *
1086 |  *  \code
1087 |  *   // Must return the number of data poins
1088 |  *   inline size_t kdtree_get_point_count() const { ... }
1089 |  *
1090 |  *
1091 |  *   // Must return the dim'th component of the idx'th point in the class:
1092 |  *   inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... }
1093 |  *
1094 |  *   // Optional bounding-box computation: return false to default to a standard
1095 |  * bbox computation loop.
1096 |  *   //   Return true if the BBOX was already computed by the class and returned
1097 |  * in "bb" so it can be avoided to redo it again.
1098 |  *   //   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3
1099 |  * for point clouds) template <class BBOX> bool kdtree_get_bbox(BBOX &bb) const
1100 |  *   {
1101 |  *      bb[0].low = ...; bb[0].high = ...;  // 0th dimension limits
1102 |  *      bb[1].low = ...; bb[1].high = ...;  // 1st dimension limits
1103 |  *      ...
1104 |  *      return true;
1105 |  *   }
1106 |  *
1107 |  *  \endcode
1108 |  *
1109 |  * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
1110 |  * \tparam Distance The distance metric to use: nanoflann::metric_L1,
1111 |  * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM
1112 |  * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will
1113 |  * be typically size_t or int
1114 |  */
1115 | template <typename Distance, class DatasetAdaptor, int DIM = -1,
1116 |           typename IndexType = size_t>
1117 | class KDTreeSingleIndexAdaptor
1118 |     : public KDTreeBaseClass<
1119 |           KDTreeSingleIndexAdaptor<Distance, DatasetAdaptor, DIM, IndexType>,
1120 |           Distance, DatasetAdaptor, DIM, IndexType> {
1121 | public:
1122 |   /** Deleted copy constructor*/
1123 |   KDTreeSingleIndexAdaptor(
1124 |       const KDTreeSingleIndexAdaptor<Distance, DatasetAdaptor, DIM, IndexType>
1125 |           &) = delete;
1126 | 
1127 |   /**
1128 |    * The dataset used by this index
1129 |    */
1130 |   const DatasetAdaptor &dataset; //!< The source of our data
1131 | 
1132 |   const KDTreeSingleIndexAdaptorParams index_params;
1133 | 
1134 |   Distance distance;
1135 | 
1136 |   typedef typename nanoflann::KDTreeBaseClass<
1137 |       nanoflann::KDTreeSingleIndexAdaptor<Distance, DatasetAdaptor, DIM,
1138 |                                           IndexType>,
1139 |       Distance, DatasetAdaptor, DIM, IndexType>
1140 |       BaseClassRef;
1141 | 
1142 |   typedef typename BaseClassRef::ElementType ElementType;
1143 |   typedef typename BaseClassRef::DistanceType DistanceType;
1144 | 
1145 |   typedef typename BaseClassRef::Node Node;
1146 |   typedef Node *NodePtr;
1147 | 
1148 |   typedef typename BaseClassRef::Interval Interval;
1149 |   /** Define "BoundingBox" as a fixed-size or variable-size container depending
1150 |    * on "DIM" */
1151 |   typedef typename BaseClassRef::BoundingBox BoundingBox;
1152 | 
1153 |   /** Define "distance_vector_t" as a fixed-size or variable-size container
1154 |    * depending on "DIM" */
1155 |   typedef typename BaseClassRef::distance_vector_t distance_vector_t;
1156 | 
1157 |   /**
1158 |    * KDTree constructor
1159 |    *
1160 |    * Refer to docs in README.md or online in
1161 |    * https://github.com/jlblancoc/nanoflann
1162 |    *
1163 |    * The KD-Tree point dimension (the length of each point in the datase, e.g. 3
1164 |    * for 3D points) is determined by means of:
1165 |    *  - The \a DIM template parameter if >0 (highest priority)
1166 |    *  - Otherwise, the \a dimensionality parameter of this constructor.
1167 |    *
1168 |    * @param inputData Dataset with the input features
1169 |    * @param params Basically, the maximum leaf node size
1170 |    */
1171 |   KDTreeSingleIndexAdaptor(const int dimensionality,
1172 |                            const DatasetAdaptor &inputData,
1173 |                            const KDTreeSingleIndexAdaptorParams &params =
1174 |                                KDTreeSingleIndexAdaptorParams())
1175 |       : dataset(inputData), index_params(params), distance(inputData) {
1176 |     BaseClassRef::root_node = NULL;
1177 |     BaseClassRef::m_size = dataset.kdtree_get_point_count();
1178 |     BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1179 |     BaseClassRef::dim = dimensionality;
1180 |     if (DIM > 0)
1181 |       BaseClassRef::dim = DIM;
1182 |     BaseClassRef::m_leaf_max_size = params.leaf_max_size;
1183 | 
1184 |     // Create a permutable array of indices to the input vectors.
1185 |     init_vind();
1186 |   }
1187 | 
1188 |   /**
1189 |    * Builds the index
1190 |    */
1191 |   void buildIndex() {
1192 |     BaseClassRef::m_size = dataset.kdtree_get_point_count();
1193 |     BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1194 |     init_vind();
1195 |     this->freeIndex(*this);
1196 |     BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1197 |     if (BaseClassRef::m_size == 0)
1198 |       return;
1199 |     computeBoundingBox(BaseClassRef::root_bbox);
1200 |     BaseClassRef::root_node =
1201 |         this->divideTree(*this, 0, BaseClassRef::m_size,
1202 |                          BaseClassRef::root_bbox); // construct the tree
1203 |   }
1204 | 
1205 |   /** \name Query methods
1206 |    * @{ */
1207 | 
1208 |   /**
1209 |    * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored
1210 |    * inside the result object.
1211 |    *
1212 |    * Params:
1213 |    *     result = the result object in which the indices of the
1214 |    * nearest-neighbors are stored vec = the vector for which to search the
1215 |    * nearest neighbors
1216 |    *
1217 |    * \tparam RESULTSET Should be any ResultSet<DistanceType>
1218 |    * \return  True if the requested neighbors could be found.
1219 |    * \sa knnSearch, radiusSearch
1220 |    */
1221 |   template <typename RESULTSET>
1222 |   bool findNeighbors(RESULTSET &result, const ElementType *vec,
1223 |                      const SearchParams &searchParams) const {
1224 |     assert(vec);
1225 |     if (this->size(*this) == 0)
1226 |       return false;
1227 |     if (!BaseClassRef::root_node)
1228 |       throw std::runtime_error(
1229 |           "[nanoflann] findNeighbors() called before building the index.");
1230 |     float epsError = 1 + searchParams.eps;
1231 | 
1232 |     distance_vector_t
1233 |         dists; // fixed or variable-sized container (depending on DIM)
1234 |     auto zero = static_cast<decltype(result.worstDist())>(0);
1235 |     assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim),
1236 |            zero); // Fill it with zeros.
1237 |     DistanceType distsq = this->computeInitialDistances(*this, vec, dists);
1238 |     searchLevel(result, vec, BaseClassRef::root_node, distsq, dists,
1239 |                 epsError); // "count_leaf" parameter removed since was neither
1240 |                            // used nor returned to the user.
1241 |     return result.full();
1242 |   }
1243 | 
1244 |   /**
1245 |    * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1].
1246 |    * Their indices are stored inside the result object. \sa radiusSearch,
1247 |    * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility
1248 |    * with the original FLANN interface. \return Number `N` of valid points in
1249 |    * the result set. Only the first `N` entries in `out_indices` and
1250 |    * `out_distances_sq` will be valid. Return may be less than `num_closest`
1251 |    * only if the number of elements in the tree is less than `num_closest`.
1252 |    */
1253 |   size_t knnSearch(const ElementType *query_point, const size_t num_closest,
1254 |                    IndexType *out_indices, DistanceType *out_distances_sq,
1255 |                    const int /* nChecks_IGNORED */ = 10) const {
1256 |     nanoflann::KNNResultSet<DistanceType, IndexType> resultSet(num_closest);
1257 |     resultSet.init(out_indices, out_distances_sq);
1258 |     this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
1259 |     return resultSet.size();
1260 |   }
1261 | 
1262 |   /**
1263 |    * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius.
1264 |    *  The output is given as a vector of pairs, of which the first element is a
1265 |    * point index and the second the corresponding distance. Previous contents of
1266 |    * \a IndicesDists are cleared.
1267 |    *
1268 |    *  If searchParams.sorted==true, the output list is sorted by ascending
1269 |    * distances.
1270 |    *
1271 |    *  For a better performance, it is advisable to do a .reserve() on the vector
1272 |    * if you have any wild guess about the number of expected matches.
1273 |    *
1274 |    *  \sa knnSearch, findNeighbors, radiusSearchCustomCallback
1275 |    * \return The number of points within the given radius (i.e. indices.size()
1276 |    * or dists.size() )
1277 |    */
1278 |   size_t
1279 |   radiusSearch(const ElementType *query_point, const DistanceType &radius,
1280 |                std::vector<std::pair<IndexType, DistanceType>> &IndicesDists,
1281 |                const SearchParams &searchParams) const {
1282 |     RadiusResultSet<DistanceType, IndexType> resultSet(radius, IndicesDists);
1283 |     const size_t nFound =
1284 |         radiusSearchCustomCallback(query_point, resultSet, searchParams);
1285 |     if (searchParams.sorted)
1286 |       std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter());
1287 |     return nFound;
1288 |   }
1289 | 
1290 |   /**
1291 |    * Just like radiusSearch() but with a custom callback class for each point
1292 |    * found in the radius of the query. See the source of RadiusResultSet<> as a
1293 |    * start point for your own classes. \sa radiusSearch
1294 |    */
1295 |   template <class SEARCH_CALLBACK>
1296 |   size_t radiusSearchCustomCallback(
1297 |       const ElementType *query_point, SEARCH_CALLBACK &resultSet,
1298 |       const SearchParams &searchParams = SearchParams()) const {
1299 |     this->findNeighbors(resultSet, query_point, searchParams);
1300 |     return resultSet.size();
1301 |   }
1302 | 
1303 |   /** @} */
1304 | 
1305 | public:
1306 |   /** Make sure the auxiliary list \a vind has the same size than the current
1307 |    * dataset, and re-generate if size has changed. */
1308 |   void init_vind() {
1309 |     // Create a permutable array of indices to the input vectors.
1310 |     BaseClassRef::m_size = dataset.kdtree_get_point_count();
1311 |     if (BaseClassRef::vind.size() != BaseClassRef::m_size)
1312 |       BaseClassRef::vind.resize(BaseClassRef::m_size);
1313 |     for (size_t i = 0; i < BaseClassRef::m_size; i++)
1314 |       BaseClassRef::vind[i] = i;
1315 |   }
1316 | 
1317 |   void computeBoundingBox(BoundingBox &bbox) {
1318 |     resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim));
1319 |     if (dataset.kdtree_get_bbox(bbox)) {
1320 |       // Done! It was implemented in derived class
1321 |     } else {
1322 |       const size_t N = dataset.kdtree_get_point_count();
1323 |       if (!N)
1324 |         throw std::runtime_error("[nanoflann] computeBoundingBox() called but "
1325 |                                  "no data points found.");
1326 |       for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
1327 |         bbox[i].low = bbox[i].high = this->dataset_get(*this, 0, i);
1328 |       }
1329 |       for (size_t k = 1; k < N; ++k) {
1330 |         for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
1331 |           if (this->dataset_get(*this, k, i) < bbox[i].low)
1332 |             bbox[i].low = this->dataset_get(*this, k, i);
1333 |           if (this->dataset_get(*this, k, i) > bbox[i].high)
1334 |             bbox[i].high = this->dataset_get(*this, k, i);
1335 |         }
1336 |       }
1337 |     }
1338 |   }
1339 | 
1340 |   /**
1341 |    * Performs an exact search in the tree starting from a node.
1342 |    * \tparam RESULTSET Should be any ResultSet<DistanceType>
1343 |    * \return true if the search should be continued, false if the results are
1344 |    * sufficient
1345 |    */
1346 |   template <class RESULTSET>
1347 |   bool searchLevel(RESULTSET &result_set, const ElementType *vec,
1348 |                    const NodePtr node, DistanceType mindistsq,
1349 |                    distance_vector_t &dists, const float epsError) const {
1350 |     /* If this is a leaf node, then do check and return. */
1351 |     if ((node->child1 == NULL) && (node->child2 == NULL)) {
1352 |       // count_leaf += (node->lr.right-node->lr.left);  // Removed since was
1353 |       // neither used nor returned to the user.
1354 |       DistanceType worst_dist = result_set.worstDist();
1355 |       for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right;
1356 |            ++i) {
1357 |         const IndexType index = BaseClassRef::vind[i]; // reorder... : i;
1358 |         DistanceType dist = distance.evalMetric(
1359 |             vec, index, (DIM > 0 ? DIM : BaseClassRef::dim));
1360 |         if (dist < worst_dist) {
1361 |           if (!result_set.addPoint(dist, BaseClassRef::vind[i])) {
1362 |             // the resultset doesn't want to receive any more points, we're done
1363 |             // searching!
1364 |             return false;
1365 |           }
1366 |         }
1367 |       }
1368 |       return true;
1369 |     }
1370 | 
1371 |     /* Which child branch should be taken first? */
1372 |     int idx = node->node_type.sub.divfeat;
1373 |     ElementType val = vec[idx];
1374 |     DistanceType diff1 = val - node->node_type.sub.divlow;
1375 |     DistanceType diff2 = val - node->node_type.sub.divhigh;
1376 | 
1377 |     NodePtr bestChild;
1378 |     NodePtr otherChild;
1379 |     DistanceType cut_dist;
1380 |     if ((diff1 + diff2) < 0) {
1381 |       bestChild = node->child1;
1382 |       otherChild = node->child2;
1383 |       cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx);
1384 |     } else {
1385 |       bestChild = node->child2;
1386 |       otherChild = node->child1;
1387 |       cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx);
1388 |     }
1389 | 
1390 |     /* Call recursively to search next level down. */
1391 |     if (!searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError)) {
1392 |       // the resultset doesn't want to receive any more points, we're done
1393 |       // searching!
1394 |       return false;
1395 |     }
1396 | 
1397 |     DistanceType dst = dists[idx];
1398 |     mindistsq = mindistsq + cut_dist - dst;
1399 |     dists[idx] = cut_dist;
1400 |     if (mindistsq * epsError <= result_set.worstDist()) {
1401 |       if (!searchLevel(result_set, vec, otherChild, mindistsq, dists,
1402 |                        epsError)) {
1403 |         // the resultset doesn't want to receive any more points, we're done
1404 |         // searching!
1405 |         return false;
1406 |       }
1407 |     }
1408 |     dists[idx] = dst;
1409 |     return true;
1410 |   }
1411 | 
1412 | public:
1413 |   /**  Stores the index in a binary file.
1414 |    *   IMPORTANT NOTE: The set of data points is NOT stored in the file, so when
1415 |    * loading the index object it must be constructed associated to the same
1416 |    * source of data points used while building it. See the example:
1417 |    * examples/saveload_example.cpp \sa loadIndex  */
1418 |   void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); }
1419 | 
1420 |   /**  Loads a previous index from a binary file.
1421 |    *   IMPORTANT NOTE: The set of data points is NOT stored in the file, so the
1422 |    * index object must be constructed associated to the same source of data
1423 |    * points used while building the index. See the example:
1424 |    * examples/saveload_example.cpp \sa loadIndex  */
1425 |   void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); }
1426 | 
1427 | }; // class KDTree
1428 | 
1429 | /** kd-tree dynamic index
1430 |  *
1431 |  * Contains the k-d trees and other information for indexing a set of points
1432 |  * for nearest-neighbor matching.
1433 |  *
1434 |  *  The class "DatasetAdaptor" must provide the following interface (can be
1435 |  * non-virtual, inlined methods):
1436 |  *
1437 |  *  \code
1438 |  *   // Must return the number of data poins
1439 |  *   inline size_t kdtree_get_point_count() const { ... }
1440 |  *
1441 |  *   // Must return the dim'th component of the idx'th point in the class:
1442 |  *   inline T kdtree_get_pt(const size_t idx, const size_t dim) const { ... }
1443 |  *
1444 |  *   // Optional bounding-box computation: return false to default to a standard
1445 |  * bbox computation loop.
1446 |  *   //   Return true if the BBOX was already computed by the class and returned
1447 |  * in "bb" so it can be avoided to redo it again.
1448 |  *   //   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3
1449 |  * for point clouds) template <class BBOX> bool kdtree_get_bbox(BBOX &bb) const
1450 |  *   {
1451 |  *      bb[0].low = ...; bb[0].high = ...;  // 0th dimension limits
1452 |  *      bb[1].low = ...; bb[1].high = ...;  // 1st dimension limits
1453 |  *      ...
1454 |  *      return true;
1455 |  *   }
1456 |  *
1457 |  *  \endcode
1458 |  *
1459 |  * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
1460 |  * \tparam Distance The distance metric to use: nanoflann::metric_L1,
1461 |  * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM
1462 |  * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will
1463 |  * be typically size_t or int
1464 |  */
1465 | template <typename Distance, class DatasetAdaptor, int DIM = -1,
1466 |           typename IndexType = size_t>
1467 | class KDTreeSingleIndexDynamicAdaptor_
1468 |     : public KDTreeBaseClass<KDTreeSingleIndexDynamicAdaptor_<
1469 |                                  Distance, DatasetAdaptor, DIM, IndexType>,
1470 |                              Distance, DatasetAdaptor, DIM, IndexType> {
1471 | public:
1472 |   /**
1473 |    * The dataset used by this index
1474 |    */
1475 |   const DatasetAdaptor &dataset; //!< The source of our data
1476 | 
1477 |   KDTreeSingleIndexAdaptorParams index_params;
1478 | 
1479 |   std::vector<int> &treeIndex;
1480 | 
1481 |   Distance distance;
1482 | 
1483 |   typedef typename nanoflann::KDTreeBaseClass<
1484 |       nanoflann::KDTreeSingleIndexDynamicAdaptor_<Distance, DatasetAdaptor, DIM,
1485 |                                                   IndexType>,
1486 |       Distance, DatasetAdaptor, DIM, IndexType>
1487 |       BaseClassRef;
1488 | 
1489 |   typedef typename BaseClassRef::ElementType ElementType;
1490 |   typedef typename BaseClassRef::DistanceType DistanceType;
1491 | 
1492 |   typedef typename BaseClassRef::Node Node;
1493 |   typedef Node *NodePtr;
1494 | 
1495 |   typedef typename BaseClassRef::Interval Interval;
1496 |   /** Define "BoundingBox" as a fixed-size or variable-size container depending
1497 |    * on "DIM" */
1498 |   typedef typename BaseClassRef::BoundingBox BoundingBox;
1499 | 
1500 |   /** Define "distance_vector_t" as a fixed-size or variable-size container
1501 |    * depending on "DIM" */
1502 |   typedef typename BaseClassRef::distance_vector_t distance_vector_t;
1503 | 
1504 |   /**
1505 |    * KDTree constructor
1506 |    *
1507 |    * Refer to docs in README.md or online in
1508 |    * https://github.com/jlblancoc/nanoflann
1509 |    *
1510 |    * The KD-Tree point dimension (the length of each point in the datase, e.g. 3
1511 |    * for 3D points) is determined by means of:
1512 |    *  - The \a DIM template parameter if >0 (highest priority)
1513 |    *  - Otherwise, the \a dimensionality parameter of this constructor.
1514 |    *
1515 |    * @param inputData Dataset with the input features
1516 |    * @param params Basically, the maximum leaf node size
1517 |    */
1518 |   KDTreeSingleIndexDynamicAdaptor_(
1519 |       const int dimensionality, const DatasetAdaptor &inputData,
1520 |       std::vector<int> &treeIndex_,
1521 |       const KDTreeSingleIndexAdaptorParams &params =
1522 |           KDTreeSingleIndexAdaptorParams())
1523 |       : dataset(inputData), index_params(params), treeIndex(treeIndex_),
1524 |         distance(inputData) {
1525 |     BaseClassRef::root_node = NULL;
1526 |     BaseClassRef::m_size = 0;
1527 |     BaseClassRef::m_size_at_index_build = 0;
1528 |     BaseClassRef::dim = dimensionality;
1529 |     if (DIM > 0)
1530 |       BaseClassRef::dim = DIM;
1531 |     BaseClassRef::m_leaf_max_size = params.leaf_max_size;
1532 |   }
1533 | 
1534 |   /** Assignment operator definiton */
1535 |   KDTreeSingleIndexDynamicAdaptor_
1536 |   operator=(const KDTreeSingleIndexDynamicAdaptor_ &rhs) {
1537 |     KDTreeSingleIndexDynamicAdaptor_ tmp(rhs);
1538 |     std::swap(BaseClassRef::vind, tmp.BaseClassRef::vind);
1539 |     std::swap(BaseClassRef::m_leaf_max_size, tmp.BaseClassRef::m_leaf_max_size);
1540 |     std::swap(index_params, tmp.index_params);
1541 |     std::swap(treeIndex, tmp.treeIndex);
1542 |     std::swap(BaseClassRef::m_size, tmp.BaseClassRef::m_size);
1543 |     std::swap(BaseClassRef::m_size_at_index_build,
1544 |               tmp.BaseClassRef::m_size_at_index_build);
1545 |     std::swap(BaseClassRef::root_node, tmp.BaseClassRef::root_node);
1546 |     std::swap(BaseClassRef::root_bbox, tmp.BaseClassRef::root_bbox);
1547 |     std::swap(BaseClassRef::pool, tmp.BaseClassRef::pool);
1548 |     return *this;
1549 |   }
1550 | 
1551 |   /**
1552 |    * Builds the index
1553 |    */
1554 |   void buildIndex() {
1555 |     BaseClassRef::m_size = BaseClassRef::vind.size();
1556 |     this->freeIndex(*this);
1557 |     BaseClassRef::m_size_at_index_build = BaseClassRef::m_size;
1558 |     if (BaseClassRef::m_size == 0)
1559 |       return;
1560 |     computeBoundingBox(BaseClassRef::root_bbox);
1561 |     BaseClassRef::root_node =
1562 |         this->divideTree(*this, 0, BaseClassRef::m_size,
1563 |                          BaseClassRef::root_bbox); // construct the tree
1564 |   }
1565 | 
1566 |   /** \name Query methods
1567 |    * @{ */
1568 | 
1569 |   /**
1570 |    * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored
1571 |    * inside the result object.
1572 |    *
1573 |    * Params:
1574 |    *     result = the result object in which the indices of the
1575 |    * nearest-neighbors are stored vec = the vector for which to search the
1576 |    * nearest neighbors
1577 |    *
1578 |    * \tparam RESULTSET Should be any ResultSet<DistanceType>
1579 |    * \return  True if the requested neighbors could be found.
1580 |    * \sa knnSearch, radiusSearch
1581 |    */
1582 |   template <typename RESULTSET>
1583 |   bool findNeighbors(RESULTSET &result, const ElementType *vec,
1584 |                      const SearchParams &searchParams) const {
1585 |     assert(vec);
1586 |     if (this->size(*this) == 0)
1587 |       return false;
1588 |     if (!BaseClassRef::root_node)
1589 |       return false;
1590 |     float epsError = 1 + searchParams.eps;
1591 | 
1592 |     // fixed or variable-sized container (depending on DIM)
1593 |     distance_vector_t dists;
1594 |     // Fill it with zeros.
1595 |     assign(dists, (DIM > 0 ? DIM : BaseClassRef::dim),
1596 |            static_cast<typename distance_vector_t::value_type>(0));
1597 |     DistanceType distsq = this->computeInitialDistances(*this, vec, dists);
1598 |     searchLevel(result, vec, BaseClassRef::root_node, distsq, dists,
1599 |                 epsError); // "count_leaf" parameter removed since was neither
1600 |                            // used nor returned to the user.
1601 |     return result.full();
1602 |   }
1603 | 
1604 |   /**
1605 |    * Find the "num_closest" nearest neighbors to the \a query_point[0:dim-1].
1606 |    * Their indices are stored inside the result object. \sa radiusSearch,
1607 |    * findNeighbors \note nChecks_IGNORED is ignored but kept for compatibility
1608 |    * with the original FLANN interface. \return Number `N` of valid points in
1609 |    * the result set. Only the first `N` entries in `out_indices` and
1610 |    * `out_distances_sq` will be valid. Return may be less than `num_closest`
1611 |    * only if the number of elements in the tree is less than `num_closest`.
1612 |    */
1613 |   size_t knnSearch(const ElementType *query_point, const size_t num_closest,
1614 |                    IndexType *out_indices, DistanceType *out_distances_sq,
1615 |                    const int /* nChecks_IGNORED */ = 10) const {
1616 |     nanoflann::KNNResultSet<DistanceType, IndexType> resultSet(num_closest);
1617 |     resultSet.init(out_indices, out_distances_sq);
1618 |     this->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
1619 |     return resultSet.size();
1620 |   }
1621 | 
1622 |   /**
1623 |    * Find all the neighbors to \a query_point[0:dim-1] within a maximum radius.
1624 |    *  The output is given as a vector of pairs, of which the first element is a
1625 |    * point index and the second the corresponding distance. Previous contents of
1626 |    * \a IndicesDists are cleared.
1627 |    *
1628 |    *  If searchParams.sorted==true, the output list is sorted by ascending
1629 |    * distances.
1630 |    *
1631 |    *  For a better performance, it is advisable to do a .reserve() on the vector
1632 |    * if you have any wild guess about the number of expected matches.
1633 |    *
1634 |    *  \sa knnSearch, findNeighbors, radiusSearchCustomCallback
1635 |    * \return The number of points within the given radius (i.e. indices.size()
1636 |    * or dists.size() )
1637 |    */
1638 |   size_t
1639 |   radiusSearch(const ElementType *query_point, const DistanceType &radius,
1640 |                std::vector<std::pair<IndexType, DistanceType>> &IndicesDists,
1641 |                const SearchParams &searchParams) const {
1642 |     RadiusResultSet<DistanceType, IndexType> resultSet(radius, IndicesDists);
1643 |     const size_t nFound =
1644 |         radiusSearchCustomCallback(query_point, resultSet, searchParams);
1645 |     if (searchParams.sorted)
1646 |       std::sort(IndicesDists.begin(), IndicesDists.end(), IndexDist_Sorter());
1647 |     return nFound;
1648 |   }
1649 | 
1650 |   /**
1651 |    * Just like radiusSearch() but with a custom callback class for each point
1652 |    * found in the radius of the query. See the source of RadiusResultSet<> as a
1653 |    * start point for your own classes. \sa radiusSearch
1654 |    */
1655 |   template <class SEARCH_CALLBACK>
1656 |   size_t radiusSearchCustomCallback(
1657 |       const ElementType *query_point, SEARCH_CALLBACK &resultSet,
1658 |       const SearchParams &searchParams = SearchParams()) const {
1659 |     this->findNeighbors(resultSet, query_point, searchParams);
1660 |     return resultSet.size();
1661 |   }
1662 | 
1663 |   /** @} */
1664 | 
1665 | public:
1666 |   void computeBoundingBox(BoundingBox &bbox) {
1667 |     resize(bbox, (DIM > 0 ? DIM : BaseClassRef::dim));
1668 | 
1669 |     if (dataset.kdtree_get_bbox(bbox)) {
1670 |       // Done! It was implemented in derived class
1671 |     } else {
1672 |       const size_t N = BaseClassRef::m_size;
1673 |       if (!N)
1674 |         throw std::runtime_error("[nanoflann] computeBoundingBox() called but "
1675 |                                  "no data points found.");
1676 |       for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
1677 |         bbox[i].low = bbox[i].high =
1678 |             this->dataset_get(*this, BaseClassRef::vind[0], i);
1679 |       }
1680 |       for (size_t k = 1; k < N; ++k) {
1681 |         for (int i = 0; i < (DIM > 0 ? DIM : BaseClassRef::dim); ++i) {
1682 |           if (this->dataset_get(*this, BaseClassRef::vind[k], i) < bbox[i].low)
1683 |             bbox[i].low = this->dataset_get(*this, BaseClassRef::vind[k], i);
1684 |           if (this->dataset_get(*this, BaseClassRef::vind[k], i) > bbox[i].high)
1685 |             bbox[i].high = this->dataset_get(*this, BaseClassRef::vind[k], i);
1686 |         }
1687 |       }
1688 |     }
1689 |   }
1690 | 
1691 |   /**
1692 |    * Performs an exact search in the tree starting from a node.
1693 |    * \tparam RESULTSET Should be any ResultSet<DistanceType>
1694 |    */
1695 |   template <class RESULTSET>
1696 |   void searchLevel(RESULTSET &result_set, const ElementType *vec,
1697 |                    const NodePtr node, DistanceType mindistsq,
1698 |                    distance_vector_t &dists, const float epsError) const {
1699 |     /* If this is a leaf node, then do check and return. */
1700 |     if ((node->child1 == NULL) && (node->child2 == NULL)) {
1701 |       // count_leaf += (node->lr.right-node->lr.left);  // Removed since was
1702 |       // neither used nor returned to the user.
1703 |       DistanceType worst_dist = result_set.worstDist();
1704 |       for (IndexType i = node->node_type.lr.left; i < node->node_type.lr.right;
1705 |            ++i) {
1706 |         const IndexType index = BaseClassRef::vind[i]; // reorder... : i;
1707 |         if (treeIndex[index] == -1)
1708 |           continue;
1709 |         DistanceType dist = distance.evalMetric(
1710 |             vec, index, (DIM > 0 ? DIM : BaseClassRef::dim));
1711 |         if (dist < worst_dist) {
1712 |           if (!result_set.addPoint(
1713 |                   static_cast<typename RESULTSET::DistanceType>(dist),
1714 |                   static_cast<typename RESULTSET::IndexType>(
1715 |                       BaseClassRef::vind[i]))) {
1716 |             // the resultset doesn't want to receive any more points, we're done
1717 |             // searching!
1718 |             return; // false;
1719 |           }
1720 |         }
1721 |       }
1722 |       return;
1723 |     }
1724 | 
1725 |     /* Which child branch should be taken first? */
1726 |     int idx = node->node_type.sub.divfeat;
1727 |     ElementType val = vec[idx];
1728 |     DistanceType diff1 = val - node->node_type.sub.divlow;
1729 |     DistanceType diff2 = val - node->node_type.sub.divhigh;
1730 | 
1731 |     NodePtr bestChild;
1732 |     NodePtr otherChild;
1733 |     DistanceType cut_dist;
1734 |     if ((diff1 + diff2) < 0) {
1735 |       bestChild = node->child1;
1736 |       otherChild = node->child2;
1737 |       cut_dist = distance.accum_dist(val, node->node_type.sub.divhigh, idx);
1738 |     } else {
1739 |       bestChild = node->child2;
1740 |       otherChild = node->child1;
1741 |       cut_dist = distance.accum_dist(val, node->node_type.sub.divlow, idx);
1742 |     }
1743 | 
1744 |     /* Call recursively to search next level down. */
1745 |     searchLevel(result_set, vec, bestChild, mindistsq, dists, epsError);
1746 | 
1747 |     DistanceType dst = dists[idx];
1748 |     mindistsq = mindistsq + cut_dist - dst;
1749 |     dists[idx] = cut_dist;
1750 |     if (mindistsq * epsError <= result_set.worstDist()) {
1751 |       searchLevel(result_set, vec, otherChild, mindistsq, dists, epsError);
1752 |     }
1753 |     dists[idx] = dst;
1754 |   }
1755 | 
1756 | public:
1757 |   /**  Stores the index in a binary file.
1758 |    *   IMPORTANT NOTE: The set of data points is NOT stored in the file, so when
1759 |    * loading the index object it must be constructed associated to the same
1760 |    * source of data points used while building it. See the example:
1761 |    * examples/saveload_example.cpp \sa loadIndex  */
1762 |   void saveIndex(FILE *stream) { this->saveIndex_(*this, stream); }
1763 | 
1764 |   /**  Loads a previous index from a binary file.
1765 |    *   IMPORTANT NOTE: The set of data points is NOT stored in the file, so the
1766 |    * index object must be constructed associated to the same source of data
1767 |    * points used while building the index. See the example:
1768 |    * examples/saveload_example.cpp \sa loadIndex  */
1769 |   void loadIndex(FILE *stream) { this->loadIndex_(*this, stream); }
1770 | };
1771 | 
1772 | /** kd-tree dynaimic index
1773 |  *
1774 |  * class to create multiple static index and merge their results to behave as
1775 |  * single dynamic index as proposed in Logarithmic Approach.
1776 |  *
1777 |  *  Example of usage:
1778 |  *  examples/dynamic_pointcloud_example.cpp
1779 |  *
1780 |  * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
1781 |  * \tparam Distance The distance metric to use: nanoflann::metric_L1,
1782 |  * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc. \tparam DIM
1783 |  * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will
1784 |  * be typically size_t or int
1785 |  */
1786 | template <typename Distance, class DatasetAdaptor, int DIM = -1,
1787 |           typename IndexType = size_t>
1788 | class KDTreeSingleIndexDynamicAdaptor {
1789 | public:
1790 |   typedef typename Distance::ElementType ElementType;
1791 |   typedef typename Distance::DistanceType DistanceType;
1792 | 
1793 | protected:
1794 |   size_t m_leaf_max_size;
1795 |   size_t treeCount;
1796 |   size_t pointCount;
1797 | 
1798 |   /**
1799 |    * The dataset used by this index
1800 |    */
1801 |   const DatasetAdaptor &dataset; //!< The source of our data
1802 | 
1803 |   std::vector<int> treeIndex; //!< treeIndex[idx] is the index of tree in which
1804 |                               //!< point at idx is stored. treeIndex[idx]=-1
1805 |                               //!< means that point has been removed.
1806 | 
1807 |   KDTreeSingleIndexAdaptorParams index_params;
1808 | 
1809 |   int dim; //!< Dimensionality of each data point
1810 | 
1811 |   typedef KDTreeSingleIndexDynamicAdaptor_<Distance, DatasetAdaptor, DIM>
1812 |       index_container_t;
1813 |   std::vector<index_container_t> index;
1814 | 
1815 | public:
1816 |   /** Get a const ref to the internal list of indices; the number of indices is
1817 |    * adapted dynamically as the dataset grows in size. */
1818 |   const std::vector<index_container_t> &getAllIndices() const { return index; }
1819 | 
1820 | private:
1821 |   /** finds position of least significant unset bit */
1822 |   int First0Bit(IndexType num) {
1823 |     int pos = 0;
1824 |     while (num & 1) {
1825 |       num = num >> 1;
1826 |       pos++;
1827 |     }
1828 |     return pos;
1829 |   }
1830 | 
1831 |   /** Creates multiple empty trees to handle dynamic support */
1832 |   void init() {
1833 |     typedef KDTreeSingleIndexDynamicAdaptor_<Distance, DatasetAdaptor, DIM>
1834 |         my_kd_tree_t;
1835 |     std::vector<my_kd_tree_t> index_(
1836 |         treeCount, my_kd_tree_t(dim /*dim*/, dataset, treeIndex, index_params));
1837 |     index = index_;
1838 |   }
1839 | 
1840 | public:
1841 |   Distance distance;
1842 | 
1843 |   /**
1844 |    * KDTree constructor
1845 |    *
1846 |    * Refer to docs in README.md or online in
1847 |    * https://github.com/jlblancoc/nanoflann
1848 |    *
1849 |    * The KD-Tree point dimension (the length of each point in the datase, e.g. 3
1850 |    * for 3D points) is determined by means of:
1851 |    *  - The \a DIM template parameter if >0 (highest priority)
1852 |    *  - Otherwise, the \a dimensionality parameter of this constructor.
1853 |    *
1854 |    * @param inputData Dataset with the input features
1855 |    * @param params Basically, the maximum leaf node size
1856 |    */
1857 |   KDTreeSingleIndexDynamicAdaptor(const int dimensionality,
1858 |                                   const DatasetAdaptor &inputData,
1859 |                                   const KDTreeSingleIndexAdaptorParams &params =
1860 |                                       KDTreeSingleIndexAdaptorParams(),
1861 |                                   const size_t maximumPointCount = 1000000000U)
1862 |       : dataset(inputData), index_params(params), distance(inputData) {
1863 |     treeCount = static_cast<size_t>(std::log2(maximumPointCount));
1864 |     pointCount = 0U;
1865 |     dim = dimensionality;
1866 |     treeIndex.clear();
1867 |     if (DIM > 0)
1868 |       dim = DIM;
1869 |     m_leaf_max_size = params.leaf_max_size;
1870 |     init();
1871 |     const size_t num_initial_points = dataset.kdtree_get_point_count();
1872 |     if (num_initial_points > 0) {
1873 |       addPoints(0, num_initial_points - 1);
1874 |     }
1875 |   }
1876 | 
1877 |   /** Deleted copy constructor*/
1878 |   KDTreeSingleIndexDynamicAdaptor(
1879 |       const KDTreeSingleIndexDynamicAdaptor<Distance, DatasetAdaptor, DIM,
1880 |                                             IndexType> &) = delete;
1881 | 
1882 |   /** Add points to the set, Inserts all points from [start, end] */
1883 |   void addPoints(IndexType start, IndexType end) {
1884 |     size_t count = end - start + 1;
1885 |     treeIndex.resize(treeIndex.size() + count);
1886 |     for (IndexType idx = start; idx <= end; idx++) {
1887 |       int pos = First0Bit(pointCount);
1888 |       index[pos].vind.clear();
1889 |       treeIndex[pointCount] = pos;
1890 |       for (int i = 0; i < pos; i++) {
1891 |         for (int j = 0; j < static_cast<int>(index[i].vind.size()); j++) {
1892 |           index[pos].vind.push_back(index[i].vind[j]);
1893 |           if (treeIndex[index[i].vind[j]] != -1)
1894 |             treeIndex[index[i].vind[j]] = pos;
1895 |         }
1896 |         index[i].vind.clear();
1897 |         index[i].freeIndex(index[i]);
1898 |       }
1899 |       index[pos].vind.push_back(idx);
1900 |       index[pos].buildIndex();
1901 |       pointCount++;
1902 |     }
1903 |   }
1904 | 
1905 |   /** Remove a point from the set (Lazy Deletion) */
1906 |   void removePoint(size_t idx) {
1907 |     if (idx >= pointCount)
1908 |       return;
1909 |     treeIndex[idx] = -1;
1910 |   }
1911 | 
1912 |   /**
1913 |    * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored
1914 |    * inside the result object.
1915 |    *
1916 |    * Params:
1917 |    *     result = the result object in which the indices of the
1918 |    * nearest-neighbors are stored vec = the vector for which to search the
1919 |    * nearest neighbors
1920 |    *
1921 |    * \tparam RESULTSET Should be any ResultSet<DistanceType>
1922 |    * \return  True if the requested neighbors could be found.
1923 |    * \sa knnSearch, radiusSearch
1924 |    */
1925 |   template <typename RESULTSET>
1926 |   bool findNeighbors(RESULTSET &result, const ElementType *vec,
1927 |                      const SearchParams &searchParams) const {
1928 |     for (size_t i = 0; i < treeCount; i++) {
1929 |       index[i].findNeighbors(result, &vec[0], searchParams);
1930 |     }
1931 |     return result.full();
1932 |   }
1933 | };
1934 | 
1935 | /** An L2-metric KD-tree adaptor for working with data directly stored in an
1936 |  * Eigen Matrix, without duplicating the data storage. Each row in the matrix
1937 |  * represents a point in the state space.
1938 |  *
1939 |  *  Example of usage:
1940 |  * \code
1941 |  * 	Eigen::Matrix<num_t,Dynamic,Dynamic>  mat;
1942 |  * 	// Fill out "mat"...
1943 |  *
1944 |  * 	typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix<num_t,Dynamic,Dynamic> >
1945 |  * my_kd_tree_t; const int max_leaf = 10; my_kd_tree_t   mat_index(mat, max_leaf
1946 |  * ); mat_index.index->buildIndex(); mat_index.index->... \endcode
1947 |  *
1948 |  *  \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality
1949 |  * for the points in the data set, allowing more compiler optimizations. \tparam
1950 |  * Distance The distance metric to use: nanoflann::metric_L1,
1951 |  * nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
1952 |  */
1953 | template <class MatrixType, int DIM = -1, class Distance = nanoflann::metric_L2>
1954 | struct KDTreeEigenMatrixAdaptor {
1955 |   typedef KDTreeEigenMatrixAdaptor<MatrixType, DIM, Distance> self_t;
1956 |   typedef typename MatrixType::Scalar num_t;
1957 |   typedef typename MatrixType::Index IndexType;
1958 |   typedef
1959 |       typename Distance::template traits<num_t, self_t>::distance_t metric_t;
1960 |   typedef KDTreeSingleIndexAdaptor<metric_t, self_t,
1961 |                                    MatrixType::ColsAtCompileTime, IndexType>
1962 |       index_t;
1963 | 
1964 |   index_t *index; //! The kd-tree index for the user to call its methods as
1965 |                   //! usual with any other FLANN index.
1966 | 
1967 |   /// Constructor: takes a const ref to the matrix object with the data points
1968 |   KDTreeEigenMatrixAdaptor(const size_t dimensionality,
1969 |                            const std::reference_wrapper<const MatrixType> &mat,
1970 |                            const int leaf_max_size = 10)
1971 |       : m_data_matrix(mat) {
1972 |     const auto dims = mat.get().cols();
1973 |     if (size_t(dims) != dimensionality)
1974 |       throw std::runtime_error(
1975 |           "Error: 'dimensionality' must match column count in data matrix");
1976 |     if (DIM > 0 && int(dims) != DIM)
1977 |       throw std::runtime_error(
1978 |           "Data set dimensionality does not match the 'DIM' template argument");
1979 |     index =
1980 |         new index_t(static_cast<int>(dims), *this /* adaptor */,
1981 |                     nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size));
1982 |     index->buildIndex();
1983 |   }
1984 | 
1985 | public:
1986 |   /** Deleted copy constructor */
1987 |   KDTreeEigenMatrixAdaptor(const self_t &) = delete;
1988 | 
1989 |   ~KDTreeEigenMatrixAdaptor() { delete index; }
1990 | 
1991 |   const std::reference_wrapper<const MatrixType> m_data_matrix;
1992 | 
1993 |   /** Query for the \a num_closest closest points to a given point (entered as
1994 |    * query_point[0:dim-1]). Note that this is a short-cut method for
1995 |    * index->findNeighbors(). The user can also call index->... methods as
1996 |    * desired. \note nChecks_IGNORED is ignored but kept for compatibility with
1997 |    * the original FLANN interface.
1998 |    */
1999 |   inline void query(const num_t *query_point, const size_t num_closest,
2000 |                     IndexType *out_indices, num_t *out_distances_sq,
2001 |                     const int /* nChecks_IGNORED */ = 10) const {
2002 |     nanoflann::KNNResultSet<num_t, IndexType> resultSet(num_closest);
2003 |     resultSet.init(out_indices, out_distances_sq);
2004 |     index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
2005 |   }
2006 | 
2007 |   /** @name Interface expected by KDTreeSingleIndexAdaptor
2008 |    * @{ */
2009 | 
2010 |   const self_t &derived() const { return *this; }
2011 |   self_t &derived() { return *this; }
2012 | 
2013 |   // Must return the number of data points
2014 |   inline size_t kdtree_get_point_count() const {
2015 |     return m_data_matrix.get().rows();
2016 |   }
2017 | 
2018 |   // Returns the dim'th component of the idx'th point in the class:
2019 |   inline num_t kdtree_get_pt(const IndexType idx, size_t dim) const {
2020 |     return m_data_matrix.get().coeff(idx, IndexType(dim));
2021 |   }
2022 | 
2023 |   // Optional bounding-box computation: return false to default to a standard
2024 |   // bbox computation loop.
2025 |   //   Return true if the BBOX was already computed by the class and returned in
2026 |   //   "bb" so it can be avoided to redo it again. Look at bb.size() to find out
2027 |   //   the expected dimensionality (e.g. 2 or 3 for point clouds)
2028 |   template <class BBOX> bool kdtree_get_bbox(BBOX & /*bb*/) const {
2029 |     return false;
2030 |   }
2031 | 
2032 |   /** @} */
2033 | 
2034 | }; // end of KDTreeEigenMatrixAdaptor
2035 |    /** @} */
2036 | 
2037 | /** @} */ // end of grouping
2038 | } // namespace nanoflann
2039 | 
2040 | #endif /* NANOFLANN_HPP_ */
2041 | 


--------------------------------------------------------------------------------
/Scancontext/tictoc.h:
--------------------------------------------------------------------------------
 1 | // Author:   Tong Qin               qintonguav@gmail.com
 2 | // 	         Shaozu Cao 		    saozu.cao@connect.ust.hk
 3 | 
 4 | #pragma once
 5 | 
 6 | #include <ctime>
 7 | #include <iostream>
 8 | #include <string>
 9 | #include <cstdlib>
10 | #include <chrono>
11 | 
12 | class TicToc
13 | {
14 | public:
15 |     TicToc()
16 |     {
17 |         tic();
18 |     }
19 | 
20 |     TicToc( bool _disp )
21 |     {
22 |         disp_ = _disp;
23 |         tic();
24 |     }
25 | 
26 |     void tic()
27 |     {
28 |         start = std::chrono::system_clock::now();
29 |     }
30 | 
31 |     void toc( std::string _about_task )
32 |     {
33 |         end = std::chrono::system_clock::now();
34 |         std::chrono::duration<double> elapsed_seconds = end - start;
35 |         double elapsed_ms = elapsed_seconds.count() * 1000;
36 | 
37 |         if( disp_ )
38 |         {
39 |           std::cout.precision(3); // 10 for sec, 3 for ms 
40 |           std::cout << _about_task << ": " << elapsed_ms << " msec." << std::endl;
41 |         }
42 |     }
43 | 
44 | private:  
45 |     std::chrono::time_point<std::chrono::system_clock> start, end;
46 |     bool disp_ = false;
47 | };
48 | 


--------------------------------------------------------------------------------
/config/sc_config.yaml:
--------------------------------------------------------------------------------
1 | %YAML:1.0
2 | 
3 | LeafSize: 0.1


--------------------------------------------------------------------------------
/img/loop.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Freeecode/loopclosure_for_aloam/1a0d70cde6787b476a513ac7dbf0cb803e144ee5/img/loop.png


--------------------------------------------------------------------------------
/img/origin.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Freeecode/loopclosure_for_aloam/1a0d70cde6787b476a513ac7dbf0cb803e144ee5/img/origin.png


--------------------------------------------------------------------------------
/launch/sc_optimization.launch:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <launch>
 3 |     <arg name="use_feature"     default="false"/>
 4 |     <arg name="config_file"     default="$(find sc_aloam)/config/sc_config.yaml"/>
 5 | 
 6 |     <node pkg="sc_aloam" type="loopclosure_aloam" name="sc_optimization" output="screen" args="$(arg use_feature)">
 7 | 
 8 |         <param name="config_file"     type="string"        value="$(arg config_file)"/>
 9 |         <param name="use_feature"     type="bool"          value="$(arg use_feature)"/>
10 |         <param name="leaf_size"                            value="0.4"/>
11 |         <remap from="/velodyne_points"               to="/velodyne_cloud_2"/>
12 |         <remap from="/odom"                          to="/aft_mapped_to_init"/>
13 |         <!--rosparam file="$(find sc_loam)/config/sc_config.yaml"/-->
14 |         <!-- <remap from="/laser_cloud_corner"       to=""/> -->
15 |         <!-- <remap from="/laser_cloud_surf"         to=""/> -->
16 |         
17 |     </node>
18 | </launch>


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>sc_aloam</name>
 4 |   <version>0.0.0</version>
 5 | 
 6 |   <description>
 7 |     This work is based on scancontext, which provided loopclosure detection for aloam
 8 |   </description>
 9 | 
10 |   <maintainer email="z691956701@gmail.com">Houzhan Zhang</maintainer>
11 | 
12 |   <license>BSD</license>
13 |   
14 |   <buildtool_depend>catkin</buildtool_depend>
15 |   <build_depend>geometry_msgs</build_depend>
16 |   <build_depend>nav_msgs</build_depend>
17 |   <build_depend>roscpp</build_depend>
18 |   <build_depend>rospy</build_depend>
19 |   <build_depend>std_msgs</build_depend>  
20 |   <build_depend>rosbag</build_depend>
21 |   <build_depend>sensor_msgs</build_depend>
22 |   <build_depend>tf</build_depend>
23 |   <build_depend>image_transport</build_depend>
24 |   <build_depend>eigen_conversions</build_depend>
25 |   <build_depend>message_generation</build_depend>
26 |   <run_depend>message_runtime</run_depend>
27 |   <run_depend>geometry_msgs</run_depend>
28 |   <run_depend>nav_msgs</run_depend>
29 |   <run_depend>sensor_msgs</run_depend>
30 |   <run_depend>roscpp</run_depend>
31 |   <run_depend>rospy</run_depend>
32 |   <run_depend>std_msgs</run_depend>
33 |   <run_depend>rosbag</run_depend>
34 |   <run_depend>tf</run_depend>
35 |   <run_depend>eigen_conversions</run_depend>
36 |   <run_depend>image_transport</run_depend>
37 | </package>
38 | 


--------------------------------------------------------------------------------
/src/SC_Optimization_node.cpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  * @brief loop closure detection for aloam
  3 |  * 
  4 |  */
  5 | 
  6 | #include <iostream>
  7 | #include <vector>
  8 | #include <string>
  9 | #include <memory>
 10 | #include <mutex>
 11 | #include <queue>
 12 | #include <thread>
 13 | 
 14 | //PCL
 15 | #include <pcl/point_cloud.h>
 16 | #include <pcl/point_types.h>
 17 | #include <pcl/registration/icp.h>
 18 | #include <pcl/filters/filter.h>
 19 | #include <pcl/filters/voxel_grid.h>
 20 | 
 21 | //ROS
 22 | #include <ros/ros.h>
 23 | #include <std_msgs/Header.h>
 24 | #include <nav_msgs/Odometry.h>
 25 | #include <nav_msgs/Path.h>
 26 | #include <sensor_msgs/PointCloud2.h>
 27 | #include "Scancontext/Scancontext.h"
 28 | 
 29 | // GTSAM
 30 | #include <gtsam/geometry/Rot3.h>
 31 | #include <gtsam/geometry/Pose3.h>
 32 | #include <gtsam/slam/PriorFactor.h>
 33 | #include <gtsam/slam/BetweenFactor.h>
 34 | #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 35 | #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 36 | #include <gtsam/nonlinear/Marginals.h>
 37 | #include <gtsam/nonlinear/Values.h>
 38 | #include <gtsam/inference/Symbol.h>
 39 | 
 40 | ros::Publisher  pub_path;
 41 | 
 42 | // Buffer
 43 | std::queue<sensor_msgs::PointCloud2ConstPtr> fullPointsBuf;
 44 | std::queue<nav_msgs::OdometryConstPtr> odometryBuf;
 45 | std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> laserCloudBuf;
 46 | std::vector<std_msgs::Header> headerBuf;
 47 | 
 48 | // Mutex
 49 | std::mutex points_mutex;
 50 | std::mutex odom_mutex;
 51 | std::mutex buf_mutex;
 52 | std::mutex graph_mutex;
 53 | std::mutex flag_mutex;
 54 | 
 55 | // median pointcloud
 56 | pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudFull;
 57 | pcl::PointCloud<pcl::PointXYZI>::Ptr final;
 58 | pcl::PointCloud<pcl::PointXYZI>::Ptr tmp;
 59 | pcl::VoxelGrid<pcl::PointXYZI> downSizeFilterScancontext;
 60 | 
 61 | // Scancontext
 62 | SCManager scManager;
 63 | 
 64 | // gtsam optimization
 65 | gtsam::NonlinearFactorGraph gtSAMgraph;
 66 | gtsam::noiseModel::Diagonal::shared_ptr priorNoisemodel;
 67 | gtsam::noiseModel::Diagonal::shared_ptr odomNoisemodel;
 68 | gtsam::noiseModel::Diagonal::shared_ptr loopNoisemode;
 69 | gtsam::noiseModel::Base::shared_ptr  robustNoiseModel;
 70 | gtsam::Values initials;
 71 | gtsam::Values results;
 72 | gtsam::Pose3 lastPose;
 73 | gtsam::Pose3 currentPose;
 74 | 
 75 | std_msgs::Header header;
 76 | bool first_msg = true;
 77 | bool optimize_flag = false;
 78 | int odom_num = 0;
 79 | int laser_num = 0;
 80 | int history_id = -1;
 81 | bool zero_flag = true;
 82 | 
 83 | void init()
 84 | {
 85 |     // allocate memory
 86 |     laserCloudFull.reset(new pcl::PointCloud<pcl::PointXYZI>);
 87 |     final.reset(new pcl::PointCloud<pcl::PointXYZI>);
 88 |     tmp.reset(new pcl::PointCloud<pcl::PointXYZI>);
 89 | 
 90 |     // set voxel filter
 91 |     float filter_size = 0.4;
 92 |     downSizeFilterScancontext.setLeafSize(filter_size, filter_size, filter_size);
 93 | 
 94 |     // noise model
 95 |     //gtsam::Vector6 PriorVector;
 96 |     gtsam::Vector PriorVector(6);
 97 |     PriorVector << 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6; 
 98 |     priorNoisemodel = gtsam::noiseModel::Diagonal::Variances(PriorVector);
 99 |     gtsam::Vector6 Constraint(6);
100 |     Constraint << 0.10, 0.10, 0.10, 0.10, 0.10, 0.10;
101 |     odomNoisemodel = gtsam::noiseModel::Diagonal::Variances(Constraint);
102 |     loopNoisemode = gtsam::noiseModel::Diagonal::Variances(Constraint);
103 |     robustNoiseModel = gtsam::noiseModel::Robust::Create(
104 |                             gtsam::noiseModel::mEstimator::Cauchy::Create(1),
105 |                             loopNoisemode);
106 | }
107 | 
108 | gtsam::Pose3 msg2Pose(nav_msgs::OdometryConstPtr &msg)
109 | {
110 |     Eigen::Quaterniond q(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x,
111 |                          msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
112 |     Eigen::Vector3d t(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
113 |     
114 |     gtsam::Rot3 rot(q.w(), q.x(), q.y(), q.z());
115 |     gtsam::Point3 trans(t.x(), t.y(), t.z());
116 |     return gtsam::Pose3(rot, trans);
117 | }
118 | 
119 | gtsam::Pose3 eigen2Pose(const Eigen::Matrix4f& transform)
120 | {
121 |     Eigen::Matrix3d rotation;
122 |     for(int i = 0; i < 3; i++)
123 |         for(int j = 0; j < 3; j++)
124 |             rotation(i,j) = transform(i,j);
125 | 
126 |     Eigen::Quaterniond q(rotation);
127 |     Eigen::Vector3d t(transform(0,3), transform(1,3), transform(2,3));
128 |    
129 |     gtsam::Rot3 rot(q.w(), q.x(), q.y(), q.z());
130 |     gtsam::Point3 trans(t.x(), t.y(), t.z());
131 |     return gtsam::Pose3(rot, trans);
132 | }
133 | 
134 | void GraphOptimization()
135 | {
136 |     ros::Rate loop(1);
137 |     while (ros::ok())
138 |     {
139 |         if(optimize_flag == false)
140 |             continue;
141 |         
142 |         ROS_INFO("Start optimizing...");
143 |         results.clear();
144 | 
145 |         graph_mutex.lock();
146 |         
147 |         gtsam::LevenbergMarquardtParams param;
148 |         param.maxIterations = 60;
149 |         param.setVerbosity("TERMINATION");  // this will show info about stopping conditions
150 |         param.setVerbosity("SILENT");
151 |         gtsam::LevenbergMarquardtOptimizer optimizer(gtSAMgraph, initials, param);
152 |         results = optimizer.optimize();
153 |  
154 |         nav_msgs::Path path;
155 |         int id = 0;
156 |         for(const gtsam::Values::ConstKeyValuePair& key_value: results)
157 |         {
158 |             gtsam::Pose3 transform = key_value.value.cast<gtsam::Pose3>();
159 |             Eigen::Quaterniond q = transform.rotation().toQuaternion();
160 |             Eigen::Vector3d t = transform.translation();        
161 | 
162 |             initials.update(id, transform);
163 |             
164 |             geometry_msgs::PoseStamped posestamp;
165 |             posestamp.header = headerBuf[id];
166 |             posestamp.pose.orientation.w = q.w();
167 |             posestamp.pose.orientation.x = q.x();
168 |             posestamp.pose.orientation.y = q.y();
169 |             posestamp.pose.orientation.z = q.z();
170 |             posestamp.pose.position.x = t.x();
171 |             posestamp.pose.position.y = t.y();
172 |             posestamp.pose.position.z = t.z();
173 | 
174 |             path.poses.push_back(posestamp);
175 | 
176 |             id++;
177 |         }
178 | 
179 |         path.header = headerBuf[id-1];
180 |         pub_path.publish(path);
181 | 
182 |         graph_mutex.unlock();
183 | 
184 |         flag_mutex.lock();
185 |         optimize_flag = false;
186 |         flag_mutex.unlock();
187 | 
188 |         loop.sleep();
189 |     }
190 | }
191 | 
192 | 
193 | bool geometryVerfication(int curr_id, int match_id, gtsam::Pose3& relative)
194 | {
195 |     double residual_q = 0.0;
196 |     double residual_t = 0.0;
197 | 
198 |     gtsam::Pose3 pose1 = initials.at<gtsam::Pose3>(curr_id);
199 |     gtsam::Pose3 pose2 = initials.at<gtsam::Pose3>(match_id);
200 |     gtsam::Pose3 transform1 = pose2.between(pose1);
201 |     gtsam::Pose3 transform = transform1.between(relative);
202 |     residual_t += std::abs(transform.translation().x()) + std::abs(transform.translation().y()) + 
203 |                         std::abs(transform.translation().z());
204 |     residual_q += std::abs(transform.rotation().toQuaternion().w()-1) + std::abs(transform.rotation().toQuaternion().x()) 
205 |                     + std::abs(transform.rotation().toQuaternion().y()) + std::abs(transform.rotation().toQuaternion().z());
206 |     
207 |     if(residual_q > 0.02 || residual_t > 0.5)
208 |         return false;
209 |     
210 |     return true;
211 | }
212 | 
213 | void odom_callback(const nav_msgs::OdometryConstPtr& msg)
214 | {
215 |     odom_mutex.lock();
216 |     odometryBuf.push(msg);
217 |     odom_num++;
218 |     odom_mutex.unlock();
219 | }
220 | 
221 | void pointcloud_callback(const sensor_msgs::PointCloud2ConstPtr& msg)
222 | {
223 |     points_mutex.lock();
224 |     fullPointsBuf.push(msg);
225 |     laser_num++;
226 |     points_mutex.unlock();
227 | }
228 | 
229 | void detect_loop_closure()
230 | {
231 |     if(fullPointsBuf.empty() || odometryBuf.empty())
232 |         return;
233 | 
234 |     buf_mutex.lock();
235 |     
236 |     double time1 = fullPointsBuf.front()->header.stamp.toSec();
237 |     double time2 = odometryBuf.front()->header.stamp.toSec();
238 |     if(!fullPointsBuf.empty() && std::abs(time1 - time2) > 0.005)
239 |     {
240 |         fullPointsBuf.pop();
241 |         buf_mutex.unlock();
242 |         return;
243 |     }
244 | 
245 |     if(!odometryBuf.empty() && std::abs(time1 - time2) > 0.005)
246 |     {
247 |         odometryBuf.pop();
248 |         buf_mutex.unlock();
249 |         return;
250 |     }
251 |     //std::cout << "laser: " << laser_num << "\nodom: " << odom_num << std::endl; 
252 | 
253 |     buf_mutex.unlock();
254 | 
255 | // Process pointcloud msg
256 |     tmp->clear();
257 |     final->clear();
258 |     laserCloudFull->clear();
259 |     
260 |     points_mutex.lock();
261 |     pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFull);
262 |     fullPointsBuf.pop();
263 |     points_mutex.unlock();
264 |     
265 |     // DownSample Pointcloud
266 |     downSizeFilterScancontext.setInputCloud(laserCloudFull);
267 |     downSizeFilterScancontext.filter(*tmp);
268 |     laserCloudBuf.push_back(tmp);
269 |     
270 |     // Make Scancontext
271 |     scManager.makeAndSaveScancontextAndKeys(*tmp);
272 | // end process pointcloud
273 |     
274 | // Process odometry msg 
275 |     odom_mutex.lock();
276 |     currentPose = msg2Pose(odometryBuf.front()); 
277 |     header = odometryBuf.front()->header;
278 |     odometryBuf.pop();
279 |     odom_mutex.unlock();
280 |     headerBuf.push_back(header);
281 | // end process odometry
282 | 
283 |     graph_mutex.lock();
284 |     int key = laserCloudBuf.size()-1;
285 |     initials.insert(key, currentPose);
286 | 
287 |     if(first_msg)
288 |     {   
289 |         // add PriorFactor; to graph
290 |         gtSAMgraph.add(gtsam::PriorFactor<gtsam::Pose3>(key, currentPose, priorNoisemodel));
291 |         first_msg = false;
292 |     }
293 |     else
294 |     {
295 |         //gtsam::Pose3 increment =  lastPose.inverse() * currentPose;
296 |         gtSAMgraph.add(gtsam::BetweenFactor<gtsam::Pose3>(key-1, key, lastPose.between(currentPose), odomNoisemodel));
297 |     }
298 |     graph_mutex.unlock();
299 | 
300 |     int current_id = laserCloudBuf.size()-1;
301 | 
302 |     // Detect loop closure
303 |     auto result = scManager.detectLoopClosureID();
304 |     history_id = result.first;
305 | 
306 |     // Verify loop closure
307 |     if(history_id != -1)
308 |     {
309 |         if(!zero_flag)
310 |             return;
311 |         if(history_id == 0)
312 |             zero_flag = false;
313 | 
314 |         pcl::IterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI> icp;
315 |         icp.setMaxCorrespondenceDistance(100);
316 |         icp.setMaximumIterations(20);
317 |         icp.setTransformationEpsilon(1e-6);
318 |         icp.setEuclideanFitnessEpsilon(1e-6);
319 |         icp.setRANSACIterations(0);
320 | 
321 |         icp.setInputSource(tmp);
322 |         icp.setInputTarget(laserCloudBuf[history_id]);
323 |         icp.align(*final);
324 | 
325 |         if(icp.hasConverged() == true && icp.getFitnessScore() < 0.001)
326 |         {
327 |             ROS_INFO("[Loop info]: from %d to %d", current_id, history_id);
328 | 
329 |             Eigen::Matrix4f loop = icp.getFinalTransformation();
330 |             gtsam::Pose3 poseFrom = eigen2Pose(loop);
331 |             gtsam::Pose3 poseTo = gtsam::Pose3::identity();
332 | 
333 |             //if(!geometryVerfication(current_id, history_id, poseFrom))
334 |             //    return;
335 |             graph_mutex.unlock();
336 |             gtSAMgraph.add(gtsam::BetweenFactor<gtsam::Pose3>(current_id, history_id, poseFrom.between(poseTo), loopNoisemode));
337 |             graph_mutex.unlock();
338 | 
339 |             flag_mutex.lock();
340 |             optimize_flag = true;
341 |             flag_mutex.unlock();
342 |         }
343 |     }
344 |     
345 |     lastPose = currentPose;
346 | }
347 | 
348 | int main(int argc, char* argv[])
349 | {
350 |     ros::init(argc, argv, "sc_optimization");
351 |     ros::NodeHandle nh("~");
352 |  
353 |     ros::Subscriber sub_odom = nh.subscribe<nav_msgs::Odometry>("/odom", 1000, odom_callback);
354 |     ros::Subscriber sub_pointcloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 1000, pointcloud_callback);
355 |     
356 |     pub_path = nh.advertise<nav_msgs::Path>("final_path", 10);
357 | 
358 |     init();
359 | 
360 |     std::thread optimization_thread{GraphOptimization};
361 | 
362 |     ros::Rate rate(30);
363 |     while (ros::ok())
364 |     {
365 |         detect_loop_closure();
366 |         ros::spinOnce();
367 |         rate.sleep();
368 |     }
369 |     
370 |     optimization_thread.join();
371 | 
372 |     return 0;
373 | }
374 | 


--------------------------------------------------------------------------------