From 4c131c103b133a9b6dab1af7b849d5771c925f54 Mon Sep 17 00:00:00 2001 From: Tim Wundenberg Date: Sat, 21 Aug 2021 12:15:03 +0200 Subject: [PATCH] add TEASER++ sample --- .gitignore | 3 +- .../Tools/TEASERpp/CMakeLists.txt | 19 +++ .../Tools/TEASERpp/teaser_cpp_ply.cc | 121 ++++++++++++++++++ 3 files changed, 142 insertions(+), 1 deletion(-) create mode 100644 PointCloudWeb.Server/Tools/TEASERpp/CMakeLists.txt create mode 100644 PointCloudWeb.Server/Tools/TEASERpp/teaser_cpp_ply.cc diff --git a/.gitignore b/.gitignore index 1a9a727..2297d8d 100644 --- a/.gitignore +++ b/.gitignore @@ -5,4 +5,5 @@ obj/ node_modules/ *.csproj.user .idea -/temp/ \ No newline at end of file +/temp/ +build/ \ No newline at end of file diff --git a/PointCloudWeb.Server/Tools/TEASERpp/CMakeLists.txt b/PointCloudWeb.Server/Tools/TEASERpp/CMakeLists.txt new file mode 100644 index 0000000..3677dbb --- /dev/null +++ b/PointCloudWeb.Server/Tools/TEASERpp/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.10) +project(teaser_cpp_ply) + +set(CMAKE_CXX_STANDARD 14) + +find_package(Eigen3 REQUIRED) +find_package(teaserpp REQUIRED) + +# Change this line to include your own executable file +add_executable(teaser_cpp_ply teaser_cpp_ply.cc) + +# Link to teaserpp & Eigen3 +target_link_libraries(teaser_cpp_ply Eigen3::Eigen teaserpp::teaser_registration teaserpp::teaser_io) + +# Copy the data files to build directory +file(COPY ../example_data/ + DESTINATION ./example_data/ + FILES_MATCHING + PATTERN *.ply) diff --git a/PointCloudWeb.Server/Tools/TEASERpp/teaser_cpp_ply.cc b/PointCloudWeb.Server/Tools/TEASERpp/teaser_cpp_ply.cc new file mode 100644 index 0000000..a459825 --- /dev/null +++ b/PointCloudWeb.Server/Tools/TEASERpp/teaser_cpp_ply.cc @@ -0,0 +1,121 @@ +// An example showing TEASER++ registration with the Stanford bunny model +#include +#include +#include + +#include + +#include +#include + +// Macro constants for generating noise and outliers +#define NOISE_BOUND 0.05 +#define N_OUTLIERS 1700 +#define OUTLIER_TRANSLATION_LB 5 +#define OUTLIER_TRANSLATION_UB 10 + +inline double getAngularError(Eigen::Matrix3d R_exp, Eigen::Matrix3d R_est) { + return std::abs(std::acos(fmin(fmax(((R_exp.transpose() * R_est).trace() - 1) / 2, -1.0), 1.0))); +} + +void addNoiseAndOutliers(Eigen::Matrix& tgt) { + // Add uniform noise + Eigen::Matrix noise = + Eigen::Matrix::Random(3, tgt.cols()) * NOISE_BOUND; + NOISE_BOUND / 2; + tgt = tgt + noise; + + // Add outliers + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_int_distribution<> dis2(0, tgt.cols() - 1); // pos of outliers + std::uniform_int_distribution<> dis3(OUTLIER_TRANSLATION_LB, + OUTLIER_TRANSLATION_UB); // random translation + std::vector expected_outlier_mask(tgt.cols(), false); + for (int i = 0; i < N_OUTLIERS; ++i) { + int c_outlier_idx = dis2(gen); + assert(c_outlier_idx < expected_outlier_mask.size()); + expected_outlier_mask[c_outlier_idx] = true; + tgt.col(c_outlier_idx).array() += dis3(gen); // random translation + } +} + +int main() { + // Load the .ply file + teaser::PLYReader reader; + teaser::PointCloud src_cloud; + auto status = reader.read("./example_data/bun_zipper_res3.ply", src_cloud); + int N = src_cloud.size(); + + // Convert the point cloud to Eigen + Eigen::Matrix src(3, N); + for (size_t i = 0; i < N; ++i) { + src.col(i) << src_cloud[i].x, src_cloud[i].y, src_cloud[i].z; + } + + // Homogeneous coordinates + Eigen::Matrix src_h; + src_h.resize(4, src.cols()); + src_h.topRows(3) = src; + src_h.bottomRows(1) = Eigen::Matrix::Ones(N); + + // Apply an arbitrary SE(3) transformation + Eigen::Matrix4d T; + // clang-format off + T << 9.96926560e-01, 6.68735757e-02, -4.06664421e-02, -1.15576939e-01, + -6.61289946e-02, 9.97617877e-01, 1.94008687e-02, -3.87705398e-02, + 4.18675510e-02, -1.66517807e-02, 9.98977765e-01, 1.14874890e-01, + 0, 0, 0, 1; + // clang-format on + + // Apply transformation + Eigen::Matrix tgt_h = T * src_h; + Eigen::Matrix tgt = tgt_h.topRows(3); + + // Add some noise & outliers + addNoiseAndOutliers(tgt); + + // Run TEASER++ registration + // Prepare solver parameters + teaser::RobustRegistrationSolver::Params params; + params.noise_bound = NOISE_BOUND; + params.cbar2 = 1; + params.estimate_scaling = false; + params.rotation_max_iterations = 100; + params.rotation_gnc_factor = 1.4; + params.rotation_estimation_algorithm = + teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS; + params.rotation_cost_threshold = 0.005; + + // Solve with TEASER++ + teaser::RobustRegistrationSolver solver(params); + std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); + solver.solve(src, tgt); + std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); + + auto solution = solver.getSolution(); + + // Compare results + std::cout << "=====================================" << std::endl; + std::cout << " TEASER++ Results " << std::endl; + std::cout << "=====================================" << std::endl; + std::cout << "Expected rotation: " << std::endl; + std::cout << T.topLeftCorner(3, 3) << std::endl; + std::cout << "Estimated rotation: " << std::endl; + std::cout << solution.rotation << std::endl; + std::cout << "Error (deg): " << getAngularError(T.topLeftCorner(3, 3), solution.rotation) + << std::endl; + std::cout << std::endl; + std::cout << "Expected translation: " << std::endl; + std::cout << T.topRightCorner(3, 1) << std::endl; + std::cout << "Estimated translation: " << std::endl; + std::cout << solution.translation << std::endl; + std::cout << "Error (m): " << (T.topRightCorner(3, 1) - solution.translation).norm() << std::endl; + std::cout << std::endl; + std::cout << "Number of correspondences: " << N << std::endl; + std::cout << "Number of outliers: " << N_OUTLIERS << std::endl; + std::cout << "Time taken (s): " + << std::chrono::duration_cast(end - begin).count() / + 1000000.0 + << std::endl; +}