diff --git a/CMakeLists.txt b/CMakeLists.txt index 0f70219..5453a38 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,24 +1,95 @@ cmake_minimum_required(VERSION 3.1) -project(lnrr) -set (${PROJECT_NAME}_VERSION_MAJOR 1) -set (${PROJECT_NAME}_VERSION_MINOR 0) -set(CMAKE_CXX_STANDARD 17) # avoid using EIGEN_MAKE_ALIGNED_OPERATOR_NEW -# add_compile_options(-march=native) -add_compile_options(-O3) -# add_compile_options(-g) -add_compile_options(-std=c++17) -# add_compile_options(-fopenmp) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") +set(LNRR_VERSION 0.1.0) +set(PROJECT_SOVERSION 0) -find_package(Eigen3 3.1 REQUIRED) -message(STATUS "[fgt] Eigen3 version: ${EIGEN3_VERSION}") +project(lnrr CXX) + +message(STATUS "[lnrr] Compiler: ${CMAKE_CXX_COMPILER_ID}") + +list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake") + +find_package(Eigen3 REQUIRED) +message(STATUS "[lnrr] Eigen3 version: ${EIGEN3_VERSION}") add_library(Library-C++ src/scan_to_model.cpp -) -target_include_directories(Library-C++ + ) +target_include_directories(Library-C++ INTERFACE + "$" + "$" + PRIVATE include ${EIGEN3_INCLUDE_DIR} + ) +set_target_properties(Library-C++ PROPERTIES + OUTPUT_NAME lnrr + VERSION ${LNRR_VERSION} + SOVERSION ${PROJECT_SOVERSION} + ) +target_compile_definitions(Library-C++ PUBLIC - $ - $ - ${EIGEN3_INCLUDE_DIR} + FGT_WITH_OPENMP + ) +if("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") + target_compile_options(Library-C++ + PUBLIC + -std=c++11 + PRIVATE + -Wall + -pedantic + # The no-nested-anon-types is for nanoflann + -Wno-nested-anon-types + ) +elseif("${CMAKE_CXX_COMPILER_ID}" MATCHES "GNU") + target_compile_options(Library-C++ + PUBLIC + -std=c++11 + # For some deprecations inside Eigen + -Wno-deprecated-declarations + PRIVATE + -Wall + -pedantic + # For pragma omp's + -Wno-unknown-pragmas + ) +elseif("${CMAKE_CXX_COMPILER_ID}" MATCHES "MSVC") + target_compile_options(Library-C++ + PUBLIC + # Eigen does relative includes + /wd4464 + # This is the famous "unreferenced inline function has been removed" + /wd4514 + # Yo, I don't care if that function wasn't inlined + /wd4710 + # Padding + /wd4820 + # Automatic inline expansion + /wd4711 + ) +endif() +install(DIRECTORY include/lnrr DESTINATION include) +install(TARGETS Library-C++ + DESTINATION lib + EXPORT lnrr-targets + ) + +include(GenerateExportHeader) +generate_export_header(Library-C++) + +include(CMakePackageConfigHelpers) +configure_file(cmake/lnrr-config.cmake + ${PROJECT_BINARY_DIR}/lnrr-config.cmake + @ONLY + ) +write_basic_package_version_file( + ${PROJECT_BINARY_DIR}/lnrr-config-version.cmake + VERSION ${LNRR_VERSION} + COMPATIBILITY AnyNewerVersion + ) +install(FILES + ${PROJECT_BINARY_DIR}/lnrr-config.cmake + ${PROJECT_BINARY_DIR}/lnrr-config-version.cmake + DESTINATION lib/cmake/lnrr ) +install(EXPORT lnrr-targets + DESTINATION lib/cmake/lnrr + NAMESPACE Lnrr:: + ) \ No newline at end of file diff --git a/README.md b/README.md index de0617f..ab4fa50 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Linewise Non-Rigid Registration (LNRR) -[fgt](https://github.com/gadomski/fgt) +[fgt](https://github.com/miguelcastillon/fgt) [cpd](https://github.com/gadomski/cpd) @@ -10,8 +10,12 @@ This code lives [on Github](https://github.com/miguelcastillon/cld). - - + + + + +[![Watch the video](https://img.youtube.com/vi/ZsPw2voKi10/maxresdefault.jpg)](https://youtu.be/ZsPw2voKi10) ## Installation @@ -38,8 +42,8 @@ Only tested with: #include int main(int argc, char** argv) { - lnrr::Matrix fixed = load_points_from_somewhere(); - lnrr::Matrix moving = load_points_from_somewhere(); + lnrr::Matrix fixed = loadPointsFromSomewhere(); + lnrr::Matrix moving = loadPointsFromSomewhere(); lnrr::ScanToModel lnrr(); lnrr.setFixed(fixed); lnrr.setMoving(moving); @@ -64,11 +68,12 @@ target_link_libraries(my-great-library ## Contributing -Github [issues](https://github.com/miguelcastillon/cld/issues) and [pull requests](https://github.com/miguelcastillon/cld/pulls), per usual. +Github [issues](https://github.com/miguelcastillon/lnrr/issues) and [pull requests](https://github.com/miguelcastillon/lnrr/pulls), per usual. ## Publications -- ToDo +- Paper information + Please cite as: ``` @article{castillon2022, diff --git a/cmake/lnrr-config.cmake b/cmake/lnrr-config.cmake new file mode 100644 index 0000000..05ea9ba --- /dev/null +++ b/cmake/lnrr-config.cmake @@ -0,0 +1 @@ +include("${CMAKE_CURRENT_LIST_DIR}/lnrr-targets.cmake") \ No newline at end of file diff --git a/docs/images/lnrr.png b/docs/images/lnrr.png new file mode 100644 index 0000000..15b1705 Binary files /dev/null and b/docs/images/lnrr.png differ diff --git a/include/ceres/cost_U_scan_to_model.h b/include/lnrr/ceres/cost_U_scan_to_model.h similarity index 97% rename from include/ceres/cost_U_scan_to_model.h rename to include/lnrr/ceres/cost_U_scan_to_model.h index 0969bd7..c6ec8ac 100644 --- a/include/ceres/cost_U_scan_to_model.h +++ b/include/lnrr/ceres/cost_U_scan_to_model.h @@ -1,9 +1,9 @@ #pragma once #include -#include -#include -#include +#include +#include +#include namespace lnrr { class CostFunctionScanToModelRot { diff --git a/include/scan_to_model.h b/include/lnrr/scan_to_model.h similarity index 71% rename from include/scan_to_model.h rename to include/lnrr/scan_to_model.h index 61ae340..b0547db 100644 --- a/include/scan_to_model.h +++ b/include/lnrr/scan_to_model.h @@ -1,20 +1,18 @@ #pragma once -#include -#include -#include -#include +#include +#include +#include +#include #include namespace lnrr { -// const size_t DEFAULT_MAX_ITERATIONS = 150; -// const double DEFAULT_OUTLIERS = 0.1; -// const double DEFAULT_THRESHOLD_TRUNCATE = 1e6; -// const double DEFAULT_TOLERANCE = 1e-5; -// const double DEFAULT_SIGMA2 = 0.0; -// const double DEFAULT_BETA = 3.0; -// const double DEFAULT_LAMBDA = 3.0; +const size_t DEFAULT_MAX_ITERATIONS = 150; +const double DEFAULT_OUTLIERS = 0.1; +const double DEFAULT_THRESHOLD_TRUNCATE = 1e6; +const double DEFAULT_TOLERANCE = 1e-5; +const double DEFAULT_SIGMA2 = 0.0; const double FGT_EPSILON = 1e-4; const double FGT_THRESHOLD = 0.2; @@ -23,36 +21,39 @@ class ScanToModel { MatrixX3 fixed_; MatrixX3 moving_; MatrixX3 moving_transformed_; - int M_; + SparseMatrix YD_; + Matrix G_; - MatrixX3 C_; - SparseMatrix D_; - MatrixX3 U_; // rotation - MatrixX3 V_; // translation SparseMatrix F_; - Matrix FG_; SparseMatrix FT_; + Matrix FG_; SparseMatrix H_; - SparseMatrix YD_; - Probabilities P_; - MatrixX3 RT_; // tranpose of the rotation matrices MatrixX3 A_; Matrix B_; Matrix GB_; + MatrixX3 C_; + SparseMatrix D_; + + MatrixX3 U_; // rotation + MatrixX3 V_; // translation + + MatrixX3 RT_; // tranpose of the rotation matrices + + Probabilities P_; double fgt_epsilon_ = FGT_EPSILON; double fgt_threshold_ = FGT_THRESHOLD; + double beta_; + double lambda_; + Vector line_sizes_; + int number_lines_; size_t max_iterations_; double outliers_; double threshold_truncate_; double sigma2_; double tolerance_; - double lambda_; - double beta_; - Vector line_sizes_; - int number_lines_; double defaultSigma2(); void computeSigma2(); @@ -64,22 +65,20 @@ class ScanToModel { SparseMatrix matrixAsSparseBlockDiag(const Matrix& input); public: - ScanToModel(const size_t& max_iterations, const double& outliers, - const double& threshold_truncate, const double& sigma2, - const double& tolerance, const double& lambda, - const double& beta, const Vector& line_sizes) - : max_iterations_(max_iterations), - outliers_(outliers), - threshold_truncate_(threshold_truncate), - sigma2_(sigma2), - tolerance_(tolerance), - lambda_(lambda), + ScanToModel(const MatrixX3& fixed, const MatrixX3& moving, + const double& beta, const double& lambda, + const Vector& line_sizes) + : fixed_(fixed), + moving_(moving), beta_(beta), + lambda_(lambda), line_sizes_(line_sizes), - number_lines_(line_sizes.rows()) {} - - // TODO: create another constructor that takes in an *structured* point - // cloud and infer line sizes from there + number_lines_(line_sizes.rows()), + max_iterations_(DEFAULT_MAX_ITERATIONS), + outliers_(DEFAULT_OUTLIERS), + threshold_truncate_(DEFAULT_THRESHOLD_TRUNCATE), + sigma2_(DEFAULT_SIGMA2), + tolerance_(DEFAULT_TOLERANCE) {} ~ScanToModel() {} @@ -92,6 +91,13 @@ class ScanToModel { void setFixed(const MatrixX3& fixed) { fixed_ = fixed; } void setMoving(const MatrixX3& moving) { moving_ = moving; } + void setLineSizes(const Vector& line_sizes) { + line_sizes_ = line_sizes; + number_lines_ = line_sizes.rows(); + } + void setBeta(const double& beta) { beta_ = beta; } + void setLambda(const double& lambda) { lambda_ = lambda; } + void setMaxIterations(const size_t& max_iterations) { max_iterations_ = max_iterations; } @@ -101,23 +107,19 @@ class ScanToModel { } void setSigma2(const double& sigma2) { sigma2_ = sigma2; } void setTolerance(const double& tolerance) { tolerance_ = tolerance; } - void setLambda(const double& lambda) { lambda_ = lambda; } - void setLineSizes(const Vector& line_sizes) { - line_sizes_ = line_sizes; - number_lines_ = line_sizes.rows(); - } // Get functions MatrixX3 getFixed() { return fixed_; } MatrixX3 getMoving() { return moving_; } + Vector getLineSizes() { return line_sizes_; } + int getNumberLines() { return number_lines_; } + double getBeta() { return beta_; } + double getLambda() { return lambda_; } size_t getMaxIterations() { return max_iterations_; } double getOutlierRate() { return outliers_; } double getThresholdTruncate() { return threshold_truncate_; } double getSigma2() { return sigma2_; } double getTolerance() { return tolerance_; } - double getLambda() { return lambda_; } - Vector getLineSizes() { return line_sizes_; } - int getNumberLines() { return number_lines_; } }; } // namespace lnrr \ No newline at end of file diff --git a/include/utils/conversions.h b/include/lnrr/utils/conversions.h similarity index 100% rename from include/utils/conversions.h rename to include/lnrr/utils/conversions.h diff --git a/include/utils/types.h b/include/lnrr/utils/types.h similarity index 100% rename from include/utils/types.h rename to include/lnrr/utils/types.h diff --git a/include/utils/utils.h b/include/lnrr/utils/utils.h similarity index 93% rename from include/utils/utils.h rename to include/lnrr/utils/utils.h index 908081b..5b23242 100644 --- a/include/utils/utils.h +++ b/include/lnrr/utils/utils.h @@ -1,6 +1,6 @@ #pragma once -#include +#include namespace lnrr { @@ -14,7 +14,6 @@ MatrixX3 computeRotationMatrices(const Matrix& G, const MatrixX3& U) { return RT; } -/// TODO: Can this be done with matrix operations (maybe kronecker product?) ? SparseMatrix matrixAsSparseBlockDiag(const Matrix& input) { SparseMatrix output(input.rows(), input.rows() * input.cols()); std::vector> tripletList; @@ -42,13 +41,11 @@ SparseMatrix computeF(const int& M, const Vector& line_sizes) { int m = 0; std::vector> tripletList; tripletList.reserve(M); - for (size_t col = 0; col < line_sizes.rows(); col++) { + for (size_t col = 0; col < line_sizes.rows(); col++) for (size_t i = 0; i < line_sizes[col]; i++) { tripletList.push_back(Eigen::Triplet(m, col, 1.0)); m++; } - } - F.setFromTriplets(tripletList.begin(), tripletList.end()); return F; } @@ -67,7 +64,6 @@ SparseMatrix computeH(const int& M, const Vector& line_sizes) { tripletList.push_back(Eigen::Triplet(m, 3 * col + 2, 1.0)); m++; } - H.setFromTriplets(tripletList.begin(), tripletList.end()); return H; } diff --git a/src/scan_to_model.cpp b/src/scan_to_model.cpp index 227676b..83c2545 100644 --- a/src/scan_to_model.cpp +++ b/src/scan_to_model.cpp @@ -1,4 +1,4 @@ -#include +#include namespace lnrr { @@ -169,12 +169,11 @@ void ScanToModel::computeSigma2() { void ScanToModel::initialize() { assert(fixed_.rows() > 0); assert(moving_.rows() > 0); - M_ = moving_.rows(); G_ = computeG(beta_, number_lines_); - F_ = computeF(M_, line_sizes_); + F_ = computeF(moving_.rows(), line_sizes_); FT_ = F_.transpose(); FG_ = F_ * G_; - H_ = computeH(M_, line_sizes_); + H_ = computeH(moving_.rows(), line_sizes_); YD_ = matrixAsSparseBlockDiag(moving_) * H_; C_ = Matrix::Zero(moving_.rows(), 3); @@ -205,13 +204,8 @@ Result ScanToModel::run() { double l = 0.0; double ntol = tolerance_ + 10.0; - while (iter < max_iterations_ && // - ntol > tolerance_ && // - sigma2_ > - 10 * - std::numeric_limits::epsilon()) // Maybe also if - // sigma2_ increases? - { + while (iter < max_iterations_ && ntol > tolerance_ && + sigma2_ > 10 * std::numeric_limits::epsilon()) { ScanToModel::computeOne(); ntol = std::abs((P_.l - l) / P_.l); l = P_.l;