diff --git a/.clang-format b/.clang-format index d1df9d2..53780a0 100644 --- a/.clang-format +++ b/.clang-format @@ -1,83 +1,22 @@ -Language: Cpp +Language: Cpp BasedOnStyle: Google # --- -#AccessModifierOffset: -4 AlignAfterOpenBracket: AlwaysBreak # Values: Align, DontAlign, AlwaysBreak AlignConsecutiveAssignments: true AlignConsecutiveDeclarations: true -#AlignEscapedNewlinesLeft: true -#AlignOperands: false AlignTrailingComments: false # Should be off, causes many dummy problems!! -#AllowAllParametersOfDeclarationOnNextLine: true -AllowShortBlocksOnASingleLine: true -#AllowShortCaseLabelsOnASingleLine: false -#AllowShortFunctionsOnASingleLine: Empty -#AllowShortIfStatementsOnASingleLine: false -#AllowShortLoopsOnASingleLine: false -#AlwaysBreakAfterDefinitionReturnType: None -#AlwaysBreakAfterReturnType: None -#AlwaysBreakBeforeMultilineStrings: true -#AlwaysBreakTemplateDeclarations: true -#BinPackArguments: false -#BinPackParameters: false -#BraceWrapping: - #AfterClass: false - #AfterControlStatement: false - #AfterEnum: false - #AfterFunction: false - #AfterNamespace: false - #AfterObjCDeclaration: false - #AfterStruct: false - #AfterUnion: false - #BeforeCatch: false - #BeforeElse: true - #IndentBraces: false -#BreakBeforeBinaryOperators: None +AllowShortBlocksOnASingleLine: false BreakBeforeBraces: Allman -#BreakBeforeTernaryOperators: true -#BreakConstructorInitializersBeforeComma: false -ColumnLimit: 80 -#CommentPragmas: '' -#ConstructorInitializerAllOnOneLineOrOnePerLine: true -#ConstructorInitializerIndentWidth: 4 -#ContinuationIndentWidth: 4 -#Cpp11BracedListStyle: true -#DerivePointerAlignment: false -#DisableFormat: false -#ExperimentalAutoDetectBinPacking: false -##FixNamespaceComments: true # Not applicable in 3.8 -#ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] -#IncludeCategories: - #- Regex: '.*' - #Priority: 1 +ColumnLimit: 100 IndentCaseLabels: true -IndentWidth: 4 +IndentWidth: 4 IndentWrappedFunctionNames: true -#KeepEmptyLinesAtTheStartOfBlocks: true -#MacroBlockBegin: '' -#MacroBlockEnd: '' MaxEmptyLinesToKeep: 1 NamespaceIndentation: None -#PenaltyBreakBeforeFirstCallParameter: 19 -#PenaltyBreakComment: 300 -#PenaltyBreakFirstLessLess: 120 -#PenaltyBreakString: 1000 -#PenaltyExcessCharacter: 1000000 -#PenaltyReturnTypeOnItsOwnLine: 200 DerivePointerAlignment: false -#PointerAlignment: Left -ReflowComments: true # Should be true, otherwise clang-format doesn't touch comments -SortIncludes: true -#SpaceAfterCStyleCast: false +ReflowComments: true # Should be true, otherwise clang-format doesn't touch comments +SortIncludes: true SpaceBeforeAssignmentOperators: true -#SpaceBeforeParens: ControlStatements -#SpaceInEmptyParentheses: false -#SpacesBeforeTrailingComments: 2 -#SpacesInAngles: false -#SpacesInContainerLiterals: true -#SpacesInCStyleCastParentheses: false -#SpacesInParentheses: false -#SpacesInSquareBrackets: false -Standard: Cpp11 -TabWidth: 4 -UseTab: Never # Available options are Never, Always, ForIndentation +Standard: Cpp11 +TabWidth: 4 +UseTab: Never # Available options are Never, Always, ForIndentation diff --git a/examples/KDTreeVectorOfVectorsAdaptor.h b/examples/KDTreeVectorOfVectorsAdaptor.h index a13487d..bced468 100644 --- a/examples/KDTreeVectorOfVectorsAdaptor.h +++ b/examples/KDTreeVectorOfVectorsAdaptor.h @@ -54,12 +54,10 @@ template < class Distance = nanoflann::metric_L2, typename IndexType = size_t> struct KDTreeVectorOfVectorsAdaptor { - using self_t = KDTreeVectorOfVectorsAdaptor< - VectorOfVectorsType, num_t, DIM, Distance, IndexType>; - using metric_t = - typename Distance::template traits::distance_t; - using index_t = - nanoflann::KDTreeSingleIndexAdaptor; + using self_t = + KDTreeVectorOfVectorsAdaptor; + using metric_t = typename Distance::template traits::distance_t; + using index_t = nanoflann::KDTreeSingleIndexAdaptor; /** The kd-tree index for the user to call its methods as usual with any * other FLANN index */ @@ -81,8 +79,7 @@ struct KDTreeVectorOfVectorsAdaptor index = new index_t( static_cast(dims), *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams( - leaf_max_size, nanoflann::KDTreeSingleIndexAdaptorFlags::None, - n_thread_build)); + leaf_max_size, nanoflann::KDTreeSingleIndexAdaptorFlags::None, n_thread_build)); } ~KDTreeVectorOfVectorsAdaptor() { delete index; } @@ -95,8 +92,8 @@ struct KDTreeVectorOfVectorsAdaptor * The user can also call index->... methods as desired. */ inline void query( - const num_t* query_point, const size_t num_closest, - IndexType* out_indices, num_t* out_distances_sq) const + const num_t* query_point, const size_t num_closest, IndexType* out_indices, + num_t* out_distances_sq) const { nanoflann::KNNResultSet resultSet(num_closest); resultSet.init(out_indices, out_distances_sq); diff --git a/examples/SO2_adaptor_example.cpp b/examples/SO2_adaptor_example.cpp index 6b45e01..d6830fc 100644 --- a/examples/SO2_adaptor_example.cpp +++ b/examples/SO2_adaptor_example.cpp @@ -45,8 +45,8 @@ void kdtree_demo(const size_t N) // construct a kd-tree index: using my_kd_tree_t = nanoflann::KDTreeSingleIndexAdaptor< - nanoflann::SO2_Adaptor>, - PointCloud_Orient, 1 /* dim */ + nanoflann::SO2_Adaptor>, PointCloud_Orient, + 1 /* dim */ >; dump_mem_usage(); @@ -64,8 +64,7 @@ void kdtree_demo(const size_t N) index.findNeighbors(resultSet, &query_pt[0]); std::cout << "knnSearch(nn=" << num_results << "): \n"; - std::cout << "ret_index=" << ret_index - << " out_dist_sqr=" << out_dist_sqr << std::endl; + std::cout << "ret_index=" << ret_index << " out_dist_sqr=" << out_dist_sqr << std::endl; } } diff --git a/examples/SO3_adaptor_example.cpp b/examples/SO3_adaptor_example.cpp index 12b6e5e..1776725 100644 --- a/examples/SO3_adaptor_example.cpp +++ b/examples/SO3_adaptor_example.cpp @@ -45,8 +45,7 @@ void kdtree_demo(const size_t N) // construct a kd-tree index: using my_kd_tree_t = nanoflann::KDTreeSingleIndexAdaptor< - nanoflann::SO3_Adaptor>, - PointCloud_Quat, 4 /* dim */ + nanoflann::SO3_Adaptor>, PointCloud_Quat, 4 /* dim */ >; dump_mem_usage(); @@ -64,8 +63,7 @@ void kdtree_demo(const size_t N) index.findNeighbors(resultSet, &query_pt[0]); std::cout << "knnSearch(nn=" << num_results << "): \n"; - std::cout << "ret_index=" << ret_index - << " out_dist_sqr=" << out_dist_sqr << std::endl; + std::cout << "ret_index=" << ret_index << " out_dist_sqr=" << out_dist_sqr << std::endl; } } diff --git a/examples/dynamic_pointcloud_example.cpp b/examples/dynamic_pointcloud_example.cpp index 97e8aee..0a0353a 100644 --- a/examples/dynamic_pointcloud_example.cpp +++ b/examples/dynamic_pointcloud_example.cpp @@ -40,8 +40,7 @@ void kdtree_demo(const size_t N) // construct a kd-tree index: using my_kd_tree_t = nanoflann::KDTreeSingleIndexDynamicAdaptor< - nanoflann::L2_Simple_Adaptor>, - PointCloud, 3 /* dim */ + nanoflann::L2_Simple_Adaptor>, PointCloud, 3 /* dim */ >; dump_mem_usage(); @@ -78,19 +77,16 @@ void kdtree_demo(const size_t N) index.findNeighbors(resultSet, query_pt, {10}); std::cout << "knnSearch(nn=" << num_results << "): \n"; - std::cout << "ret_index=" << ret_index - << " out_dist_sqr=" << out_dist_sqr << std::endl; + std::cout << "ret_index=" << ret_index << " out_dist_sqr=" << out_dist_sqr << std::endl; std::cout << "point: (" - << "point: (" << cloud.pts[ret_index].x << ", " - << cloud.pts[ret_index].y << ", " << cloud.pts[ret_index].z - << ")" << std::endl; + << "point: (" << cloud.pts[ret_index].x << ", " << cloud.pts[ret_index].y << ", " + << cloud.pts[ret_index].z << ")" << std::endl; std::cout << std::endl; } { // do a knn search searching for more than one result const size_t num_results = 5; - std::cout << "Searching for " << num_results << " elements" - << std::endl; + std::cout << "Searching for " << num_results << " elements" << std::endl; size_t ret_index[num_results]; num_t out_dist_sqr[num_results]; nanoflann::KNNResultSet resultSet(num_results); @@ -105,8 +101,8 @@ void kdtree_demo(const size_t N) << "index: " << ret_index[i] << ",\t" << "dist: " << out_dist_sqr[i] << ",\t" << "point: (" << cloud.pts[ret_index[i]].x << ", " - << cloud.pts[ret_index[i]].y << ", " - << cloud.pts[ret_index[i]].z << ")" << std::endl; + << cloud.pts[ret_index[i]].y << ", " << cloud.pts[ret_index[i]].z << ")" + << std::endl; } std::cout << std::endl; } @@ -115,18 +111,16 @@ void kdtree_demo(const size_t N) std::cout << "Unsorted radius search" << std::endl; const num_t radiusSqr = 1; std::vector> indices_dists; - nanoflann::RadiusResultSet resultSet( - radiusSqr, indices_dists); + nanoflann::RadiusResultSet resultSet(radiusSqr, indices_dists); index.findNeighbors(resultSet, query_pt); - nanoflann::ResultItem worst_pair = - resultSet.worst_item(); - std::cout << "Worst pair: idx=" << worst_pair.first - << " dist=" << worst_pair.second << std::endl; + nanoflann::ResultItem worst_pair = resultSet.worst_item(); + std::cout << "Worst pair: idx=" << worst_pair.first << " dist=" << worst_pair.second + << std::endl; std::cout << "point: (" << cloud.pts[worst_pair.first].x << ", " - << cloud.pts[worst_pair.first].y << ", " - << cloud.pts[worst_pair.first].z << ")" << std::endl; + << cloud.pts[worst_pair.first].y << ", " << cloud.pts[worst_pair.first].z << ")" + << std::endl; std::cout << std::endl; } } diff --git a/examples/example_with_cmake/pointcloud_example.cpp b/examples/example_with_cmake/pointcloud_example.cpp index 29325dd..16b28c4 100644 --- a/examples/example_with_cmake/pointcloud_example.cpp +++ b/examples/example_with_cmake/pointcloud_example.cpp @@ -45,8 +45,7 @@ void kdtree_demo(const size_t N) // construct a kd-tree index: using my_kd_tree_t = nanoflann::KDTreeSingleIndexAdaptor< - nanoflann::L2_Simple_Adaptor>, - PointCloud, 3 /* dim */ + nanoflann::L2_Simple_Adaptor>, PointCloud, 3 /* dim */ >; dump_mem_usage(); @@ -61,26 +60,23 @@ void kdtree_demo(const size_t N) num_t out_dist_sqr; nanoflann::KNNResultSet resultSet(num_results); resultSet.init(&ret_index, &out_dist_sqr); - index.findNeighbors( - resultSet, &query_pt[0], nanoflann::SearchParams(10)); + index.findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10)); std::cout << "knnSearch(nn=" << num_results << "): \n"; - std::cout << "ret_index=" << ret_index - << " out_dist_sqr=" << out_dist_sqr << std::endl; + std::cout << "ret_index=" << ret_index << " out_dist_sqr=" << out_dist_sqr << std::endl; } { // Unsorted radius search: const num_t radius = 1; std::vector> indices_dists; - nanoflann::RadiusResultSet resultSet( - radius, indices_dists); + nanoflann::RadiusResultSet resultSet(radius, indices_dists); index.findNeighbors(resultSet, query_pt, nanoflann::SearchParams()); // Get worst (furthest) point, without sorting: std::pair worst_pair = resultSet.worst_item(); - std::cout << "Worst pair: idx=" << worst_pair.first - << " dist=" << worst_pair.second << std::endl; + std::cout << "Worst pair: idx=" << worst_pair.first << " dist=" << worst_pair.second + << std::endl; } } diff --git a/examples/example_with_pkgconfig/pointcloud_example.cpp b/examples/example_with_pkgconfig/pointcloud_example.cpp index 29325dd..16b28c4 100644 --- a/examples/example_with_pkgconfig/pointcloud_example.cpp +++ b/examples/example_with_pkgconfig/pointcloud_example.cpp @@ -45,8 +45,7 @@ void kdtree_demo(const size_t N) // construct a kd-tree index: using my_kd_tree_t = nanoflann::KDTreeSingleIndexAdaptor< - nanoflann::L2_Simple_Adaptor>, - PointCloud, 3 /* dim */ + nanoflann::L2_Simple_Adaptor>, PointCloud, 3 /* dim */ >; dump_mem_usage(); @@ -61,26 +60,23 @@ void kdtree_demo(const size_t N) num_t out_dist_sqr; nanoflann::KNNResultSet resultSet(num_results); resultSet.init(&ret_index, &out_dist_sqr); - index.findNeighbors( - resultSet, &query_pt[0], nanoflann::SearchParams(10)); + index.findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10)); std::cout << "knnSearch(nn=" << num_results << "): \n"; - std::cout << "ret_index=" << ret_index - << " out_dist_sqr=" << out_dist_sqr << std::endl; + std::cout << "ret_index=" << ret_index << " out_dist_sqr=" << out_dist_sqr << std::endl; } { // Unsorted radius search: const num_t radius = 1; std::vector> indices_dists; - nanoflann::RadiusResultSet resultSet( - radius, indices_dists); + nanoflann::RadiusResultSet resultSet(radius, indices_dists); index.findNeighbors(resultSet, query_pt, nanoflann::SearchParams()); // Get worst (furthest) point, without sorting: std::pair worst_pair = resultSet.worst_item(); - std::cout << "Worst pair: idx=" << worst_pair.first - << " dist=" << worst_pair.second << std::endl; + std::cout << "Worst pair: idx=" << worst_pair.first << " dist=" << worst_pair.second + << std::endl; } } diff --git a/examples/examples_gui/nanoflann_gui_example_R3/nanoflann_gui_example_R3.cpp b/examples/examples_gui/nanoflann_gui_example_R3/nanoflann_gui_example_R3.cpp index 860e94f..ef65cc2 100644 --- a/examples/examples_gui/nanoflann_gui_example_R3/nanoflann_gui_example_R3.cpp +++ b/examples/examples_gui/nanoflann_gui_example_R3/nanoflann_gui_example_R3.cpp @@ -44,7 +44,10 @@ mrpt::opengl::CPointCloud::Ptr pc_to_viz(const PointCloud& pc) { auto gl = mrpt::opengl::CPointCloud::Create(); - for (const auto pt : pc.pts) { gl->insertPoint(pt.x, pt.y, pt.z); } + for (const auto pt : pc.pts) + { + gl->insertPoint(pt.x, pt.y, pt.z); + } gl->setPointSize(3.0f); return gl; } @@ -91,8 +94,7 @@ void kdtree_demo(const size_t N) // construct a kd-tree index: using my_kd_tree_t = nanoflann::KDTreeSingleIndexAdaptor< - nanoflann::L2_Simple_Adaptor>, - PointCloud, 3 /* dim */ + nanoflann::L2_Simple_Adaptor>, PointCloud, 3 /* dim */ >; mrpt::system::CTimeLogger profiler; @@ -127,22 +129,19 @@ void kdtree_demo(const size_t N) const size_t nnToSearch = (rng.drawUniform32bit() % 10) + 1; #endif const double queryPt[3] = { - rng.drawUniform(-0.3, maxRangeXY + 0.3), - rng.drawUniform(-0.3, maxRangeXY + 0.3), + rng.drawUniform(-0.3, maxRangeXY + 0.3), rng.drawUniform(-0.3, maxRangeXY + 0.3), rng.drawUniform(-0.3, maxRangeZ + 0.3)}; mrpt::system::CTimeLoggerEntry tle2(profiler, "query"); #if defined(USE_RADIUS_SEARCH) indicesDists.clear(); - nanoflann::RadiusResultSet resultSet( - sqRadius, indicesDists); + nanoflann::RadiusResultSet resultSet(sqRadius, indicesDists); index.findNeighbors(resultSet, queryPt); #else #if defined(USE_RKNN_SEARCH) - nanoflann::RKNNResultSet resultSet( - nnToSearch, sqRadius); + nanoflann::RKNNResultSet resultSet(nnToSearch, sqRadius); #elif defined(USE_KNN_SEARCH) nanoflann::KNNResultSet resultSet(nnToSearch); #else @@ -162,20 +161,17 @@ void kdtree_demo(const size_t N) #endif tle2.stop(); - std::cout << "\nQuery point: (" << queryPt[0] << "," << queryPt[1] - << "," << queryPt[2] << ") => " << resultSet.size() - << " results.\n"; + std::cout << "\nQuery point: (" << queryPt[0] << "," << queryPt[1] << "," << queryPt[2] + << ") => " << resultSet.size() << " results.\n"; if (!resultSet.empty()) { #if defined(USE_RADIUS_SEARCH) - nanoflann::ResultItem worstPair = - resultSet.worst_item(); + nanoflann::ResultItem worstPair = resultSet.worst_item(); std::cout << "Worst pair: idx=" << worstPair.first << " dist=" << std::sqrt(worstPair.second) << std::endl; #else - std::cout << "nnToSearch=" << nnToSearch - << " actual found=" << indices.size() + std::cout << "nnToSearch=" << nnToSearch << " actual found=" << indices.size() << " Worst found dist=" << worstDist << std::endl; #endif } diff --git a/examples/examples_gui/nanoflann_gui_example_bearings/nanoflann_gui_example_bearings.cpp b/examples/examples_gui/nanoflann_gui_example_bearings/nanoflann_gui_example_bearings.cpp index 8726ae5..389fd38 100644 --- a/examples/examples_gui/nanoflann_gui_example_bearings/nanoflann_gui_example_bearings.cpp +++ b/examples/examples_gui/nanoflann_gui_example_bearings/nanoflann_gui_example_bearings.cpp @@ -79,8 +79,7 @@ struct Bearing mrpt::math::TPoint3D bearing_to_point(const Bearing& b) { const auto R = - mrpt::poses::CPose3D::FromYawPitchRoll(b.yaw, b.pitch, 0.0 /*roll*/) - .getRotationMatrix(); + mrpt::poses::CPose3D::FromYawPitchRoll(b.yaw, b.pitch, 0.0 /*roll*/).getRotationMatrix(); return {R(0, 0), R(1, 0), R(2, 0)}; } #endif @@ -175,9 +174,7 @@ inline T my_angDistance(T from, T to) // // ********************************* IMPORTANT ******************************** // -template < - class T, class DataSource, typename _DistanceType = T, - typename IndexType = uint32_t> +template struct ThetaPhiMetric_Adaptor { using ElementType = T; @@ -185,17 +182,12 @@ struct ThetaPhiMetric_Adaptor const DataSource& data_source; - ThetaPhiMetric_Adaptor(const DataSource& _data_source) - : data_source(_data_source) - { - } + ThetaPhiMetric_Adaptor(const DataSource& _data_source) : data_source(_data_source) {} - DistanceType evalMetric( - const T* a, const IndexType b_idx, size_t /*size = 2*/) const + DistanceType evalMetric(const T* a, const IndexType b_idx, size_t /*size = 2*/) const { DistanceType result = - mrpt::square( - my_angDistance(a[0], data_source.kdtree_get_pt(b_idx, 0))) + + mrpt::square(my_angDistance(a[0], data_source.kdtree_get_pt(b_idx, 0))) + mrpt::square(a[1] - data_source.kdtree_get_pt(b_idx, 1)); return result; @@ -217,8 +209,7 @@ struct ThetaPhiMetric_Adaptor }; using my_kd_tree_t = nanoflann::KDTreeSingleIndexAdaptor< - ThetaPhiMetric_Adaptor, BearingsDataset, - 2 /* dim */ + ThetaPhiMetric_Adaptor, BearingsDataset, 2 /* dim */ >; void kdtree_demo(const size_t N) @@ -241,8 +232,7 @@ void kdtree_demo(const size_t N) auto glPts = pc_to_viz(data); scene->insert(glPts); scene->insert(mrpt::opengl::stock_objects::CornerXYZSimple()); - auto glAxis = - mrpt::opengl::CAxis::Create(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0, 0.25); + auto glAxis = mrpt::opengl::CAxis::Create(-1.0, -1.0, -1.0, 1.0, 1.0, 1.0, 0.25); glAxis->setTextScale(0.1); scene->insert(glAxis); @@ -276,18 +266,17 @@ void kdtree_demo(const size_t N) for (size_t i = 0; i < N; i++) #endif { - const double queryPt[2] = { - data.kdtree_get_pt(i, 0), data.kdtree_get_pt(i, 1)}; + const double queryPt[2] = {data.kdtree_get_pt(i, 0), data.kdtree_get_pt(i, 1)}; mrpt::system::CTimeLoggerEntry tle2(profiler, "query"); - const auto numNN = index.knnSearch( - queryPt, NUM_NEIGHBORS, nn_indices.data(), nn_distances.data()); + const auto numNN = + index.knnSearch(queryPt, NUM_NEIGHBORS, nn_indices.data(), nn_distances.data()); tle2.stop(); - std::cout << "\nQuery point: (" << queryPt[0] << "," << queryPt[1] - << ") => " << numNN << " results.\n"; + std::cout << "\nQuery point: (" << queryPt[0] << "," << queryPt[1] << ") => " << numNN + << " results.\n"; #ifdef SHOW_GUI bool stop = false; @@ -305,8 +294,7 @@ void kdtree_demo(const size_t N) glFoundPts->clear(); for (size_t j = 0; j < numNN; j++) { - const auto pt = - bearing_to_point(data.samples.at(nn_indices[j])); + const auto pt = bearing_to_point(data.samples.at(nn_indices[j])); glFoundPts->insertPoint(pt.x, pt.y, pt.z); } @@ -324,10 +312,8 @@ void kdtree_demo(const size_t N) int main() { - std::cout << my_angDistance(mrpt::DEG2RAD(179.0), mrpt::DEG2RAD(-179.0)) - << std::endl; - std::cout << my_angDistance(mrpt::DEG2RAD(-179.0), mrpt::DEG2RAD(179.0)) - << std::endl; + std::cout << my_angDistance(mrpt::DEG2RAD(179.0), mrpt::DEG2RAD(-179.0)) << std::endl; + std::cout << my_angDistance(mrpt::DEG2RAD(-179.0), mrpt::DEG2RAD(179.0)) << std::endl; kdtree_demo(NUM_SAMPLES); return 0; diff --git a/examples/matrix_example.cpp b/examples/matrix_example.cpp index cbddc7b..58645fe 100644 --- a/examples/matrix_example.cpp +++ b/examples/matrix_example.cpp @@ -43,8 +43,7 @@ void generateRandomPointCloud( mat.resize(N, dim); for (size_t i = 0; i < N; i++) for (size_t d = 0; d < dim; d++) - mat(i, d) = - max_range * (rand() % 1000) / typename Der::Scalar(1000); + mat(i, d) = max_range * (rand() % 1000) / typename Der::Scalar(1000); std::cout << "done\n"; } @@ -64,8 +63,7 @@ void kdtree_demo(const size_t nSamples, const size_t dim) // Query point: std::vector query_pt(dim); - for (size_t d = 0; d < dim; d++) - query_pt[d] = max_range * (rand() % 1000) / num_t(1000); + for (size_t d = 0; d < dim; d++) query_pt[d] = max_range * (rand() % 1000) / num_t(1000); // ------------------------------------------------------------ // construct a kd-tree index: diff --git a/examples/pointcloud_adaptor_example.cpp b/examples/pointcloud_adaptor_example.cpp index ace3f77..6d46276 100644 --- a/examples/pointcloud_adaptor_example.cpp +++ b/examples/pointcloud_adaptor_example.cpp @@ -51,10 +51,7 @@ struct PointCloudAdaptor inline const Derived& derived() const { return obj; } // Must return the number of data points - inline size_t kdtree_get_point_count() const - { - return derived().pts.size(); - } + inline size_t kdtree_get_point_count() const { return derived().pts.size(); } // Returns the dim'th component of the idx'th point in the class: // Since this is inlined and the "dim" argument is typically an immediate @@ -114,8 +111,7 @@ void kdtree_demo(const size_t N) index.findNeighbors(resultSet, &query_pt[0]); std::cout << "knnSearch(nn=" << num_results << "): \n"; - std::cout << "ret_index=" << ret_index - << " out_dist_sqr=" << out_dist_sqr << std::endl; + std::cout << "ret_index=" << ret_index << " out_dist_sqr=" << out_dist_sqr << std::endl; }; my_kd_tree_t index1(3 /*dim*/, pc2kd, {10 /* max leaf */}); diff --git a/examples/pointcloud_custom_metric.cpp b/examples/pointcloud_custom_metric.cpp index 6e6db8c..f645544 100644 --- a/examples/pointcloud_custom_metric.cpp +++ b/examples/pointcloud_custom_metric.cpp @@ -40,9 +40,7 @@ using namespace nanoflann; // the metric class My_Custom_Metric_Adaptor, whose constructor accepts // arbitrary parameters: -template < - class T, class DataSource, typename _DistanceType = T, - typename IndexType = uint32_t> +template struct My_Custom_Metric_Adaptor { using ElementType = T; @@ -57,14 +55,12 @@ struct My_Custom_Metric_Adaptor { } - inline DistanceType evalMetric( - const T* a, const IndexType b_idx, size_t size) const + inline DistanceType evalMetric(const T* a, const IndexType b_idx, size_t size) const { DistanceType result = DistanceType(); for (size_t i = 0; i < size; ++i) { - const DistanceType diff = - a[i] - data_source.kdtree_get_pt(b_idx, i); + const DistanceType diff = a[i] - data_source.kdtree_get_pt(b_idx, i); result += std::pow(diff, _myParam); } return result; @@ -90,8 +86,7 @@ static void kdtree_custom_metric_demo(const size_t N) // construct a kd-tree index: using my_kd_tree_t = KDTreeSingleIndexAdaptor< - My_Custom_Metric_Adaptor>, PointCloud, - 3 /* dim */ + My_Custom_Metric_Adaptor>, PointCloud, 3 /* dim */ >; dump_mem_usage(); @@ -112,22 +107,19 @@ static void kdtree_custom_metric_demo(const size_t N) index.findNeighbors(resultSet, &query_pt[0]); std::cout << "knnSearch(nn=" << num_results << "\n"; - std::cout << "ret_index=" << ret_index - << " out_dist_sqr=" << out_dist_sqr << endl; + std::cout << "ret_index=" << ret_index << " out_dist_sqr=" << out_dist_sqr << endl; } { // Unsorted radius search: const num_t radius = 1; std::vector> indices_dists; - RadiusResultSet resultSet(radius, indices_dists); + RadiusResultSet resultSet(radius, indices_dists); index.findNeighbors(resultSet, query_pt); // Get worst (furthest) point, without sorting: - nanoflann::ResultItem worst_pair = - resultSet.worst_item(); - cout << "Worst pair: idx=" << worst_pair.first - << " dist=" << worst_pair.second << endl; + nanoflann::ResultItem worst_pair = resultSet.worst_item(); + cout << "Worst pair: idx=" << worst_pair.first << " dist=" << worst_pair.second << endl; } } diff --git a/examples/pointcloud_custom_resultset.cpp b/examples/pointcloud_custom_resultset.cpp index 89553ab..9d9dd1f 100644 --- a/examples/pointcloud_custom_resultset.cpp +++ b/examples/pointcloud_custom_resultset.cpp @@ -46,13 +46,11 @@ class MyCustomResultSet public: const DistanceType radius; - std::vector>& - m_indices_dists; + std::vector>& m_indices_dists; explicit MyCustomResultSet( - DistanceType radius_, - std::vector>& - indices_dists) + DistanceType radius_, + std::vector>& indices_dists) : radius(radius_), m_indices_dists(indices_dists) { init(); @@ -73,9 +71,7 @@ class MyCustomResultSet */ bool addPoint(DistanceType dist, IndexType index) { - printf( - "addPoint() called: dist=%f index=%u\n", dist, - static_cast(index)); + printf("addPoint() called: dist=%f index=%u\n", dist, static_cast(index)); if (dist < radius) m_indices_dists.emplace_back(index, dist); return true; @@ -85,9 +81,7 @@ class MyCustomResultSet void sort() { - std::sort( - m_indices_dists.begin(), m_indices_dists.end(), - nanoflann::IndexDist_Sorter()); + std::sort(m_indices_dists.begin(), m_indices_dists.end(), nanoflann::IndexDist_Sorter()); } }; @@ -102,8 +96,7 @@ void kdtree_demo(const size_t N) // construct a kd-tree index: using my_kd_tree_t = nanoflann::KDTreeSingleIndexAdaptor< - nanoflann::L2_Simple_Adaptor>, - PointCloud, 3 /* dim */ + nanoflann::L2_Simple_Adaptor>, PointCloud, 3 /* dim */ >; my_kd_tree_t index(3 /*dim*/, cloud, {10 /* max leaf */}); @@ -113,13 +106,11 @@ void kdtree_demo(const size_t N) const num_t squaredRadius = 1; std::vector> indices_dists; - MyCustomResultSet resultSet( - squaredRadius, indices_dists); + MyCustomResultSet resultSet(squaredRadius, indices_dists); index.findNeighbors(resultSet, query_pt); - std::cout << "Found: " << indices_dists.size() << " NN points." - << std::endl; + std::cout << "Found: " << indices_dists.size() << " NN points." << std::endl; } } diff --git a/examples/pointcloud_example.cpp b/examples/pointcloud_example.cpp index 1d61e99..d959682 100644 --- a/examples/pointcloud_example.cpp +++ b/examples/pointcloud_example.cpp @@ -50,8 +50,7 @@ void kdtree_demo(const size_t N) // construct a kd-tree index: using my_kd_tree_t = nanoflann::KDTreeSingleIndexAdaptor< - nanoflann::L2_Simple_Adaptor>, - PointCloud, 3 /* dim */ + nanoflann::L2_Simple_Adaptor>, PointCloud, 3 /* dim */ >; dump_mem_usage(); @@ -69,24 +68,21 @@ void kdtree_demo(const size_t N) index.findNeighbors(resultSet, &query_pt[0]); std::cout << "knnSearch(nn=" << num_results << "): \n"; - std::cout << "ret_index=" << ret_index - << " out_dist_sqr=" << out_dist_sqr << std::endl; + std::cout << "ret_index=" << ret_index << " out_dist_sqr=" << out_dist_sqr << std::endl; } { // radius search: const num_t squaredRadius = 1; std::vector> indices_dists; - nanoflann::RadiusResultSet resultSet( - squaredRadius, indices_dists); + nanoflann::RadiusResultSet resultSet(squaredRadius, indices_dists); index.findNeighbors(resultSet, query_pt); // Get worst (furthest) point, without sorting: - nanoflann::ResultItem worst_pair = - resultSet.worst_item(); - std::cout << "Worst pair: idx=" << worst_pair.first - << " squaredDist=" << worst_pair.second << std::endl; + nanoflann::ResultItem worst_pair = resultSet.worst_item(); + std::cout << "Worst pair: idx=" << worst_pair.first << " squaredDist=" << worst_pair.second + << std::endl; } } diff --git a/examples/pointcloud_kdd_radius.cpp b/examples/pointcloud_kdd_radius.cpp index 78e760e..e3f6781 100644 --- a/examples/pointcloud_kdd_radius.cpp +++ b/examples/pointcloud_kdd_radius.cpp @@ -46,8 +46,7 @@ void kdtree_demo(const size_t N) // construct a kd-tree index: using my_kd_tree_t = nanoflann::KDTreeSingleIndexAdaptor< - nanoflann::L2_Simple_Adaptor>, - PointCloud, 3 /* dim */ + nanoflann::L2_Simple_Adaptor>, PointCloud, 3 /* dim */ >; my_kd_tree_t index(3 /*dim*/, cloud, {10 /* max leaf */}); @@ -68,8 +67,7 @@ void kdtree_demo(const size_t N) std::vector ret_index(num_results); std::vector out_dist_sqr(num_results); - num_results = index.knnSearch( - &query_pt[0], num_results, &ret_index[0], &out_dist_sqr[0]); + num_results = index.knnSearch(&query_pt[0], num_results, &ret_index[0], &out_dist_sqr[0]); // In case of less points in the tree than requested: ret_index.resize(num_results); @@ -77,8 +75,8 @@ void kdtree_demo(const size_t N) cout << "knnSearch(): num_results=" << num_results << "\n"; for (size_t i = 0; i < num_results; i++) - cout << "idx[" << i << "]=" << ret_index[i] << " dist[" << i - << "]=" << out_dist_sqr[i] << endl; + cout << "idx[" << i << "]=" << ret_index[i] << " dist[" << i << "]=" << out_dist_sqr[i] + << endl; cout << "\n"; } @@ -86,17 +84,15 @@ void kdtree_demo(const size_t N) // radiusSearch(): Perform a search for the points within search_radius // ---------------------------------------------------------------- { - const num_t search_radius = static_cast(0.1); + const num_t search_radius = static_cast(0.1); std::vector> ret_matches; // nanoflanSearchParamsameters params; // params.sorted = false; - const size_t nMatches = - index.radiusSearch(&query_pt[0], search_radius, ret_matches); + const size_t nMatches = index.radiusSearch(&query_pt[0], search_radius, ret_matches); - cout << "radiusSearch(): radius=" << search_radius << " -> " << nMatches - << " matches\n"; + cout << "radiusSearch(): radius=" << search_radius << " -> " << nMatches << " matches\n"; for (size_t i = 0; i < nMatches; i++) cout << "idx[" << i << "]=" << ret_matches[i].first << " dist[" << i << "]=" << ret_matches[i].second << endl; diff --git a/examples/saveload_example.cpp b/examples/saveload_example.cpp index 25c5c71..3ad3583 100644 --- a/examples/saveload_example.cpp +++ b/examples/saveload_example.cpp @@ -45,16 +45,14 @@ void kdtree_save_load_demo(const size_t N) // construct a kd-tree index: using my_kd_tree_t = nanoflann::KDTreeSingleIndexAdaptor< - nanoflann::L2_Simple_Adaptor>, - PointCloud, 3 /* dim */ + nanoflann::L2_Simple_Adaptor>, PointCloud, 3 /* dim */ >; // Construct the index and save it: // -------------------------------------------- { my_kd_tree_t index( - 3 /*dim*/, cloud, - nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); + 3 /*dim*/, cloud, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); std::ofstream f("index.bin", std::ofstream::binary); @@ -74,8 +72,8 @@ void kdtree_save_load_demo(const size_t N) my_kd_tree_t index( 3 /*dim*/, cloud, nanoflann::KDTreeSingleIndexAdaptorParams( - 10 /* max leaf */, nanoflann::KDTreeSingleIndexAdaptorFlags:: - SkipInitialBuildIndex)); + 10 /* max leaf */, + nanoflann::KDTreeSingleIndexAdaptorFlags::SkipInitialBuildIndex)); std::ifstream f("index.bin", std::ofstream::binary); @@ -93,8 +91,7 @@ void kdtree_save_load_demo(const size_t N) index.findNeighbors(resultSet, &query_pt[0]); std::cout << "knnSearch(nn=" << num_results << "): \n"; - std::cout << "ret_index=" << ret_index - << " out_dist_sqr=" << out_dist_sqr << std::endl; + std::cout << "ret_index=" << ret_index << " out_dist_sqr=" << out_dist_sqr << std::endl; } // Stress test: try to save an empty index diff --git a/examples/utils.h b/examples/utils.h index 44ed5e2..a2a53cd 100644 --- a/examples/utils.h +++ b/examples/utils.h @@ -87,8 +87,7 @@ void generateRandomPointCloudRanges( } template -void generateRandomPointCloud( - PointCloud& pc, const size_t N, const T max_range = 10) +void generateRandomPointCloud(PointCloud& pc, const size_t N, const T max_range = 10) { generateRandomPointCloudRanges(pc, N, max_range, max_range, max_range); } @@ -143,8 +142,7 @@ void generateRandomPointCloud_Quat(PointCloud_Quat& point, const size_t N) T theta, X, Y, Z, sinAng, cosAng, mag; for (size_t i = 0; i < N; i++) { - theta = static_cast( - nanoflann::pi_const() * (((double)rand()) / RAND_MAX)); + theta = static_cast(nanoflann::pi_const() * (((double)rand()) / RAND_MAX)); // Generate random value in [-1, 1] X = static_cast(2 * (((double)rand()) / RAND_MAX) - 1); Y = static_cast(2 * (((double)rand()) / RAND_MAX) - 1); @@ -199,16 +197,14 @@ struct PointCloud_Orient }; template -void generateRandomPointCloud_Orient( - PointCloud_Orient& point, const size_t N) +void generateRandomPointCloud_Orient(PointCloud_Orient& point, const size_t N) { // Generating Random Orientations point.pts.resize(N); for (size_t i = 0; i < N; i++) { point.pts[i].theta = static_cast( - (2 * nanoflann::pi_const() * - (((double)rand()) / RAND_MAX)) - + (2 * nanoflann::pi_const() * (((double)rand()) / RAND_MAX)) - nanoflann::pi_const()); } } diff --git a/examples/vector_of_vectors_example.cpp b/examples/vector_of_vectors_example.cpp index 56fa98d..c40c928 100644 --- a/examples/vector_of_vectors_example.cpp +++ b/examples/vector_of_vectors_example.cpp @@ -48,8 +48,7 @@ void generateRandomPointCloud( for (size_t i = 0; i < N; i++) { samples[i].resize(dim); - for (size_t d = 0; d < dim; d++) - samples[i][d] = max_range * (rand() % 1000) / (1000.0); + for (size_t d = 0; d < dim; d++) samples[i][d] = max_range * (rand() % 1000) / (1000.0); } std::cout << "done\n"; } @@ -65,14 +64,12 @@ void kdtree_demo(const size_t nSamples, const size_t dim) // Query point: std::vector query_pt(dim); - for (size_t d = 0; d < dim; d++) - query_pt[d] = max_range * (rand() % 1000) / (1000.0); + for (size_t d = 0; d < dim; d++) query_pt[d] = max_range * (rand() % 1000) / (1000.0); // construct a kd-tree index: // Dimensionality set at run-time (default: L2) // ------------------------------------------------------------ - typedef KDTreeVectorOfVectorsAdaptor - my_kd_tree_t; + typedef KDTreeVectorOfVectorsAdaptor my_kd_tree_t; my_kd_tree_t mat_index(dim /*dim*/, samples, 10 /* max leaf */); diff --git a/include/nanoflann.hpp b/include/nanoflann.hpp index 152ed4d..426e5ff 100644 --- a/include/nanoflann.hpp +++ b/include/nanoflann.hpp @@ -36,8 +36,8 @@ * * nanoflann does not require compiling or installing, just an * #include in your code. - * - * Macros that are observed in this file: + * + * Macros that are observed in this file: * - NANOFLANN_NO_THREADS: If defined, single thread will be enforced. * - NANOFLANN_FIRST_MATCH: If defined, in case of a tie in distances the item with the smallest * index will be returned. @@ -71,8 +71,7 @@ #define NANOFLANN_VERSION 0x180 // Avoid conflicting declaration of min/max macros in Windows headers -#if !defined(NOMINMAX) && \ - (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64)) +#if !defined(NOMINMAX) && (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64)) #define NOMINMAX #ifdef max #undef max @@ -120,8 +119,7 @@ struct has_resize : std::false_type }; template -struct has_resize().resize(1), 0)> - : std::true_type +struct has_resize().resize(1), 0)> : std::true_type { }; @@ -131,8 +129,7 @@ struct has_assign : std::false_type }; template -struct has_assign().assign(1, 0), 0)> - : std::true_type +struct has_assign().assign(1, 0), 0)> : std::true_type { }; @@ -151,11 +148,10 @@ inline typename std::enable_if::value, void>::type resize( * std::array) It raises an exception if the expected size does not match */ template -inline typename std::enable_if::value, void>::type - resize(Container& c, const size_t nElements) +inline typename std::enable_if::value, void>::type resize( + Container& c, const size_t nElements) { - if (nElements != c.size()) - throw std::logic_error("Attempt to resize a fixed size container."); + if (nElements != c.size()) throw std::logic_error("Attempt to resize a fixed size container."); } /** @@ -172,8 +168,8 @@ inline typename std::enable_if::value, void>::type assign( * Free function to assign to a std::array */ template -inline typename std::enable_if::value, void>::type - assign(Container& c, const size_t nElements, const T& value) +inline typename std::enable_if::value, void>::type assign( + Container& c, const size_t nElements, const T& value) { for (size_t i = 0; i < nElements; i++) c[i] = value; } @@ -201,8 +197,7 @@ template struct ResultItem { ResultItem() = default; - ResultItem(const IndexType index, const DistanceType distance) - : first(index), second(distance) + ResultItem(const IndexType index, const DistanceType distance) : first(index), second(distance) { } @@ -214,9 +209,7 @@ struct ResultItem * @{ */ /** Result set for KNN searches (N-closest neighbors) */ -template < - typename _DistanceType, typename _IndexType = size_t, - typename _CountType = size_t> +template class KNNResultSet { public: @@ -260,8 +253,7 @@ class KNNResultSet /** If defined and two points have the same distance, the one with * the lowest-index will be returned first. */ #ifdef NANOFLANN_FIRST_MATCH - if ((dists[i - 1] > dist) || - ((dist == dists[i - 1]) && (indices[i - 1] > index))) + if ((dists[i - 1] > dist) || ((dist == dists[i - 1]) && (indices[i - 1] > index))) { #else if (dists[i - 1] > dist) @@ -291,9 +283,8 @@ class KNNResultSet //! full, or the maximum possible distance, if not full yet. DistanceType worstDist() const noexcept { - return (count < capacity || !count) - ? std::numeric_limits::max() - : dists[count - 1]; + return (count < capacity || !count) ? std::numeric_limits::max() + : dists[count - 1]; } void sort() @@ -303,9 +294,7 @@ class KNNResultSet }; /** Result set for RKNN searches (N-closest neighbors with a maximum radius) */ -template < - typename _DistanceType, typename _IndexType = size_t, - typename _CountType = size_t> +template class RKNNResultSet { public: @@ -321,8 +310,7 @@ class RKNNResultSet DistanceType maximumSearchDistanceSquared; public: - explicit RKNNResultSet( - CountType capacity_, DistanceType maximumSearchDistanceSquared_) + explicit RKNNResultSet(CountType capacity_, DistanceType maximumSearchDistanceSquared_) : indices(nullptr), dists(nullptr), capacity(capacity_), @@ -356,8 +344,7 @@ class RKNNResultSet /** If defined and two points have the same distance, the one with * the lowest-index will be returned first. */ #ifdef NANOFLANN_FIRST_MATCH - if ((dists[i - 1] > dist) || - ((dist == dists[i - 1]) && (indices[i - 1] > index))) + if ((dists[i - 1] > dist) || ((dist == dists[i - 1]) && (indices[i - 1] > index))) { #else if (dists[i - 1] > dist) @@ -387,8 +374,7 @@ class RKNNResultSet //! full, or the maximum possible distance, if not full yet. DistanceType worstDist() const noexcept { - return (count < capacity || !count) ? maximumSearchDistanceSquared - : dists[count - 1]; + return (count < capacity || !count) ? maximumSearchDistanceSquared : dists[count - 1]; } void sort() @@ -413,8 +399,7 @@ class RadiusResultSet std::vector>& m_indices_dists; explicit RadiusResultSet( - DistanceType radius_, - std::vector>& indices_dists) + DistanceType radius_, std::vector>& indices_dists) : radius(radius_), m_indices_dists(indices_dists) { init(); @@ -450,16 +435,12 @@ class RadiusResultSet throw std::runtime_error( "Cannot invoke RadiusResultSet::worst_item() on " "an empty list of results."); - auto it = std::max_element( - m_indices_dists.begin(), m_indices_dists.end(), IndexDist_Sorter()); + auto it = + std::max_element(m_indices_dists.begin(), m_indices_dists.end(), IndexDist_Sorter()); return *it; } - void sort() - { - std::sort( - m_indices_dists.begin(), m_indices_dists.end(), IndexDist_Sorter()); - } + void sort() { std::sort(m_indices_dists.begin(), m_indices_dists.end(), IndexDist_Sorter()); } }; /** @} */ @@ -513,9 +494,7 @@ struct Metric * \tparam IndexType Type of the arguments with which the data can be * accessed (e.g. float, double, int64_t, T*) */ -template < - class T, class DataSource, typename _DistanceType = T, - typename IndexType = size_t> +template struct L1_Adaptor { using ElementType = T; @@ -530,9 +509,8 @@ struct L1_Adaptor DistanceType worst_dist = -1) const { DistanceType result = DistanceType(); - const size_t multof4 = (size >> 2) - << 2; // largest multiple of 4, i.e. 1 << 2 - size_t d; + const size_t multof4 = (size >> 2) << 2; // largest multiple of 4, i.e. 1 << 2 + size_t d; /* Process 4 items with each loop for efficiency. */ if (worst_dist <= 0) @@ -540,14 +518,14 @@ struct L1_Adaptor /* No checks with worst_dist. */ for (d = 0; d < multof4; d += 4) { - const DistanceType diff0 = std::abs( - a[d + 0] - data_source.kdtree_get_pt(b_idx, d + 0)); - const DistanceType diff1 = std::abs( - a[d + 1] - data_source.kdtree_get_pt(b_idx, d + 1)); - const DistanceType diff2 = std::abs( - a[d + 2] - data_source.kdtree_get_pt(b_idx, d + 2)); - const DistanceType diff3 = std::abs( - a[d + 3] - data_source.kdtree_get_pt(b_idx, d + 3)); + const DistanceType diff0 = + std::abs(a[d + 0] - data_source.kdtree_get_pt(b_idx, d + 0)); + const DistanceType diff1 = + std::abs(a[d + 1] - data_source.kdtree_get_pt(b_idx, d + 1)); + const DistanceType diff2 = + std::abs(a[d + 2] - data_source.kdtree_get_pt(b_idx, d + 2)); + const DistanceType diff3 = + std::abs(a[d + 3] - data_source.kdtree_get_pt(b_idx, d + 3)); /* Parentheses break dependency chain: */ result += (diff0 + diff1) + (diff2 + diff3); } @@ -557,17 +535,20 @@ struct L1_Adaptor /* Check with worst_dist. */ for (d = 0; d < multof4; d += 4) { - const DistanceType diff0 = std::abs( - a[d + 0] - data_source.kdtree_get_pt(b_idx, d + 0)); - const DistanceType diff1 = std::abs( - a[d + 1] - data_source.kdtree_get_pt(b_idx, d + 1)); - const DistanceType diff2 = std::abs( - a[d + 2] - data_source.kdtree_get_pt(b_idx, d + 2)); - const DistanceType diff3 = std::abs( - a[d + 3] - data_source.kdtree_get_pt(b_idx, d + 3)); + const DistanceType diff0 = + std::abs(a[d + 0] - data_source.kdtree_get_pt(b_idx, d + 0)); + const DistanceType diff1 = + std::abs(a[d + 1] - data_source.kdtree_get_pt(b_idx, d + 1)); + const DistanceType diff2 = + std::abs(a[d + 2] - data_source.kdtree_get_pt(b_idx, d + 2)); + const DistanceType diff3 = + std::abs(a[d + 3] - data_source.kdtree_get_pt(b_idx, d + 3)); /* Parentheses break dependency chain: */ result += (diff0 + diff1) + (diff2 + diff3); - if (result > worst_dist) { return result; } + if (result > worst_dist) + { + return result; + } } } /* Process last 0-3 components. Unrolled loop with fall-through switch. @@ -575,14 +556,11 @@ struct L1_Adaptor switch (size - multof4) { case 3: - result += std::abs( - a[d + 2] - data_source.kdtree_get_pt(b_idx, d + 2)); + result += std::abs(a[d + 2] - data_source.kdtree_get_pt(b_idx, d + 2)); case 2: - result += std::abs( - a[d + 1] - data_source.kdtree_get_pt(b_idx, d + 1)); + result += std::abs(a[d + 1] - data_source.kdtree_get_pt(b_idx, d + 1)); case 1: - result += std::abs( - a[d + 0] - data_source.kdtree_get_pt(b_idx, d + 0)); + result += std::abs(a[d + 0] - data_source.kdtree_get_pt(b_idx, d + 0)); case 0: break; } @@ -606,9 +584,7 @@ struct L1_Adaptor * \tparam IndexType Type of the arguments with which the data can be * accessed (e.g. float, double, int64_t, T*) */ -template < - class T, class DataSource, typename _DistanceType = T, - typename IndexType = size_t> +template struct L2_Adaptor { using ElementType = T; @@ -623,9 +599,8 @@ struct L2_Adaptor DistanceType worst_dist = -1) const { DistanceType result = DistanceType(); - const size_t multof4 = (size >> 2) - << 2; // largest multiple of 4, i.e. 1 << 2 - size_t d; + const size_t multof4 = (size >> 2) << 2; // largest multiple of 4, i.e. 1 << 2 + size_t d; /* Process 4 items with each loop for efficiency. */ if (worst_dist <= 0) @@ -633,17 +608,12 @@ struct L2_Adaptor /* No checks with worst_dist. */ for (d = 0; d < multof4; d += 4) { - const DistanceType diff0 = - a[d + 0] - data_source.kdtree_get_pt(b_idx, d + 0); - const DistanceType diff1 = - a[d + 1] - data_source.kdtree_get_pt(b_idx, d + 1); - const DistanceType diff2 = - a[d + 2] - data_source.kdtree_get_pt(b_idx, d + 2); - const DistanceType diff3 = - a[d + 3] - data_source.kdtree_get_pt(b_idx, d + 3); + const DistanceType diff0 = a[d + 0] - data_source.kdtree_get_pt(b_idx, d + 0); + const DistanceType diff1 = a[d + 1] - data_source.kdtree_get_pt(b_idx, d + 1); + const DistanceType diff2 = a[d + 2] - data_source.kdtree_get_pt(b_idx, d + 2); + const DistanceType diff3 = a[d + 3] - data_source.kdtree_get_pt(b_idx, d + 3); /* Parentheses break dependency chain: */ - result += (diff0 * diff0 + diff1 * diff1) + - (diff2 * diff2 + diff3 * diff3); + result += (diff0 * diff0 + diff1 * diff1) + (diff2 * diff2 + diff3 * diff3); } } else @@ -651,18 +621,16 @@ struct L2_Adaptor /* Check with worst_dist. */ for (d = 0; d < multof4; d += 4) { - const DistanceType diff0 = - a[d + 0] - data_source.kdtree_get_pt(b_idx, d + 0); - const DistanceType diff1 = - a[d + 1] - data_source.kdtree_get_pt(b_idx, d + 1); - const DistanceType diff2 = - a[d + 2] - data_source.kdtree_get_pt(b_idx, d + 2); - const DistanceType diff3 = - a[d + 3] - data_source.kdtree_get_pt(b_idx, d + 3); + const DistanceType diff0 = a[d + 0] - data_source.kdtree_get_pt(b_idx, d + 0); + const DistanceType diff1 = a[d + 1] - data_source.kdtree_get_pt(b_idx, d + 1); + const DistanceType diff2 = a[d + 2] - data_source.kdtree_get_pt(b_idx, d + 2); + const DistanceType diff3 = a[d + 3] - data_source.kdtree_get_pt(b_idx, d + 3); /* Parentheses break dependency chain: */ - result += (diff0 * diff0 + diff1 * diff1) + - (diff2 * diff2 + diff3 * diff3); - if (result > worst_dist) { return result; } + result += (diff0 * diff0 + diff1 * diff1) + (diff2 * diff2 + diff3 * diff3); + if (result > worst_dist) + { + return result; + } } } /* Process last 0-3 components. Unrolled loop with fall-through switch. @@ -703,9 +671,7 @@ struct L2_Adaptor * \tparam IndexType Type of the arguments with which the data can be * accessed (e.g. float, double, int64_t, T*) */ -template < - class T, class DataSource, typename _DistanceType = T, - typename IndexType = size_t> +template struct L2_Simple_Adaptor { using ElementType = T; @@ -713,19 +679,14 @@ struct L2_Simple_Adaptor const DataSource& data_source; - L2_Simple_Adaptor(const DataSource& _data_source) - : data_source(_data_source) - { - } + L2_Simple_Adaptor(const DataSource& _data_source) : data_source(_data_source) {} - inline DistanceType evalMetric( - const T* a, const IndexType b_idx, size_t size) const + inline DistanceType evalMetric(const T* a, const IndexType b_idx, size_t size) const { DistanceType result = DistanceType(); for (size_t i = 0; i < size; ++i) { - const DistanceType diff = - a[i] - data_source.kdtree_get_pt(b_idx, i); + const DistanceType diff = a[i] - data_source.kdtree_get_pt(b_idx, i); result += diff * diff; } return result; @@ -749,9 +710,7 @@ struct L2_Simple_Adaptor * \tparam IndexType Type of the arguments with which the data can be * accessed (e.g. float, double, int64_t, T*) */ -template < - class T, class DataSource, typename _DistanceType = T, - typename IndexType = size_t> +template struct SO2_Adaptor { using ElementType = T; @@ -761,11 +720,9 @@ struct SO2_Adaptor SO2_Adaptor(const DataSource& _data_source) : data_source(_data_source) {} - inline DistanceType evalMetric( - const T* a, const IndexType b_idx, size_t size) const + inline DistanceType evalMetric(const T* a, const IndexType b_idx, size_t size) const { - return accum_dist( - a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1), size - 1); + return accum_dist(a[size - 1], data_source.kdtree_get_pt(b_idx, size - 1), size - 1); } /** Note: this assumes that input angles are already in the range [-pi,pi] @@ -794,24 +751,17 @@ struct SO2_Adaptor * \tparam IndexType Type of the arguments with which the data can be * accessed (e.g. float, double, int64_t, T*) */ -template < - class T, class DataSource, typename _DistanceType = T, - typename IndexType = size_t> +template struct SO3_Adaptor { using ElementType = T; using DistanceType = _DistanceType; - L2_Simple_Adaptor - distance_L2_Simple; + L2_Simple_Adaptor distance_L2_Simple; - SO3_Adaptor(const DataSource& _data_source) - : distance_L2_Simple(_data_source) - { - } + SO3_Adaptor(const DataSource& _data_source) : distance_L2_Simple(_data_source) {} - inline DistanceType evalMetric( - const T* a, const IndexType b_idx, size_t size) const + inline DistanceType evalMetric(const T* a, const IndexType b_idx, size_t size) const { return distance_L2_Simple.evalMetric(a, b_idx, size); } @@ -885,8 +835,7 @@ enum class KDTreeSingleIndexAdaptorFlags inline std::underlying_type::type operator&( KDTreeSingleIndexAdaptorFlags lhs, KDTreeSingleIndexAdaptorFlags rhs) { - using underlying = - typename std::underlying_type::type; + using underlying = typename std::underlying_type::type; return static_cast(lhs) & static_cast(rhs); } @@ -894,13 +843,10 @@ inline std::underlying_type::type operator&( struct KDTreeSingleIndexAdaptorParams { KDTreeSingleIndexAdaptorParams( - size_t _leaf_max_size = 10, - KDTreeSingleIndexAdaptorFlags _flags = - KDTreeSingleIndexAdaptorFlags::None, - unsigned int _n_thread_build = 1) - : leaf_max_size(_leaf_max_size), - flags(_flags), - n_thread_build(_n_thread_build) + size_t _leaf_max_size = 10, + KDTreeSingleIndexAdaptorFlags _flags = KDTreeSingleIndexAdaptorFlags::None, + unsigned int _n_thread_build = 1) + : leaf_max_size(_leaf_max_size), flags(_flags), n_thread_build(_n_thread_build) { } @@ -912,10 +858,7 @@ struct KDTreeSingleIndexAdaptorParams /** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */ struct SearchParameters { - SearchParameters(float eps_ = 0, bool sorted_ = true) - : eps(eps_), sorted(sorted_) - { - } + SearchParameters(float eps_ = 0, bool sorted_ = true) : eps(eps_), sorted(sorted_) {} float eps; //!< search for eps-approximate neighbours (default: 0) bool sorted; //!< only for radius search, require neighbours sorted by @@ -954,8 +897,8 @@ class PooledAllocator using Size = size_t; Size remaining_ = 0; //!< Number of bytes left in current block of storage - void* base_ = nullptr; //!< Pointer to base of current block of storage - void* loc_ = nullptr; //!< Current location in block to next allocate + void* base_ = nullptr; //!< Pointer to base of current block of storage + void* loc_ = nullptr; //!< Current location in block to next allocate void internal_init() { @@ -1012,12 +955,14 @@ class PooledAllocator wastedMemory += remaining_; /* Allocate new storage. */ - const Size blocksize = - size > BLOCKSIZE ? size + WORDSIZE : BLOCKSIZE + WORDSIZE; + const Size blocksize = size > BLOCKSIZE ? size + WORDSIZE : BLOCKSIZE + WORDSIZE; // use the standard C malloc to allocate memory void* m = ::malloc(blocksize); - if (!m) { throw std::bad_alloc(); } + if (!m) + { + throw std::bad_alloc(); + } /* Fill first word of new block with pointer to previous block. */ static_cast(m)[0] = base_; @@ -1194,8 +1139,7 @@ class KDTreeBaseClass Size veclen(const Derived& obj) const { return DIM > 0 ? DIM : obj.dim_; } /// Helper accessor to the dataset points: - ElementType dataset_get( - const Derived& obj, IndexType element, Dimension component) const + ElementType dataset_get(const Derived& obj, IndexType element, Dimension component) const { return obj.dataset_.kdtree_get_pt(element, component); } @@ -1215,8 +1159,8 @@ class KDTreeBaseClass * Compute the minimum and maximum element values in the specified dimension */ void computeMinMax( - const Derived& obj, Offset ind, Size count, Dimension element, - ElementType& min_elem, ElementType& max_elem) const + const Derived& obj, Offset ind, Size count, Dimension element, ElementType& min_elem, + ElementType& max_elem) const { min_elem = dataset_get(obj, vAcc_[ind], element); max_elem = min_elem; @@ -1237,12 +1181,11 @@ class KDTreeBaseClass * @param bbox bounding box used as input for splitting and output for * parent node */ - NodePtr divideTree( - Derived& obj, const Offset left, const Offset right, BoundingBox& bbox) + NodePtr divideTree(Derived& obj, const Offset left, const Offset right, BoundingBox& bbox) { assert(left < obj.dataset_.kdtree_get_point_count()); - NodePtr node = obj.pool_.template allocate(); // allocate memory + NodePtr node = obj.pool_.template allocate(); // allocate memory const auto dims = (DIM > 0 ? DIM : obj.dim_); /* If too few exemplars remain, then make this a leaf node. */ @@ -1281,12 +1224,12 @@ class KDTreeBaseClass /* Recurse on left */ BoundingBox left_bbox(bbox); left_bbox[cutfeat].high = cutval; - node->child1 = this->divideTree(obj, left, left + idx, left_bbox); + node->child1 = this->divideTree(obj, left, left + idx, left_bbox); /* Recurse on right */ BoundingBox right_bbox(bbox); right_bbox[cutfeat].low = cutval; - node->child2 = this->divideTree(obj, left + idx, right, right_bbox); + node->child2 = this->divideTree(obj, left + idx, right, right_bbox); node->node_type.sub.divlow = left_bbox[cutfeat].high; node->node_type.sub.divhigh = right_bbox[cutfeat].low; @@ -1318,7 +1261,7 @@ class KDTreeBaseClass std::atomic& thread_count, std::mutex& mutex) { std::unique_lock lock(mutex); - NodePtr node = obj.pool_.template allocate(); // allocate memory + NodePtr node = obj.pool_.template allocate(); // allocate memory lock.unlock(); const auto dims = (DIM > 0 ? DIM : obj.dim_); @@ -1367,19 +1310,21 @@ class KDTreeBaseClass /* Concurrent thread for right recursion */ right_future = std::async( - std::launch::async, &KDTreeBaseClass::divideTreeConcurrent, - this, std::ref(obj), left + idx, right, - std::ref(right_bbox), std::ref(thread_count), + std::launch::async, &KDTreeBaseClass::divideTreeConcurrent, this, std::ref(obj), + left + idx, right, std::ref(right_bbox), std::ref(thread_count), std::ref(mutex)); } - else { --thread_count; } + else + { + --thread_count; + } /* Recurse on left in this thread */ BoundingBox left_bbox(bbox); left_bbox[cutfeat].high = cutval; - node->child1 = this->divideTreeConcurrent( - obj, left, left + idx, left_bbox, thread_count, mutex); + node->child1 = + this->divideTreeConcurrent(obj, left, left + idx, left_bbox, thread_count, mutex); if (right_future.valid()) { @@ -1410,8 +1355,8 @@ class KDTreeBaseClass } void middleSplit_( - const Derived& obj, const Offset ind, const Size count, Offset& index, - Dimension& cutfeat, DistanceType& cutval, const BoundingBox& bbox) + const Derived& obj, const Offset ind, const Size count, Offset& index, Dimension& cutfeat, + DistanceType& cutval, const BoundingBox& bbox) { const auto dims = (DIM > 0 ? DIM : obj.dim_); const auto EPS = static_cast(0.00001); @@ -1489,9 +1434,7 @@ class KDTreeBaseClass Offset lim1, lim2; planeSplit(obj, ind, count, cutfeat, cutval, lim1, lim2); - index = (lim1 > count / 2) ? lim1 - : (lim2 < count / 2) ? lim2 - : count / 2; + index = (lim1 > count / 2) ? lim1 : (lim2 < count / 2) ? lim2 : count / 2; } /** @@ -1504,9 +1447,8 @@ class KDTreeBaseClass * dataset[ind[lim2..count]][cutfeat] > cutval */ void planeSplit( - const Derived& obj, const Offset ind, const Size count, - const Dimension cutfeat, const DistanceType& cutval, Offset& lim1, - Offset& lim2) + const Derived& obj, const Offset ind, const Size count, const Dimension cutfeat, + const DistanceType& cutval, Offset& lim1, Offset& lim2) { // Dutch National Flag algorithm for three-way partitioning Offset left = 0; @@ -1528,7 +1470,10 @@ class KDTreeBaseClass std::swap(vAcc_[ind + mid], vAcc_[ind + right]); right--; } - else { mid++; } + else + { + mid++; + } } lim1 = left; @@ -1536,8 +1481,7 @@ class KDTreeBaseClass } DistanceType computeInitialDistances( - const Derived& obj, const ElementType* vec, - distance_vector_t& dists) const + const Derived& obj, const ElementType* vec, distance_vector_t& dists) const { assert(vec); DistanceType dist = DistanceType(); @@ -1546,34 +1490,43 @@ class KDTreeBaseClass { if (vec[i] < obj.root_bbox_[i].low) { - dists[i] = - obj.distance_.accum_dist(vec[i], obj.root_bbox_[i].low, i); + dists[i] = obj.distance_.accum_dist(vec[i], obj.root_bbox_[i].low, i); dist += dists[i]; } if (vec[i] > obj.root_bbox_[i].high) { - dists[i] = - obj.distance_.accum_dist(vec[i], obj.root_bbox_[i].high, i); + dists[i] = obj.distance_.accum_dist(vec[i], obj.root_bbox_[i].high, i); dist += dists[i]; } } return dist; } - static void save_tree( - const Derived& obj, std::ostream& stream, const NodeConstPtr tree) + static void save_tree(const Derived& obj, std::ostream& stream, const NodeConstPtr tree) { save_value(stream, *tree); - if (tree->child1 != nullptr) { save_tree(obj, stream, tree->child1); } - if (tree->child2 != nullptr) { save_tree(obj, stream, tree->child2); } + if (tree->child1 != nullptr) + { + save_tree(obj, stream, tree->child1); + } + if (tree->child2 != nullptr) + { + save_tree(obj, stream, tree->child2); + } } static void load_tree(Derived& obj, std::istream& stream, NodePtr& tree) { tree = obj.pool_.template allocate(); load_value(stream, *tree); - if (tree->child1 != nullptr) { load_tree(obj, stream, tree->child1); } - if (tree->child2 != nullptr) { load_tree(obj, stream, tree->child2); } + if (tree->child1 != nullptr) + { + load_tree(obj, stream, tree->child1); + } + if (tree->child2 != nullptr) + { + load_tree(obj, stream, tree->child2); + } } /** Stores the index in a binary file. @@ -1648,19 +1601,16 @@ class KDTreeBaseClass * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType Will * be typically size_t or int */ -template < - typename Distance, class DatasetAdaptor, int32_t DIM = -1, - typename index_t = uint32_t> +template class KDTreeSingleIndexAdaptor : public KDTreeBaseClass< - KDTreeSingleIndexAdaptor, - Distance, DatasetAdaptor, DIM, index_t> + KDTreeSingleIndexAdaptor, Distance, + DatasetAdaptor, DIM, index_t> { public: /** Deleted copy constructor*/ explicit KDTreeSingleIndexAdaptor( - const KDTreeSingleIndexAdaptor< - Distance, DatasetAdaptor, DIM, index_t>&) = delete; + const KDTreeSingleIndexAdaptor&) = delete; /** The data source used by this index */ const DatasetAdaptor& dataset_; @@ -1670,9 +1620,8 @@ class KDTreeSingleIndexAdaptor Distance distance_; using Base = typename nanoflann::KDTreeBaseClass< - nanoflann::KDTreeSingleIndexAdaptor< - Distance, DatasetAdaptor, DIM, index_t>, - Distance, DatasetAdaptor, DIM, index_t>; + nanoflann::KDTreeSingleIndexAdaptor, Distance, + DatasetAdaptor, DIM, index_t>; using Offset = typename Base::Offset; using Size = typename Base::Size; @@ -1735,9 +1684,7 @@ class KDTreeSingleIndexAdaptor } private: - void init( - const Dimension dimensionality, - const KDTreeSingleIndexAdaptorParams& params) + void init(const Dimension dimensionality, const KDTreeSingleIndexAdaptorParams& params) { Base::size_ = dataset_.kdtree_get_point_count(); Base::size_at_index_build_ = Base::size_; @@ -1750,12 +1697,10 @@ class KDTreeSingleIndexAdaptor } else { - Base::n_thread_build_ = - std::max(std::thread::hardware_concurrency(), 1u); + Base::n_thread_build_ = std::max(std::thread::hardware_concurrency(), 1u); } - if (!(params.flags & - KDTreeSingleIndexAdaptorFlags::SkipInitialBuildIndex)) + if (!(params.flags & KDTreeSingleIndexAdaptorFlags::SkipInitialBuildIndex)) { // Build KD-tree: buildIndex(); @@ -1778,8 +1723,7 @@ class KDTreeSingleIndexAdaptor // construct the tree if (Base::n_thread_build_ == 1) { - Base::root_node_ = - this->divideTree(*this, 0, Base::size_, Base::root_bbox_); + Base::root_node_ = this->divideTree(*this, 0, Base::size_, Base::root_bbox_); } else { @@ -1815,8 +1759,7 @@ class KDTreeSingleIndexAdaptor */ template bool findNeighbors( - RESULTSET& result, const ElementType* vec, - const SearchParameters& searchParams = {}) const + RESULTSET& result, const ElementType* vec, const SearchParameters& searchParams = {}) const { assert(vec); if (this->size(*this) == 0) return false; @@ -1874,8 +1817,7 @@ class KDTreeSingleIndexAdaptor // If this is a leaf node, then do check and return. if (!node->child1) // (if one node is nullptr, both are) { - for (Offset i = node->node_type.lr.left; - i < node->node_type.lr.right; ++i) + for (Offset i = node->node_type.lr.left; i < node->node_type.lr.right; ++i) { if (contains(bbox, Base::vAcc_[i])) { @@ -1918,8 +1860,8 @@ class KDTreeSingleIndexAdaptor * number of elements in the tree is less than `num_closest`. */ Size knnSearch( - const ElementType* query_point, const Size num_closest, - IndexType* out_indices, DistanceType* out_distances) const + const ElementType* query_point, const Size num_closest, IndexType* out_indices, + DistanceType* out_distances) const { nanoflann::KNNResultSet resultSet(num_closest); resultSet.init(out_indices, out_distances); @@ -1949,12 +1891,10 @@ class KDTreeSingleIndexAdaptor Size radiusSearch( const ElementType* query_point, const DistanceType& radius, std::vector>& IndicesDists, - const SearchParameters& searchParams = {}) const + const SearchParameters& searchParams = {}) const { - RadiusResultSet resultSet( - radius, IndicesDists); - const Size nFound = - radiusSearchCustomCallback(query_point, resultSet, searchParams); + RadiusResultSet resultSet(radius, IndicesDists); + const Size nFound = radiusSearchCustomCallback(query_point, resultSet, searchParams); return nFound; } @@ -1989,12 +1929,10 @@ class KDTreeSingleIndexAdaptor * number of elements in the tree is less than `num_closest`. */ Size rknnSearch( - const ElementType* query_point, const Size num_closest, - IndexType* out_indices, DistanceType* out_distances, - const DistanceType& radius) const + const ElementType* query_point, const Size num_closest, IndexType* out_indices, + DistanceType* out_distances, const DistanceType& radius) const { - nanoflann::RKNNResultSet resultSet( - num_closest, radius); + nanoflann::RKNNResultSet resultSet(num_closest, radius); resultSet.init(out_indices, out_distances); findNeighbors(resultSet, query_point); return resultSet.size(); @@ -2010,8 +1948,7 @@ class KDTreeSingleIndexAdaptor // Create a permutable array of indices to the input vectors. Base::size_ = dataset_.kdtree_get_point_count(); if (Base::vAcc_.size() != Base::size_) Base::vAcc_.resize(Base::size_); - for (IndexType i = 0; i < static_cast(Base::size_); i++) - Base::vAcc_[i] = i; + for (IndexType i = 0; i < static_cast(Base::size_); i++) Base::vAcc_[i] = i; } void computeBoundingBox(BoundingBox& bbox) @@ -2031,15 +1968,13 @@ class KDTreeSingleIndexAdaptor "no data points found."); for (Dimension i = 0; i < dims; ++i) { - bbox[i].low = bbox[i].high = - this->dataset_get(*this, Base::vAcc_[0], i); + bbox[i].low = bbox[i].high = this->dataset_get(*this, Base::vAcc_[0], i); } for (Offset k = 1; k < N; ++k) { for (Dimension i = 0; i < dims; ++i) { - const auto val = - this->dataset_get(*this, Base::vAcc_[k], i); + const auto val = this->dataset_get(*this, Base::vAcc_[k], i); if (val < bbox[i].low) bbox[i].low = val; if (val > bbox[i].high) bbox[i].high = val; } @@ -2066,19 +2001,17 @@ class KDTreeSingleIndexAdaptor */ template bool searchLevel( - RESULTSET& result_set, const ElementType* vec, const NodePtr node, - DistanceType mindist, distance_vector_t& dists, - const float epsError) const + RESULTSET& result_set, const ElementType* vec, const NodePtr node, DistanceType mindist, + distance_vector_t& dists, const float epsError) const { // If this is a leaf node, then do check and return. if (!node->child1) // (if one node is nullptr, both are) { - for (Offset i = node->node_type.lr.left; - i < node->node_type.lr.right; ++i) + for (Offset i = node->node_type.lr.left; i < node->node_type.lr.right; ++i) { const IndexType accessor = Base::vAcc_[i]; // reorder... : i; - DistanceType dist = distance_.evalMetric( - vec, accessor, (DIM > 0 ? DIM : Base::dim_)); + DistanceType dist = + distance_.evalMetric(vec, accessor, (DIM > 0 ? DIM : Base::dim_)); if (dist < result_set.worstDist()) { if (!result_set.addPoint(dist, Base::vAcc_[i])) @@ -2105,15 +2038,13 @@ class KDTreeSingleIndexAdaptor { bestChild = node->child1; otherChild = node->child2; - cut_dist = - distance_.accum_dist(val, node->node_type.sub.divhigh, idx); + cut_dist = distance_.accum_dist(val, node->node_type.sub.divhigh, idx); } else { bestChild = node->child2; otherChild = node->child1; - cut_dist = - distance_.accum_dist(val, node->node_type.sub.divlow, idx); + cut_dist = distance_.accum_dist(val, node->node_type.sub.divlow, idx); } /* Call recursively to search next level down. */ @@ -2129,8 +2060,7 @@ class KDTreeSingleIndexAdaptor dists[idx] = cut_dist; if (mindist * epsError <= result_set.worstDist()) { - if (!searchLevel( - result_set, vec, otherChild, mindist, dists, epsError)) + if (!searchLevel(result_set, vec, otherChild, mindist, dists, epsError)) { // the resultset doesn't want to receive any more points, we're // done searching! @@ -2147,10 +2077,7 @@ class KDTreeSingleIndexAdaptor * when loading the index object it must be constructed associated to the * same source of data points used while building it. See the example: * examples/saveload_example.cpp \sa loadIndex */ - void saveIndex(std::ostream& stream) const - { - Base::saveIndex(*this, stream); - } + void saveIndex(std::ostream& stream) const { Base::saveIndex(*this, stream); } /** Loads a previous index from a binary file. * IMPORTANT NOTE: The set of data points is NOT stored in the file, so @@ -2198,14 +2125,11 @@ class KDTreeSingleIndexAdaptor * \tparam IndexType Type of the arguments with which the data can be * accessed (e.g. float, double, int64_t, T*) */ -template < - typename Distance, class DatasetAdaptor, int32_t DIM = -1, - typename IndexType = uint32_t> +template class KDTreeSingleIndexDynamicAdaptor_ : public KDTreeBaseClass< - KDTreeSingleIndexDynamicAdaptor_< - Distance, DatasetAdaptor, DIM, IndexType>, - Distance, DatasetAdaptor, DIM, IndexType> + KDTreeSingleIndexDynamicAdaptor_, Distance, + DatasetAdaptor, DIM, IndexType> { public: /** @@ -2220,8 +2144,7 @@ class KDTreeSingleIndexDynamicAdaptor_ Distance distance_; using Base = typename nanoflann::KDTreeBaseClass< - nanoflann::KDTreeSingleIndexDynamicAdaptor_< - Distance, DatasetAdaptor, DIM, IndexType>, + nanoflann::KDTreeSingleIndexDynamicAdaptor_, Distance, DatasetAdaptor, DIM, IndexType>; using ElementType = typename Base::ElementType; @@ -2261,12 +2184,8 @@ class KDTreeSingleIndexDynamicAdaptor_ KDTreeSingleIndexDynamicAdaptor_( const Dimension dimensionality, const DatasetAdaptor& inputData, std::vector& treeIndex, - const KDTreeSingleIndexAdaptorParams& params = - KDTreeSingleIndexAdaptorParams()) - : dataset_(inputData), - index_params_(params), - treeIndex_(treeIndex), - distance_(inputData) + const KDTreeSingleIndexAdaptorParams& params = KDTreeSingleIndexAdaptorParams()) + : dataset_(inputData), index_params_(params), treeIndex_(treeIndex), distance_(inputData) { Base::size_ = 0; Base::size_at_index_build_ = 0; @@ -2280,18 +2199,15 @@ class KDTreeSingleIndexDynamicAdaptor_ } else { - Base::n_thread_build_ = - std::max(std::thread::hardware_concurrency(), 1u); + Base::n_thread_build_ = std::max(std::thread::hardware_concurrency(), 1u); } } /** Explicitly default the copy constructor */ - KDTreeSingleIndexDynamicAdaptor_( - const KDTreeSingleIndexDynamicAdaptor_& rhs) = default; + KDTreeSingleIndexDynamicAdaptor_(const KDTreeSingleIndexDynamicAdaptor_& rhs) = default; /** Assignment operator definiton */ - KDTreeSingleIndexDynamicAdaptor_ operator=( - const KDTreeSingleIndexDynamicAdaptor_& rhs) + KDTreeSingleIndexDynamicAdaptor_ operator=(const KDTreeSingleIndexDynamicAdaptor_& rhs) { KDTreeSingleIndexDynamicAdaptor_ tmp(rhs); std::swap(Base::vAcc_, tmp.Base::vAcc_); @@ -2319,8 +2235,7 @@ class KDTreeSingleIndexDynamicAdaptor_ // construct the tree if (Base::n_thread_build_ == 1) { - Base::root_node_ = - this->divideTree(*this, 0, Base::size_, Base::root_bbox_); + Base::root_node_ = this->divideTree(*this, 0, Base::size_, Base::root_bbox_); } else { @@ -2360,8 +2275,7 @@ class KDTreeSingleIndexDynamicAdaptor_ */ template bool findNeighbors( - RESULTSET& result, const ElementType* vec, - const SearchParameters& searchParams = {}) const + RESULTSET& result, const ElementType* vec, const SearchParameters& searchParams = {}) const { assert(vec); if (this->size(*this) == 0) return false; @@ -2397,9 +2311,8 @@ class KDTreeSingleIndexDynamicAdaptor_ * number of elements in the tree is less than `num_closest`. */ Size knnSearch( - const ElementType* query_point, const Size num_closest, - IndexType* out_indices, DistanceType* out_distances, - const SearchParameters& searchParams = {}) const + const ElementType* query_point, const Size num_closest, IndexType* out_indices, + DistanceType* out_distances, const SearchParameters& searchParams = {}) const { nanoflann::KNNResultSet resultSet(num_closest); resultSet.init(out_indices, out_distances); @@ -2429,12 +2342,10 @@ class KDTreeSingleIndexDynamicAdaptor_ Size radiusSearch( const ElementType* query_point, const DistanceType& radius, std::vector>& IndicesDists, - const SearchParameters& searchParams = {}) const + const SearchParameters& searchParams = {}) const { - RadiusResultSet resultSet( - radius, IndicesDists); - const size_t nFound = - radiusSearchCustomCallback(query_point, resultSet, searchParams); + RadiusResultSet resultSet(radius, IndicesDists); + const size_t nFound = radiusSearchCustomCallback(query_point, resultSet, searchParams); return nFound; } @@ -2473,15 +2384,13 @@ class KDTreeSingleIndexDynamicAdaptor_ "no data points found."); for (Dimension i = 0; i < dims; ++i) { - bbox[i].low = bbox[i].high = - this->dataset_get(*this, Base::vAcc_[0], i); + bbox[i].low = bbox[i].high = this->dataset_get(*this, Base::vAcc_[0], i); } for (Offset k = 1; k < N; ++k) { for (Dimension i = 0; i < dims; ++i) { - const auto val = - this->dataset_get(*this, Base::vAcc_[k], i); + const auto val = this->dataset_get(*this, Base::vAcc_[k], i); if (val < bbox[i].low) bbox[i].low = val; if (val > bbox[i].high) bbox[i].high = val; } @@ -2495,26 +2404,22 @@ class KDTreeSingleIndexDynamicAdaptor_ */ template void searchLevel( - RESULTSET& result_set, const ElementType* vec, const NodePtr node, - DistanceType mindist, distance_vector_t& dists, - const float epsError) const + RESULTSET& result_set, const ElementType* vec, const NodePtr node, DistanceType mindist, + distance_vector_t& dists, const float epsError) const { // If this is a leaf node, then do check and return. if (!node->child1) // (if one node is nullptr, both are) { - for (Offset i = node->node_type.lr.left; - i < node->node_type.lr.right; ++i) + for (Offset i = node->node_type.lr.left; i < node->node_type.lr.right; ++i) { const IndexType index = Base::vAcc_[i]; // reorder... : i; if (treeIndex_[index] == -1) continue; - DistanceType dist = distance_.evalMetric( - vec, index, (DIM > 0 ? DIM : Base::dim_)); + DistanceType dist = distance_.evalMetric(vec, index, (DIM > 0 ? DIM : Base::dim_)); if (dist < result_set.worstDist()) { if (!result_set.addPoint( static_cast(dist), - static_cast( - Base::vAcc_[i]))) + static_cast(Base::vAcc_[i]))) { // the resultset doesn't want to receive any more // points, we're done searching! @@ -2538,15 +2443,13 @@ class KDTreeSingleIndexDynamicAdaptor_ { bestChild = node->child1; otherChild = node->child2; - cut_dist = - distance_.accum_dist(val, node->node_type.sub.divhigh, idx); + cut_dist = distance_.accum_dist(val, node->node_type.sub.divhigh, idx); } else { bestChild = node->child2; otherChild = node->child1; - cut_dist = - distance_.accum_dist(val, node->node_type.sub.divlow, idx); + cut_dist = distance_.accum_dist(val, node->node_type.sub.divlow, idx); } /* Call recursively to search next level down. */ @@ -2592,21 +2495,17 @@ class KDTreeSingleIndexDynamicAdaptor_ * Dimensionality of data points (e.g. 3 for 3D points) \tparam IndexType * Will be typically size_t or int */ -template < - typename Distance, class DatasetAdaptor, int32_t DIM = -1, - typename IndexType = uint32_t> +template class KDTreeSingleIndexDynamicAdaptor { public: using ElementType = typename Distance::ElementType; using DistanceType = typename Distance::DistanceType; - using Offset = typename KDTreeSingleIndexDynamicAdaptor_< - Distance, DatasetAdaptor, DIM>::Offset; - using Size = typename KDTreeSingleIndexDynamicAdaptor_< - Distance, DatasetAdaptor, DIM>::Size; - using Dimension = typename KDTreeSingleIndexDynamicAdaptor_< - Distance, DatasetAdaptor, DIM>::Dimension; + using Offset = typename KDTreeSingleIndexDynamicAdaptor_::Offset; + using Size = typename KDTreeSingleIndexDynamicAdaptor_::Size; + using Dimension = + typename KDTreeSingleIndexDynamicAdaptor_::Dimension; protected: Size leaf_max_size_; @@ -2627,17 +2526,14 @@ class KDTreeSingleIndexDynamicAdaptor Dimension dim_; //!< Dimensionality of each data point - using index_container_t = KDTreeSingleIndexDynamicAdaptor_< - Distance, DatasetAdaptor, DIM, IndexType>; + using index_container_t = + KDTreeSingleIndexDynamicAdaptor_; std::vector index_; public: /** Get a const ref to the internal list of indices; the number of indices * is adapted dynamically as the dataset grows in size. */ - const std::vector& getAllIndices() const - { - return index_; - } + const std::vector& getAllIndices() const { return index_; } private: /** finds position of least significant unset bit */ @@ -2655,11 +2551,10 @@ class KDTreeSingleIndexDynamicAdaptor /** Creates multiple empty trees to handle dynamic support */ void init() { - using my_kd_tree_t = KDTreeSingleIndexDynamicAdaptor_< - Distance, DatasetAdaptor, DIM, IndexType>; + using my_kd_tree_t = + KDTreeSingleIndexDynamicAdaptor_; std::vector index( - treeCount_, - my_kd_tree_t(dim_ /*dim*/, dataset_, treeIndex_, index_params_)); + treeCount_, my_kd_tree_t(dim_ /*dim*/, dataset_, treeIndex_, index_params_)); index_ = index; } @@ -2683,9 +2578,8 @@ class KDTreeSingleIndexDynamicAdaptor */ explicit KDTreeSingleIndexDynamicAdaptor( const int dimensionality, const DatasetAdaptor& inputData, - const KDTreeSingleIndexAdaptorParams& params = - KDTreeSingleIndexAdaptorParams(), - const size_t maximumPointCount = 1000000000U) + const KDTreeSingleIndexAdaptorParams& params = KDTreeSingleIndexAdaptorParams(), + const size_t maximumPointCount = 1000000000U) : dataset_(inputData), index_params_(params), distance_(inputData) { treeCount_ = static_cast(std::log2(maximumPointCount)) + 1; @@ -2696,13 +2590,15 @@ class KDTreeSingleIndexDynamicAdaptor leaf_max_size_ = params.leaf_max_size; init(); const size_t num_initial_points = dataset_.kdtree_get_point_count(); - if (num_initial_points > 0) { addPoints(0, num_initial_points - 1); } + if (num_initial_points > 0) + { + addPoints(0, num_initial_points - 1); + } } /** Deleted copy constructor*/ explicit KDTreeSingleIndexDynamicAdaptor( - const KDTreeSingleIndexDynamicAdaptor< - Distance, DatasetAdaptor, DIM, IndexType>&) = delete; + const KDTreeSingleIndexDynamicAdaptor&) = delete; /** Add points to the set, Inserts all points from [start, end] */ void addPoints(IndexType start, IndexType end) @@ -2725,12 +2621,10 @@ class KDTreeSingleIndexDynamicAdaptor for (int i = 0; i < pos; i++) { - for (int j = 0; j < static_cast(index_[i].vAcc_.size()); - j++) + for (int j = 0; j < static_cast(index_[i].vAcc_.size()); j++) { index_[pos].vAcc_.push_back(index_[i].vAcc_[j]); - if (treeIndex_[index_[i].vAcc_[j]] != -1) - treeIndex_[index_[i].vAcc_[j]] = pos; + if (treeIndex_[index_[i].vAcc_[j]] != -1) treeIndex_[index_[i].vAcc_[j]] = pos; } index_[i].vAcc_.clear(); } @@ -2771,8 +2665,7 @@ class KDTreeSingleIndexDynamicAdaptor */ template bool findNeighbors( - RESULTSET& result, const ElementType* vec, - const SearchParameters& searchParams = {}) const + RESULTSET& result, const ElementType* vec, const SearchParameters& searchParams = {}) const { for (size_t i = 0; i < treeCount_; i++) { @@ -2812,17 +2705,13 @@ template < bool row_major = true> struct KDTreeEigenMatrixAdaptor { - using self_t = - KDTreeEigenMatrixAdaptor; + using self_t = KDTreeEigenMatrixAdaptor; using num_t = typename MatrixType::Scalar; using IndexType = typename MatrixType::Index; - using metric_t = typename Distance::template traits< - num_t, self_t, IndexType>::distance_t; + using metric_t = typename Distance::template traits::distance_t; using index_t = KDTreeSingleIndexAdaptor< - metric_t, self_t, - row_major ? MatrixType::ColsAtCompileTime - : MatrixType::RowsAtCompileTime, + metric_t, self_t, row_major ? MatrixType::ColsAtCompileTime : MatrixType::RowsAtCompileTime, IndexType>; index_t* index_; //! The kd-tree index for the user to call its methods as @@ -2834,8 +2723,7 @@ struct KDTreeEigenMatrixAdaptor /// Constructor: takes a const ref to the matrix object with the data points explicit KDTreeEigenMatrixAdaptor( - const Dimension dimensionality, - const std::reference_wrapper& mat, + const Dimension dimensionality, const std::reference_wrapper& mat, const int leaf_max_size = 10, const unsigned int n_thread_build = 1) : m_data_matrix(mat) { @@ -2851,8 +2739,7 @@ struct KDTreeEigenMatrixAdaptor index_ = new index_t( dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams( - leaf_max_size, nanoflann::KDTreeSingleIndexAdaptorFlags::None, - n_thread_build)); + leaf_max_size, nanoflann::KDTreeSingleIndexAdaptorFlags::None, n_thread_build)); } public: @@ -2872,8 +2759,8 @@ struct KDTreeEigenMatrixAdaptor * distances. */ void query( - const num_t* query_point, const Size num_closest, - IndexType* out_indices, num_t* out_distances) const + const num_t* query_point, const Size num_closest, IndexType* out_indices, + num_t* out_distances) const { nanoflann::KNNResultSet resultSet(num_closest); resultSet.init(out_indices, out_distances); diff --git a/tests/test_main.cpp b/tests/test_main.cpp index 19f72e3..8821470 100644 --- a/tests/test_main.cpp +++ b/tests/test_main.cpp @@ -60,19 +60,16 @@ void L2_vs_L2_simple_test(const size_t N, const size_t num_results) // construct a kd-tree index: using my_kd_tree_simple_t = KDTreeSingleIndexAdaptor< - L2_Simple_Adaptor>, PointCloud, - 3 /* dim */ + L2_Simple_Adaptor>, PointCloud, 3 /* dim */ >; using my_kd_tree_t = KDTreeSingleIndexAdaptor< L2_Adaptor>, PointCloud, 3 /* dim */ >; - my_kd_tree_simple_t index1( - 3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); + my_kd_tree_simple_t index1(3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); - my_kd_tree_t index2( - 3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); + my_kd_tree_t index2(3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); // do a knn search std::vector ret_index(num_results); @@ -88,8 +85,14 @@ void L2_vs_L2_simple_test(const size_t N, const size_t num_results) index2.findNeighbors(resultSet, &query_pt[0]); - if (N >= num_results) { EXPECT_EQ(resultSet.size(), num_results); } - else { EXPECT_EQ(resultSet.size(), N); } + if (N >= num_results) + { + EXPECT_EQ(resultSet.size(), num_results); + } + else + { + EXPECT_EQ(resultSet.size(), N); + } for (size_t i = 0; i < resultSet.size(); i++) { @@ -109,12 +112,11 @@ void L2_vs_L2_simple_test(const size_t N, const size_t num_results) const num_t maxRadiusSqrSearch = 10.0 * 10.0; std::vector> + typename my_kd_tree_simple_t::IndexType, typename my_kd_tree_simple_t::DistanceType>> radiusIdxs; - nanoflann::RadiusResultSet - radiusResults(maxRadiusSqrSearch, radiusIdxs); + nanoflann::RadiusResultSet radiusResults( + maxRadiusSqrSearch, radiusIdxs); radiusResults.init(); nanoflann::SearchParameters searchParams; searchParams.sorted = true; @@ -135,21 +137,18 @@ using namespace nanoflann; template void generateRandomPointCloud( - std::vector>& samples, const size_t N, const size_t dim, - const NUM max_range) + std::vector>& samples, const size_t N, const size_t dim, const NUM max_range) { samples.resize(N); for (size_t i = 0; i < N; i++) { samples[i].resize(dim); - for (size_t d = 0; d < dim; d++) - samples[i][d] = max_range * (rand() % 1000) / NUM(1000.0); + for (size_t d = 0; d < dim; d++) samples[i][d] = max_range * (rand() % 1000) / NUM(1000.0); } } template -void L1_vs_bruteforce_test( - const size_t nSamples, const size_t DIM, const size_t numToSearch) +void L1_vs_bruteforce_test(const size_t nSamples, const size_t DIM, const size_t numToSearch) { std::vector> samples; @@ -196,8 +195,7 @@ void L1_vs_bruteforce_test( for (size_t i = 0; i < nSamples; i++) { double dist = 0.0; - for (size_t d = 0; d < DIM; d++) - dist += std::abs(query_pt[d] - samples[i][d]); + for (size_t d = 0; d < DIM; d++) dist += std::abs(query_pt[d] - samples[i][d]); bf_nn.emplace(dist, i); } } @@ -218,16 +216,15 @@ void L1_vs_bruteforce_test( // indices may be not in the (rare) case of a tie: EXPECT_NEAR(bf_idx2dist.at(ret_indexes[i]), out_dists_sqr[i], 1e-3) - << "For: numToSearch=" << numToSearch - << " out_dists_sqr[i]=" << out_dists_sqr[i] << "\n"; + << "For: numToSearch=" << numToSearch << " out_dists_sqr[i]=" << out_dists_sqr[i] + << "\n"; } } } template void rknn_L1_vs_bruteforce_test( - const size_t nSamples, const size_t DIM, const size_t numToSearch, - const NUM maxRadiusSqr) + const size_t nSamples, const size_t DIM, const size_t numToSearch, const NUM maxRadiusSqr) { std::vector> samples; @@ -273,8 +270,7 @@ void rknn_L1_vs_bruteforce_test( for (size_t i = 0; i < nSamples; i++) { double dist = 0.0; - for (size_t d = 0; d < DIM; d++) - dist += std::abs(query_pt[d] - samples[i][d]); + for (size_t d = 0; d < DIM; d++) dist += std::abs(query_pt[d] - samples[i][d]); if (dist <= maxRadiusSqr) bf_nn.emplace(dist, i); } @@ -293,20 +289,19 @@ void rknn_L1_vs_bruteforce_test( { // Distances must be in exact order: EXPECT_NEAR(it->first, out_dists_sqr[i], 1e-3) - << "For: numToSearch=" << numToSearch - << " out_dists_sqr[i]=" << out_dists_sqr[i] << "\n"; + << "For: numToSearch=" << numToSearch << " out_dists_sqr[i]=" << out_dists_sqr[i] + << "\n"; // indices may be not in the (rare) case of a tie: EXPECT_NEAR(bf_idx2dist.at(ret_indexes[i]), out_dists_sqr[i], 1e-3) - << "For: numToSearch=" << numToSearch - << " out_dists_sqr[i]=" << out_dists_sqr[i] << "\n"; + << "For: numToSearch=" << numToSearch << " out_dists_sqr[i]=" << out_dists_sqr[i] + << "\n"; } } } template -void L2_vs_bruteforce_test( - const size_t nSamples, const size_t DIM, const size_t numToSearch) +void L2_vs_bruteforce_test(const size_t nSamples, const size_t DIM, const size_t numToSearch) { std::vector> samples; @@ -323,8 +318,7 @@ void L2_vs_bruteforce_test( // construct a kd-tree index: // Dimensionality set at run-time (default: L2) // ------------------------------------------------------------ - typedef KDTreeVectorOfVectorsAdaptor>, NUM> - my_kd_tree_t; + typedef KDTreeVectorOfVectorsAdaptor>, NUM> my_kd_tree_t; my_kd_tree_t mat_index(DIM /*dim*/, samples, 10 /* max leaf */); @@ -353,8 +347,7 @@ void L2_vs_bruteforce_test( { double dist = 0.0; for (size_t d = 0; d < DIM; d++) - dist += (query_pt[d] - samples[i][d]) * - (query_pt[d] - samples[i][d]); + dist += (query_pt[d] - samples[i][d]) * (query_pt[d] - samples[i][d]); bf_nn.emplace(dist, i); } } @@ -375,8 +368,8 @@ void L2_vs_bruteforce_test( // indices may be not in the (rare) case of a tie: EXPECT_NEAR(bf_idx2dist.at(ret_indexes[i]), out_dists_sqr[i], 1e-3) - << "For: numToSearch=" << numToSearch - << " out_dists_sqr[i]=" << out_dists_sqr[i] << "\n"; + << "For: numToSearch=" << numToSearch << " out_dists_sqr[i]=" << out_dists_sqr[i] + << "\n"; } } } @@ -391,19 +384,15 @@ void box_L2_vs_bruteforce_test(const size_t nSamples, const size_t DIM) // Generate points: generateRandomPointCloud(samples, nSamples, DIM, max_range); - typedef KDTreeVectorOfVectorsAdaptor>, NUM> - my_kd_tree_t; + typedef KDTreeVectorOfVectorsAdaptor>, NUM> my_kd_tree_t; // Query box: typename my_kd_tree_t::index_t::BoundingBox query_box(DIM); for (size_t d = 0; d < DIM; d++) { - query_box[d].low = - static_cast(max_range * (rand() % 1000) / (1000.0)); - query_box[d].high = - static_cast(max_range * (rand() % 1000) / (1000.0)); - if (query_box[d].low > query_box[d].high) - std::swap(query_box[d].low, query_box[d].high); + query_box[d].low = static_cast(max_range * (rand() % 1000) / (1000.0)); + query_box[d].high = static_cast(max_range * (rand() % 1000) / (1000.0)); + if (query_box[d].low > query_box[d].high) std::swap(query_box[d].low, query_box[d].high); } // construct a kd-tree index: @@ -438,8 +427,7 @@ void box_L2_vs_bruteforce_test(const size_t nSamples, const size_t DIM) template void rknn_L2_vs_bruteforce_test( - const size_t nSamples, const size_t DIM, const size_t numToSearch, - const NUM maxRadiusSqr) + const size_t nSamples, const size_t DIM, const size_t numToSearch, const NUM maxRadiusSqr) { std::vector> samples; @@ -456,8 +444,7 @@ void rknn_L2_vs_bruteforce_test( // construct a kd-tree index: // Dimensionality set at run-time (default: L2) // ------------------------------------------------------------ - typedef KDTreeVectorOfVectorsAdaptor>, NUM> - my_kd_tree_t; + typedef KDTreeVectorOfVectorsAdaptor>, NUM> my_kd_tree_t; my_kd_tree_t mat_index(DIM /*dim*/, samples, 10 /* max leaf */); @@ -485,8 +472,7 @@ void rknn_L2_vs_bruteforce_test( { double dist = 0.0; for (size_t d = 0; d < DIM; d++) - dist += (query_pt[d] - samples[i][d]) * - (query_pt[d] - samples[i][d]); + dist += (query_pt[d] - samples[i][d]) * (query_pt[d] - samples[i][d]); if (dist <= maxRadiusSqr) bf_nn.emplace(dist, i); } @@ -505,13 +491,13 @@ void rknn_L2_vs_bruteforce_test( { // Distances must be in exact order: EXPECT_NEAR(it->first, out_dists_sqr[i], 1e-3) - << "For: numToSearch=" << numToSearch - << " out_dists_sqr[i]=" << out_dists_sqr[i] << "\n"; + << "For: numToSearch=" << numToSearch << " out_dists_sqr[i]=" << out_dists_sqr[i] + << "\n"; // indices may be not in the (rare) case of a tie: EXPECT_NEAR(bf_idx2dist.at(ret_indexes[i]), out_dists_sqr[i], 1e-3) - << "For: numToSearch=" << numToSearch - << " out_dists_sqr[i]=" << out_dists_sqr[i] << "\n"; + << "For: numToSearch=" << numToSearch << " out_dists_sqr[i]=" << out_dists_sqr[i] + << "\n"; } } } @@ -528,13 +514,11 @@ void SO3_vs_bruteforce_test(const size_t nSamples) // construct a kd-tree index: typedef KDTreeSingleIndexAdaptor< - SO3_Adaptor>, PointCloud_Quat, - 4 /* dim */ + SO3_Adaptor>, PointCloud_Quat, 4 /* dim */ > my_kd_tree_t; - my_kd_tree_t index( - 4 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); + my_kd_tree_t index(4 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); // do a knn search const size_t num_results = 1; std::vector ret_indexes(num_results); @@ -581,13 +565,11 @@ void SO2_vs_bruteforce_test(const size_t nSamples) // construct a kd-tree index: typedef KDTreeSingleIndexAdaptor< - SO2_Adaptor>, PointCloud_Orient, - 1 /* dim */ + SO2_Adaptor>, PointCloud_Orient, 1 /* dim */ > my_kd_tree_t; - my_kd_tree_t index( - 1 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); + my_kd_tree_t index(1 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); // do a knn search const size_t num_results = 1; std::vector ret_indexes(num_results); @@ -639,8 +621,7 @@ void L2_dynamic_vs_bruteforce_test(const size_t nSamples) > my_kd_tree_t; - my_kd_tree_t index( - 3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); + my_kd_tree_t index(3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); // Generate points: generateRandomPointCloud(cloud, nSamples, max_range); @@ -731,8 +712,7 @@ void L2_dynamic_vs_bruteforce_test(const size_t nSamples) } template -void L2_concurrent_build_vs_bruteforce_test( - const size_t nSamples, const size_t DIM) +void L2_concurrent_build_vs_bruteforce_test(const size_t nSamples, const size_t DIM) { std::vector> samples; @@ -749,11 +729,9 @@ void L2_concurrent_build_vs_bruteforce_test( // construct a kd-tree index: // Dimensionality set at run-time (default: L2) // ------------------------------------------------------------ - typedef KDTreeVectorOfVectorsAdaptor>, NUM> - my_kd_tree_t; + typedef KDTreeVectorOfVectorsAdaptor>, NUM> my_kd_tree_t; - my_kd_tree_t mat_index( - DIM /*dim*/, samples, 10 /* max leaf */, 0 /* concurrent build */); + my_kd_tree_t mat_index(DIM /*dim*/, samples, 10 /* max leaf */, 0 /* concurrent build */); // do a knn search const size_t num_results = 1; @@ -773,8 +751,7 @@ void L2_concurrent_build_vs_bruteforce_test( { double dist = 0.0; for (size_t d = 0; d < DIM; d++) - dist += (query_pt[d] - samples[i][d]) * - (query_pt[d] - samples[i][d]); + dist += (query_pt[d] - samples[i][d]) * (query_pt[d] - samples[i][d]); if (dist < min_dist_L2) { min_dist_L2 = dist; @@ -807,8 +784,7 @@ void L2_concurrent_build_vs_L2_test(const size_t nSamples, const size_t DIM) // construct a kd-tree index: // Dimensionality set at run-time (default: L2) // ------------------------------------------------------------ - typedef KDTreeVectorOfVectorsAdaptor>, NUM> - my_kd_tree_t; + typedef KDTreeVectorOfVectorsAdaptor>, NUM> my_kd_tree_t; my_kd_tree_t mat_index_concurrent_build( DIM /*dim*/, samples, 10 /* max leaf */, 0 /* concurrent build */); @@ -837,8 +813,7 @@ void L2_dynamic_sorted_test(const size_t N, const size_t num_results) std::vector dynamic_dist(num_results); KNNResultSet dynamic_knn_result(num_results); std::vector> radius_results_vec; - RadiusResultSet dynamic_radius_result( - 10.0 * 10.0, radius_results_vec); + RadiusResultSet dynamic_radius_result(10.0 * 10.0, radius_results_vec); // Prepare search params to sort result const auto search_params = SearchParameters(0, true); @@ -846,10 +821,8 @@ void L2_dynamic_sorted_test(const size_t N, const size_t num_results) dynamic_knn_result.init(&dynamic_idx[0], &dynamic_dist[0]); dynamic_radius_result.init(); - dynamic_index.findNeighbors( - dynamic_knn_result, &query_pt[0], search_params); - dynamic_index.findNeighbors( - dynamic_radius_result, &query_pt[0], search_params); + dynamic_index.findNeighbors(dynamic_knn_result, &query_pt[0], search_params); + dynamic_index.findNeighbors(dynamic_radius_result, &query_pt[0], search_params); // Check size const size_t expected_size = std::min(N, num_results); @@ -934,13 +907,11 @@ TEST(kdtree, robust_empty_tree) // construct a kd-tree index: typedef KDTreeSingleIndexAdaptor< - L2_Simple_Adaptor>, PointCloud, - 3 /* dim */ + L2_Simple_Adaptor>, PointCloud, 3 /* dim */ > my_kd_tree_simple_t; - my_kd_tree_simple_t index1( - 3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); + my_kd_tree_simple_t index1(3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); // Now we will try to search in the tree, and WE EXPECT a result of // no neighbors found if the error detection works fine: @@ -1073,8 +1044,7 @@ TEST(kdtree, robust_nonempty_tree) my_kd_tree_simple_t; my_kd_tree_simple_t index1( - 3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */), - max_point_count); + 3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */), max_point_count); // Try a search and expect a neighbor to exist because the dynamic tree was // passed a non-empty cloud @@ -1096,8 +1066,7 @@ TEST(kdtree, add_and_remove_points) L2_Simple_Adaptor>, PointCloud, 3> my_kd_tree_simple_t; - my_kd_tree_simple_t index( - 3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); + my_kd_tree_simple_t index(3 /*dim*/, cloud, KDTreeSingleIndexAdaptorParams(10 /* max leaf */)); const auto query = [&index]() -> size_t {