diff --git a/CMakeLists.txt b/CMakeLists.txt index fd98dc5..746a561 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -50,6 +50,7 @@ find_external_dependency("Eigen3" "Eigen3::Eigen" "${CMAKE_CURRENT_LIST_DIR}/cma find_package(OpenMP QUIET REQUIRED) robin_download_pmc() robin_download_xenium() +robin_download_nanoflann() add_subdirectory(${pmc_SOURCE_DIR} ${pmc_BINARY_DIR}) # @@ -66,6 +67,7 @@ add_library(robin::robin ALIAS robin) target_link_libraries(robin PUBLIC Eigen3::Eigen xenium + nanoflann PRIVATE OpenMP::OpenMP_CXX PRIVATE pmc) target_include_directories( @@ -112,7 +114,7 @@ export( ) install( - TARGETS xenium pmc + TARGETS xenium pmc nanoflann EXPORT robinTargets LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} diff --git a/cmake/DownloadExternal.cmake b/cmake/DownloadExternal.cmake index 1b8f729..f22fba3 100644 --- a/cmake/DownloadExternal.cmake +++ b/cmake/DownloadExternal.cmake @@ -90,6 +90,39 @@ function(robin_download_xenium) install(DIRECTORY ${xenium_SOURCE_DIR}/xenium DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) endfunction() +# nanoflann +function(robin_download_nanoflann) + download_project(PROJ nanoflann + GIT_REPOSITORY https://github.com/jlblancoc/nanoflann.git + GIT_TAG v1.6.2 + QUIET + ) + add_library(nanoflann INTERFACE) + target_include_directories(nanoflann + INTERFACE + $ + $) + + install( + TARGETS nanoflann + EXPORT nanoflannTargets + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + ) + + export( + EXPORT nanoflannTargets + FILE ${CMAKE_CURRENT_BINARY_DIR}/nanoflannTargets.cmake + NAMESPACE nanoflann:: + ) + + install( + EXPORT nanoflannTargets + FILE nanoflannTargets.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/nanoflann/ + NAMESPACE nanoflann:: + ) +endfunction() + # For handling Eigen3 library function(find_external_dependency PACKAGE_NAME TARGET_NAME INCLUDED_CMAKE_PATH) string(TOUPPER ${PACKAGE_NAME} PACKAGE_NAME_UP) diff --git a/src/robin.cpp b/src/robin.cpp index da6cbcf..353828f 100644 --- a/src/robin.cpp +++ b/src/robin.cpp @@ -6,6 +6,51 @@ #include +#include + +namespace { + +/** + * @brief Convert a directed half-edge adjacency list (where only edges i->j with j>i are stored) + * into a full undirected graph of the requested storage type. + */ +robin::IGraph* BuildGraphFromHalfEdges(std::vector>& half_adj_list, + robin::GraphsStorageType graph_storage_type) { + const size_t N = half_adj_list.size(); + + size_t total_edges = 0; + for (size_t i = 0; i < N; ++i) { + total_edges += half_adj_list[i].size(); + } + + if (graph_storage_type == robin::GraphsStorageType::ADJ_LIST) { + std::vector> full_adj_list(N); + for (size_t i = 0; i < N; ++i) { + for (size_t j : half_adj_list[i]) { + full_adj_list[i].push_back(j); + full_adj_list[j].push_back(i); + } + } + return new robin::AdjListGraph(std::move(full_adj_list), total_edges); + } + + // CSR and ATOMIC_CSR use edge-list constructors + std::vector> edge_list; + edge_list.reserve(total_edges); + for (size_t i = 0; i < N; ++i) { + for (size_t j : half_adj_list[i]) { + edge_list.emplace_back(i, j); + } + } + + if (graph_storage_type == robin::GraphsStorageType::CSR) { + return new robin::CSRGraph(edge_list); + } + return new robin::AtomicCSRGraph(edge_list); +} + +} // anonymous namespace + std::vector robin::FindInlierStructure(const IGraph* g, InlierGraphStructure graph_structure) { // identify inlier structures @@ -27,65 +72,100 @@ std::vector robin::FindInlierStructure(const IGraph* g, robin::IGraph* robin::MakeVecAvgInvGraph(const Eigen::MatrixXd& measurements, double noise_bound, GraphsStorageType graph_storage_type) { - // Measurements set - VectorY y_set(measurements); + const size_t N = measurements.cols(); + const size_t dim = measurements.rows(); - // Compatibility check function - robin::SvaCompCheck sva_comp_check(noise_bound); + // Transpose to row-major (N x dim) for nanoflann + Eigen::Matrix data_rowmajor = + measurements.transpose(); - // Compatibility graph constructor - robin::CompGraphConstructor graph_constructor; - graph_constructor.SetCompCheckFunction(&sva_comp_check); - graph_constructor.SetMeasurements(&y_set); + // Build KD-tree using nanoflann's Eigen matrix adaptor + using kd_tree_t = nanoflann::KDTreeEigenMatrixAdaptor< + Eigen::Matrix, + -1 /* dimensionality at runtime */, + nanoflann::metric_L2>; - // InvGraphConstructor - auto* g = graph_constructor.BuildCompGraph(graph_storage_type); - return g; -} - -robin::IGraph* robin::MakeRotAvgInvGraph(const std::vector& measurements, - So3Distance dist_type, double noise_bound, - GraphsStorageType graph_storage_type) { - // Measurements sets - So3Y y_set(measurements); + kd_tree_t tree(dim, std::cref(data_rowmajor), {10 /* max leaf size */}); - // InvFunc - switch (dist_type) { - case So3Distance::CHORDAL_DISTANCE: { - // Distance function - So3ChordalDist inv_func; + // nanoflann radiusSearch uses squared L2 distance + const double search_radius_sq = noise_bound * noise_bound; - // Compatibility check function - robin::SraCompCheck comp_check(&inv_func, noise_bound); + std::vector> half_adj_list(N); - // InvGraphConstructor - robin::CompGraphConstructor, 2> graph_constructor; - graph_constructor.SetCompCheckFunction(&comp_check); - graph_constructor.SetMeasurements(&y_set); + nanoflann::SearchParameters params; + params.sorted = false; - // Build graph - auto* g = graph_constructor.BuildCompGraph(graph_storage_type); +#pragma omp parallel for schedule(dynamic, 64) + for (size_t i = 0; i < N; ++i) { + std::vector> matches; + tree.index->radiusSearch(data_rowmajor.row(i).data(), search_radius_sq, matches, params); - return g; + for (const auto& match : matches) { + if (match.first > i) { + half_adj_list[i].push_back(match.first); + } + } } - case robin::So3Distance::GEODESIC_DISTANCE: { - // InvFunc - So3GeodesicDist inv_func; - // Compatibility check function - robin::SraCompCheck comp_check(&inv_func, noise_bound); + return BuildGraphFromHalfEdges(half_adj_list, graph_storage_type); +} - // InvGraphConstructor - robin::CompGraphConstructor, 2> graph_constructor; - graph_constructor.SetCompCheckFunction(&comp_check); - graph_constructor.SetMeasurements(&y_set); +robin::IGraph* robin::MakeRotAvgInvGraph(const std::vector& measurements, + So3Distance dist_type, double noise_bound, + GraphsStorageType graph_storage_type) { + const size_t N = measurements.size(); - // Build graph - auto* g = graph_constructor.BuildCompGraph(graph_storage_type); + // Embed rotations as 9D vectors (flatten each 3x3 matrix) + Eigen::Matrix embedded(N, 9); + for (size_t i = 0; i < N; ++i) { + Eigen::Map> flat(measurements[i].data()); + embedded.row(i) = flat; + } - return g; + // Build KD-tree in R^9 + using kd_tree_t = nanoflann::KDTreeEigenMatrixAdaptor< + Eigen::Matrix, + 9, + nanoflann::metric_L2>; + + kd_tree_t tree(9, std::cref(embedded), {10 /* max leaf size */}); + + // L2 in R^9 equals the Frobenius (chordal) distance. + // For geodesic mode, use chordal upper bound as a pre-filter: + // chordal = 2*sqrt(2)*sin(geodesic/2), then refine with exact geodesic check. + double search_radius_sq; + if (dist_type == So3Distance::GEODESIC_DISTANCE) { + double chordal_ub = 2.0 * std::sqrt(2.0) * std::sin(noise_bound / 2.0); + search_radius_sq = chordal_ub * chordal_ub; + } else { + search_radius_sq = noise_bound * noise_bound; } + + std::vector> half_adj_list(N); + So3GeodesicDist geodesic_fn; + + nanoflann::SearchParameters params; + params.sorted = false; + +#pragma omp parallel for schedule(dynamic, 64) + for (size_t i = 0; i < N; ++i) { + std::vector> matches; + tree.index->radiusSearch(embedded.row(i).data(), search_radius_sq, matches, params); + + for (const auto& match : matches) { + if (match.first > i) { + bool keep = true; + if (dist_type == So3Distance::GEODESIC_DISTANCE) { + keep = (geodesic_fn(measurements[i], measurements[match.first]) <= noise_bound); + } + if (keep) { + half_adj_list[i].push_back(match.first); + } + } + } } + + return BuildGraphFromHalfEdges(half_adj_list, graph_storage_type); } robin::IGraph* robin::Make3dRegInvGraph(const Eigen::Matrix3Xd& src_3d_points, diff --git a/tests/robin_test.cpp b/tests/robin_test.cpp index 6a8fa57..ae2b724 100644 --- a/tests/robin_test.cpp +++ b/tests/robin_test.cpp @@ -250,6 +250,153 @@ TEST_CASE("single rotation averaging") { } } +TEST_CASE("SVA KD-tree produces correct results across graph types") { + // Tests that the KD-tree based MakeVecAvgInvGraph produces the same inlier + // structure as expected for all graph storage types. + size_t N = 10; + size_t dimension = 5; + Eigen::VectorXd exp_vec(dimension, 1); + + Eigen::Matrix random_noises = + Eigen::MatrixXd::Random(dimension, N); + random_noises.colwise().normalize(); + random_noises /= 10; + double noise_bound = 0.1; + + // manually create outliers + random_noises.col(0) *= 10; + random_noises.col(9) *= 20; + std::vector expected_inliers = {1, 2, 3, 4, 5, 6, 7, 8}; + + Eigen::MatrixXd measurements = Eigen::MatrixXd::Zero(dimension, N); + for (size_t i = 0; i < random_noises.cols(); ++i) { + measurements.col(i) = exp_vec + random_noises.col(i); + } + + SECTION("ADJ_LIST matches expected") { + auto* g = robin::MakeVecAvgInvGraph(measurements, 2 * noise_bound, + robin::GraphsStorageType::ADJ_LIST); + auto inliers = robin::FindInlierStructure(g, robin::InlierGraphStructure::MAX_CLIQUE); + std::sort(inliers.begin(), inliers.end()); + REQUIRE_THAT(inliers, Catch::Equals(expected_inliers)); + delete g; + } + + SECTION("CSR matches expected") { + auto* g = robin::MakeVecAvgInvGraph(measurements, 2 * noise_bound, + robin::GraphsStorageType::CSR); + auto inliers = robin::FindInlierStructure(g, robin::InlierGraphStructure::MAX_CLIQUE); + std::sort(inliers.begin(), inliers.end()); + REQUIRE_THAT(inliers, Catch::Equals(expected_inliers)); + delete g; + } + + SECTION("ATOMIC_CSR matches expected") { + auto* g = robin::MakeVecAvgInvGraph(measurements, 2 * noise_bound, + robin::GraphsStorageType::ATOMIC_CSR); + auto inliers = robin::FindInlierStructure(g, robin::InlierGraphStructure::MAX_CLIQUE); + std::sort(inliers.begin(), inliers.end()); + REQUIRE_THAT(inliers, Catch::Equals(expected_inliers)); + delete g; + } +} + +TEST_CASE("SRA KD-tree chordal equivalence") { + // Tests that the KD-tree based MakeRotAvgInvGraph with chordal distance + // correctly identifies outliers. + Eigen::Quaternion exp_mean_quat = Eigen::Quaternion::UnitRandom(); + Eigen::Matrix3d exp_mean_mat = exp_mean_quat.normalized().toRotationMatrix(); + + double noise_ceiling = 0.1; // radian + std::random_device rd; + std::mt19937 e2(rd()); + std::uniform_real_distribution<> dist(0, noise_ceiling); + + size_t N = 10; + std::vector measurements; + for (size_t i = 0; i < N; ++i) { + double noise = dist(e2); + Eigen::AngleAxis random_pertubation = + Eigen::AngleAxis(noise, Eigen::Vector3d::UnitX()); + measurements.emplace_back(exp_mean_mat * random_pertubation.toRotationMatrix()); + } + + SECTION("no outliers") { + auto* g = robin::MakeRotAvgInvGraph(measurements, robin::So3Distance::CHORDAL_DISTANCE, + 2 * noise_ceiling, robin::GraphsStorageType::ADJ_LIST); + auto inliers = robin::FindInlierStructure(g, robin::InlierGraphStructure::MAX_CLIQUE); + std::sort(inliers.begin(), inliers.end()); + + std::vector expected_inliers = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; + REQUIRE_THAT(inliers, Catch::Equals(expected_inliers)); + delete g; + } + + SECTION("with outliers") { + measurements[0] = measurements[0] * Eigen::AngleAxis((10 + dist(e2)) * noise_ceiling, + Eigen::Vector3d::UnitY()); + measurements[3] = measurements[3] * Eigen::AngleAxis((10 + dist(e2)) * noise_ceiling, + Eigen::Vector3d::UnitZ()); + + std::vector expected_inliers = {1, 2, 4, 5, 6, 7, 8, 9}; + + auto* g = robin::MakeRotAvgInvGraph(measurements, robin::So3Distance::CHORDAL_DISTANCE, + 2 * noise_ceiling, robin::GraphsStorageType::ADJ_LIST); + auto inliers = robin::FindInlierStructure(g, robin::InlierGraphStructure::MAX_CLIQUE); + std::sort(inliers.begin(), inliers.end()); + REQUIRE_THAT(inliers, Catch::Equals(expected_inliers)); + delete g; + } +} + +TEST_CASE("SRA KD-tree geodesic equivalence") { + // Tests that the KD-tree based MakeRotAvgInvGraph with geodesic distance + // correctly identifies outliers (same test structure as the existing SRA test). + Eigen::Quaternion exp_mean_quat = Eigen::Quaternion::UnitRandom(); + Eigen::Matrix3d exp_mean_mat = exp_mean_quat.normalized().toRotationMatrix(); + + double noise_ceiling = 0.1; + std::random_device rd; + std::mt19937 e2(rd()); + std::uniform_real_distribution<> dist(0, noise_ceiling); + + size_t N = 10; + std::vector measurements; + for (size_t i = 0; i < N; ++i) { + double noise = dist(e2); + Eigen::AngleAxis random_pertubation = + Eigen::AngleAxis(noise, Eigen::Vector3d::UnitX()); + measurements.emplace_back(exp_mean_mat * random_pertubation.toRotationMatrix()); + } + + SECTION("no outliers") { + auto* g = robin::MakeRotAvgInvGraph(measurements, robin::So3Distance::GEODESIC_DISTANCE, + 2 * noise_ceiling, robin::GraphsStorageType::ADJ_LIST); + auto inliers = robin::FindInlierStructure(g, robin::InlierGraphStructure::MAX_CLIQUE); + std::sort(inliers.begin(), inliers.end()); + + std::vector expected_inliers = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; + REQUIRE_THAT(inliers, Catch::Equals(expected_inliers)); + delete g; + } + + SECTION("with outliers CSR") { + measurements[0] = measurements[0] * Eigen::AngleAxis((10 + dist(e2)) * noise_ceiling, + Eigen::Vector3d::UnitY()); + measurements[3] = measurements[3] * Eigen::AngleAxis((10 + dist(e2)) * noise_ceiling, + Eigen::Vector3d::UnitZ()); + + std::vector expected_inliers = {1, 2, 4, 5, 6, 7, 8, 9}; + + auto* g = robin::MakeRotAvgInvGraph(measurements, robin::So3Distance::GEODESIC_DISTANCE, + 2 * noise_ceiling, robin::GraphsStorageType::CSR); + auto inliers = robin::FindInlierStructure(g, robin::InlierGraphStructure::MAX_CLIQUE); + std::sort(inliers.begin(), inliers.end()); + REQUIRE_THAT(inliers, Catch::Equals(expected_inliers)); + delete g; + } +} + TEST_CASE("3d reg") { // an arbitrary transformation matrix Eigen::Matrix4d T;