├── .gitignore ├── cmake ├── lnrr-config.cmake └── uninstall_target.cmake.in ├── docs └── images │ ├── lnrr.png │ ├── initial.png │ └── registered.png ├── examples ├── data │ ├── model.pcd │ ├── model_dense.pcd │ ├── scans │ │ ├── scan00.pcd │ │ ├── scan01.pcd │ │ ├── scan02.pcd │ │ ├── scan03.pcd │ │ ├── scan04.pcd │ │ ├── scan05.pcd │ │ ├── scan06.pcd │ │ ├── scan07.pcd │ │ ├── scan08.pcd │ │ ├── scan09.pcd │ │ ├── scan10.pcd │ │ ├── scan11.pcd │ │ ├── scan12.pcd │ │ ├── scan13.pcd │ │ ├── scan14.pcd │ │ ├── scan15.pcd │ │ ├── scan16.pcd │ │ ├── scan17.pcd │ │ ├── scan18.pcd │ │ ├── scan19.pcd │ │ ├── scan20.pcd │ │ ├── scan21.pcd │ │ ├── scan22.pcd │ │ ├── scan23.pcd │ │ ├── scan24.pcd │ │ ├── scan25.pcd │ │ ├── scan26.pcd │ │ ├── scan27.pcd │ │ ├── scan28.pcd │ │ ├── scan29.pcd │ │ ├── scan30.pcd │ │ ├── scan31.pcd │ │ ├── scan32.pcd │ │ ├── scan33.pcd │ │ ├── scan34.pcd │ │ ├── scan35.pcd │ │ └── scan36.pcd │ └── output.pcd ├── CMakeLists.txt └── lnrr.cpp ├── .vscode ├── c_cpp_properties.json └── settings.json ├── src ├── CMakeLists.txt └── scan_to_model.cpp ├── tests ├── jacobian_G_test.cpp ├── data │ └── data_E_step_test.py ├── jacobian_YR_T_test.cpp ├── CMakeLists.txt ├── E_step_test.cpp ├── M_step_test.cpp └── conversions_test.cpp ├── include └── lnrr │ ├── utils │ ├── conversions.h │ ├── types.h │ ├── test_utils.h │ └── operations.h │ ├── scan_to_model.h │ └── ceres │ └── cost_W.h ├── .clang-format_cirs ├── .clang-format ├── .github └── workflows │ └── build-linux.yml ├── CMakeLists.txt ├── README.md └── LICENSE.txt /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | -------------------------------------------------------------------------------- /cmake/lnrr-config.cmake: -------------------------------------------------------------------------------- 1 | include("${CMAKE_CURRENT_LIST_DIR}/lnrr-targets.cmake") -------------------------------------------------------------------------------- /docs/images/lnrr.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/docs/images/lnrr.png -------------------------------------------------------------------------------- /docs/images/initial.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/docs/images/initial.png -------------------------------------------------------------------------------- /examples/data/model.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/model.pcd -------------------------------------------------------------------------------- /docs/images/registered.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/docs/images/registered.png -------------------------------------------------------------------------------- /examples/data/model_dense.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/model_dense.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan00.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan00.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan01.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan01.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan02.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan02.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan03.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan03.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan04.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan04.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan05.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan05.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan06.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan06.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan07.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan07.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan08.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan08.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan09.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan09.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan10.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan10.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan11.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan11.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan12.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan12.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan13.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan13.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan14.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan14.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan15.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan15.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan16.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan16.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan17.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan17.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan18.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan18.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan19.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan19.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan20.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan20.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan21.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan21.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan22.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan22.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan23.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan23.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan24.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan24.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan25.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan25.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan26.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan26.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan27.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan27.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan28.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan28.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan29.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan29.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan30.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan30.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan31.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan31.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan32.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan32.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan33.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan33.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan34.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan34.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan35.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan35.pcd -------------------------------------------------------------------------------- /examples/data/scans/scan36.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/miguelcastillon/lnrr/HEAD/examples/data/scans/scan36.pcd -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(lnrr-examples CXX) 2 | cmake_minimum_required(VERSION 3.16) 3 | 4 | find_package(lnrr REQUIRED) 5 | find_package(Fgt REQUIRED) 6 | find_package(Ceres 2.2 REQUIRED) 7 | find_package(PCL 1.10 REQUIRED) 8 | 9 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") 10 | 11 | add_executable(lnrr_example 12 | lnrr.cpp 13 | ) 14 | target_link_libraries(lnrr_example 15 | Lnrr::Library-C++ 16 | ) 17 | -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Linux", 5 | "includePath": [ 6 | "${workspaceFolder}/include/**", 7 | "${workspaceFolder}/extern/**", 8 | "/usr/include/**", 9 | "/usr/local/include/**", 10 | "/home/miguel/libraries/**" 11 | ], 12 | "defines": [], 13 | "compilerPath": "/usr/bin/clang", 14 | "cStandard": "c11", 15 | "cppStandard": "c++14", 16 | "intelliSenseMode": "linux-clang-x64" 17 | } 18 | ], 19 | "version": 4 20 | } -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(CMAKE_BUILD_TYPE MATCHES Debug) 2 | add_definitions(-DDEBUG) 3 | message(WARNING "Compiling Debug mode. Information will be printed but performance will be slow. Compile on Release for best performance.") 4 | endif(CMAKE_BUILD_TYPE MATCHES Debug) 5 | 6 | add_library(Library-C++ 7 | SHARED 8 | scan_to_model.cpp 9 | ) 10 | set_target_properties(Library-C++ PROPERTIES 11 | OUTPUT_NAME lnrr 12 | VERSION ${LNRR_VERSION} 13 | SOVERSION ${PROJECT_SOVERSION} 14 | ) 15 | target_include_directories(Library-C++ PUBLIC 16 | "$" 17 | "$" 18 | ${EIGEN3_INCLUDE_DIR} 19 | ${CERES_INCLUDE_DIR} 20 | ${PCL_INCLUDE_DIRS} 21 | ) 22 | target_link_libraries(Library-C++ 23 | PUBLIC 24 | Fgt::Library-C++ 25 | ${CERES_LIBRARIES} 26 | ${PCL_LIBRARIES} 27 | ) -------------------------------------------------------------------------------- /tests/jacobian_G_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace lnrr; 6 | 7 | Matrix computeT(const Matrix& G, const MatrixX6& W, const double& epsilon) { 8 | return G * (W + epsilon * Matrix::Ones(W.rows(), W.cols())); 9 | } 10 | 11 | const int L = 10; 12 | const double BETA = 1.0; 13 | const double EPSILON = 1e-3; 14 | 15 | struct Data { 16 | MatrixX6 W0; 17 | }; 18 | 19 | Data data0 = {MatrixX6::Random(L, 6)}; 20 | Data data1 = {MatrixX6::Random(L, 6)}; 21 | Data data2 = {MatrixX6::Random(L, 6)}; 22 | std::vector data = {data0, data1, data2}; 23 | 24 | TEST(Tests_Jacobian_G, ComputeJacobianG) { 25 | for (auto d : data) { 26 | Matrix G = computeG(BETA, L); 27 | Matrix T0 = computeT(G, d.W0, 0.0); 28 | Matrix T1 = computeT(G, d.W0, EPSILON); 29 | Matrix jTW = computeJacobianG(G); 30 | Vector epsilon_vector = Vector::Ones(6 * L) * EPSILON; 31 | Vector jTW_eps_vec = jTW * epsilon_vector; 32 | Eigen::Map jTW_eps(&jTW_eps_vec[0], L, 6); 33 | Matrix T1_approx = T0 + jTW_eps; 34 | EXPECT_TRUE(T1.isApprox(T1_approx, 1e-6)); 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /tests/data/data_E_step_test.py: -------------------------------------------------------------------------------- 1 | from cmath import pi 2 | import numpy as np 3 | 4 | # Input variables 5 | X = np.array([[0,0.1,0.2],[1,0.1,0.2],[2,0.1,0.2],[3,0.1,0.2],[4,0.1,0.2],[5,0.1,0.2],[6,0.1,0.2],[7,0.1,0.2],[8,0.1,0.2],[9,0.1,0.2]]) 6 | Y = np.array([[0,1,0],[1,1,1],[2,1,2],[3,1,3],[4,1,3],[5,1,2],[6,1,1],[7,1,0]]) 7 | w = 0.1 8 | sigma = 1 9 | distance_threshold = 5 10 | 11 | # Other variables 12 | N = X.shape[0] 13 | M = Y.shape[0] 14 | 15 | def exp(x, y, sigma2): 16 | if np.linalg.norm(x - y) < distance_threshold: 17 | return np.exp(-np.linalg.norm(x - y)**2 / (2*sigma2)) 18 | else: 19 | return 0 20 | 21 | def computeP(w, sigma, X, Y): 22 | sigma2 = sigma**2 23 | c = (w*M)/((1-w)*N)*(2*pi*sigma2)**(3/2) 24 | 25 | K = np.zeros((M, N)) 26 | for n in range(N): 27 | for m in range(M): 28 | K[m, n] = exp(X[n], Y[m], sigma2) 29 | 30 | P = np.zeros((M, N)) 31 | for n in range(N): 32 | for m in range(M): 33 | if K[m, n] > 0: 34 | d=0 35 | for l in range(M): 36 | d += exp(X[n], Y[l], sigma2) 37 | P[m, n] = K[m, n] / (c+d) 38 | return P 39 | 40 | P = computeP(w, sigma, X, Y) 41 | print("P: ", P) 42 | # P1 = P.sum(axis=1) 43 | # PT1 = P.sum(axis=0) 44 | # print("P1: ", P1) 45 | # print("PT1: ", PT1) 46 | # print("PX: ", np.matmul(P, X)) 47 | 48 | 49 | -------------------------------------------------------------------------------- /include/lnrr/utils/conversions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace lnrr { 8 | namespace conversions { 9 | 10 | template 11 | Eigen::Quaternion rpy2Quaternion(const T& roll, const T& pitch, 12 | const T& yaw) { 13 | Eigen::AngleAxis rollAngle(roll, Eigen::Matrix::UnitX()); 14 | Eigen::AngleAxis pitchAngle(pitch, Eigen::Matrix::UnitY()); 15 | Eigen::AngleAxis yawAngle(yaw, Eigen::Matrix::UnitZ()); 16 | 17 | Eigen::Quaternion q = yawAngle * pitchAngle * rollAngle; 18 | return q; 19 | } 20 | 21 | template 22 | Eigen::Matrix quaternion2rpy(const Eigen::Quaternion& q) { 23 | return q.toRotationMatrix().eulerAngles(2, 1, 0).colwise().reverse(); 24 | } 25 | 26 | template 27 | Eigen::Matrix rpy2RotationMatrix(const T& roll, const T& pitch, 28 | const T& yaw) { 29 | return rpy2Quaternion(roll, pitch, yaw).toRotationMatrix(); 30 | } 31 | 32 | template 33 | Eigen::Matrix 34 | rotationMatrix2rpy(const Eigen::Matrix& rotation_matrix) { 35 | return rotation_matrix.eulerAngles(2, 1, 0).colwise().reverse(); 36 | } 37 | 38 | } // namespace conversions 39 | } // namespace lnrr 40 | -------------------------------------------------------------------------------- /.clang-format_cirs: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | Language: Cpp 4 | AccessModifierOffset: -2 5 | ConstructorInitializerIndentWidth: 2 6 | AlignEscapedNewlinesLeft: false 7 | AlignTrailingComments: true 8 | AllowAllParametersOfDeclarationOnNextLine: false 9 | AllowShortIfStatementsOnASingleLine: false 10 | AllowShortLoopsOnASingleLine: false 11 | AllowShortFunctionsOnASingleLine: None 12 | AllowShortLoopsOnASingleLine: false 13 | AlwaysBreakTemplateDeclarations: true 14 | AlwaysBreakBeforeMultilineStrings: false 15 | BreakBeforeBinaryOperators: false 16 | BreakBeforeTernaryOperators: false 17 | BreakConstructorInitializersBeforeComma: true 18 | BinPackParameters: true 19 | ColumnLimit: 120 20 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 21 | DerivePointerBinding: true 22 | ExperimentalAutoDetectBinPacking: false 23 | IndentCaseLabels: true 24 | MaxEmptyLinesToKeep: 1 25 | NamespaceIndentation: None 26 | ObjCSpaceBeforeProtocolList: true 27 | PenaltyBreakBeforeFirstCallParameter: 19 28 | PenaltyBreakComment: 60 29 | PenaltyBreakString: 1 30 | PenaltyBreakFirstLessLess: 1000 31 | PenaltyExcessCharacter: 1000 32 | PenaltyReturnTypeOnItsOwnLine: 90 33 | PointerBindsToType: false 34 | SpacesBeforeTrailingComments: 2 35 | Cpp11BracedListStyle: false 36 | Standard: Auto 37 | IndentWidth: 2 38 | TabWidth: 2 39 | UseTab: Never 40 | BreakBeforeBraces: Allman 41 | IndentFunctionDeclarationAfterType: false 42 | SpacesInParentheses: false 43 | SpacesInAngles: false 44 | SpaceInEmptyParentheses: false 45 | SpacesInCStyleCastParentheses: false 46 | SpaceAfterControlStatementKeyword: true 47 | SpaceBeforeAssignmentOperators: true 48 | ContinuationIndentWidth: 4 49 | ... 50 | 51 | -------------------------------------------------------------------------------- /cmake/uninstall_target.cmake.in: -------------------------------------------------------------------------------- 1 | if(NOT EXISTS "@PROJECT_BINARY_DIR@/install_manifest.txt") 2 | message(FATAL_ERROR "Cannot find install manifest: \"@PROJECT_BINARY_DIR@/install_manifest.txt\"") 3 | endif(NOT EXISTS "@PROJECT_BINARY_DIR@/install_manifest.txt") 4 | 5 | file(READ "@PROJECT_BINARY_DIR@/install_manifest.txt" files) 6 | string(REGEX REPLACE "\n" ";" files "${files}") 7 | foreach(file ${files}) 8 | message(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"") 9 | if(EXISTS "$ENV{DESTDIR}${file}" OR IS_SYMLINK "$ENV{DESTDIR}${file}") 10 | exec_program("@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\"" 11 | OUTPUT_VARIABLE rm_out RETURN_VALUE rm_retval) 12 | if(NOT "${rm_retval}" STREQUAL 0) 13 | message(FATAL_ERROR "Problem when removing \"$ENV{DESTDIR}${file}\"") 14 | endif(NOT "${rm_retval}" STREQUAL 0) 15 | else(EXISTS "$ENV{DESTDIR}${file}" OR IS_SYMLINK "$ENV{DESTDIR}${file}") 16 | message(STATUS "File \"$ENV{DESTDIR}${file}\" does not exist.") 17 | endif(EXISTS "$ENV{DESTDIR}${file}" OR IS_SYMLINK "$ENV{DESTDIR}${file}") 18 | endforeach(file) 19 | 20 | # remove lnrr directory in include (removes all files in it!) 21 | message(STATUS "Uninstalling \"@INSTALL_INCLUDE_DIR@\"") 22 | if(EXISTS "@INSTALL_INCLUDE_DIR@") 23 | exec_program("@CMAKE_COMMAND@" 24 | ARGS "-E remove_directory \"@INSTALL_INCLUDE_DIR@\"" 25 | OUTPUT_VARIABLE rm_out RETURN_VALUE rm_retval) 26 | if(NOT "${rm_retval}" STREQUAL 0) 27 | message(FATAL_ERROR 28 | "Problem when removing \"@INSTALL_INCLUDE_DIR@\"") 29 | endif(NOT "${rm_retval}" STREQUAL 0) 30 | else(EXISTS "@INSTALL_INCLUDE_DIR@") 31 | message(STATUS 32 | "Directory \"@INSTALL_INCLUDE_DIR@\" does not exist.") 33 | endif(EXISTS "@INSTALL_INCLUDE_DIR@") -------------------------------------------------------------------------------- /include/lnrr/utils/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace lnrr { 10 | 11 | class lnrr_error : public std::runtime_error { 12 | public: 13 | explicit lnrr_error(const std::string what_arg) 14 | : std::runtime_error(what_arg) {} 15 | }; 16 | 17 | typedef Eigen::MatrixXd Matrix; 18 | typedef Eigen::SparseMatrix Sparse; 19 | typedef Eigen::Matrix3d Matrix3; 20 | typedef Eigen::MatrixX3d MatrixX3; 21 | typedef Eigen::Matrix MatrixX6; 22 | typedef Eigen::VectorXd Vector; 23 | typedef Eigen::Matrix VectorInt; 24 | typedef Eigen::Vector3d Vector3; 25 | 26 | template 27 | using MatrixT = Eigen::Matrix; 28 | template 29 | using SparseT = Eigen::SparseMatrix; 30 | template 31 | using Matrix3T = Eigen::Matrix; 32 | template 33 | using MatrixX3T = Eigen::Matrix; 34 | template 35 | using MatrixX6T = Eigen::Matrix; 36 | template 37 | using VectorT = Eigen::Matrix; 38 | template 39 | using Vector3T = Eigen::Matrix; 40 | 41 | typedef pcl::PointCloud PointCloud; 42 | typedef pcl::PointCloud::Ptr PointCloudPtr; 43 | 44 | template 45 | struct RigidTransform { 46 | Vector3T xyz; 47 | Matrix3T rot_mat; 48 | }; 49 | 50 | struct Result { 51 | MatrixX3 points; 52 | std::vector> line_transforms; 53 | double sigma2; 54 | std::chrono::microseconds runtime; 55 | size_t iterations; 56 | }; 57 | 58 | struct Probabilities { 59 | Vector p1; 60 | Vector pt1; 61 | Matrix px; 62 | double l; 63 | }; 64 | } // namespace lnrr -------------------------------------------------------------------------------- /tests/jacobian_YR_T_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace lnrr; 6 | 7 | Matrix computeT(const Matrix& G, const MatrixX6& W, const double& epsilon) { 8 | return G * (W + epsilon * Matrix::Ones(W.rows(), W.cols())); 9 | } 10 | 11 | const int L = 10; 12 | const int NUMBER_POINTS_LINE = 10; 13 | const int M = L * NUMBER_POINTS_LINE; 14 | const VectorInt LINE_SIZES = VectorInt::Ones(L) * NUMBER_POINTS_LINE; 15 | const Matrix G = Matrix::Identity(L, L); // If G is identity, then T = W 16 | const double EPSILON = 1e-5; 17 | 18 | struct Data { 19 | MatrixX3 moving; 20 | MatrixX6 W0; 21 | }; 22 | 23 | Data data0 = {MatrixX3::Random(M, 3), MatrixX6::Random(L, 6)}; 24 | Data data1 = {MatrixX3::Random(M, 3), MatrixX6::Random(L, 6)}; 25 | Data data2 = {MatrixX3::Random(M, 3), MatrixX6::Random(L, 6)}; 26 | std::vector data = {data0, data1, data2}; 27 | 28 | TEST(Tests_Jacobian_YR_T, ComputeJacobianYR_T) { 29 | for (auto d : data) { 30 | MatrixX6 W1 = d.W0 + EPSILON * Matrix::Ones(d.W0.rows(), d.W0.cols()); 31 | 32 | MatrixX3 YR0 = computeTransformedMoving(d.moving, G, d.W0, LINE_SIZES); 33 | MatrixX3 YR1 = computeTransformedMoving(d.moving, G, W1, LINE_SIZES); 34 | std::vector> T_lines = 35 | computeTransformations(G, d.W0); 36 | 37 | Sparse jYRT = 38 | computeJacobianPointComposition(T_lines, d.moving, LINE_SIZES); 39 | 40 | Vector epsilon_vector = Vector::Ones(6 * L) * EPSILON; 41 | Vector jYRT_eps_vec = jYRT * epsilon_vector; 42 | 43 | Eigen::Map jYRT_eps(&jYRT_eps_vec[0], M, 3); 44 | 45 | MatrixX3 YR1_approx = YR0 + jYRT_eps; 46 | EXPECT_TRUE(YR1.isApprox(YR1_approx, 1e-4)) 47 | << "YR1 - YR1_approx:\n" 48 | << YR1 - YR1_approx << std::endl; 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: LLVM 4 | AccessModifierOffset: -4 5 | ConstructorInitializerIndentWidth: 4 6 | AlignEscapedNewlinesLeft: false 7 | AlignTrailingComments: true 8 | AllowAllParametersOfDeclarationOnNextLine: true 9 | AllowShortBlocksOnASingleLine: false 10 | AllowShortIfStatementsOnASingleLine: false 11 | AllowShortLoopsOnASingleLine: false 12 | AllowShortFunctionsOnASingleLine: All 13 | AlwaysBreakTemplateDeclarations: true 14 | AlwaysBreakBeforeMultilineStrings: false 15 | BreakBeforeBinaryOperators: false 16 | BreakBeforeTernaryOperators: true 17 | BreakConstructorInitializersBeforeComma: false 18 | BinPackParameters: true 19 | ColumnLimit: 80 20 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 21 | DerivePointerAlignment: false 22 | ExperimentalAutoDetectBinPacking: false 23 | IndentCaseLabels: false 24 | IndentWrappedFunctionNames: false 25 | IndentFunctionDeclarationAfterType: false 26 | MaxEmptyLinesToKeep: 1 27 | KeepEmptyLinesAtTheStartOfBlocks: true 28 | NamespaceIndentation: None 29 | ObjCSpaceAfterProperty: false 30 | ObjCSpaceBeforeProtocolList: true 31 | PenaltyBreakBeforeFirstCallParameter: 19 32 | PenaltyBreakComment: 300 33 | PenaltyBreakString: 1000 34 | PenaltyBreakFirstLessLess: 120 35 | PenaltyExcessCharacter: 1000000 36 | PenaltyReturnTypeOnItsOwnLine: 60 37 | PointerAlignment: Left 38 | SpacesBeforeTrailingComments: 1 39 | Cpp11BracedListStyle: true 40 | Standard: Cpp11 41 | IndentWidth: 4 42 | TabWidth: 8 43 | UseTab: Never 44 | BreakBeforeBraces: Attach 45 | SpacesInParentheses: false 46 | SpacesInAngles: false 47 | SpaceInEmptyParentheses: false 48 | SpacesInCStyleCastParentheses: false 49 | SpacesInContainerLiterals: true 50 | SpaceBeforeAssignmentOperators: true 51 | ContinuationIndentWidth: 4 52 | CommentPragmas: '^ IWYU pragma:' 53 | ForEachMacros: [ foreach, Q_FOREACH, BOOST_FOREACH ] 54 | SpaceBeforeParens: ControlStatements 55 | DisableFormat: false 56 | ... 57 | 58 | -------------------------------------------------------------------------------- /.github/workflows/build-linux.yml: -------------------------------------------------------------------------------- 1 | # Based on GTSAM file (by @ProfFan) 2 | name: CI Linux 3 | 4 | on: [push, pull_request] 5 | 6 | jobs: 7 | build: 8 | runs-on: ubuntu-20.04 9 | steps: 10 | - name: Install dependencies 11 | run: | 12 | sudo apt -y update 13 | sudo apt -y upgrade 14 | sudo apt install -y git \ 15 | cmake \ 16 | openssh-client \ 17 | libgoogle-glog-dev \ 18 | libgflags-dev \ 19 | libgtest-dev \ 20 | libatlas-base-dev \ 21 | libeigen3-dev \ 22 | libsuitesparse-dev \ 23 | libpcl-dev 24 | - name: Checkout 25 | uses: actions/checkout@v3 26 | - name: Install Ceres 27 | run: | 28 | git clone https://ceres-solver.googlesource.com/ceres-solver 29 | cd ceres-solver && mkdir build && cd build 30 | cmake .. \ 31 | -DBUILD_TESTING=OFF \ 32 | -DBUILD_EXAMPLES=OFF 33 | make -j4 && sudo make install 34 | cd ../.. 35 | - name: Install FGT 36 | run: | 37 | git clone https://github.com/miguelcastillon/fgt_threshold 38 | cd fgt_threshold && mkdir build && cd build 39 | cmake .. \ 40 | -DCMAKE_BUILD_TYPE=Release \ 41 | -DWITH_OPENMP=ON \ 42 | -DBUILD_SHARED_LIBS=ON \ 43 | -DWITH_TESTS=OFF 44 | make 45 | sudo make install 46 | cd ../.. 47 | - name: Build 48 | run: | 49 | mkdir build 50 | cd build 51 | cmake .. 52 | make 53 | sudo make install 54 | - name: Test 55 | working-directory: ./build 56 | run: | 57 | ctest 58 | - name: Compile example 59 | working-directory: ./examples 60 | run: | 61 | mkdir build 62 | cd build 63 | cmake .. 64 | make 65 | -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "files.associations": { 3 | "core": "cpp", 4 | "cctype": "cpp", 5 | "clocale": "cpp", 6 | "cmath": "cpp", 7 | "cstdarg": "cpp", 8 | "cstddef": "cpp", 9 | "cstdio": "cpp", 10 | "cstdlib": "cpp", 11 | "cstring": "cpp", 12 | "ctime": "cpp", 13 | "cwchar": "cpp", 14 | "cwctype": "cpp", 15 | "array": "cpp", 16 | "atomic": "cpp", 17 | "strstream": "cpp", 18 | "*.tcc": "cpp", 19 | "bitset": "cpp", 20 | "chrono": "cpp", 21 | "complex": "cpp", 22 | "condition_variable": "cpp", 23 | "cstdint": "cpp", 24 | "deque": "cpp", 25 | "list": "cpp", 26 | "unordered_map": "cpp", 27 | "vector": "cpp", 28 | "exception": "cpp", 29 | "algorithm": "cpp", 30 | "functional": "cpp", 31 | "iterator": "cpp", 32 | "map": "cpp", 33 | "memory": "cpp", 34 | "memory_resource": "cpp", 35 | "numeric": "cpp", 36 | "optional": "cpp", 37 | "random": "cpp", 38 | "ratio": "cpp", 39 | "set": "cpp", 40 | "string": "cpp", 41 | "string_view": "cpp", 42 | "system_error": "cpp", 43 | "tuple": "cpp", 44 | "type_traits": "cpp", 45 | "utility": "cpp", 46 | "fstream": "cpp", 47 | "initializer_list": "cpp", 48 | "iomanip": "cpp", 49 | "iosfwd": "cpp", 50 | "iostream": "cpp", 51 | "istream": "cpp", 52 | "limits": "cpp", 53 | "mutex": "cpp", 54 | "new": "cpp", 55 | "ostream": "cpp", 56 | "sstream": "cpp", 57 | "stdexcept": "cpp", 58 | "streambuf": "cpp", 59 | "thread": "cpp", 60 | "cfenv": "cpp", 61 | "cinttypes": "cpp", 62 | "typeindex": "cpp", 63 | "typeinfo": "cpp", 64 | "variant": "cpp", 65 | "bit": "cpp", 66 | "numericaldiff": "cpp", 67 | "*.in": "cpp", 68 | "csetjmp": "cpp", 69 | "csignal": "cpp", 70 | "any": "cpp", 71 | "hash_map": "cpp", 72 | "hash_set": "cpp", 73 | "codecvt": "cpp", 74 | "forward_list": "cpp", 75 | "unordered_set": "cpp", 76 | "filesystem": "cpp", 77 | "regex": "cpp", 78 | "slist": "cpp", 79 | "future": "cpp", 80 | "shared_mutex": "cpp", 81 | "valarray": "cpp", 82 | "geometry": "cpp" 83 | } 84 | } -------------------------------------------------------------------------------- /tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | enable_testing() 2 | include(FetchContent) 3 | FetchContent_Declare( 4 | googletest 5 | GIT_REPOSITORY https://github.com/google/googletest.git 6 | GIT_TAG release-1.12.1 7 | ) 8 | FetchContent_MakeAvailable(googletest) 9 | add_library(GTest::GTest INTERFACE IMPORTED) 10 | target_link_libraries(GTest::GTest INTERFACE gtest_main) 11 | 12 | include(GoogleTest) 13 | 14 | ################## 15 | 16 | add_executable(conversions_test conversions_test.cpp) 17 | target_include_directories(conversions_test PUBLIC 18 | "$" 19 | "$" 20 | ${EIGEN3_INCLUDE_DIR} 21 | ${CERES_INCLUDE_DIR} 22 | ) 23 | target_link_libraries(conversions_test 24 | GTest::gtest_main 25 | ) 26 | gtest_discover_tests(conversions_test) 27 | 28 | ################## 29 | 30 | add_executable(E_step_test E_step_test.cpp) 31 | target_include_directories(E_step_test PUBLIC 32 | "$" 33 | "$" 34 | ${EIGEN3_INCLUDE_DIR} 35 | ${CERES_INCLUDE_DIR} 36 | ) 37 | target_link_libraries(E_step_test 38 | GTest::gtest_main 39 | Library-C++ 40 | ) 41 | gtest_discover_tests(E_step_test) 42 | 43 | ################## 44 | 45 | add_executable(jacobian_G_test jacobian_G_test.cpp) 46 | target_include_directories(jacobian_G_test PUBLIC 47 | "$" 48 | "$" 49 | ${EIGEN3_INCLUDE_DIR} 50 | ${CERES_INCLUDE_DIR} 51 | ) 52 | target_link_libraries(jacobian_G_test 53 | GTest::gtest_main 54 | Library-C++ 55 | ) 56 | gtest_discover_tests(jacobian_G_test) 57 | 58 | ################## 59 | 60 | add_executable(jacobian_YR_T_test jacobian_YR_T_test.cpp) 61 | target_include_directories(jacobian_YR_T_test PUBLIC 62 | "$" 63 | "$" 64 | ${EIGEN3_INCLUDE_DIR} 65 | ${CERES_INCLUDE_DIR} 66 | ) 67 | target_link_libraries(jacobian_YR_T_test 68 | GTest::gtest_main 69 | Library-C++ 70 | ) 71 | gtest_discover_tests(jacobian_YR_T_test) 72 | 73 | ################## 74 | 75 | add_executable(M_step_test M_step_test.cpp) 76 | target_include_directories(M_step_test PUBLIC 77 | "$" 78 | "$" 79 | ${EIGEN3_INCLUDE_DIR} 80 | ${CERES_INCLUDE_DIR} 81 | ) 82 | target_link_libraries(M_step_test 83 | GTest::gtest_main 84 | Library-C++ 85 | ) 86 | gtest_discover_tests(M_step_test) 87 | 88 | ################## 89 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(lnrr CXX) 3 | set(LNRR_VERSION 0.1.0) 4 | set(PROJECT_SOVERSION 0) 5 | 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CXX_STANDARD_REQUIRED ON) 8 | add_compile_options(-O3) 9 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp") 10 | 11 | option(BUILD_TESTS "Build test suite" ON) 12 | 13 | list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake") 14 | 15 | find_package(OpenMP REQUIRED) 16 | find_package(Eigen3 3.3.7 REQUIRED) 17 | find_package(Ceres 2.2 REQUIRED) 18 | find_package(Fgt REQUIRED) 19 | find_package(PCL 1.10 REQUIRED) 20 | 21 | add_subdirectory(src) 22 | 23 | if(BUILD_TESTS) 24 | include(CTest) 25 | add_subdirectory(tests) 26 | endif() 27 | 28 | if(NOT CMAKE_BUILD_TYPE) 29 | set(default_cmake_build_type "Release") 30 | message("Setting build type to '${default_cmake_build_type}' as none was specified.") 31 | set(CMAKE_BUILD_TYPE "${default_cmake_build_type}" CACHE 32 | STRING "Choose the type of build." FORCE) 33 | # Set the possible values of build type for cmake-gui 34 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS 35 | "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 36 | endif() 37 | message(STATUS "CMAKE_BUILD_TYPE: ${CMAKE_BUILD_TYPE}") 38 | 39 | 40 | 41 | include(CMakePackageConfigHelpers) 42 | write_basic_package_version_file(${PROJECT_BINARY_DIR}/lnrr-config-version.cmake VERSION ${LNRR_VERSION} COMPATIBILITY AnyNewerVersion) 43 | configure_file(cmake/lnrr-config.cmake ${PROJECT_BINARY_DIR}/lnrr-config.cmake @ONLY) 44 | install(FILES ${PROJECT_BINARY_DIR}/lnrr-config.cmake ${PROJECT_BINARY_DIR}/lnrr-config-version.cmake DESTINATION lib/cmake/lnrr) 45 | 46 | 47 | # if("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") 48 | # target_compile_options(Library-C++ 49 | # PUBLIC 50 | # -std=c++11 51 | # PRIVATE 52 | # -Wall 53 | # -pedantic 54 | # -Wno-nested-anon-types 55 | # ) 56 | # elseif("${CMAKE_CXX_COMPILER_ID}" MATCHES "GNU") 57 | # target_compile_options(Library-C++ 58 | # PUBLIC 59 | # -std=c++11 60 | # -Wno-deprecated-declarations 61 | # PRIVATE 62 | # -Wall 63 | # -pedantic 64 | # -Wno-unknown-pragmas 65 | # ) 66 | # endif() 67 | install(TARGETS Library-C++ DESTINATION lib EXPORT lnrr-targets) 68 | install(DIRECTORY include/lnrr DESTINATION include) 69 | install(EXPORT lnrr-targets NAMESPACE Lnrr:: DESTINATION lib/cmake/lnrr) 70 | 71 | # Create uninstall command 72 | configure_file("${CMAKE_SOURCE_DIR}/cmake/uninstall_target.cmake.in" 73 | "${CMAKE_BINARY_DIR}/uninstall_target.cmake" IMMEDIATE @ONLY) 74 | add_custom_target(uninstall "${CMAKE_COMMAND}" -P 75 | "${CMAKE_BINARY_DIR}/uninstall_target.cmake") 76 | -------------------------------------------------------------------------------- /tests/E_step_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace lnrr; 5 | 6 | struct Data { 7 | MatrixX3 X; 8 | MatrixX3 Y; 9 | Matrix P; 10 | double w; 11 | double sigma; 12 | double distance_threshold; 13 | }; 14 | 15 | Data data0 = {MatrixX3::Random(100, 3), 16 | data0.X, 17 | Matrix::Identity(data0.Y.rows(), data0.X.rows()), 18 | 0.0, 19 | 1.0, 20 | 0.0001}; 21 | Data data1 = {(MatrixX3(10, 3) << 0, 0.1, 0.2, 1, 0.1, 0.2, 2, 0.1, 0.2, 3, 0.1, 22 | 0.2, 4, 0.1, 0.2, 5, 0.1, 0.2, 6, 0.1, 0.2, 7, 0.1, 0.2, 8, 0.1, 23 | 0.2, 9, 0.1, 0.2) 24 | .finished(), 25 | (MatrixX3(8, 3) << 0, 1, 0, 1, 1, 1, 2, 1, 2, 3, 1, 3, 4, 1, 3, 5, 26 | 1, 2, 6, 1, 1, 7, 1, 0) 27 | .finished(), 28 | (Matrix(8, 10) << 2.76376098e-01, 1.67818281e-01, 4.59473521e-02, 29 | 4.54614881e-03, 1.37281799e-04, 0.00000000e+00, 0.00000000e+00, 30 | 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.24183786e-01, 31 | 2.04973711e-01, 1.52550581e-01, 4.10290544e-02, 3.36786987e-03, 32 | 8.43733422e-05, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 33 | 0.00000000e+00, 7.55162380e-03, 3.38819265e-02, 6.85453947e-02, 34 | 5.01130002e-02, 1.11817218e-02, 7.61470553e-04, 1.87395640e-05, 35 | 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 6.21479537e-05, 36 | 7.57964848e-04, 4.16824974e-03, 8.28362322e-03, 5.02427145e-03, 37 | 9.30062233e-04, 6.22175435e-05, 1.87670559e-06, 0.00000000e+00, 38 | 0.00000000e+00, 1.87670559e-06, 6.22175435e-05, 9.30062233e-04, 39 | 5.02427145e-03, 8.28362322e-03, 4.16824974e-03, 7.57964848e-04, 40 | 6.21479537e-05, 2.38225320e-06, 0.00000000e+00, 0.00000000e+00, 41 | 1.87395640e-05, 7.61470553e-04, 1.11817218e-02, 5.01130002e-02, 42 | 6.85453947e-02, 3.38819265e-02, 7.55162380e-03, 7.86857183e-04, 43 | 2.96405030e-05, 0.00000000e+00, 0.00000000e+00, 8.43733422e-05, 44 | 3.36786987e-03, 4.10290544e-02, 1.52550581e-01, 2.04973711e-01, 45 | 1.24183786e-01, 3.51734481e-02, 3.60162990e-03, 0.00000000e+00, 46 | 0.00000000e+00, 0.00000000e+00, 1.37281799e-04, 4.54614881e-03, 47 | 4.59473521e-02, 1.67818281e-01, 2.76376098e-01, 2.12786961e-01, 48 | 5.92275314e-02) 49 | .finished(), 50 | 0.1, 51 | 1.0, 52 | 5}; 53 | 54 | std::vector data = {data0, data1}; 55 | 56 | const double BETA = 1.0; 57 | const double LAMBDA = 1.0; 58 | 59 | /////////////////// 60 | ////// TESTS ////// 61 | /////////////////// 62 | 63 | TEST(Tests_E_Step, computeP) { 64 | for (auto d : data) { 65 | VectorInt line_sizes = VectorInt::Ones(d.Y.rows()); 66 | ScanToModel scan_to_model(d.X, d.Y, BETA, LAMBDA, line_sizes); 67 | scan_to_model.setThresholdTruncate(d.distance_threshold); 68 | scan_to_model.setOutlierRate(d.w); 69 | scan_to_model.setSigma2(pow(d.sigma, 2)); 70 | scan_to_model.initialize(); 71 | scan_to_model.computeP(); 72 | Probabilities P = scan_to_model.getP(); 73 | EXPECT_TRUE(P.p1.isApprox(d.P.rowwise().sum(), 1e-4)) 74 | << "Expected P\n:" << d.P << std::endl 75 | << "Expected P1\n:" << d.P.rowwise().sum() << std::endl 76 | << "Returned P1\n:" << P.p1 << std::endl; 77 | EXPECT_TRUE(P.pt1.isApprox(d.P.colwise().sum().transpose(), 1e-4)) 78 | << "Expected P\n:" << d.P << std::endl 79 | << "Expected Pt1\n:" << d.P.colwise().sum().transpose() << std::endl 80 | << "Returned Pt1\n:" << P.pt1 << std::endl; 81 | EXPECT_TRUE(P.px.isApprox(d.P * d.X, 1e-4)) 82 | << "Expected P\n:" << d.P << std::endl 83 | << "Expected PX\n:" << d.P * d.X << std::endl 84 | << "Returned PX\n:" << P.px << std::endl; 85 | } 86 | } 87 | 88 | TEST(Tests_E_Step, computeP_FGT) { 89 | for (auto d : data) { 90 | VectorInt line_sizes = VectorInt::Ones(d.Y.rows()); 91 | ScanToModel scan_to_model(d.X, d.Y, BETA, LAMBDA, line_sizes); 92 | scan_to_model.setThresholdTruncate(d.distance_threshold); 93 | scan_to_model.setOutlierRate(d.w); 94 | scan_to_model.setSigma2(pow(d.sigma, 2)); 95 | scan_to_model.initialize(); 96 | scan_to_model.computeP_FGT(); 97 | Probabilities P = scan_to_model.getP(); 98 | EXPECT_TRUE(P.p1.isApprox(d.P.rowwise().sum(), 1e-4)) 99 | << "Expected P\n:" << d.P << std::endl 100 | << "Expected P1\n:" << d.P.rowwise().sum() << std::endl 101 | << "Returned P1\n:" << P.p1 << std::endl; 102 | EXPECT_TRUE(P.pt1.isApprox(d.P.colwise().sum().transpose(), 1e-4)) 103 | << "Expected P\n:" << d.P << std::endl 104 | << "Expected Pt1\n:" << d.P.colwise().sum().transpose() << std::endl 105 | << "Returned Pt1\n:" << P.pt1 << std::endl; 106 | EXPECT_TRUE(P.px.isApprox(d.P * d.X, 1e-4)) 107 | << "Expected P\n:" << d.P << std::endl 108 | << "Expected PX\n:" << d.P * d.X << std::endl 109 | << "Returned PX\n:" << P.px << std::endl; 110 | } 111 | } -------------------------------------------------------------------------------- /include/lnrr/scan_to_model.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace lnrr { 11 | const size_t DEFAULT_MAX_ITERATIONS = 150; 12 | const double DEFAULT_OUTLIERS = 0.1; 13 | const double DEFAULT_THRESHOLD_TRUNCATE = 1e6; 14 | const double DEFAULT_TOLERANCE = 1e-5; 15 | const double DEFAULT_SIGMA2 = 0.0; 16 | const double FGT_EPSILON = 1e-4; 17 | const double FGT_THRESHOLD = 0.2; 18 | 19 | class ScanToModel { 20 | private: 21 | MatrixX3 fixed_; 22 | MatrixX3 moving_; 23 | MatrixX3 moving_transformed_; 24 | 25 | Probabilities P_; 26 | 27 | Matrix G_; 28 | Sparse jacobianG_; 29 | MatrixX6 W_; 30 | 31 | double fgt_epsilon_ = FGT_EPSILON; 32 | double fgt_threshold_ = FGT_THRESHOLD; 33 | 34 | double beta_; 35 | double lambda_; 36 | VectorInt line_sizes_; 37 | int number_lines_; 38 | size_t max_iterations_; 39 | double outliers_; 40 | double threshold_truncate_2_; 41 | double sigma2_; 42 | double tolerance_; 43 | 44 | double defaultSigma2(); 45 | void computeSigma2(); 46 | 47 | public: 48 | ScanToModel(const MatrixX3& fixed, const MatrixX3& moving, 49 | const double& beta, const double& lambda, 50 | const VectorInt& line_sizes) 51 | : fixed_(fixed), 52 | moving_(moving), 53 | beta_(beta), 54 | lambda_(lambda), 55 | line_sizes_(line_sizes), 56 | number_lines_(line_sizes.rows()), 57 | max_iterations_(DEFAULT_MAX_ITERATIONS), 58 | outliers_(DEFAULT_OUTLIERS), 59 | threshold_truncate_2_(DEFAULT_THRESHOLD_TRUNCATE), 60 | sigma2_(DEFAULT_SIGMA2), 61 | tolerance_(DEFAULT_TOLERANCE) { 62 | assert(line_sizes.sum() == moving.rows() && 63 | "The vector line_sizes must agree with the number of points of " 64 | "each line in the scan"); 65 | assert(beta > 0.0 && "beta must be positive non-zero"); 66 | assert(lambda > 0.0 && "lambda must be positive non-zero"); 67 | } 68 | 69 | ScanToModel(const PointCloudPtr& fixed, 70 | const std::vector& moving, const double& beta, 71 | const double& lambda) 72 | : beta_(beta), 73 | lambda_(lambda), 74 | max_iterations_(DEFAULT_MAX_ITERATIONS), 75 | outliers_(DEFAULT_OUTLIERS), 76 | threshold_truncate_2_(DEFAULT_THRESHOLD_TRUNCATE), 77 | sigma2_(DEFAULT_SIGMA2), 78 | tolerance_(DEFAULT_TOLERANCE) { 79 | assert(beta > 0.0 && "beta must be positive non-zero"); 80 | assert(lambda > 0.0 && "lambda must be positive non-zero"); 81 | 82 | pclModelToEigen(fixed, fixed_); 83 | pclScanToEigen(moving, moving_, line_sizes_); 84 | number_lines_ = line_sizes_.rows(); 85 | 86 | assert(line_sizes_.sum() == moving_.rows() && 87 | "The vector line_sizes must agree with the number of points of " 88 | "each line in the scan"); 89 | assert(fixed_.rows() > 0 && "The model must have at least one point"); 90 | assert(moving_.rows() > 0 && "The scan must have at least one point"); 91 | assert(number_lines_ > 0 && "The scan must have at least one line"); 92 | } 93 | 94 | ~ScanToModel() {} 95 | 96 | void initialize(); 97 | void computeP(); 98 | void computeP_FGT(); 99 | void computeW(); 100 | void computeOne(); 101 | MatrixX3 getTransformedMoving() { 102 | return computeTransformedMoving(moving_, G_, W_, line_sizes_); 103 | } 104 | Result run(); 105 | 106 | // Set functions 107 | void setP(const Probabilities& P) { P_ = P; } 108 | void setFixed(const MatrixX3& fixed) { fixed_ = fixed; } 109 | void setMoving(const MatrixX3& moving) { moving_ = moving; } 110 | void setLineSizes(const VectorInt& line_sizes) { 111 | line_sizes_ = line_sizes; 112 | number_lines_ = line_sizes.rows(); 113 | } 114 | void setBeta(const double& beta) { beta_ = beta; } 115 | void setLambda(const double& lambda) { lambda_ = lambda; } 116 | 117 | void setMaxIterations(const size_t& max_iterations) { 118 | max_iterations_ = max_iterations; 119 | } 120 | void setOutlierRate(const double& outliers) { outliers_ = outliers; } 121 | void setThresholdTruncate(const double& threshold) { 122 | threshold_truncate_2_ = pow(threshold, 2); 123 | } 124 | void setSigma2(const double& sigma2) { sigma2_ = sigma2; } 125 | void setTolerance(const double& tolerance) { tolerance_ = tolerance; } 126 | 127 | // Get functions 128 | Probabilities getP() { return P_; } 129 | MatrixX6 getW() { return W_; } 130 | MatrixX3 getFixed() { return fixed_; } 131 | MatrixX3 getMoving() { return moving_; } 132 | VectorInt getLineSizes() { return line_sizes_; } 133 | int getNumberLines() { return number_lines_; } 134 | double getBeta() { return beta_; } 135 | double getLambda() { return lambda_; } 136 | size_t getMaxIterations() { return max_iterations_; } 137 | double getOutlierRate() { return outliers_; } 138 | double getThresholdTruncate() { return threshold_truncate_2_; } 139 | double getSigma2() { return sigma2_; } 140 | double getTolerance() { return tolerance_; } 141 | }; 142 | } // namespace lnrr -------------------------------------------------------------------------------- /include/lnrr/utils/test_utils.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace lnrr; 6 | 7 | MatrixX3 readFromTxtFile(const std::string& file_name) { 8 | std::ifstream file(file_name); 9 | std::string line; 10 | std::vector values; 11 | while (std::getline(file, line, ',')) { 12 | std::stringstream line_stream(line); 13 | double value; 14 | while (line_stream >> value) 15 | values.push_back(value); 16 | } 17 | size_t rows = values.size() / 3; 18 | 19 | MatrixX3 result(rows, 3); 20 | for (size_t i = 0; i < rows; i++) { 21 | result(i, 0) = values[i * 3]; 22 | result(i, 1) = values[i * 3 + 1]; 23 | result(i, 2) = values[i * 3 + 2]; 24 | } 25 | file.close(); 26 | return result; 27 | } 28 | 29 | VectorInt readLineSizesFromTxtFile(const std::string& file_name) { 30 | std::ifstream file(file_name); 31 | std::string line; 32 | std::vector values; 33 | while (std::getline(file, line)) { 34 | std::stringstream line_stream(line); 35 | double value; 36 | while (line_stream >> value) 37 | values.push_back(value); 38 | } 39 | file.close(); 40 | VectorInt vector(values.size()); 41 | for (size_t i = 0; i < values.size(); i++) 42 | vector[i] = values[i]; 43 | return vector; 44 | } 45 | 46 | void writeResultToTxtFile(const std::string& file_name, 47 | const MatrixX3& points) { 48 | std::ofstream file(file_name); 49 | for (size_t i = 0; i < points.rows(); i++) 50 | file << points(i, 0) << " " << points(i, 1) << " " << points(i, 2) 51 | << std::endl; 52 | file.close(); 53 | } 54 | 55 | std::vector loadPCDFilesFromPath(const std::string& path) { 56 | std::vector clouds; 57 | std::vector files; 58 | boost::filesystem::path dir(path); 59 | boost::filesystem::directory_iterator end_iter; 60 | if (boost::filesystem::exists(dir) && boost::filesystem::is_directory(dir)) 61 | for (boost::filesystem::directory_iterator dir_itr(dir); 62 | dir_itr != end_iter; ++dir_itr) 63 | if (boost::filesystem::is_regular_file(dir_itr->status())) 64 | files.push_back(dir_itr->path().string()); 65 | std::sort(files.begin(), files.end()); 66 | for (auto& file : files) { 67 | PointCloudPtr cloud(new PointCloud); 68 | pcl::io::loadPCDFile(file, *cloud); 69 | clouds.push_back(cloud); 70 | } 71 | return clouds; 72 | } 73 | 74 | // wrote result to pcd file 75 | void eigenCloudToPCL(const MatrixX3& result, PointCloudPtr cloud) { 76 | if (cloud == nullptr) 77 | cloud.reset(new PointCloud); 78 | cloud->width = result.rows(); 79 | cloud->height = 1; 80 | cloud->points.resize(cloud->width * cloud->height); 81 | for (size_t i = 0; i < result.rows(); i++) { 82 | cloud->points[i].x = result(i, 0); 83 | cloud->points[i].y = result(i, 1); 84 | cloud->points[i].z = result(i, 2); 85 | } 86 | } 87 | 88 | void writeResultToPCDFile(const std::string& file_name, 89 | const MatrixX3& result) { 90 | PointCloudPtr cloud(new PointCloud); 91 | eigenCloudToPCL(result, cloud); 92 | pcl::io::savePCDFile(file_name, *cloud); 93 | } 94 | 95 | ////////////////////////////// 96 | 97 | double generateRandomNumber(const double& min, const double& max) { 98 | // std::random_device r; 99 | // std::default_random_engine re{r()}; 100 | std::default_random_engine re{static_cast(time(0))}; 101 | std::uniform_real_distribution unif(min, max); 102 | return unif(re); 103 | } 104 | 105 | RigidTransform 106 | generateRandomTransformation(const double& max_translation, 107 | const double& max_rotation_deg) { 108 | RigidTransform t; 109 | t.xyz = max_translation * Vector3::Random(); 110 | double angle = M_PI / 180.0 * 111 | generateRandomNumber(-max_rotation_deg, max_rotation_deg); 112 | Vector3 axis = Vector3(generateRandomNumber(-1.0, 1.0), 113 | generateRandomNumber(-1.0, 1.0), 114 | generateRandomNumber(-1.0, 1.0)) 115 | .normalized(); 116 | assert(abs(axis.norm() - 1.0) < 1e-8); 117 | // just in case random vector generation 118 | // does something strange 119 | if (abs(angle) > 0.0) 120 | t.rot_mat = Matrix3(Eigen::AngleAxisd(angle, axis)); 121 | else 122 | t.rot_mat.setIdentity(); 123 | 124 | return t; 125 | } 126 | 127 | std::vector> 128 | generateRandomTransformations(const int& number_transformations, 129 | const double& max_translation, 130 | const double& max_rotation_deg) { 131 | std::vector> t(number_transformations); 132 | for (size_t i = 0; i < number_transformations; i++) 133 | t[i] = generateRandomTransformation(max_translation, max_rotation_deg); 134 | return t; 135 | } 136 | 137 | RigidTransform invertTransformation(const RigidTransform& T) { 138 | RigidTransform T_inv; 139 | T_inv.rot_mat = T.rot_mat.transpose(); 140 | T_inv.xyz = -T_inv.rot_mat * T.xyz; 141 | return T_inv; 142 | } 143 | 144 | std::vector> 145 | invertTransformations(const std::vector>& T) { 146 | std::vector> T_inv(T.size()); 147 | for (size_t i = 0; i < T.size(); i++) 148 | T_inv[i] = invertTransformation(T[i]); 149 | return T_inv; 150 | } -------------------------------------------------------------------------------- /tests/M_step_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace lnrr; 6 | 7 | struct Data { 8 | std::vector> T; 9 | MatrixX3 X; 10 | MatrixX3 Y; 11 | Probabilities P; 12 | }; 13 | 14 | const double LAMBDA = 1.0e-30; 15 | const double BETA = 1e-6; 16 | const int L = 3; 17 | const Matrix G = Matrix::Identity(L, L); // If G is identity, then T = W 18 | const int NUMBER_POINTS_LINE = 3; 19 | const int M = L * NUMBER_POINTS_LINE; 20 | const VectorInt LINE_SIZES = VectorInt::Ones(L) * NUMBER_POINTS_LINE; 21 | 22 | // No rotation 23 | Data data_translation0 = { 24 | generateRandomTransformations(L, 1.0e-3, 0.0), 25 | MatrixX3::Random(M, 3), 26 | computeTransformedMoving(data_translation0.X, data_translation0.T, 27 | LINE_SIZES), 28 | {Vector::Ones(M), Vector::Ones(M), data_translation0.X, 0.0}}; 29 | Data data_translation1 = { 30 | generateRandomTransformations(L, 1.0, 0.0), 31 | MatrixX3::Random(M, 3), 32 | computeTransformedMoving(data_translation1.X, data_translation1.T, 33 | LINE_SIZES), 34 | {Vector::Ones(M), Vector::Ones(M), data_translation1.X, 0.0}}; 35 | Data data_translation2 = { 36 | generateRandomTransformations(L, 10.0, 0.0), 37 | MatrixX3::Random(M, 3), 38 | computeTransformedMoving(data_translation2.X, data_translation2.T, 39 | LINE_SIZES), 40 | {Vector::Ones(M), Vector::Ones(M), data_translation2.X, 0.0}}; 41 | 42 | // No translation 43 | Data data_rotation0 = { 44 | generateRandomTransformations(L, 0.0, 0.1), 45 | MatrixX3::Random(M, 3), 46 | computeTransformedMoving(data_rotation0.X, data_rotation0.T, LINE_SIZES), 47 | {Vector::Ones(M), Vector::Ones(M), data_rotation0.X, 0.0}}; 48 | Data data_rotation1 = { 49 | generateRandomTransformations(L, 0.0, 1.0), 50 | MatrixX3::Random(M, 3), 51 | computeTransformedMoving(data_rotation1.X, data_rotation1.T, LINE_SIZES), 52 | {Vector::Ones(M), Vector::Ones(M), data_rotation1.X, 0.0}}; 53 | Data data_rotation2 = { 54 | generateRandomTransformations(L, 0.0, 50.0), 55 | MatrixX3::Random(M, 3), 56 | computeTransformedMoving(data_rotation2.X, data_rotation2.T, LINE_SIZES), 57 | {Vector::Ones(M), Vector::Ones(M), data_rotation2.X, 0.0}}; 58 | 59 | // Both 60 | Data data_both0 = { 61 | generateRandomTransformations(L, 1e-3, 0.1), 62 | MatrixX3::Random(M, 3), 63 | computeTransformedMoving(data_both0.X, data_both0.T, LINE_SIZES), 64 | {Vector::Ones(M), Vector::Ones(M), data_both0.X, 0.0}}; 65 | Data data_both1 = { 66 | generateRandomTransformations(L, 0.10, 1.0), 67 | MatrixX3::Random(M, 3), 68 | computeTransformedMoving(data_both1.X, data_both1.T, LINE_SIZES), 69 | {Vector::Ones(M), Vector::Ones(M), data_both1.X, 0.0}}; 70 | Data data_both2 = { 71 | generateRandomTransformations(L, 1.0, 5.0), 72 | MatrixX3::Random(M, 3), 73 | computeTransformedMoving(data_both2.X, data_both2.T, LINE_SIZES), 74 | {Vector::Ones(M), Vector::Ones(M), data_both2.X, 0.0}}; 75 | 76 | std::vector data_translation = {data_translation0, data_translation1, 77 | data_translation2}; 78 | std::vector data_rotation = {data_rotation0, data_rotation1, 79 | data_rotation2}; 80 | std::vector data_both = {data_both0, data_both1, data_both2}; 81 | 82 | /////////////////// 83 | ////// TESTS ////// 84 | /////////////////// 85 | 86 | void test_computeW(const std::vector& data, 87 | const bool& zero_translation) { 88 | for (auto d : data) { 89 | ScanToModel scan_to_model(d.X, d.Y, BETA, LAMBDA, LINE_SIZES); 90 | scan_to_model.initialize(); 91 | scan_to_model.setP(d.P); 92 | Probabilities P = scan_to_model.getP(); 93 | try { 94 | scan_to_model.computeW(); 95 | } catch (const lnrr_error& e) { 96 | std::cerr << e.what() << '\n'; 97 | } 98 | 99 | std::vector> T_computed = 100 | computeTransformations(G, scan_to_model.getW()); 101 | std::cout << "W:\n" << scan_to_model.getW() << std::endl; 102 | std::vector> T_inverse = 103 | invertTransformations(T_computed); 104 | for (size_t i = 0; i < d.T.size(); i++) { 105 | if (zero_translation) { 106 | EXPECT_TRUE(d.T[i].xyz.isMuchSmallerThan(1e-3)) 107 | << "i:" << i << std::endl 108 | << "Expected translation:\n" 109 | << d.T[i].xyz << std::endl 110 | << "Returned translation:\n" 111 | << T_inverse[i].xyz << std::endl; 112 | } else { 113 | EXPECT_TRUE(d.T[i].xyz.isApprox(T_inverse[i].xyz, 1e-3)) 114 | << "i:" << i << std::endl 115 | << "Expected translation:\n" 116 | << d.T[i].xyz << std::endl 117 | << "Returned translation:\n" 118 | << T_inverse[i].xyz << std::endl; 119 | } 120 | 121 | EXPECT_TRUE(d.T[i].rot_mat.isApprox(T_inverse[i].rot_mat, 1e-3)) 122 | << "i:" << i << std::endl 123 | << "Expected rot_mat:\n" 124 | << d.T[i].rot_mat << std::endl 125 | << "Returned rot_mat:\n" 126 | << T_inverse[i].rot_mat << std::endl; 127 | } 128 | } 129 | } 130 | 131 | TEST(Tests_M_Step, computeW_translation) { 132 | test_computeW(data_translation, false); 133 | } 134 | TEST(Tests_M_Step, computeW_rotation) { test_computeW(data_rotation, true); } 135 | TEST(Tests_M_Step, computeW_both) { test_computeW(data_both, false); } 136 | -------------------------------------------------------------------------------- /tests/conversions_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | struct RotationEquivalence { 5 | double roll; 6 | double pitch; 7 | double yaw; 8 | Eigen::Quaterniond q; 9 | Eigen::Matrix3d matrix; 10 | }; 11 | 12 | RotationEquivalence data1 = {0.0, 0.0, 0.0, 13 | Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), 14 | Eigen::Matrix3d::Identity()}; 15 | RotationEquivalence data2 = { 16 | 0.0, 0.0, M_PI_4, Eigen::Quaterniond(0.9238795, 0.0, 0.0, 0.3826834), 17 | (Eigen::Matrix3d() << 0.7071068, -0.7071068, 0.0000000, 0.7071068, 18 | 0.7071068, 0.0000000, 0.0000000, 0.0000000, 1.0000000) 19 | .finished()}; 20 | RotationEquivalence data3 = { 21 | 0.0, M_PI_4, 0.0, Eigen::Quaterniond(0.9238795, 0.0, 0.3826834, 0.0), 22 | (Eigen::Matrix3d() << 0.7071068, 0.0000000, 0.7071068, 0.0000000, 1.0000000, 23 | 0.0000000, -0.7071068, 0.0000000, 0.7071068) 24 | .finished()}; 25 | RotationEquivalence data4 = { 26 | M_PI_4, 0.0, 0.0, Eigen::Quaterniond(0.9238795, 0.3826834, 0.0, 0.0), 27 | (Eigen::Matrix3d() << 1.0000000, 0.0000000, 0.0000000, 0.0000000, 0.7071068, 28 | -0.7071068, 0.0000000, 0.7071068, 0.7071068) 29 | .finished()}; 30 | RotationEquivalence data5 = { 31 | M_PI_4, M_PI_4, M_PI_4, 32 | Eigen::Quaterniond(0.8446232, 0.1913417, 0.4619398, 0.1913417), 33 | (Eigen::Matrix3d() << 0.5, -0.1464466, 0.8535534, 0.5, 0.8535534, 34 | -0.1464466, -0.7071068, 0.5, 0.5) 35 | .finished()}; 36 | RotationEquivalence data6 = { 37 | 0.122173, -0.5410521, 3.0194196, 38 | Eigen::Quaterniond(0.0424344, 0.2698338, 0.0424344, 0.9610351), 39 | (Eigen::Matrix3d() << -0.8507781, -0.0586615, 0.5222408, 0.1044624, 40 | -0.9927973, 0.0586615, 0.5150381, 0.1044624, 0.8507781) 41 | .finished()}; 42 | 43 | std::vector data = {data1, data2, data3, 44 | data4, data5, data6}; 45 | 46 | TEST(ConversionsTests, TestRpy2Quaternion) { 47 | for (size_t i = 0; i < data.size(); i++) { 48 | Eigen::Quaterniond q_function = lnrr::conversions::rpy2Quaternion( 49 | data[i].roll, data[i].pitch, data[i].yaw); 50 | EXPECT_NEAR( 51 | 0.0, 52 | (data[i].q.toRotationMatrix() - q_function.toRotationMatrix()) 53 | .cwiseAbs() 54 | .sum(), 55 | 1e-6) 56 | << "i: " << i << std::endl 57 | << "roll: " << data[i].roll << std::endl 58 | << "pitch: " << data[i].pitch << std::endl 59 | << "yaw: " << data[i].yaw << std::endl 60 | << "q.w(): " << data[i].q.w() << ", " 61 | << "q.vec(): " << data[i].q.vec() << std::endl 62 | << "q_function.w(): " << q_function.w() << ", " 63 | << "q_function.vec(): " << q_function.vec() << std::endl; 64 | } 65 | } 66 | 67 | TEST(ConversionsTests, TestQuaternion2rpy) { 68 | for (size_t i = 0; i < data.size(); i++) { 69 | Eigen::Vector3d rpy = lnrr::conversions::quaternion2rpy(data[i].q); 70 | EXPECT_NEAR(data[i].roll, rpy[0], 1e-6) 71 | << "i: " << i << std::endl 72 | << "q.w(): " << data[i].q.w() << ", " 73 | << "q.vec(): " << data[i].q.vec() << std::endl 74 | << "roll: " << data[i].roll << std::endl 75 | << "roll_function: " << rpy[0] << std::endl; 76 | EXPECT_NEAR(data[i].pitch, rpy[1], 1e-6) 77 | << "i: " << i << std::endl 78 | << "q.w(): " << data[i].q.w() << ", " 79 | << "q.vec(): " << data[i].q.vec() << std::endl 80 | << "pitch: " << data[i].pitch << std::endl 81 | << "pitch_function: " << rpy[1] << std::endl; 82 | EXPECT_NEAR(data[i].yaw, rpy[2], 1e-6) 83 | << "i: " << i << std::endl 84 | << "q.w(): " << data[i].q.w() << ", " 85 | << "q.vec(): " << data[i].q.vec() << std::endl 86 | << "yaw: " << data[i].yaw << std::endl 87 | << "yaw_function: " << rpy[2] << std::endl; 88 | } 89 | } 90 | 91 | TEST(ConversionsTests, TestRpy2RotationMatrix) { 92 | for (size_t i = 0; i < data.size(); i++) { 93 | Eigen::Matrix3d matrix_function = lnrr::conversions::rpy2RotationMatrix( 94 | data[i].roll, data[i].pitch, data[i].yaw); 95 | EXPECT_NEAR(0.0, (data[i].matrix - matrix_function).cwiseAbs().sum(), 96 | 1e-6) 97 | << "i: " << i << std::endl 98 | << "roll: " << data[i].roll << std::endl 99 | << "pitch: " << data[i].pitch << std::endl 100 | << "yaw: " << data[i].yaw << std::endl 101 | << "matrix: " << data[i].matrix << std::endl 102 | << "matrix_function: " << matrix_function << std::endl; 103 | } 104 | } 105 | 106 | TEST(ConversionsTests, TestRotationMatrix2rpy) { 107 | for (size_t i = 0; i < data.size(); i++) { 108 | Eigen::Vector3d rpy = 109 | lnrr::conversions::rotationMatrix2rpy(data[i].matrix); 110 | EXPECT_NEAR(data[i].roll, rpy[0], 1e-6) 111 | << "i: " << i << std::endl 112 | << "matrix: " << data[i].matrix << std::endl 113 | << "roll: " << data[i].roll << std::endl 114 | << "roll_function: " << rpy[0] << std::endl; 115 | EXPECT_NEAR(data[i].pitch, rpy[1], 1e-6) 116 | << "i: " << i << std::endl 117 | << "matrix: " << data[i].matrix << std::endl 118 | << "pitch: " << data[i].pitch << std::endl 119 | << "pitch_function: " << rpy[1] << std::endl; 120 | EXPECT_NEAR(data[i].yaw, rpy[2], 1e-6) 121 | << "i: " << i << std::endl 122 | << "matrix: " << data[i].matrix << std::endl 123 | << "yaw: " << data[i].yaw << std::endl 124 | << "yaw_function: " << rpy[2] << std::endl; 125 | } 126 | } -------------------------------------------------------------------------------- /examples/lnrr.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | const double VOXEL_SIZE = 0.01; 8 | const int NUMBER_ITERATIONS = 15; 9 | 10 | using namespace lnrr; 11 | 12 | int main(int argc, char const* argv[]) { 13 | std::string filename_model, folder_scans, filename_output; 14 | double beta, lambda, threshold_truncate; 15 | if (argc == 7) { 16 | filename_model = argv[1]; 17 | folder_scans = argv[2]; 18 | filename_output = argv[3]; 19 | beta = atof(argv[4]); 20 | lambda = atof(argv[5]); 21 | threshold_truncate = atof(argv[6]); 22 | } else { 23 | std::cout << "Usage: " << argv[0] 24 | << " " 25 | " " 26 | " " 27 | " " 28 | " " 29 | " " 30 | << std::endl; 31 | return -1; 32 | } 33 | std::cout << "Read input parameters:" << std::endl; 34 | std::cout << " -- filename_model: " << filename_model << std::endl; 35 | std::cout << " -- folder_scans: " << folder_scans << std::endl; 36 | std::cout << " -- filename_output: " << filename_output << std::endl; 37 | std::cout << " -- beta: " << beta << std::endl; 38 | std::cout << " -- lambda: " << lambda << std::endl; 39 | std::cout << " -- threshold_truncate: " << threshold_truncate 40 | << std::endl; 41 | 42 | PointCloudPtr model(new PointCloud); 43 | pcl::io::loadPCDFile(filename_model, *model); 44 | 45 | std::vector scan = loadPCDFilesFromPath(folder_scans); 46 | 47 | // subsample model 48 | PointCloudPtr model_subsampled(new PointCloud); 49 | pcl::VoxelGrid voxel_grid; 50 | voxel_grid.setInputCloud(model); 51 | voxel_grid.setLeafSize(VOXEL_SIZE / 2.0, VOXEL_SIZE / 2.0, 52 | VOXEL_SIZE / 2.0); 53 | voxel_grid.filter(*model_subsampled); 54 | 55 | // subsample scans (take only half of the lines) 56 | std::vector scan_subsampled; 57 | for (size_t i = 0; i < scan.size(); i += 3) { 58 | PointCloudPtr scan_subsampled_i(new PointCloud); 59 | voxel_grid.setInputCloud(scan[i]); 60 | voxel_grid.setLeafSize(VOXEL_SIZE, VOXEL_SIZE, VOXEL_SIZE); 61 | voxel_grid.filter(*scan_subsampled_i); 62 | scan_subsampled.push_back(scan_subsampled_i); 63 | } 64 | 65 | int number_lines = scan_subsampled.size(); 66 | int number_points = 0; 67 | for (size_t i = 0; i < number_lines; i++) 68 | number_points += scan_subsampled[i]->size(); 69 | 70 | std::cout << "Number of points in model: " << model_subsampled->size() 71 | << std::endl; 72 | std::cout << "Number of lines in scan: " << number_lines << std::endl; 73 | std::cout << "Number of points in scan: " << number_points << std::endl; 74 | 75 | std::cout << "Initializing..." << std::endl; 76 | ScanToModel scan_to_model(model_subsampled, scan_subsampled, beta, lambda); 77 | scan_to_model.setMaxIterations(NUMBER_ITERATIONS); 78 | scan_to_model.setThresholdTruncate(threshold_truncate); 79 | std::cout << "Starting registration (compile LNRR in DEBUG mode to get " 80 | "information at each iteration)" 81 | << std::endl; 82 | scan_to_model.initialize(); 83 | // Result result = scan_to_model.run(); 84 | 85 | // compute one step of the algorithm and visualize the resulting point 86 | // clouds 87 | pcl::transformPointCloud(*model_subsampled, *model_subsampled, 88 | Eigen::Vector3d(0.0, 0.0, 0.0), 89 | conversions::rpy2Quaternion(M_PI_2, 0.0, 0.0)); 90 | pcl::visualization::PCLVisualizer::Ptr viewer( 91 | new pcl::visualization::PCLVisualizer("LNRR")); 92 | // viewer->setCameraPosition(0, 0, 0.5, 0, 0, -1, 0); 93 | viewer->setBackgroundColor(0.9, 0.9, 0.9); 94 | viewer->addPointCloud(model_subsampled, "model"); 95 | viewer->setPointCloudRenderingProperties( 96 | pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 1.0, "model"); 97 | PointCloudPtr scan_aggregated(new PointCloud); 98 | for (size_t i = 0; i < number_lines; i++) 99 | *scan_aggregated += *scan_subsampled[i]; 100 | viewer->addPointCloud(scan_aggregated, "scan"); 101 | viewer->setPointCloudRenderingProperties( 102 | pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "scan"); 103 | 104 | viewer->setPointCloudRenderingProperties( 105 | pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "model"); 106 | viewer->setPointCloudRenderingProperties( 107 | pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "scan"); 108 | 109 | // viewer->spin(); 110 | 111 | for (int i = 0; i < NUMBER_ITERATIONS; i++) { 112 | scan_to_model.computeOne(); 113 | PointCloudPtr scan_transformed(new PointCloud); 114 | eigenCloudToPCL(scan_to_model.getTransformedMoving(), scan_transformed); 115 | // visualize the resulting point clouds 116 | viewer->updatePointCloud(scan_transformed, "scan"); 117 | // viewer->addCoordinateSystem(1.0); 118 | viewer->spinOnce(); 119 | std::cout << "Finished teration " << i + 1 << " / " << NUMBER_ITERATIONS 120 | << std::endl; 121 | } 122 | 123 | std::cout 124 | << "Registration finished, writing registered scan in output file." 125 | << std::endl; 126 | 127 | writeResultToPCDFile(filename_output, scan_to_model.getTransformedMoving()); 128 | // std::cout << "Registration finished after " << result.iterations 129 | // << " iterations in " << (double)result.runtime.count() / 1e6 130 | // << " seconds." << std::endl; 131 | viewer->spin(); 132 | std::cout << "Exiting." << std::endl; 133 | return 0; 134 | } 135 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Linewise Non-Rigid Registration (LNRR) 2 | 3 | This repository provides a basic implementation of the non-rigid point cloud registration method described in [1]. 4 | For a detailed explanation of the method and its motivation, please refer to the [original paper](https://doi.org/10.1109/LRA.2022.3180038). 5 | In a nutshell, the method is able to find the set of rigid transformations that need to be applied to *each line* in the scan in order to match an existing model: 6 | 7 | 8 | 9 | You can also watch the video that summarizes this work: 10 | 11 | [![Watch the video](https://img.youtube.com/vi/4QDZ7z1WER8/mqdefault.jpg)](https://youtu.be/4QDZ7z1WER8) 12 | 13 | Additionally, you can watch the presentation of this work at IROS 2022: 14 | 15 | [![Watch the video](https://img.youtube.com/vi/tp0ob9yHagQ/mqdefault.jpg)](https://youtu.be/tp0ob9yHagQ) 16 | 17 | Thank you for citing the original publication [1] if you use our method in academic work: 18 | ``` 19 | @ARTICLE{9788023, 20 | author={Castillón, Miguel and Ridao, Pere and Siegwart, Roland and Cadena, César}, 21 | journal={IEEE Robotics and Automation Letters}, 22 | title={Linewise Non-Rigid Point Cloud Registration}, 23 | year={2022}, 24 | volume={7}, 25 | number={3}, 26 | pages={7044-7051}, 27 | doi={10.1109/LRA.2022.3180038}} 28 | ``` 29 | 30 | If you want to know more about our underwater 3D scanner, check out [the paper](https://doi.org/10.1109/TMECH.2022.3170504) [2] and [this blog post](https://miguelcastillon.github.io/project/underwater-3d-scanner/). 31 | 32 | ## Publications 33 | 34 | [1] M. Castillón, P. Ridao, R. Siegwart and C. Cadena, "Linewise Non-Rigid Point Cloud Registration," in IEEE Robotics and Automation Letters, doi: 10.1109/LRA.2022.3180038. [[pdf](https://doi.org/10.1109/LRA.2022.3180038)] 35 | 36 | [2] M. Castillón, J. Forest and P. Ridao, "Underwater 3D Scanner to Counteract Refraction: Calibration and Experimental Results," in IEEE/ASME Transactions on Mechatronics, doi: 10.1109/TMECH.2022.3170504. [[pdf](https://doi.org/10.1109/TMECH.2022.3170504)] 37 | 38 | 39 | ## Installation 40 | 41 | ### Dependencies 42 | 43 | Our methods depends on [CMake](https://cmake.org/), [Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page), [PCL](https://pointclouds.org/) and [Ceres](http://ceres-solver.org/index.html) [please note that so far, it has only been tested on Ubuntu 20.04 + Eigen 3.3.7 + PCL 1.10 + Ceres 2.2.] 44 | Moreover, our method uses Fast Gauss Transforms to compute the correspondence probability between each pair of points. 45 | Therefore, our method depends on 46 | [fgt](https://github.com/miguelcastillon/fgt_threshold), which is a fork of [this repository](https://github.com/gadomski/fgt). 47 | 48 | In summary, first do: 49 | ```bash 50 | sudo apt install \ 51 | cmake \ 52 | libeigen3-dev \ 53 | libpcl-dev 54 | ``` 55 | Then, in your `libraries` folder, compile Ceres: 56 | ```bash 57 | git clone https://ceres-solver.googlesource.com/ceres-solver 58 | cd ceres-solver && mkdir build && cd build 59 | cmake .. \ 60 | -DBUILD_TESTING=OFF \ 61 | -DBUILD_EXAMPLES=OFF 62 | make -jX 63 | sudo make install 64 | cd ../.. 65 | ``` 66 | Then compile fgt: 67 | ```bash 68 | git clone https://github.com/miguelcastillon/fgt_threshold 69 | cd fgt_threshold && mkdir build && cd build 70 | cmake .. \ 71 | -DCMAKE_BUILD_TYPE=Release \ 72 | -DWITH_OPENMP=ON \ 73 | -DBUILD_SHARED_LIBS=ON \ 74 | -DWITH_TESTS=OFF 75 | make -jX 76 | sudo make install 77 | cd ../.. 78 | ``` 79 | 80 | ### Compilation 81 | As usual, just download and unzip this repository in your preferred location and `cd` into it. 82 | Then: 83 | ```bash 84 | git clone https://github.com/miguelcastillon/lnrr 85 | cd lnrr && mkdir build && cd build 86 | cmake [OPTIONS] .. 87 | make -jX 88 | sudo make install 89 | ``` 90 | 91 | There are several options that you can pass to CMake: 92 | - `-DCMAKE_BUILD_TYPE=Debug` to get information for each iteration (default is Release). 93 | - `-DBUILD_TESTS=OFF` to disable the compilation of the tests (default is ON). 94 | 95 | If at any point you want to uninstall the library, just do: 96 | ```bash 97 | sudo make uninstall 98 | ``` 99 | 100 | 101 | ## Usage 102 | 103 | ```cpp 104 | #include 105 | 106 | int main(int argc, char** argv) { 107 | lnrr::PointCloudPtr fixed = loadModel(); 108 | std::vector moving = loadScan(); 109 | double beta = ...; 110 | double lambda = ...; 111 | 112 | lnrr::ScanToModel lnrr(fixed, moving, beta, lambda); 113 | lnrr::Result result = lnrr.run(); 114 | return 0; 115 | } 116 | ``` 117 | 118 | And your `CMakeLists.txt` should include: 119 | ```cmake 120 | find_package(OpenMP REQUIRED) 121 | find_package(Ceres REQUIRED) 122 | find_package(Fgt REQUIRED) 123 | find_package(Lnrr REQUIRED) 124 | 125 | add_library(my-new-library 126 | my_program.cpp 127 | ) 128 | target_link_libraries(my-new-library 129 | PUBLIC 130 | Lnrr::Library-C++ 131 | ) 132 | ``` 133 | 134 | Check the folder `examples` to see how to write a `.cpp` file and a `CMakeLists.txt` for your project. 135 | 136 | ### Example 137 | 138 | There is an example in the `examples` folder. 139 | Inside, you can run 140 | ```bash 141 | mkdir build 142 | cd build 143 | cmake .. 144 | make -jX 145 | ``` 146 | Then, from `examples/build` you can run it like this: 147 | ```bash 148 | ./lnrr_example \ 149 | ../data/model.pcd \ 150 | ../data/scans/ \ 151 | ../data/output.pcd \ 152 | 5 \ 153 | 30000 \ 154 | 0.03 155 | ``` 156 | 157 | ## Contributing 158 | 159 | Please feel free to create [issues](https://github.com/miguelcastillon/lnrr/issues) and [pull requests](https://github.com/miguelcastillon/lnrr/pulls), they will be much appreciated. 160 | 161 | ## Documentation 162 | 163 | Please be aware that this repository is only a simple implementation of the method and may therefore unfortunately not always show a robust behaviour. 164 | There is no documentation yet but we hope the code is self-explanatory. 165 | 166 | ## License 167 | 168 | This library is GPL2, copyright 2022 Miguel Castillón. See LICENSE.txt for the full license text. 169 | 170 | In the creation of this library we have drawn inspiration from [this cpd implementation](https://github.com/gadomski/cpd) by Gadomski. 171 | -------------------------------------------------------------------------------- /include/lnrr/ceres/cost_W.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace lnrr { 9 | class CostFunctionW { 10 | public: 11 | CostFunctionW(const Matrix& moving, const Matrix& G, 12 | const Sparse& jacobianG, const Vector& PX_vec, 13 | const Vector& P1, const VectorInt& line_sizes, 14 | const double& lambda) 15 | : moving_(moving), 16 | G_(G), 17 | jacobianG_(jacobianG), 18 | PX_vec_(PX_vec), 19 | P1_(P1), 20 | line_sizes_(line_sizes), 21 | lambda_(lambda) {} 22 | 23 | template 24 | bool operator()(T const* const* parameters, T* residual) const { 25 | #ifdef DEBUG 26 | auto tic = std::chrono::high_resolution_clock::now(); 27 | #endif 28 | int number_lines_scan = G_.rows(); 29 | const Eigen::Map> W_matrix(parameters[0], 30 | number_lines_scan, 6); 31 | MatrixT G_T = G_.cast(); 32 | #ifdef DEBUG 33 | auto toc = std::chrono::high_resolution_clock::now(); 34 | double time_A = 35 | std::chrono::duration_cast(toc - tic) 36 | .count(); 37 | 38 | tic = std::chrono::high_resolution_clock::now(); 39 | #endif 40 | // This line is the second bottleneck (10% - 40%) 41 | std::vector> T_lines = 42 | computeTransformations(G_T, MatrixX6T(W_matrix)); 43 | #ifdef DEBUG 44 | toc = std::chrono::high_resolution_clock::now(); 45 | double time_B = 46 | std::chrono::duration_cast(toc - tic) 47 | .count(); 48 | 49 | tic = std::chrono::high_resolution_clock::now(); 50 | #endif 51 | MatrixX3T moving_T = moving_.cast(); 52 | #ifdef DEBUG 53 | toc = std::chrono::high_resolution_clock::now(); 54 | double time_C = 55 | std::chrono::duration_cast(toc - tic) 56 | .count(); 57 | 58 | tic = std::chrono::high_resolution_clock::now(); 59 | #endif 60 | SparseT F = 61 | computeJacobianPointComposition(T_lines, moving_T, line_sizes_); 62 | #ifdef DEBUG 63 | toc = std::chrono::high_resolution_clock::now(); 64 | double time_D = 65 | std::chrono::duration_cast(toc - tic) 66 | .count(); 67 | 68 | tic = std::chrono::high_resolution_clock::now(); 69 | #endif 70 | MatrixX3T moving_transformed = 71 | computeTransformedMoving(moving_T, T_lines, line_sizes_); 72 | #ifdef DEBUG 73 | toc = std::chrono::high_resolution_clock::now(); 74 | double time_E = 75 | std::chrono::duration_cast(toc - tic) 76 | .count(); 77 | 78 | tic = std::chrono::high_resolution_clock::now(); 79 | #endif 80 | MatrixX6T lambdaGW = T(lambda_) * G_T * W_matrix; 81 | #ifdef DEBUG 82 | toc = std::chrono::high_resolution_clock::now(); 83 | double time_F = 84 | std::chrono::duration_cast(toc - tic) 85 | .count(); 86 | 87 | tic = std::chrono::high_resolution_clock::now(); 88 | #endif 89 | const Eigen::Map> lambdaGW_vec(lambdaGW.data(), 90 | lambdaGW.size()); 91 | #ifdef DEBUG 92 | toc = std::chrono::high_resolution_clock::now(); 93 | double time_G = 94 | std::chrono::duration_cast(toc - tic) 95 | .count(); 96 | 97 | tic = std::chrono::high_resolution_clock::now(); 98 | #endif 99 | const MatrixT dP1Yr = 100 | P1_.cast().asDiagonal() * moving_transformed; 101 | #ifdef DEBUG 102 | toc = std::chrono::high_resolution_clock::now(); 103 | double time_H = 104 | std::chrono::duration_cast(toc - tic) 105 | .count(); 106 | 107 | tic = std::chrono::high_resolution_clock::now(); 108 | #endif 109 | const Eigen::Map> dP1Yr_vec(dP1Yr.data(), 110 | dP1Yr.size()); 111 | #ifdef DEBUG 112 | toc = std::chrono::high_resolution_clock::now(); 113 | double time_I = 114 | std::chrono::duration_cast(toc - tic) 115 | .count(); 116 | 117 | tic = std::chrono::high_resolution_clock::now(); 118 | #endif 119 | // This matrix multiplication is the bottleneck (70% - 90%) 120 | SparseT aux0 = (F * jacobianG_.cast()).transpose(); 121 | #ifdef DEBUG 122 | toc = std::chrono::high_resolution_clock::now(); 123 | double time_J = 124 | std::chrono::duration_cast(toc - tic) 125 | .count(); 126 | 127 | tic = std::chrono::high_resolution_clock::now(); 128 | #endif 129 | VectorT aux1 = (-PX_vec_.cast() + dP1Yr_vec); 130 | #ifdef DEBUG 131 | toc = std::chrono::high_resolution_clock::now(); 132 | double time_K = 133 | std::chrono::duration_cast(toc - tic) 134 | .count(); 135 | 136 | tic = std::chrono::high_resolution_clock::now(); 137 | #endif 138 | const VectorT res = aux0 * aux1 + lambdaGW_vec; 139 | #ifdef DEBUG 140 | toc = std::chrono::high_resolution_clock::now(); 141 | double time_L = 142 | std::chrono::duration_cast(toc - tic) 143 | .count(); 144 | 145 | double time_total = time_A + time_B + time_C + time_D + time_E + 146 | time_F + time_G + time_H + time_I + time_J + 147 | time_K + time_L; 148 | #if true 149 | std::cout << "****************" << std::endl; 150 | std::cout << " Times cost_W.h " << std::endl; 151 | std::cout << "****************" << std::endl; 152 | std::cout << "time_A: " << time_A << " (" << time_A * 100 / time_total 153 | << "%)" << std::endl; 154 | std::cout << "time_B: " << time_B << " (" << time_B * 100 / time_total 155 | << "%)" << std::endl; 156 | std::cout << "time_C: " << time_C << " (" << time_C * 100 / time_total 157 | << "%)" << std::endl; 158 | std::cout << "time_D: " << time_D << " (" << time_D * 100 / time_total 159 | << "%)" << std::endl; 160 | std::cout << "time_E: " << time_E << " (" << time_E * 100 / time_total 161 | << "%)" << std::endl; 162 | std::cout << "time_F: " << time_F << " (" << time_F * 100 / time_total 163 | << "%)" << std::endl; 164 | std::cout << "time_G: " << time_G << " (" << time_G * 100 / time_total 165 | << "%)" << std::endl; 166 | std::cout << "time_H: " << time_H << " (" << time_H * 100 / time_total 167 | << "%)" << std::endl; 168 | std::cout << "time_I: " << time_I << " (" << time_I * 100 / time_total 169 | << "%)" << std::endl; 170 | std::cout << "time_J: " << time_J << " (" << time_J * 100 / time_total 171 | << "%)" << std::endl; 172 | std::cout << "time_K: " << time_K << " (" << time_K * 100 / time_total 173 | << "%)" << std::endl; 174 | std::cout << "time_L: " << time_L << " (" << time_L * 100 / time_total 175 | << "%)" << std::endl; 176 | std::cout << " ........ " << std::endl; 177 | #endif 178 | #endif 179 | 180 | for (Eigen::Index i = 0; i < res.rows(); i++) 181 | residual[i] = res(i); 182 | return true; 183 | } 184 | 185 | private: 186 | MatrixX3 moving_; 187 | Matrix G_; 188 | Sparse jacobianG_; 189 | Vector PX_vec_; 190 | Vector P1_; 191 | VectorInt line_sizes_; 192 | double lambda_; 193 | }; 194 | } // namespace lnrr -------------------------------------------------------------------------------- /include/lnrr/utils/operations.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace lnrr { 10 | 11 | inline Matrix computeG(const double& beta, const int& number_lines) { 12 | double k = -2.0 * beta * beta; 13 | Matrix G = Matrix(number_lines, number_lines); 14 | for (int i = 0; i < number_lines; i++) 15 | for (int j = 0; j < number_lines; j++) 16 | G(i, j) = std::abs(i - j); 17 | G = (G.array().pow(2) / k).exp(); 18 | return G; 19 | } 20 | 21 | template 22 | SparseT computeJacobianG(const MatrixT& G) { 23 | assert(G.rows() == G.cols()); 24 | int number_lines = G.rows(); 25 | SparseT JG(number_lines * 6, number_lines * 6); 26 | std::vector> tripletList; 27 | tripletList.reserve(6 * number_lines * number_lines); 28 | 29 | for (Eigen::Index block = 0; block < 6; block++) 30 | for (Eigen::Index i = 0; i < number_lines; i++) 31 | for (Eigen::Index j = 0; j < number_lines; j++) 32 | tripletList.push_back( 33 | Eigen::Triplet(block * number_lines + i, 34 | block * number_lines + j, G(i, j))); 35 | 36 | JG.setFromTriplets(tripletList.begin(), tripletList.end()); 37 | return JG; 38 | } 39 | 40 | template 41 | SparseT computeJacobianPointComposition( 42 | std::vector> T_lines, MatrixX3T& moving, 43 | const Eigen::Matrix& line_sizes) { 44 | int number_lines = T_lines.size(); 45 | int number_points_M = moving.rows(); 46 | assert(number_lines > 0); 47 | assert(number_points_M > 0); 48 | 49 | SparseT J(3 * number_points_M, 6 * number_lines); 50 | std::vector> tripletList; 51 | tripletList.reserve(3 * 6 * number_lines); 52 | 53 | int m = 0; 54 | for (int l = 0; l < number_lines; l++) { 55 | Matrix3T rotation_matrix_l = T_lines[l].rot_mat; 56 | MatrixT jacobian_block(3, 6); 57 | jacobian_block.block(0, 0, 3, 3) = rotation_matrix_l; 58 | for (size_t k = 0; k < line_sizes[l]; k++) { 59 | // Skew-symmetric matrix 60 | Matrix3T moving_m_x = Matrix3T::Zero(); 61 | moving_m_x(0, 1) = -moving(m, 2); 62 | moving_m_x(1, 0) = moving(m, 2); 63 | moving_m_x(0, 2) = moving(m, 1); 64 | moving_m_x(2, 0) = -moving(m, 1); 65 | moving_m_x(1, 2) = -moving(m, 0); 66 | moving_m_x(2, 1) = moving(m, 0); 67 | jacobian_block.block(0, 3, 3, 3) = -rotation_matrix_l * moving_m_x; 68 | 69 | for (size_t i = 0; i < jacobian_block.rows(); i++) 70 | for (size_t j = 0; j < jacobian_block.cols(); j++) 71 | tripletList.push_back(Eigen::Triplet( 72 | i * number_points_M + m, j * number_lines + l, 73 | jacobian_block(i, j))); 74 | m++; 75 | } 76 | } 77 | J.setFromTriplets(tripletList.begin(), tripletList.end()); 78 | return J; 79 | } 80 | 81 | template 82 | std::vector> computeTransformations(const MatrixT& G, 83 | const MatrixX6T& W) { 84 | #ifdef DEBUG 85 | auto tic = std::chrono::high_resolution_clock::now(); 86 | #endif 87 | 88 | assert(G.rows() == G.cols()); 89 | assert(G.rows() == W.rows()); 90 | assert(W.cols() == 6); 91 | 92 | int number_lines = G.rows(); 93 | 94 | std::vector> T_lines; 95 | T_lines.resize(number_lines); 96 | 97 | MatrixT GW = G * W; 98 | #ifdef DEBUG 99 | auto toc = std::chrono::high_resolution_clock::now(); 100 | double time_1 = 101 | std::chrono::duration_cast(toc - tic) 102 | .count(); 103 | 104 | tic = std::chrono::high_resolution_clock::now(); 105 | #endif 106 | 107 | // This for loop is the bottleneck 108 | #pragma omp parallel for 109 | for (size_t l = 0; l < number_lines; l++) { 110 | T_lines[l].xyz(0) = GW(l, 0); 111 | T_lines[l].xyz(1) = GW(l, 1); 112 | T_lines[l].xyz(2) = GW(l, 2); 113 | Vector3T rotation; 114 | rotation(0) = GW(l, 3); 115 | rotation(1) = GW(l, 4); 116 | rotation(2) = GW(l, 5); 117 | T angle = rotation.norm(); 118 | if (abs(angle) > T(0.0)) { 119 | Vector3T axis = rotation / angle; 120 | T_lines[l].rot_mat = 121 | Eigen::AngleAxis(angle, axis).toRotationMatrix(); 122 | } else { 123 | T_lines[l].rot_mat.setIdentity(); 124 | } 125 | } 126 | 127 | #ifdef DEBUG 128 | toc = std::chrono::high_resolution_clock::now(); 129 | double time_2 = 130 | std::chrono::duration_cast(toc - tic) 131 | .count(); 132 | 133 | double time_total = time_1 + time_2; 134 | #if false 135 | std::cout << "*******" << std::endl; 136 | std::cout << "time_1: " << time_1 << " (" << time_1 * 100 / time_total 137 | << "%)" << std::endl; 138 | std::cout << "time_2: " << time_2 << " (" << time_2 * 100 / time_total 139 | << "%)" << std::endl; 140 | std::cout << "*******" << std::endl; 141 | #endif 142 | #endif 143 | 144 | return T_lines; 145 | } 146 | 147 | template 148 | MatrixX3T computeTransformedMoving( 149 | const MatrixX3T& moving, const MatrixT& G, const MatrixX6T& W, 150 | const Eigen::Matrix& line_sizes) { 151 | 152 | assert(G.rows() == W.rows()); 153 | assert(G.rows() == line_sizes.size()); 154 | assert(moving.rows() == line_sizes.sum()); 155 | 156 | int number_lines = G.rows(); 157 | 158 | std::vector> T_lines = computeTransformations(G, W); 159 | MatrixX3T transformed_moving(moving.rows(), 3); 160 | int m = 0; 161 | // #pragma omp parallel private(m) 162 | for (size_t l = 0; l < number_lines; l++) { 163 | for (size_t j = 0; j < line_sizes[l]; j++) { 164 | Vector3T y_m; 165 | y_m(0) = moving(m, 0); 166 | y_m(1) = moving(m, 1); 167 | y_m(2) = moving(m, 2); 168 | Vector3T result = T_lines[l].rot_mat * y_m + T_lines[l].xyz; 169 | transformed_moving(m, 0) = result(0); 170 | transformed_moving(m, 1) = result(1); 171 | transformed_moving(m, 2) = result(2); 172 | m++; 173 | } 174 | } 175 | assert(m == moving.rows()); 176 | return transformed_moving; 177 | } 178 | 179 | template 180 | MatrixX3T computeTransformedMoving( 181 | const MatrixX3T& moving, std::vector> transforms, 182 | const Eigen::Matrix& line_sizes) { 183 | 184 | assert(transforms.size() == line_sizes.size()); 185 | assert(moving.rows() == line_sizes.sum()); 186 | 187 | int number_lines = transforms.size(); 188 | 189 | MatrixX3T transformed_moving(moving.rows(), 3); 190 | int m = 0; 191 | // #pragma omp parallel private(m) 192 | for (size_t l = 0; l < number_lines; l++) { 193 | for (size_t j = 0; j < line_sizes[l]; j++) { 194 | Vector3T y_m; 195 | y_m(0) = moving(m, 0); 196 | y_m(1) = moving(m, 1); 197 | y_m(2) = moving(m, 2); 198 | Vector3T result = 199 | transforms[l].rot_mat * y_m + transforms[l].xyz; 200 | transformed_moving(m, 0) = result(0); 201 | transformed_moving(m, 1) = result(1); 202 | transformed_moving(m, 2) = result(2); 203 | m++; 204 | } 205 | } 206 | assert(m == moving.rows()); 207 | return transformed_moving; 208 | } 209 | 210 | template 211 | void pclModelToEigen(const PointCloudPtr& cloud, MatrixX3T& eigen_cloud) { 212 | assert(!cloud->isOrganized()); // height == 1 213 | eigen_cloud.resize(cloud->size(), 3); 214 | for (size_t i = 0; i < cloud->size(); i++) { 215 | eigen_cloud(i, 0) = cloud->points[i].x; 216 | eigen_cloud(i, 1) = cloud->points[i].y; 217 | eigen_cloud(i, 2) = cloud->points[i].z; 218 | } 219 | } 220 | 221 | template 222 | void pclScanToEigen(const std::vector& cloud, 223 | MatrixX3T& eigen_cloud, VectorInt& line_sizes) { 224 | int number_lines = cloud.size(); 225 | int number_points = 0; 226 | for (size_t i = 0; i < number_lines; i++) 227 | number_points += cloud[i]->size(); 228 | eigen_cloud.resize(number_points, 3); 229 | line_sizes.resize(number_lines); 230 | int m = 0; 231 | for (size_t i = 0; i < number_lines; i++) { 232 | line_sizes[i] = cloud[i]->size(); 233 | for (size_t j = 0; j < cloud[i]->size(); j++) { 234 | eigen_cloud(m, 0) = cloud[i]->points[j].x; 235 | eigen_cloud(m, 1) = cloud[i]->points[j].y; 236 | eigen_cloud(m, 2) = cloud[i]->points[j].z; 237 | m++; 238 | } 239 | } 240 | assert(m == number_points); 241 | } 242 | 243 | } // namespace lnrr 244 | -------------------------------------------------------------------------------- /src/scan_to_model.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace lnrr { 4 | 5 | void ScanToModel::computeP() { 6 | double ksig = -2.0 * sigma2_; 7 | size_t cols = fixed_.cols(); 8 | double outlier_tmp = (outliers_ * moving_transformed_.rows() * 9 | std::pow(-ksig * M_PI, 0.5 * cols)) / 10 | ((1 - outliers_) * fixed_.rows()); 11 | Vector p = Vector::Zero(moving_transformed_.rows()); 12 | Vector p1 = Vector::Zero(moving_transformed_.rows()); 13 | Vector p1_max = Vector::Zero(moving_transformed_.rows()); 14 | Vector pt1 = Vector::Zero(fixed_.rows()); 15 | Matrix px = Matrix::Zero(moving_transformed_.rows(), cols); 16 | double l = 0.0; 17 | 18 | for (Matrix::Index i = 0; i < fixed_.rows(); ++i) { 19 | double sp = 0; 20 | for (Matrix::Index j = 0; j < moving_transformed_.rows(); ++j) { 21 | double razn = (fixed_.row(i) - moving_transformed_.row(j)) 22 | .array() 23 | .pow(2) 24 | .sum(); 25 | if (razn >= threshold_truncate_2_) { 26 | p(j) = 0.0; 27 | } else { 28 | p(j) = std::exp(razn / ksig); 29 | sp += p(j); 30 | } 31 | } 32 | sp += outlier_tmp; 33 | pt1(i) = 1 - outlier_tmp / sp; 34 | for (Matrix::Index j = 0; j < moving_transformed_.rows(); ++j) { 35 | p1(j) += p(j) / sp; 36 | px.row(j) += fixed_.row(i) * p(j) / sp; 37 | if (p(j) / sp > p1_max(j)) { 38 | p1_max(j) = p(j) / sp; 39 | } 40 | } 41 | l += -std::log(sp); 42 | } 43 | l += cols * fixed_.rows() * std::log(sigma2_) / 2; 44 | P_ = {p1, pt1, px, l}; 45 | } 46 | 47 | void ScanToModel::computeP_FGT() { 48 | double bandwidth = std::sqrt(2.0 * sigma2_); 49 | size_t cols = fixed_.cols(); 50 | std::unique_ptr transform; 51 | transform = std::unique_ptr( 52 | new fgt::DirectTree(moving_transformed_, bandwidth, fgt_epsilon_)); 53 | auto kt1 = transform->compute(fixed_, threshold_truncate_2_); 54 | double ndi = outliers_ / (1.0 - outliers_) * moving_transformed_.rows() / 55 | fixed_.rows() * std::pow(2.0 * M_PI * sigma2_, 0.5 * cols); 56 | Eigen::ArrayXd denom_p = kt1.array() + ndi; 57 | Vector pt1 = 1 - ndi / denom_p; 58 | 59 | transform = std::unique_ptr( 60 | new fgt::DirectTree(fixed_, bandwidth, fgt_epsilon_)); 61 | Vector p1 = transform->compute(moving_transformed_, 1 / denom_p, 62 | threshold_truncate_2_); 63 | Matrix px(moving_transformed_.rows(), cols); 64 | for (size_t i = 0; i < cols; ++i) { 65 | px.col(i) = transform->compute(moving_transformed_, 66 | fixed_.col(i).array() / denom_p, 67 | threshold_truncate_2_); 68 | } 69 | double l = 70 | -denom_p.log().sum() + cols * fixed_.rows() * std::log(sigma2_) / 2; 71 | P_ = {p1, pt1, px, l}; 72 | } 73 | 74 | /// TODO: Are we sure this sigma equals to equation in Fig. 2 of [Myronenko 75 | /// 2010] ?? 76 | double ScanToModel::defaultSigma2() { 77 | return ((moving_.rows() * (fixed_.transpose() * fixed_).trace()) + 78 | (fixed_.rows() * (moving_.transpose() * moving_).trace()) - 79 | 2 * fixed_.colwise().sum() * moving_.colwise().sum().transpose()) / 80 | (fixed_.rows() * moving_.rows() * fixed_.cols()) / 81 | 1000.; // 1000 here is a magic number 82 | } 83 | 84 | void ScanToModel::computeW() { 85 | ceres::Problem problem; 86 | ceres::LossFunction* loss = NULL; 87 | 88 | const Eigen::Map PX_vec(P_.px.data(), P_.px.size()); 89 | 90 | ceres::DynamicAutoDiffCostFunction* cost = 91 | new ceres::DynamicAutoDiffCostFunction( 92 | new CostFunctionW(moving_, G_, jacobianG_ / sigma2_, PX_vec, P_.p1, 93 | line_sizes_, lambda_)); 94 | std::vector parameter_blocks; 95 | parameter_blocks.push_back(W_.data()); 96 | cost->AddParameterBlock(W_.size()); 97 | cost->SetNumResiduals(W_.size()); 98 | problem.AddResidualBlock(cost, loss, parameter_blocks); 99 | ceres::Solver::Options options; 100 | options.minimizer_progress_to_stdout = true; 101 | options.check_gradients = false; 102 | 103 | /** 104 | * In general, we would like to find a *good enough* W, so that we can 105 | * recompute a better P matrix. 106 | * However, in the M_step_test we only do 1 iteration (because we assume P 107 | * is known). In that case, not increasing the max_num_iterations may lead 108 | * to a failing test. 109 | */ 110 | // options.max_num_iterations = 500; 111 | // options.parameter_tolerance = 1e-5; 112 | 113 | #ifdef DEBUG 114 | options.logging_type = ceres::PER_MINIMIZER_ITERATION; 115 | #else 116 | options.logging_type = ceres::SILENT; 117 | #endif 118 | 119 | ceres::Solver::Summary summary; 120 | Solve(options, &problem, &summary); 121 | 122 | #ifdef DEBUG 123 | std::cout << summary.FullReport() << std::endl; 124 | #endif 125 | 126 | #ifdef DEBUG 127 | if (summary.termination_type != ceres::CONVERGENCE) { 128 | throw(lnrr_error("Numerical solver has not converged. Final status: " + 129 | summary.termination_type)); 130 | } 131 | #endif 132 | return; 133 | } 134 | 135 | void ScanToModel::computeSigma2() { 136 | sigma2_ = 0.0; 137 | sigma2_ += (fixed_.transpose() * P_.pt1.asDiagonal() * fixed_).trace(); 138 | sigma2_ -= 2 * (P_.px.transpose() * moving_transformed_).trace(); 139 | sigma2_ += (moving_transformed_.transpose() * P_.p1.asDiagonal() * 140 | moving_transformed_) 141 | .trace(); 142 | sigma2_ /= (P_.p1.sum() * 3.0); 143 | } 144 | 145 | void ScanToModel::initialize() { 146 | assert(fixed_.rows() > 0); 147 | assert(moving_.rows() > 0); 148 | 149 | W_ = MatrixX6::Zero(number_lines_, 6); 150 | G_ = computeG(beta_, number_lines_); 151 | jacobianG_ = computeJacobianG(G_); 152 | 153 | if (sigma2_ == 0.0) 154 | sigma2_ = defaultSigma2(); 155 | moving_transformed_ = getTransformedMoving(); 156 | } 157 | 158 | void ScanToModel::computeOne() { 159 | assert(fixed_.rows() > 0); 160 | assert(moving_.rows() > 0); 161 | assert(number_lines_ > 0); 162 | assert(W_.rows() == number_lines_ && 163 | "Are you sure you called initialize()?"); 164 | assert(G_.rows() == number_lines_ && 165 | "Are you sure you called initialize()?"); 166 | #ifdef DEBUG 167 | auto tic = std::chrono::high_resolution_clock::now(); 168 | std::cout << "Computing P..." << std::endl; 169 | #endif 170 | computeP_FGT(); 171 | #ifdef DEBUG 172 | std::cout << "P computed." << std::endl; 173 | auto toc = std::chrono::high_resolution_clock::now(); 174 | double time_E = 175 | std::chrono::duration_cast(toc - tic) 176 | .count(); 177 | #endif 178 | 179 | #ifdef DEBUG 180 | tic = std::chrono::high_resolution_clock::now(); 181 | std::cout << "Computing M..." << std::endl; 182 | #endif 183 | try { 184 | computeW(); 185 | } catch (const lnrr_error& e) { 186 | std::cerr << e.what() << '\n'; 187 | } 188 | 189 | #ifdef DEBUG 190 | std::cout << "M computed." << std::endl; 191 | toc = std::chrono::high_resolution_clock::now(); 192 | double time_M = 193 | std::chrono::duration_cast(toc - tic) 194 | .count(); 195 | #endif 196 | 197 | #ifdef DEBUG 198 | tic = std::chrono::high_resolution_clock::now(); 199 | std::cout << "Computing other..." << std::endl; 200 | #endif 201 | moving_transformed_ = getTransformedMoving(); 202 | computeSigma2(); 203 | #ifdef DEBUG 204 | std::cout << "Other computed." << std::endl; 205 | toc = std::chrono::high_resolution_clock::now(); 206 | double time_other = 207 | std::chrono::duration_cast(toc - tic) 208 | .count(); 209 | 210 | double time_total = time_E + time_M + time_other; 211 | std::cout << "-----" << std::endl; 212 | std::cout << "time_E: " << time_E << " (" << time_E * 100 / time_total 213 | << "%)" << std::endl; 214 | std::cout << "time_M: " << time_M << " (" << time_M * 100 / time_total 215 | << "%)" << std::endl; 216 | std::cout << "time_other: " << time_other << " (" 217 | << time_other * 100 / time_total << "%)" << std::endl; 218 | std::cout << std::endl; 219 | #endif 220 | } 221 | 222 | Result ScanToModel::run() { 223 | auto tic = std::chrono::high_resolution_clock::now(); 224 | 225 | #ifdef DEBUG 226 | std::cout << "Initializing non-rigid registration in DEBUG mode" 227 | << std::endl; 228 | #endif 229 | initialize(); 230 | #ifdef DEBUG 231 | std::cout << "Initialization complete" << std::endl; 232 | #endif 233 | 234 | size_t iter = 0; 235 | 236 | double l = 0.0; 237 | double ntol = tolerance_ + 10.0; 238 | 239 | while (iter < max_iterations_ && ntol > tolerance_ && 240 | sigma2_ > 10 * std::numeric_limits::epsilon()) { 241 | ScanToModel::computeOne(); 242 | ntol = std::abs((P_.l - l) / P_.l); 243 | l = P_.l; 244 | ++iter; 245 | #ifdef DEBUG 246 | std::cout << "Iteration " << iter << " complete. " << std::endl; 247 | std::cout << " --> Tolerance = " << ntol << ". "; 248 | std::cout << "Convergence criteria: " << tolerance_ << std::endl; 249 | std::cout << " --> sigma2 = " << sigma2_ << ". "; 250 | std::cout << "Convergence criteria: " 251 | << 10 * std::numeric_limits::epsilon() << std::endl; 252 | #endif 253 | } 254 | 255 | auto toc = std::chrono::high_resolution_clock::now(); 256 | Result result; 257 | result.runtime = 258 | std::chrono::duration_cast(toc - tic); 259 | result.points = moving_transformed_; 260 | result.sigma2 = sigma2_; 261 | result.iterations = iter; 262 | result.line_transforms = computeTransformations(G_, W_); 263 | 264 | #ifdef DEBUG 265 | std::cout << "Non-rigid registration complete: "; 266 | std::cout << result.iterations << " iterations, "; 267 | std::cout << result.runtime.count() / 1e6 << " seconds. "; 268 | std::cout << "Final sigma2 = " << result.sigma2 << std::endl; 269 | #endif 270 | 271 | return result; 272 | } 273 | 274 | } // namespace lnrr -------------------------------------------------------------------------------- /examples/data/output.pcd: -------------------------------------------------------------------------------- 1 | # .PCD v0.7 - Point Cloud Data file format 2 | VERSION 0.7 3 | FIELDS x y z 4 | SIZE 4 4 4 5 | TYPE F F F 6 | COUNT 1 1 1 7 | WIDTH 300 8 | HEIGHT 1 9 | VIEWPOINT 0 0 0 1 0 0 0 10 | POINTS 300 11 | DATA ascii 12 | -0.088146769 0.12440126 -0.0011540971 13 | -0.088185318 0.13418579 -0.00062325521 14 | -0.088120133 0.12634745 0.0044991905 15 | -0.088158287 0.13320638 0.0042518047 16 | -0.08782015 0.12648016 0.019061845 17 | -0.078185514 0.14543466 -0.015463651 18 | -0.078761511 0.14993282 -0.010497948 19 | -0.07797765 0.072908595 -0.0011068716 20 | -0.078295015 0.073889643 0.00033078925 21 | -0.078722581 0.15312681 -0.0025427958 22 | -0.078881532 0.15343037 0.00073869328 23 | -0.078316212 0.075868249 0.0071712211 24 | -0.078716695 0.10273775 0.012950136 25 | -0.078753784 0.10574653 0.012168825 26 | -0.078838035 0.15208343 0.01049052 27 | -0.07847701 0.15325056 0.0055886586 28 | -0.078237191 0.080358729 0.014113467 29 | -0.078496829 0.088497952 0.015511218 30 | -0.078399599 0.097821735 0.014999284 31 | -0.078730375 0.10819545 0.017055687 32 | -0.078956231 0.14768983 0.019818263 33 | -0.0789015 0.10982801 0.028043902 34 | -0.078820094 0.11641382 0.03263608 35 | -0.079084992 0.1372911 0.030654807 36 | -0.078724682 0.14373849 0.025127342 37 | -0.078735255 0.12075128 0.034326293 38 | -0.078957506 0.12756237 0.035533164 39 | -0.06912899 0.17376728 -0.065072469 40 | -0.06853652 0.17047186 -0.065503657 41 | -0.069094859 0.17717902 -0.067328945 42 | -0.068576276 0.17691545 -0.065902539 43 | -0.069181427 0.17109525 -0.05543552 44 | -0.068716735 0.17068222 -0.055227298 45 | -0.068819121 0.167062 -0.046351191 46 | -0.069285929 0.16408715 -0.037227932 47 | -0.068874687 0.16332214 -0.035834495 48 | -0.069187008 0.16549537 -0.040796936 49 | -0.068677239 0.16527663 -0.040686157 50 | -0.069337681 0.15871878 -0.026373778 51 | -0.068733908 0.15849705 -0.026136393 52 | -0.069400057 0.1512254 -0.012848233 53 | -0.068841137 0.15311933 -0.01683805 54 | -0.069024399 0.15491459 -0.019772828 55 | -0.069334798 0.14987832 -0.0076633818 56 | -0.068996921 0.14974214 -0.0068981028 57 | -0.068814024 0.056221485 0.0037749175 58 | -0.068497293 0.055766568 0.0010739533 59 | -0.069488041 0.14999101 0.0026403598 60 | -0.069004714 0.14997406 0.0036156212 61 | -0.068917282 0.058917392 0.0098032681 62 | -0.06852363 0.059106678 0.010899487 63 | -0.069602907 0.14826052 0.01748566 64 | -0.069329977 0.14927351 0.013418943 65 | -0.069023371 0.061700642 0.015952265 66 | -0.068358056 0.062113993 0.017344298 67 | -0.069197677 0.066500932 0.02223647 68 | -0.068524569 0.067417324 0.023038005 69 | -0.069409713 0.094678126 0.026477629 70 | -0.068890333 0.099798366 0.024877835 71 | -0.069373228 0.10289703 0.026234569 72 | -0.068883725 0.10302355 0.026204407 73 | -0.06906639 0.14257255 0.027627466 74 | -0.069684453 0.14564708 0.022752926 75 | -0.069128796 0.14511995 0.024133692 76 | -0.069278829 0.078507558 0.028220162 77 | -0.068671629 0.07755664 0.028156953 78 | -0.069317214 0.092436746 0.027446058 79 | -0.06882406 0.086876161 0.028603619 80 | -0.06943211 0.093429551 0.027014745 81 | -0.06943509 0.10458647 0.033117529 82 | -0.069000833 0.10467093 0.032141373 83 | -0.069631942 0.12832247 0.036130093 84 | -0.069045641 0.12666374 0.036135685 85 | -0.069587074 0.13771901 0.031741973 86 | -0.06923636 0.1373482 0.0316181 87 | -0.069551714 0.10673384 0.037518207 88 | -0.068985254 0.10832777 0.038447622 89 | -0.069475695 0.11773796 0.04017007 90 | -0.069020122 0.11648864 0.03995727 91 | -0.058593355 0.13574974 -0.011100187 92 | -0.058657102 0.14505659 -0.0086269854 93 | -0.058765944 0.14875832 -0.0037899825 94 | -0.058586452 0.1507604 -0.003871952 95 | -0.057920206 0.025443688 0.0041845101 96 | -0.058095597 0.033222441 0.0074264617 97 | -0.058128711 0.050170783 0.0076509868 98 | -0.058855984 0.14741263 0.0063894824 99 | -0.058128625 0.025577975 0.014761035 100 | -0.057997782 0.035940345 0.013590417 101 | -0.058209624 0.052563246 0.014561221 102 | -0.05866085 0.14573503 0.01783099 103 | -0.058155783 0.025574014 0.023828123 104 | -0.058128443 0.038818859 0.024329277 105 | -0.058518417 0.05807225 0.024773546 106 | -0.058294773 0.061171886 0.027910603 107 | -0.058607984 0.098299332 0.030148534 108 | -0.058639023 0.1033082 0.027560383 109 | -0.058441173 0.11535884 0.030490223 110 | -0.058699451 0.12417764 0.027606573 111 | -0.058758307 0.1344813 0.02406715 112 | -0.058751825 0.14190817 0.023924574 113 | -0.058206208 0.026215531 0.031600997 114 | -0.058352619 0.032250419 0.033989821 115 | -0.058154613 0.066842869 0.032049164 116 | -0.058371998 0.073552214 0.033821497 117 | -0.058404412 0.084731072 0.034248862 118 | -0.05848508 0.094040431 0.032292791 119 | -0.058611907 0.11511405 0.031389765 120 | -0.047258399 0.16216902 -0.0061412635 121 | -0.047321655 0.14954565 0.0026018177 122 | -0.047368787 0.15850103 -0.0011093728 123 | -0.047167014 0.12393978 0.012664518 124 | -0.046127342 0.026183952 0.020003514 125 | -0.047149606 0.11145653 0.022659278 126 | -0.047118083 0.11801084 0.018282002 127 | -0.047149215 0.12282857 0.0151914 128 | -0.046541914 0.026426001 0.026097883 129 | -0.0467733 0.039717335 0.031136446 130 | -0.046597049 0.04661392 0.029961869 131 | -0.046787329 0.055145487 0.030844916 132 | -0.046842217 0.10068415 0.032303296 133 | -0.046995714 0.10653903 0.02734087 134 | -0.046558786 0.028512349 0.034426358 135 | -0.046513088 0.034361128 0.034684703 136 | -0.046507239 0.060505491 0.032834519 137 | -0.046842534 0.066198021 0.034348417 138 | -0.046935365 0.076586463 0.036566716 139 | -0.046992347 0.086211801 0.036080237 140 | -0.047056817 0.095056012 0.034685858 141 | -0.034786724 0.17182615 -0.015203439 142 | -0.034783714 0.173039 -0.01326953 143 | -0.034609172 0.14809784 -0.0073422729 144 | -0.034670897 0.16042842 -0.0069327359 145 | -0.034578186 0.16760108 -0.0055015869 146 | -0.034662891 0.17317437 -0.010643784 147 | -0.034447204 0.15051712 -0.0023216435 148 | -0.034375958 0.15500417 -0.0020217311 149 | -0.033696063 0.028791426 0.0094344914 150 | -0.034494497 0.1206851 0.014612586 151 | -0.033767298 0.029035892 0.018473729 152 | -0.034254156 0.11735424 0.020440508 153 | -0.033911653 0.029461866 0.026345704 154 | -0.03383958 0.048480444 0.031670779 155 | -0.033852056 0.054990739 0.032757584 156 | -0.033896971 0.10036268 0.034369394 157 | -0.034017395 0.10709978 0.030148059 158 | -0.033954177 0.11230292 0.02650978 159 | -0.033629771 0.029887049 0.040019844 160 | -0.033777211 0.036721732 0.040433202 161 | -0.033787947 0.042472322 0.03467419 162 | -0.034129556 0.060254745 0.034252949 163 | -0.033937618 0.065312751 0.035127949 164 | -0.034213122 0.077138126 0.036509782 165 | -0.033911534 0.086019494 0.038713649 166 | -0.034033619 0.095081635 0.03705164 167 | -0.033447374 0.032582309 0.042995542 168 | -0.021030091 0.18011177 -0.024323149 169 | -0.020828435 0.18209112 -0.023658866 170 | -0.021119568 0.15908027 -0.016309856 171 | -0.021017453 0.16750465 -0.016799184 172 | -0.021095058 0.17776464 -0.016920503 173 | -0.021146968 0.18366024 -0.018628001 174 | -0.020970222 0.15779899 -0.0084348759 175 | -0.021198645 0.16278635 -0.011795565 176 | -0.019767646 0.036345407 0.0090064304 177 | -0.020362169 0.12453004 0.013760448 178 | -0.019777026 0.036263224 0.01805073 179 | -0.020320935 0.12001156 0.024211124 180 | -0.020532729 0.12196872 0.01968715 181 | -0.019839428 0.036563147 0.027278576 182 | -0.02047151 0.10908489 0.034485918 183 | -0.020397531 0.11572447 0.029270273 184 | -0.01986799 0.032258864 0.040231958 185 | -0.019839728 0.047767162 0.039921939 186 | -0.019972106 0.055119123 0.039038718 187 | -0.020291613 0.063521206 0.041479237 188 | -0.020241817 0.094962582 0.040831041 189 | -0.020239616 0.10449638 0.036830191 190 | -0.019727943 0.037146367 0.047035765 191 | -0.019800315 0.043014791 0.045699697 192 | -0.020066468 0.067659482 0.044639327 193 | -0.020015134 0.076149441 0.049153674 194 | -0.020138463 0.086048745 0.048695553 195 | -0.020424789 0.090817362 0.045254409 196 | -0.0076011131 0.16941607 -0.028242426 197 | -0.0070907855 0.1697876 -0.027841415 198 | -0.0075928131 0.17742905 -0.029466949 199 | -0.0071413503 0.17530671 -0.028623311 200 | -0.0075324355 0.16907617 -0.024397412 201 | -0.0072408346 0.16897334 -0.024340596 202 | -0.006071581 0.039491512 0.0082602398 203 | -0.0068190885 0.13099663 0.011892919 204 | -0.0065067154 0.1312138 0.010543937 205 | -0.0066730585 0.13126206 0.0097982483 206 | -0.0063442043 0.039504483 0.018854538 207 | -0.0059512588 0.039460447 0.013861431 208 | -0.0068923789 0.13047181 0.014922128 209 | -0.0062584844 0.12887657 0.019294176 210 | -0.00626816 0.039758589 0.024566073 211 | -0.0058204294 0.039855801 0.025868986 212 | -0.006675439 0.11708882 0.032792754 213 | -0.0060808752 0.1186408 0.031955071 214 | -0.0066678864 0.12121277 0.030084657 215 | -0.0062469449 0.12353583 0.027806345 216 | -0.0060941717 0.035312124 0.037626013 217 | -0.0055504711 0.036795001 0.039783426 218 | -0.0061387708 0.045842491 0.04101428 219 | -0.0056635248 0.045055941 0.040865954 220 | -0.0065290965 0.099806868 0.041976135 221 | -0.0060486156 0.10001503 0.041797489 222 | -0.0064872685 0.10290778 0.037279125 223 | -0.0060421871 0.10515392 0.03714801 224 | -0.0065467153 0.11187155 0.035577945 225 | -0.005986094 0.11324196 0.03496087 226 | -0.0056900913 0.049848802 0.042132489 227 | -0.0061560664 0.056951802 0.045683939 228 | -0.0056277486 0.054366767 0.044462584 229 | -0.0056692101 0.065981448 0.047956057 230 | -0.0061972355 0.072059058 0.049101278 231 | -0.0057488675 0.07447122 0.049643021 232 | -0.0063148499 0.085403875 0.050266083 233 | -0.0057669259 0.085481569 0.050317053 234 | -0.0063230051 0.096385106 0.046467341 235 | -0.005835834 0.095987566 0.046686523 236 | 0.0073555238 0.037372712 0.005121897 237 | 0.006598698 0.13418466 0.0084943408 238 | 0.0075308308 0.040384222 0.013371291 239 | 0.0068979287 0.13222922 0.015730223 240 | 0.0075735501 0.040540136 0.02319929 241 | 0.0071643316 0.12226225 0.027846245 242 | 0.0070950091 0.12779199 0.022810096 243 | 0.0077533247 0.039674494 0.034417719 244 | 0.0076010833 0.04772095 0.036672127 245 | 0.0073308367 0.10229625 0.03764113 246 | 0.007289737 0.10918054 0.033066355 247 | 0.0072262678 0.11695454 0.031213179 248 | 0.0077330424 0.05138959 0.040500615 249 | 0.00785123 0.058776345 0.043608941 250 | 0.0078101358 0.069037497 0.045880567 251 | 0.0076374998 0.079170875 0.046452515 252 | 0.0076693147 0.089901462 0.046399824 253 | 0.0075408099 0.09718626 0.043760501 254 | 0.018912742 0.13242401 0.0037443491 255 | 0.019636625 0.037576251 0.011018875 256 | 0.019042939 0.13051131 0.011366173 257 | 0.019792799 0.037533719 0.019733761 258 | 0.019410249 0.11824661 0.023430388 259 | 0.019111056 0.12550393 0.018816663 260 | 0.019767568 0.039862555 0.028487515 261 | 0.019785976 0.048519589 0.030162677 262 | 0.019918466 0.056624379 0.03414214 263 | 0.019616639 0.10016844 0.034311391 264 | 0.019387385 0.1074739 0.030282581 265 | 0.019734535 0.060439598 0.035667572 266 | 0.019846054 0.067412317 0.037383396 267 | 0.019890122 0.077356808 0.039335243 268 | 0.019689811 0.087446816 0.038592964 269 | 0.019570857 0.094596133 0.03741397 270 | 0.029721495 0.036314916 -0.0029635285 271 | 0.029092418 0.12438319 -0.00061240594 272 | 0.029764652 0.038444992 0.0060559851 273 | 0.029218977 0.12234786 0.0068463786 274 | 0.02973008 0.043485351 0.015881753 275 | 0.029966772 0.050947327 0.019849421 276 | 0.02963233 0.105409 0.020927353 277 | 0.02948403 0.11129089 0.018091902 278 | 0.029439194 0.11797776 0.013107824 279 | 0.030143037 0.054975793 0.021822577 280 | 0.030020321 0.060229272 0.024549389 281 | 0.029909004 0.071477219 0.025671806 282 | 0.029805163 0.080944799 0.028261119 283 | 0.029877651 0.091268562 0.027476884 284 | 0.029687691 0.10092634 0.023812044 285 | 0.037988782 0.037167173 -0.0064711808 286 | 0.03747486 0.10803867 -0.0049521066 287 | 0.038187589 0.040446654 0.003211909 288 | 0.037761554 0.10207909 0.005383756 289 | 0.037532784 0.10651995 0.00035130882 290 | 0.038080022 0.043915525 0.007695436 291 | 0.03807861 0.050334305 0.0089234691 292 | 0.038062643 0.060312513 0.010630564 293 | 0.037983634 0.07031054 0.012465926 294 | 0.037883315 0.079527512 0.01303704 295 | 0.037840243 0.090343319 0.012031196 296 | 0.037613988 0.09692093 0.0091504734 297 | 0.044398978 0.040241946 -0.010366268 298 | 0.044840891 0.040592298 -0.010139377 299 | 0.044125799 0.069177344 -0.0095777074 300 | 0.044619754 0.069361672 -0.0089755999 301 | 0.044471119 0.04292291 -0.0022079726 302 | 0.04495002 0.043428648 -0.0019468646 303 | 0.044525076 0.048615564 0.0031864836 304 | 0.045093484 0.048550073 0.0027125354 305 | 0.044409081 0.061483108 0.0023728614 306 | 0.044921279 0.062787771 0.0014126635 307 | 0.044310153 0.066322066 -0.0017227033 308 | 0.044689588 0.067520581 -0.0036778396 309 | 0.044491455 0.055268552 0.0048014051 310 | 0.045092426 0.053182505 0.0043660803 311 | 0.045072805 0.056982193 0.004439468 312 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | --------------------------------------------------------------------------------