[MAYBE MERGE] Add research code directly

libpointmatcher/CMakeLists.txt

@@ -29,6 +29,9 @@ set(POINTMATCHER_SRC
 	pointmatcher/TransformationsImpl.cpp
 	pointmatcher/TransformationCheckersImpl.cpp
 	pointmatcher/InspectorsImpl.cpp
+	pointmatcher/point_cloud_registration.cpp
+	pointmatcher/utils.cpp
+	pointmatcher/l_curve_optimization.cpp
 #ErrorMinimizers
 	pointmatcher/ErrorMinimizers/PointToPlane.cpp
 	pointmatcher/ErrorMinimizers/PointToPlaneWithCov.cpp

libpointmatcher/CatkinBuild.cmake

@@ -11,6 +11,7 @@ endif(NOT CMAKE_BUILD_TYPE STREQUAL "Debug")
 set(CATKIN_PACKAGE_DEPENDENCIES
   libnabo
   message_logger
+  qpmad
 )
 
 list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR})
@@ -20,6 +21,11 @@ find_package(catkin REQUIRED
     ${CATKIN_PACKAGE_DEPENDENCIES}
 )
 find_package(Eigen3 REQUIRED)
+find_package(Ceres REQUIRED)
+find_package(Sophus REQUIRED)
+find_package(NLOPT REQUIRED)
+find_package(BSplineInterpolation REQUIRED)
+find_package(Splinter REQUIRED)
 
 find_package(Boost REQUIRED COMPONENTS chrono date_time filesystem program_options system thread timer)
 
@@ -28,6 +34,10 @@ catkin_package(
   INCLUDE_DIRS
     ${CMAKE_SOURCE_DIR}
     ${EIGEN3_INCLUDE_DIR}
+    ${CERES_INCLUDE_DIRS}
+    ${SOPHUS_INCLUDE_DIR}
+    ${NLOPT_INCLUDE_DIRS}
+    ${Splinter_INCLUDE_DIR}
   LIBRARIES
     yaml_cpp_pm
     pointmatcher
@@ -75,9 +85,19 @@ target_include_directories(pointmatcher SYSTEM PRIVATE
   ${Boost_INCLUDE_DIRS}
   ${catkin_INCLUDE_DIRS}
   ${yaml_cpp_pm_INCLUDE_DIRS}
+  ${CERES_INCLUDE_DIRS}
+  ${SOPHUS_INCLUDE_DIR}
+  ${NLOPT_INCLUDE_DIRS}
+  ${BSplineInterpolation_INCLUDE_DIRS}
+  ${Splinter_INCLUDE_DIR}
 )
 target_link_libraries(pointmatcher
   ${catkin_LIBRARIES}
+  ${CERES_LIBRARIES}
+  ${SOPHUS_LIBRARIES}
+  ${NLOPT_LIBRARIES}
+  ${BSplineInterpolation_LIBRARIES}
+  ${Splinter_LIBRARIES}
   Boost::chrono
   Boost::date_time
   Boost::filesystem

libpointmatcher/FindNLOPT.cmake

@@ -0,0 +1,48 @@
+# Copyright (c) 2011-2019, The DART development contributors
+# All rights reserved.
+#
+# The list of contributors can be found at:
+#   https://github.com/dartsim/dart/blob/master/LICENSE
+#
+# This file is provided under the "BSD-style" License
+
+# Find NLOPT
+#
+# This sets the following variables:
+#   NLOPT_FOUND
+#   NLOPT_INCLUDE_DIRS
+#   NLOPT_LIBRARIES
+#   NLOPT_DEFINITIONS
+#   NLOPT_VERSION
+#
+# and the following targets:
+#   NLOPT::nlopt
+
+find_package(PkgConfig QUIET)
+
+# Check to see if pkgconfig is installed.
+pkg_check_modules(PC_NLOPT nlopt QUIET)
+
+# Definitions
+set(NLOPT_DEFINITIONS ${PC_NLOPT_CFLAGS_OTHER})
+
+# Include directories
+find_path(NLOPT_INCLUDE_DIRS
+    NAMES nlopt.h
+    HINTS ${PC_NLOPT_INCLUDEDIR}
+    PATHS "${CMAKE_INSTALL_PREFIX}/include")
+
+# Libraries
+find_library(NLOPT_LIBRARIES
+    NAMES nlopt nlopt_cxx
+    HINTS ${PC_NLOPT_LIBDIR})
+
+# Version
+set(NLOPT_VERSION ${PC_NLOPT_VERSION})
+
+# Set (NAME)_FOUND if all the variables and the version are satisfied.
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(NLOPT
+    FAIL_MESSAGE  DEFAULT_MSG
+    REQUIRED_VARS NLOPT_INCLUDE_DIRS NLOPT_LIBRARIES
+    VERSION_VAR   NLOPT_VERSION)

libpointmatcher/FindSplinter.cmake

@@ -0,0 +1,16 @@
+# https://github.com/bgrimstad/splinter
+#
+# Splinter_ROOT root search path
+# Splinter_INCLUDE_DIR inlcude directories
+# Splinter_LIBRARIES libraries
+#
+
+
+find_path(Splinter_INCLUDE_DIR NAMES data_table.h PATHS /usr/local/include/SPLINTER)
+
+find_library(Splinter_LIBRARIES NAMES splinter-4-0 PATHS /usr/local/lib /usr/local/lib64)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Splinter DEFAULT_MSG
+                                  Splinter_INCLUDE_DIR Splinter_LIBRARIES)
+mark_as_advanced(Splinter_INCLUDE_DIR Splinter_LIBRARIES)

libpointmatcher/package.xml

@@ -16,11 +16,13 @@
   <build_depend>boost</build_depend>
   <build_depend>libnabo</build_depend>
   <build_depend>eigen</build_depend>
+  <build_depend>qpmad</build_depend>
   <build_depend>message_logger</build_depend>
 
   <run_depend>boost</run_depend>
   <run_depend>libnabo</run_depend>
   <run_depend>eigen</run_depend>
+  <run_depend>qpmad</run_depend>
   <run_depend>message_logger</run_depend>
 
   <test_depend>cmake_code_coverage</test_depend>