ceres, scanICP and cpptest

.gitignore

@@ -0,0 +1,52 @@
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+.vscode/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE

LOAM_NOTED

@@ -0,0 +1 @@
+Subproject commit e7fbd408c81ac3f84229355a48e1d9c2116cfa9c

ceres_test/CMakeLists.txt

@@ -0,0 +1,205 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ceres_test)
+
+set(OpenCV_DIR "/opt/ros/kinetic/share/OpenCV-3.3.1-dev/")
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  OpenCV REQUIRED
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+find_package(Ceres REQUIRED)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES ceres_test
+#  CATKIN_DEPENDS roscpp rospy
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+  ${CERES_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/ceres_test.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME}_node src/ceres_test.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_node
+  ${catkin_LIBRARIES}
+  ${CERES_LIBRARIES}
+  ${OpenCV_LIBS}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_ceres_test.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

ceres_test/package.xml

@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ceres_test</name>
+  <version>0.0.0</version>
+  <description>The ceres_test package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="youchen@todo.todo">youchen</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/ceres_test</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

ceres_test/src/ceres_test.cpp

@@ -0,0 +1,65 @@
+#include<iostream>
+#include<opencv2/core/core.hpp>
+#include<ceres/ceres.h>
+
+using namespace std;
+using namespace cv;
+
+//构建代价函数结构体，abc为待优化参数，residual为残差。
+struct CURVE_FITTING_COST
+{
+  CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}
+  template <typename T>
+  bool operator()(const T* const abc,T* residual)const
+  {
+    residual[0]=_y-ceres::exp(abc[0]*_x*_x+abc[1]*_x+abc[2]);
+    return true;
+  }
+  const double _x,_y;
+};
+
+//主函数
+int main()
+{
+  //参数初始化设置，abc初始化为0，白噪声方差为1（使用OpenCV的随机数产生器）。
+  double a=3,b=2,c=1;
+  double w=1;
+  RNG rng;
+  double abc[3]={0,0,0};
+
+//生成待拟合曲线的数据散点，储存在Vector里，x_data，y_data。
+  vector<double> x_data,y_data;
+  for(int i=0;i<1000;i++)
+  {
+    double x=i/1000.0;
+    x_data.push_back(x);
+    y_data.push_back(exp(a*x*x+b*x+c)+rng.gaussian(w));
+  }
+
+//反复使用AddResidualBlock方法（逐个散点，反复1000次）
+//将每个点的残差累计求和构建最小二乘优化式
+//不使用核函数，待优化参数是abc
+  ceres::Problem problem;
+  for(int i=0;i<1000;i++)
+  {
+    problem.AddResidualBlock(
+      new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,3>(
+        new CURVE_FITTING_COST(x_data[i],y_data[i])
+      ),
+      nullptr,
+      abc
+    );
+  }
+
+//配置求解器并求解，输出结果
+  ceres::Solver::Options options;
+  options.linear_solver_type=ceres::DENSE_QR;
+  options.minimizer_progress_to_stdout=true;
+  ceres::Solver::Summary summary;
+  ceres::Solve(options,&problem,&summary);
+  cout<<"a= "<<abc[0]<<endl;
+  cout<<"b= "<<abc[1]<<endl;
+  cout<<"c= "<<abc[2]<<endl;
+return 0;
+}
+

cpp_pkgs/CMakeLists.txt

@@ -0,0 +1,196 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(cpp_pkgs)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES cpp_pkgs
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/cpp_pkgs.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/cpp_pkgs_node.cpp)
+add_executable(basicTest src/basicTest.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_cpp_pkgs.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

cpp_pkgs/package.xml

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>cpp_pkgs</name>
+  <version>0.0.0</version>
+  <description>The cpp_pkgs package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="youchen@todo.todo">youchen</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/cpp_pkgs</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

cpp_pkgs/src/basicTest.cpp

@@ -0,0 +1,82 @@
+//预处理指令
+#ifndef __BASICTEST_CPP__
+#define __BASICTEST_CPP__
+#include <iostream>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#define trace(x, format) \
+printf(#x " = %" #format "\n", x)
+#define trace2(i) trace(x ## i, d)
+#define assert(cond) \
+if(!cond) _assert(#cond,__FILE__,__LINE__)
+//预定义宏 __FILE__
+//预处理运算符 #字符串化符 ##加标记 
+void _assert(char * cond, char *fname, long lineno){
+    fprintf(stderr, "Assertion failed: %s, file %s, line %ld\n",cond, fname, lineno);
+    abort();
+}
+
+int main(){
+    int mmm = 10;
+    assert((mmm<11));
+    int a[] = {0,1,2,3,4};
+    // traverse
+    size_t n = sizeof a / sizeof a[0];
+    for (size_t i = 0; i < n; i++)
+    {
+        std::cout<<a[i]<<' ';
+    }
+    std::cout<<std::endl;
+
+    //pointer
+    int *ppp = a+n-1;
+    // inverse traverse
+    while(ppp>=a){
+        std::cout << *ppp--<<' ';
+    }
+    std::cout<<std::endl;
+    ppp = a+n-1;
+    for (size_t i = 0; i < n; i++)
+    {
+        std::cout << ppp[-i]<<' ';
+    }
+    std::cout<<std::endl;
+
+    //test 2.2
+    char s[] = "desolate", *p = s;
+    std::cout<<*p++<<std::endl;//d
+    std::cout<<*p<<std::endl;//e
+    std::cout<<*(p++)<<std::endl;//e
+    std::cout<<*p<<std::endl;//s
+    std::cout<<(*p)++<<std::endl;//s
+    std::cout<<*p<<std::endl;//t
+    std::cout<<*++p<<std::endl;//o
+    std::cout<<*p<<std::endl;//o
+    std::cout<<*(++p)<<std::endl;//l
+    std::cout<<*p<<std::endl;//l
+    std::cout<<++*p<<std::endl;//m
+    std::cout<<++(*p)<<std::endl;//n
+    std::cout<<strcmp(s, "detonate")<<std::endl;//0
+
+    int arr[][2][3] = {{{0,1},{2,3}},{{4,5},{6,7}},{{8,9},{10,11}}};
+    int (*par)[2][3] = arr;
+    void *vp;
+    std::cout<<sizeof par<<' '<<sizeof vp<<std::endl;//8
+    vp = (char *)arr[0][0];
+    std::cout<<sizeof (*par) <<std::endl;//24
+    std::cout<<sizeof vp<<std::endl;//8
+
+
+    std::cout<<"__DATE__: "<<__DATE__<<std::endl;
+    std::cout<<"__FILE__: "<<__FILE__<<std::endl;
+    std::cout<<"__LINE__: "<<__LINE__<<std::endl;
+    std::cout<<"__TIME__: "<<__TIME__<<std::endl;
+
+    int x = 66, x1=12;
+    trace(x, c);
+    trace2(1);
+    
+}
+
+#endif

scan_matcher_ICP/CMakeLists.txt

@@ -0,0 +1,239 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(scan_matcher_ICP)
+add_compile_options(-std=c++11)
+
+# -mavx causes a lot of errors!!
+add_definitions(-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
+set(CMAKE_CXX_FLAGS "-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
+
+# pcl 1.7 causes a segfault when it is built with debug mode
+set(CMAKE_BUILD_TYPE "RELEASE")
+find_package(OpenMP)
+if (OPENMP_FOUND)
+    set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+    set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+endif()
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  sensor_msgs
+  tf
+  geometry_msgs
+  nav_msgs
+  pcl_ros
+  pcl_conversions
+  PCL REQUIRED
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+# Find csm project
+find_package(PkgConfig)
+pkg_check_modules(csm REQUIRED csm)
+
+include_directories(
+  include 
+  ${catkin_INCLUDE_DIRS} 
+  ${csm_INCLUDE_DIRS} 
+  ${PCL_INCLUDE_DIRS}
+  ./src/
+  )
+
+link_directories(${csm_LIBRARY_DIRS})
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  INCLUDE_DIRS include
+#  LIBRARIES scan_matcher_ICP
+  CATKIN_DEPENDS roscpp sensor_msgs tf geometry_msgs nav_msgs pcl_ros pcl_conversions
+  DEPENDS PCL 
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/scan_matcher_ICP.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/scan_matcher_ICP_node.cpp)
+
+# add_executable(laser_scan_matcher src/laser_scan_matcher.cpp)
+# target_link_libraries(laser_scan_matcher ${catkin_LIBRARIES} ${csm_LIBRARIES} )
+add_library(ndt_omp
+  src/pclomp/voxel_grid_covariance_omp.cpp
+  src/pclomp/ndt_omp.cpp
+  src/pclomp/gicp_omp.cpp
+)
+#Create node
+add_executable(scan_matcher_ICP_node src/laser_scan_matcher_node.cpp src/laser_scan_matcher.cpp)
+add_dependencies(scan_matcher_ICP_node
+  ndt_omp
+)
+target_link_libraries(scan_matcher_ICP_node ${catkin_LIBRARIES} ${csm_LIBRARIES} ndt_omp)
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_scan_matcher_ICP.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

scan_matcher_ICP/include/pclomp/gicp_omp.h

@@ -0,0 +1,373 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_GICP_OMP_H_
+#define PCL_GICP_OMP_H_
+
+#include <pcl/registration/icp.h>
+#include <pcl/registration/bfgs.h>
+
+namespace pclomp
+{
+  /** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the 
+    * generalized iterative closest point algorithm as described by Alex Segal et al. in 
+    * http://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf
+    * The approach is based on using anistropic cost functions to optimize the alignment 
+    * after closest point assignments have been made.
+    * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and 
+    * FLANN.
+    * \author Nizar Sallem
+    * \ingroup registration
+    */
+  template <typename PointSource, typename PointTarget>
+  class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint<PointSource, PointTarget>
+  {
+    public:
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::reg_name_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::getClassName;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::indices_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::target_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::input_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::tree_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::tree_reciprocal_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::nr_iterations_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::max_iterations_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::previous_transformation_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::final_transformation_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::transformation_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::transformation_epsilon_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::converged_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::corr_dist_threshold_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::inlier_threshold_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::min_number_correspondences_;
+      using pcl::IterativeClosestPoint<PointSource, PointTarget>::update_visualizer_;
+
+      typedef pcl::PointCloud<PointSource> PointCloudSource;
+      typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
+      typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
+
+      typedef pcl::PointCloud<PointTarget> PointCloudTarget;
+      typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
+      typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;
+
+      typedef pcl::PointIndices::Ptr PointIndicesPtr;
+      typedef pcl::PointIndices::ConstPtr PointIndicesConstPtr;
+
+      typedef std::vector< Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d> > MatricesVector;
+      typedef boost::shared_ptr< MatricesVector > MatricesVectorPtr;
+      typedef boost::shared_ptr< const MatricesVector > MatricesVectorConstPtr;
+      
+      typedef typename pcl::Registration<PointSource, PointTarget>::KdTree InputKdTree;
+      typedef typename pcl::Registration<PointSource, PointTarget>::KdTreePtr InputKdTreePtr;
+
+      typedef boost::shared_ptr< GeneralizedIterativeClosestPoint<PointSource, PointTarget> > Ptr;
+      typedef boost::shared_ptr< const GeneralizedIterativeClosestPoint<PointSource, PointTarget> > ConstPtr;
+
+
+      typedef Eigen::Matrix<double, 6, 1> Vector6d;
+
+      /** \brief Empty constructor. */
+      GeneralizedIterativeClosestPoint () 
+        : k_correspondences_(20)
+        , gicp_epsilon_(0.001)
+        , rotation_epsilon_(2e-3)
+        , mahalanobis_(0)
+        , max_inner_iterations_(20)
+      {
+        min_number_correspondences_ = 4;
+        reg_name_ = "GeneralizedIterativeClosestPoint";
+        max_iterations_ = 200;
+        transformation_epsilon_ = 5e-4;
+        corr_dist_threshold_ = 5.;
+        rigid_transformation_estimation_ = 
+          boost::bind (&GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationBFGS, 
+                       this, _1, _2, _3, _4, _5); 
+      }
+      
+      /** \brief Provide a pointer to the input dataset
+        * \param cloud the const boost shared pointer to a PointCloud message
+        */
+      // PCL_DEPRECATED ("[pcl::registration::GeneralizedIterativeClosestPoint::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead.")
+      void
+      setInputCloud (const PointCloudSourceConstPtr &cloud);
+
+      /** \brief Provide a pointer to the input dataset
+        * \param cloud the const boost shared pointer to a PointCloud message
+        */
+      inline void
+      setInputSource (const PointCloudSourceConstPtr &cloud)
+      {
+
+        if (cloud->points.empty ())
+        {
+          PCL_ERROR ("[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
+          return;
+        }
+        PointCloudSource input = *cloud;
+        // Set all the point.data[3] values to 1 to aid the rigid transformation
+        for (size_t i = 0; i < input.size (); ++i)
+          input[i].data[3] = 1.0;
+        
+        pcl::IterativeClosestPoint<PointSource, PointTarget>::setInputSource (cloud);
+        input_covariances_.reset ();
+      }
+
+      /** \brief Provide a pointer to the covariances of the input source (if computed externally!). 
+        * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself.
+        * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances).
+        * \param[in] target the input point cloud target
+        */
+      inline void 
+      setSourceCovariances (const MatricesVectorPtr& covariances)
+      {
+        input_covariances_ = covariances;
+      }
+      
+      /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to)
+        * \param[in] target the input point cloud target
+        */
+      inline void 
+      setInputTarget (const PointCloudTargetConstPtr &target)
+      {
+        pcl::IterativeClosestPoint<PointSource, PointTarget>::setInputTarget(target);
+        target_covariances_.reset ();
+      }
+
+      /** \brief Provide a pointer to the covariances of the input target (if computed externally!). 
+        * If not set, GeneralizedIterativeClosestPoint will compute the covariances itself.
+        * Make sure to set the covariances AFTER setting the input source point cloud (setting the input source point cloud will reset the covariances).
+        * \param[in] target the input point cloud target
+        */
+	    inline void 
+      setTargetCovariances (const MatricesVectorPtr& covariances)
+      {
+        target_covariances_ = covariances;
+      }
+      
+      /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using an iterative
+        * non-linear Levenberg-Marquardt approach.
+        * \param[in] cloud_src the source point cloud dataset
+        * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src
+        * \param[in] cloud_tgt the target point cloud dataset
+        * \param[in] indices_tgt the vector of indices describing the correspondences of the interst points from \a indices_src
+        * \param[out] transformation_matrix the resultant transformation matrix
+        */
+      void
+      estimateRigidTransformationBFGS (const PointCloudSource &cloud_src,
+                                       const std::vector<int> &indices_src,
+                                       const PointCloudTarget &cloud_tgt,
+                                       const std::vector<int> &indices_tgt,
+                                       Eigen::Matrix4f &transformation_matrix);
+      
+      /** \brief \return Mahalanobis distance matrix for the given point index */
+      inline const Eigen::Matrix3d& mahalanobis(size_t index) const
+      {
+        assert(index < mahalanobis_.size());
+        return mahalanobis_[index];
+      }
+
+      /** \brief Computes rotation matrix derivative.
+        * rotation matrix is obtainded from rotation angles x[3], x[4] and x[5]
+        * \return d/d_rx, d/d_ry and d/d_rz respectively in g[3], g[4] and g[5]
+        * param x array representing 3D transformation
+        * param R rotation matrix
+        * param g gradient vector
+        */
+      void
+      computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const;
+
+      /** \brief Set the rotation epsilon (maximum allowable difference between two 
+        * consecutive rotations) in order for an optimization to be considered as having 
+        * converged to the final solution.
+        * \param epsilon the rotation epsilon
+        */
+      inline void 
+      setRotationEpsilon (double epsilon) { rotation_epsilon_ = epsilon; }
+
+      /** \brief Get the rotation epsilon (maximum allowable difference between two 
+        * consecutive rotations) as set by the user.
+        */
+      inline double 
+      getRotationEpsilon () { return (rotation_epsilon_); }
+
+      /** \brief Set the number of neighbors used when selecting a point neighbourhood
+        * to compute covariances. 
+        * A higher value will bring more accurate covariance matrix but will make 
+        * covariances computation slower.
+        * \param k the number of neighbors to use when computing covariances
+        */
+      void
+      setCorrespondenceRandomness (int k) { k_correspondences_ = k; }
+
+      /** \brief Get the number of neighbors used when computing covariances as set by 
+        * the user 
+        */
+      int
+      getCorrespondenceRandomness () { return (k_correspondences_); }
+
+      /** set maximum number of iterations at the optimization step
+        * \param[in] max maximum number of iterations for the optimizer
+        */
+      void
+      setMaximumOptimizerIterations (int max) { max_inner_iterations_ = max; }
+
+      ///\return maximum number of iterations at the optimization step
+      int
+      getMaximumOptimizerIterations () { return (max_inner_iterations_); }
+
+    protected:
+
+      /** \brief The number of neighbors used for covariances computation. 
+        * default: 20
+        */
+      int k_correspondences_;
+
+      /** \brief The epsilon constant for gicp paper; this is NOT the convergence 
+        * tolerence 
+        * default: 0.001
+        */
+      double gicp_epsilon_;
+
+      /** The epsilon constant for rotation error. (In GICP the transformation epsilon 
+        * is split in rotation part and translation part).
+        * default: 2e-3
+        */
+      double rotation_epsilon_;
+
+      /** \brief base transformation */
+      Eigen::Matrix4f base_transformation_;
+
+      /** \brief Temporary pointer to the source dataset. */
+      const PointCloudSource *tmp_src_;
+
+      /** \brief Temporary pointer to the target dataset. */
+      const PointCloudTarget  *tmp_tgt_;
+
+      /** \brief Temporary pointer to the source dataset indices. */
+      const std::vector<int> *tmp_idx_src_;
+
+      /** \brief Temporary pointer to the target dataset indices. */
+      const std::vector<int> *tmp_idx_tgt_;
+
+      
+      /** \brief Input cloud points covariances. */
+      MatricesVectorPtr input_covariances_;
+
+      /** \brief Target cloud points covariances. */
+      MatricesVectorPtr target_covariances_;
+
+      /** \brief Mahalanobis matrices holder. */
+      std::vector<Eigen::Matrix3d> mahalanobis_;
+      
+      /** \brief maximum number of optimizations */
+      int max_inner_iterations_;
+
+      /** \brief compute points covariances matrices according to the K nearest 
+        * neighbors. K is set via setCorrespondenceRandomness() methode.
+        * \param cloud pointer to point cloud
+        * \param tree KD tree performer for nearest neighbors search
+        * \param[out] cloud_covariances covariances matrices for each point in the cloud
+        */
+      template<typename PointT>
+      void computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
+                              const typename pcl::search::KdTree<PointT>::ConstPtr tree,
+                              MatricesVector& cloud_covariances);
+
+      /** \return trace of mat1^t . mat2 
+        * \param mat1 matrix of dimension nxm
+        * \param mat2 matrix of dimension nxp
+        */
+      inline double 
+      matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const
+      {
+        double r = 0.;
+        size_t n = mat1.rows();
+        // tr(mat1^t.mat2)
+        for(size_t i = 0; i < n; i++)
+          for(size_t j = 0; j < n; j++)
+            r += mat1 (j, i) * mat2 (i,j);
+        return r;
+      }
+
+      /** \brief Rigid transformation computation method  with initial guess.
+        * \param output the transformed input point cloud dataset using the rigid transformation found
+        * \param guess the initial guess of the transformation to compute
+        */
+      void 
+      computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess);
+
+      /** \brief Search for the closest nearest neighbor of a given point.
+        * \param query the point to search a nearest neighbour for
+        * \param index vector of size 1 to store the index of the nearest neighbour found
+        * \param distance vector of size 1 to store the distance to nearest neighbour found
+        */
+      inline bool 
+      searchForNeighbors (const PointSource &query, std::vector<int>& index, std::vector<float>& distance) const
+      {
+        int k = tree_->nearestKSearch (query, 1, index, distance);
+        if (k == 0)
+          return (false);
+        return (true);
+      }
+
+      /// \brief compute transformation matrix from transformation matrix
+      void applyState(Eigen::Matrix4f &t, const Vector6d& x) const;
+      
+      /// \brief optimization functor structure
+      struct OptimizationFunctorWithIndices : public BFGSDummyFunctor<double,6>
+      {
+        OptimizationFunctorWithIndices (const GeneralizedIterativeClosestPoint* gicp)
+          : BFGSDummyFunctor<double,6> (), gicp_(gicp) {}
+        double operator() (const Vector6d& x);
+        void  df(const Vector6d &x, Vector6d &df);
+        void fdf(const Vector6d &x, double &f, Vector6d &df);
+
+        const GeneralizedIterativeClosestPoint *gicp_;
+      };
+      
+      boost::function<void(const pcl::PointCloud<PointSource> &cloud_src,
+                           const std::vector<int> &src_indices,
+                           const pcl::PointCloud<PointTarget> &cloud_tgt,
+                           const std::vector<int> &tgt_indices,
+                           Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_;
+  };
+}
+
+#endif  //#ifndef PCL_GICP_H_

scan_matcher_ICP/include/pclomp/gicp_omp_impl.hpp

@@ -0,0 +1,529 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010, Willow Garage, Inc.
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+#ifndef PCL_REGISTRATION_IMPL_GICP_OMP_HPP_
+#define PCL_REGISTRATION_IMPL_GICP_OMP_HPP_
+
+#include <chrono>
+#include <atomic>
+#include <pcl/registration/boost.h>
+#include <pcl/registration/exceptions.h>
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget> void
+pclomp::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::setInputCloud (
+    const typename pclomp::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::PointCloudSourceConstPtr &cloud)
+{
+  setInputSource (cloud);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget> 
+template<typename PointT> void
+pclomp::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
+                                                                                    const typename pcl::search::KdTree<PointT>::ConstPtr kdtree,
+                                                                                    MatricesVector& cloud_covariances)
+{
+  if (k_correspondences_ > int (cloud->size ()))
+  {
+    PCL_ERROR ("[pclomp::GeneralizedIterativeClosestPoint::computeCovariances] Number or points in cloud (%lu) is less than k_correspondences_ (%lu)!\n", cloud->size (), k_correspondences_);
+    return;
+  }
+
+  // We should never get there but who knows
+  if(cloud_covariances.size () < cloud->size ())
+    cloud_covariances.resize (cloud->size ());
+
+  std::vector<std::vector<int>> nn_indecies_array(omp_get_max_threads());
+  std::vector<std::vector<float>> nn_dist_sq_array(omp_get_max_threads());
+
+#pragma omp parallel for
+  for(int i=0; i<cloud->size(); i++) {
+    auto& nn_indecies = nn_indecies_array[omp_get_thread_num()];
+	auto& nn_dist_sq = nn_dist_sq_array[omp_get_thread_num()];
+
+    const PointT &query_point = cloud->at(i);
+    Eigen::Vector3d mean = Eigen::Vector3d::Zero();
+    Eigen::Matrix3d &cov = cloud_covariances[i];
+    // Zero out the cov and mean
+    cov.setZero ();
+
+    // Search for the K nearest neighbours
+    kdtree->nearestKSearch(query_point, k_correspondences_, nn_indecies, nn_dist_sq);
+    
+    // Find the covariance matrix
+    for(int j = 0; j < k_correspondences_; j++) {
+      const PointT &pt = (*cloud)[nn_indecies[j]];
+      
+      mean[0] += pt.x;
+      mean[1] += pt.y;
+      mean[2] += pt.z;
+      
+      cov(0,0) += pt.x*pt.x;
+      
+      cov(1,0) += pt.y*pt.x;
+      cov(1,1) += pt.y*pt.y;
+      
+      cov(2,0) += pt.z*pt.x;
+      cov(2,1) += pt.z*pt.y;
+      cov(2,2) += pt.z*pt.z;    
+    }
+  
+    mean /= static_cast<double> (k_correspondences_);
+    // Get the actual covariance
+    for (int k = 0; k < 3; k++)
+      for (int l = 0; l <= k; l++) 
+      {
+        cov(k,l) /= static_cast<double> (k_correspondences_);
+        cov(k,l) -= mean[k]*mean[l];
+        cov(l,k) = cov(k,l);
+      }
+    
+    // Compute the SVD (covariance matrix is symmetric so U = V')
+    Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov, Eigen::ComputeFullU);
+    cov.setZero ();
+    Eigen::Matrix3d U = svd.matrixU ();
+    // Reconstitute the covariance matrix with modified singular values using the column     // vectors in V.
+    for(int k = 0; k < 3; k++) {
+      Eigen::Vector3d col = U.col(k);
+      double v = 1.; // biggest 2 singular values replaced by 1
+      if(k == 2)   // smallest singular value replaced by gicp_epsilon
+        v = gicp_epsilon_;
+      cov+= v * col * col.transpose(); 
+    }
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget> void
+pclomp::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d& g) const
+{
+  Eigen::Matrix3d dR_dPhi;
+  Eigen::Matrix3d dR_dTheta;
+  Eigen::Matrix3d dR_dPsi;
+
+  double phi = x[3], theta = x[4], psi = x[5];
+  
+  double cphi = cos(phi), sphi = sin(phi);
+  double ctheta = cos(theta), stheta = sin(theta);
+  double cpsi = cos(psi), spsi = sin(psi);
+      
+  dR_dPhi(0,0) = 0.;
+  dR_dPhi(1,0) = 0.;
+  dR_dPhi(2,0) = 0.;
+
+  dR_dPhi(0,1) = sphi*spsi + cphi*cpsi*stheta;
+  dR_dPhi(1,1) = -cpsi*sphi + cphi*spsi*stheta;
+  dR_dPhi(2,1) = cphi*ctheta;
+
+  dR_dPhi(0,2) = cphi*spsi - cpsi*sphi*stheta;
+  dR_dPhi(1,2) = -cphi*cpsi - sphi*spsi*stheta;
+  dR_dPhi(2,2) = -ctheta*sphi;
+
+  dR_dTheta(0,0) = -cpsi*stheta;
+  dR_dTheta(1,0) = -spsi*stheta;
+  dR_dTheta(2,0) = -ctheta;
+
+  dR_dTheta(0,1) = cpsi*ctheta*sphi;
+  dR_dTheta(1,1) = ctheta*sphi*spsi;
+  dR_dTheta(2,1) = -sphi*stheta;
+
+  dR_dTheta(0,2) = cphi*cpsi*ctheta;
+  dR_dTheta(1,2) = cphi*ctheta*spsi;
+  dR_dTheta(2,2) = -cphi*stheta;
+
+  dR_dPsi(0,0) = -ctheta*spsi;
+  dR_dPsi(1,0) = cpsi*ctheta;
+  dR_dPsi(2,0) = 0.;
+
+  dR_dPsi(0,1) = -cphi*cpsi - sphi*spsi*stheta;
+  dR_dPsi(1,1) = -cphi*spsi + cpsi*sphi*stheta;
+  dR_dPsi(2,1) = 0.;
+
+  dR_dPsi(0,2) = cpsi*sphi - cphi*spsi*stheta;
+  dR_dPsi(1,2) = sphi*spsi + cphi*cpsi*stheta;
+  dR_dPsi(2,2) = 0.;
+      
+  g[3] = matricesInnerProd(dR_dPhi, R);
+  g[4] = matricesInnerProd(dR_dTheta, R);
+  g[5] = matricesInnerProd(dR_dPsi, R);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget> void
+pclomp::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src,
+                                                                                                  const std::vector<int> &indices_src, 
+                                                                                                  const PointCloudTarget &cloud_tgt, 
+                                                                                                  const std::vector<int> &indices_tgt, 
+                                                                                                  Eigen::Matrix4f &transformation_matrix)
+{
+  if (indices_src.size () < 4)     // need at least 4 samples
+  {
+    PCL_THROW_EXCEPTION (pcl::NotEnoughPointsException,
+                         "[pclomp::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () << " points!");
+    return;
+  }
+  // Set the initial solution
+  Vector6d x = Vector6d::Zero ();
+  x[0] = transformation_matrix (0,3);
+  x[1] = transformation_matrix (1,3);
+  x[2] = transformation_matrix (2,3);
+  x[3] = atan2 (transformation_matrix (2,1), transformation_matrix (2,2));
+  x[4] = asin (-transformation_matrix (2,0));
+  x[5] = atan2 (transformation_matrix (1,0), transformation_matrix (0,0));
+
+  // Set temporary pointers
+  tmp_src_ = &cloud_src;
+  tmp_tgt_ = &cloud_tgt;
+  tmp_idx_src_ = &indices_src;
+  tmp_idx_tgt_ = &indices_tgt;
+
+  // Optimize using forward-difference approximation LM
+  const double gradient_tol = 1e-2;
+  OptimizationFunctorWithIndices functor(this);
+  BFGS<OptimizationFunctorWithIndices> bfgs (functor);
+  bfgs.parameters.sigma = 0.01;
+  bfgs.parameters.rho = 0.01;
+  bfgs.parameters.tau1 = 9;
+  bfgs.parameters.tau2 = 0.05;
+  bfgs.parameters.tau3 = 0.5;
+  bfgs.parameters.order = 3;
+
+  int inner_iterations_ = 0;
+  int result = bfgs.minimizeInit (x);
+  result = BFGSSpace::Running;
+  do
+  {
+    inner_iterations_++;
+    result = bfgs.minimizeOneStep (x);
+    if(result)
+    {
+      break;
+    }
+    result = bfgs.testGradient(gradient_tol);
+  } while(result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_);
+  if(result == BFGSSpace::NoProgress || result == BFGSSpace::Success || inner_iterations_ == max_inner_iterations_)
+  {
+    PCL_DEBUG ("[pcl::registration::TransformationEstimationBFGS::estimateRigidTransformation]");
+    PCL_DEBUG ("BFGS solver finished with exit code %i \n", result);
+    transformation_matrix.setIdentity();
+    applyState(transformation_matrix, x);
+  }
+  else
+    PCL_THROW_EXCEPTION(pcl::SolverDidntConvergeException,
+                        "[pcl::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!");
+}
+
+////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget> inline double
+pclomp::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::operator() (const Vector6d& x)
+{
+  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
+  gicp_->applyState(transformation_matrix, x);
+  double f = 0;
+  std::vector<double> f_array(omp_get_max_threads(), 0.0);
+  int m = static_cast<int> (gicp_->tmp_idx_src_->size ());
+#pragma omp parallel for
+  for (int i = 0; i < m; ++i)
+  {
+    // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
+    pcl::Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
+    // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
+    pcl::Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
+    Eigen::Vector4f pp (transformation_matrix * p_src);
+    // Estimate the distance (cost function)
+    // The last coordiante is still guaranteed to be set to 1.0
+	// Eigen::AlignedVector3<double> res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
+	// Eigen::AlignedVector3<double> temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
+    Eigen::Vector4d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2], 0.0);
+	Eigen::Matrix4d maha = Eigen::Matrix4d::Zero();	
+	maha.block<3, 3>(0, 0) = gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]);
+    Eigen::Vector4d temp (maha * res);
+    //increment= res'*temp/num_matches = temp'*M*temp/num_matches (we postpone 1/num_matches after the loop closes)
+	double ret = double(res.transpose() * temp);
+    f_array[omp_get_thread_num()] += ret;
+  }
+  f = std::accumulate(f_array.begin(), f_array.end(), 0.0);
+  return f/m;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget> inline void
+pclomp::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::df (const Vector6d& x, Vector6d& g)
+{
+  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
+  gicp_->applyState(transformation_matrix, x);
+  //Eigen::Vector3d g_t = g.head<3> ();
+  std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> R_array(omp_get_max_threads());
+  std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>> g_array(omp_get_max_threads());
+
+  for (int i = 0; i < R_array.size(); i++) {
+	  R_array[i].setZero();
+	  g_array[i].setZero();
+  }
+
+  int m = static_cast<int> (gicp_->tmp_idx_src_->size ());
+#pragma omp parallel for
+  for (int i = 0; i < m; ++i)
+  {
+    // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
+    pcl::Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
+    // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
+    pcl::Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
+
+    Eigen::Vector4f pp (transformation_matrix * p_src);
+    // The last coordiante is still guaranteed to be set to 1.0
+    Eigen::Vector4d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2], 0.0);
+    // temp = M*res
+
+	Eigen::Matrix4d maha = Eigen::Matrix4d::Zero();
+	maha.block<3, 3>(0, 0) = gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]);
+
+    Eigen::Vector4d temp (maha * res);
+    // Increment translation gradient
+    // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes)
+    // Increment rotation gradient
+	pp = gicp_->base_transformation_ * p_src;
+
+    Eigen::Vector4d p_src3 (pp[0], pp[1], pp[2], 0.0);
+	g_array[omp_get_thread_num()] += temp;
+	R_array[omp_get_thread_num()] += p_src3 * temp.transpose();
+  }
+
+  g.setZero();
+  Eigen::Matrix4d R = Eigen::Matrix4d::Zero();
+  for (int i = 0; i < R_array.size(); i++) {
+	  R += R_array[i];
+	  g.head<3>() += g_array[i].head<3>();
+  }
+
+  g.head<3>() *= 2.0 / m;
+  R *= 2.0 / m;
+
+  gicp_->computeRDerivative(x, R.block<3, 3>(0, 0), g);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget> inline void
+pclomp::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::fdf (const Vector6d& x, double& f, Vector6d& g)
+{
+  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
+  gicp_->applyState(transformation_matrix, x);
+  f = 0;
+  g.setZero ();
+  Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
+  const int m = static_cast<const int> (gicp_->tmp_idx_src_->size ());
+  for (int i = 0; i < m; ++i)
+  {
+    // The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
+    pcl::Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
+    // The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
+    pcl::Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
+    Eigen::Vector4f pp (transformation_matrix * p_src);
+    // The last coordiante is still guaranteed to be set to 1.0
+    Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
+    // temp = M*res
+    Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
+    // Increment total error
+    f+= double(res.transpose() * temp);
+    // Increment translation gradient
+    // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes)
+    g.head<3> ()+= temp;
+    pp = gicp_->base_transformation_ * p_src;
+    Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
+    // Increment rotation gradient
+    R+= p_src3 * temp.transpose();    
+  }
+  f/= double(m);
+  g.head<3> ()*= double(2.0/m);
+  R*= 2.0/m;
+  gicp_->computeRDerivative(x, R, g);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget> inline void
+pclomp::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
+{
+  pcl::IterativeClosestPoint<PointSource, PointTarget>::initComputeReciprocal ();
+  using namespace std;
+  // Difference between consecutive transforms
+  double delta = 0;
+  // Get the size of the target
+  const size_t N = indices_->size ();
+  // Set the mahalanobis matrices to identity
+  mahalanobis_.resize (N, Eigen::Matrix3d::Identity ());
+  // Compute target cloud covariance matrices
+  if ((!target_covariances_) || (target_covariances_->empty ()))
+  {
+    target_covariances_.reset (new MatricesVector);  
+    computeCovariances<PointTarget> (target_, tree_, *target_covariances_);
+  }
+  // Compute input cloud covariance matrices
+  if ((!input_covariances_) || (input_covariances_->empty ()))
+  {
+    input_covariances_.reset (new MatricesVector);
+    computeCovariances<PointSource> (input_, tree_reciprocal_, *input_covariances_);
+  }
+
+  base_transformation_ = guess;
+  nr_iterations_ = 0;
+  converged_ = false;
+  double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
+
+  std::vector<std::vector<int>> nn_indices_array(omp_get_max_threads());
+  std::vector<std::vector<float>> nn_dists_array(omp_get_max_threads());
+  for (auto& nn_indices : nn_indices_array) { nn_indices.resize(1); }
+  for (auto& nn_dists : nn_dists_array) { nn_dists.resize(1); }
+
+  while(!converged_)
+  {
+    std::atomic<size_t> cnt;
+    cnt = 0;
+    std::vector<int> source_indices (indices_->size ());
+    std::vector<int> target_indices (indices_->size ());
+
+    // guess corresponds to base_t and transformation_ to t
+    Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero ();
+    for(size_t i = 0; i < 4; i++)
+      for(size_t j = 0; j < 4; j++)
+        for(size_t k = 0; k < 4; k++)
+          transform_R(i,j)+= double(transformation_(i,k)) * double(guess(k,j));
+
+    const Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> ();
+
+#pragma omp parallel for
+    for (int i = 0; i < N; i++)
+    {
+      auto& nn_indices = nn_indices_array[omp_get_thread_num()];
+	  auto& nn_dists = nn_dists_array[omp_get_thread_num()];
+
+      PointSource query = output[i];
+      query.getVector4fMap () = guess * query.getVector4fMap ();
+      query.getVector4fMap () = transformation_ * query.getVector4fMap ();
+
+      if (!searchForNeighbors (query, nn_indices, nn_dists))
+      {
+        PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[i]);
+        continue;
+      }
+      
+      // Check if the distance to the nearest neighbor is smaller than the user imposed threshold
+      if (nn_dists[0] < dist_threshold)
+      {
+        const Eigen::Matrix3d &C1 = (*input_covariances_)[i];
+        const Eigen::Matrix3d &C2 = (*target_covariances_)[nn_indices[0]];
+        Eigen::Matrix3d &M = mahalanobis_[i];
+        // M = R*C1
+        M = R * C1;
+        // temp = M*R' + C2 = R*C1*R' + C2
+        Eigen::Matrix3d temp = M * R.transpose();        
+        temp+= C2;
+        // M = temp^-1
+        M = temp.inverse ();
+        int c = cnt++;
+        source_indices[c] = static_cast<int> (i);
+        target_indices[c] = nn_indices[0];
+      }
+    }
+    // Resize to the actual number of valid correspondences
+    source_indices.resize(cnt); target_indices.resize(cnt);
+    /* optimize transformation using the current assignment and Mahalanobis metrics*/
+    previous_transformation_ = transformation_;
+    //optimization right here
+    try
+    {
+      rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_);
+      /* compute the delta from this iteration */
+      delta = 0.;
+      for(int k = 0; k < 4; k++) {
+        for(int l = 0; l < 4; l++) {
+          double ratio = 1;
+          if(k < 3 && l < 3) // rotation part of the transform
+            ratio = 1./rotation_epsilon_;
+          else
+            ratio = 1./transformation_epsilon_;
+          double c_delta = ratio*fabs(previous_transformation_(k,l) - transformation_(k,l));
+          if(c_delta > delta)
+            delta = c_delta;
+        }
+      }
+    } 
+    catch (pcl::PCLException &e)
+    {
+      PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ());
+      break;
+    }
+    nr_iterations_++;
+    // Check for convergence
+    if (nr_iterations_ >= max_iterations_ || delta < 1)
+    {
+      converged_ = true;
+      previous_transformation_ = transformation_;
+      PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n",
+                 getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ());
+    } 
+    else
+      PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ());
+  }
+  //for some reason the static equivalent methode raises an error
+  // final_transformation_.block<3,3> (0,0) = (transformation_.block<3,3> (0,0)) * (guess.block<3,3> (0,0));
+  // final_transformation_.block <3, 1> (0, 3) = transformation_.block <3, 1> (0, 3) + guess.rightCols<1>.block <3, 1> (0, 3);
+  final_transformation_.topLeftCorner (3,3) = previous_transformation_.topLeftCorner (3,3) * guess.topLeftCorner (3,3);
+  final_transformation_(0,3) = previous_transformation_(0,3) + guess(0,3);
+  final_transformation_(1,3) = previous_transformation_(1,3) + guess(1,3);
+  final_transformation_(2,3) = previous_transformation_(2,3) + guess(2,3);
+
+  // Transform the point cloud
+  pcl::transformPointCloud (*input_, output, final_transformation_);
+}
+
+template <typename PointSource, typename PointTarget> void
+pclomp::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::applyState(Eigen::Matrix4f &t, const Vector6d& x) const
+{
+  // !!! CAUTION Stanford GICP uses the Z Y X euler angles convention
+  Eigen::Matrix3f R;
+  R = Eigen::AngleAxisf (static_cast<float> (x[5]), Eigen::Vector3f::UnitZ ())
+    * Eigen::AngleAxisf (static_cast<float> (x[4]), Eigen::Vector3f::UnitY ())
+    * Eigen::AngleAxisf (static_cast<float> (x[3]), Eigen::Vector3f::UnitX ());
+  t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix ();
+  Eigen::Vector4f T (static_cast<float> (x[0]), static_cast<float> (x[1]), static_cast<float> (x[2]), 0.0f);
+  t.col (3) += T;
+}
+
+#endif //PCL_REGISTRATION_IMPL_GICP_HPP_

scan_matcher_ICP/include/pclomp/ndt_omp.h

@@ -0,0 +1,500 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-2012, Willow Garage, Inc.
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_REGISTRATION_NDT_OMP_H_
+#define PCL_REGISTRATION_NDT_OMP_H_
+
+#include <pcl/registration/registration.h>
+#include "voxel_grid_covariance_omp.h"
+
+#include <unsupported/Eigen/NonLinearOptimization>
+
+namespace pclomp
+{
+	enum NeighborSearchMethod {
+		KDTREE,
+		DIRECT26,
+		DIRECT7,
+		DIRECT1
+	};
+
+	/** \brief A 3D Normal Distribution Transform registration implementation for point cloud data.
+	  * \note For more information please see
+	  * <b>Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform —
+	  * an Efficient Representation for Registration, Surface Analysis, and Loop Detection.
+	  * PhD thesis, Orebro University. Orebro Studies in Technology 36.</b>,
+	  * <b>More, J., and Thuente, D. (1994). Line Search Algorithm with Guaranteed Sufficient Decrease
+	  * In ACM Transactions on Mathematical Software.</b> and
+	  * Sun, W. and Yuan, Y, (2006) Optimization Theory and Methods: Nonlinear Programming. 89-100
+	  * \note Math refactored by Todor Stoyanov.
+	  * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
+	  */
+	template<typename PointSource, typename PointTarget>
+	class NormalDistributionsTransform : public pcl::Registration<PointSource, PointTarget>
+	{
+	protected:
+
+		typedef typename pcl::Registration<PointSource, PointTarget>::PointCloudSource PointCloudSource;
+		typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
+		typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
+
+		typedef typename pcl::Registration<PointSource, PointTarget>::PointCloudTarget PointCloudTarget;
+		typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
+		typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;
+
+		typedef pcl::PointIndices::Ptr PointIndicesPtr;
+		typedef pcl::PointIndices::ConstPtr PointIndicesConstPtr;
+
+		/** \brief Typename of searchable voxel grid containing mean and covariance. */
+		typedef pclomp::VoxelGridCovariance<PointTarget> TargetGrid;
+		/** \brief Typename of pointer to searchable voxel grid. */
+		typedef TargetGrid* TargetGridPtr;
+		/** \brief Typename of const pointer to searchable voxel grid. */
+		typedef const TargetGrid* TargetGridConstPtr;
+		/** \brief Typename of const pointer to searchable voxel grid leaf. */
+		typedef typename TargetGrid::LeafConstPtr TargetGridLeafConstPtr;
+
+
+	public:
+
+		typedef boost::shared_ptr< NormalDistributionsTransform<PointSource, PointTarget> > Ptr;
+		typedef boost::shared_ptr< const NormalDistributionsTransform<PointSource, PointTarget> > ConstPtr;
+
+
+		/** \brief Constructor.
+		  * Sets \ref outlier_ratio_ to 0.35, \ref step_size_ to 0.05 and \ref resolution_ to 1.0
+		  */
+		NormalDistributionsTransform();
+
+		/** \brief Empty destructor */
+		virtual ~NormalDistributionsTransform() {}
+
+    void setNumThreads(int n) {
+      num_threads_ = n;
+    }
+
+		/** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to).
+		  * \param[in] cloud the input point cloud target
+		  */
+		inline void
+			setInputTarget(const PointCloudTargetConstPtr &cloud)
+		{
+			pcl::Registration<PointSource, PointTarget>::setInputTarget(cloud);
+			init();
+		}
+
+		/** \brief Set/change the voxel grid resolution.
+		  * \param[in] resolution side length of voxels
+		  */
+		inline void
+			setResolution(float resolution)
+		{
+			// Prevents unnessary voxel initiations
+			if (resolution_ != resolution)
+			{
+				resolution_ = resolution;
+				if (input_)
+					init();
+			}
+		}
+
+		/** \brief Get voxel grid resolution.
+		  * \return side length of voxels
+		  */
+		inline float
+			getResolution() const
+		{
+			return (resolution_);
+		}
+
+		/** \brief Get the newton line search maximum step length.
+		  * \return maximum step length
+		  */
+		inline double
+			getStepSize() const
+		{
+			return (step_size_);
+		}
+
+		/** \brief Set/change the newton line search maximum step length.
+		  * \param[in] step_size maximum step length
+		  */
+		inline void
+			setStepSize(double step_size)
+		{
+			step_size_ = step_size;
+		}
+
+		/** \brief Get the point cloud outlier ratio.
+		  * \return outlier ratio
+		  */
+		inline double
+			getOulierRatio() const
+		{
+			return (outlier_ratio_);
+		}
+
+		/** \brief Set/change the point cloud outlier ratio.
+		  * \param[in] outlier_ratio outlier ratio
+		  */
+		inline void
+			setOulierRatio(double outlier_ratio)
+		{
+			outlier_ratio_ = outlier_ratio;
+		}
+
+		inline void setNeighborhoodSearchMethod(NeighborSearchMethod method) {
+			search_method = method;
+		}
+
+		/** \brief Get the registration alignment probability.
+		  * \return transformation probability
+		  */
+		inline double
+			getTransformationProbability() const
+		{
+			return (trans_probability_);
+		}
+
+		/** \brief Get the number of iterations required to calculate alignment.
+		  * \return final number of iterations
+		  */
+		inline int
+			getFinalNumIteration() const
+		{
+			return (nr_iterations_);
+		}
+
+		/** \brief Convert 6 element transformation vector to affine transformation.
+		  * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
+		  * \param[out] trans affine transform corresponding to given transfomation vector
+		  */
+		static void
+			convertTransform(const Eigen::Matrix<double, 6, 1> &x, Eigen::Affine3f &trans)
+		{
+			trans = Eigen::Translation<float, 3>(float(x(0)), float(x(1)), float(x(2))) *
+				Eigen::AngleAxis<float>(float(x(3)), Eigen::Vector3f::UnitX()) *
+				Eigen::AngleAxis<float>(float(x(4)), Eigen::Vector3f::UnitY()) *
+				Eigen::AngleAxis<float>(float(x(5)), Eigen::Vector3f::UnitZ());
+		}
+
+		/** \brief Convert 6 element transformation vector to transformation matrix.
+		  * \param[in] x transformation vector of the form [x, y, z, roll, pitch, yaw]
+		  * \param[out] trans 4x4 transformation matrix corresponding to given transfomation vector
+		  */
+		static void
+			convertTransform(const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix4f &trans)
+		{
+			Eigen::Affine3f _affine;
+			convertTransform(x, _affine);
+			trans = _affine.matrix();
+		}
+
+		// negative log likelihood function
+		// lower is better
+		double calculateScore(const PointCloudSource& cloud) const;
+
+	protected:
+
+		using pcl::Registration<PointSource, PointTarget>::reg_name_;
+		using pcl::Registration<PointSource, PointTarget>::getClassName;
+		using pcl::Registration<PointSource, PointTarget>::input_;
+		using pcl::Registration<PointSource, PointTarget>::indices_;
+		using pcl::Registration<PointSource, PointTarget>::target_;
+		using pcl::Registration<PointSource, PointTarget>::nr_iterations_;
+		using pcl::Registration<PointSource, PointTarget>::max_iterations_;
+		using pcl::Registration<PointSource, PointTarget>::previous_transformation_;
+		using pcl::Registration<PointSource, PointTarget>::final_transformation_;
+		using pcl::Registration<PointSource, PointTarget>::transformation_;
+		using pcl::Registration<PointSource, PointTarget>::transformation_epsilon_;
+		using pcl::Registration<PointSource, PointTarget>::converged_;
+		using pcl::Registration<PointSource, PointTarget>::corr_dist_threshold_;
+		using pcl::Registration<PointSource, PointTarget>::inlier_threshold_;
+
+		using pcl::Registration<PointSource, PointTarget>::update_visualizer_;
+
+		/** \brief Estimate the transformation and returns the transformed source (input) as output.
+		  * \param[out] output the resultant input transfomed point cloud dataset
+		  */
+		virtual void
+			computeTransformation(PointCloudSource &output)
+		{
+			computeTransformation(output, Eigen::Matrix4f::Identity());
+		}
+
+		/** \brief Estimate the transformation and returns the transformed source (input) as output.
+		  * \param[out] output the resultant input transfomed point cloud dataset
+		  * \param[in] guess the initial gross estimation of the transformation
+		  */
+		virtual void
+			computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess);
+
+		/** \brief Initiate covariance voxel structure. */
+		void inline
+			init()
+		{
+			target_cells_.setLeafSize(resolution_, resolution_, resolution_);
+			target_cells_.setInputCloud(target_);
+			// Initiate voxel structure.
+			target_cells_.filter(true);
+		}
+
+		/** \brief Compute derivatives of probability function w.r.t. the transformation vector.
+		  * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
+		  * \param[out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector
+		  * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
+		  * \param[in] trans_cloud transformed point cloud
+		  * \param[in] p the current transform vector
+		  * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation.
+		  */
+		double
+			computeDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient,
+				Eigen::Matrix<double, 6, 6> &hessian,
+				PointCloudSource &trans_cloud,
+				Eigen::Matrix<double, 6, 1> &p,
+				bool compute_hessian = true);
+
+		/** \brief Compute individual point contirbutions to derivatives of probability function w.r.t. the transformation vector.
+		  * \note Equation 6.10, 6.12 and 6.13 [Magnusson 2009].
+		  * \param[in,out] score_gradient the gradient vector of the probability function w.r.t. the transformation vector
+		  * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
+		  * \param[in] x_trans transformed point minus mean of occupied covariance voxel
+		  * \param[in] c_inv covariance of occupied covariance voxel
+		  * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation.
+		  */
+		double
+			updateDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient,
+				Eigen::Matrix<double, 6, 6> &hessian,
+				const Eigen::Matrix<float, 4, 6> &point_gradient_,
+				const Eigen::Matrix<float, 24, 6> &point_hessian_,
+				const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv,
+				bool compute_hessian = true) const;
+
+		/** \brief Precompute anglular components of derivatives.
+		  * \note Equation 6.19 and 6.21 [Magnusson 2009].
+		  * \param[in] p the current transform vector
+		  * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation.
+		  */
+		void
+			computeAngleDerivatives(Eigen::Matrix<double, 6, 1> &p, bool compute_hessian = true);
+
+		/** \brief Compute point derivatives.
+		  * \note Equation 6.18-21 [Magnusson 2009].
+		  * \param[in] x point from the input cloud
+		  * \param[in] compute_hessian flag to calculate hessian, unnessissary for step calculation.
+		  */
+		void
+			computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<double, 3, 6>& point_gradient_, Eigen::Matrix<double, 18, 6>& point_hessian_, bool compute_hessian = true) const;
+
+		void
+			computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<float, 4, 6>& point_gradient_, Eigen::Matrix<float, 24, 6>& point_hessian_, bool compute_hessian = true) const;
+
+		/** \brief Compute hessian of probability function w.r.t. the transformation vector.
+		  * \note Equation 6.13 [Magnusson 2009].
+		  * \param[out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
+		  * \param[in] trans_cloud transformed point cloud
+		  * \param[in] p the current transform vector
+		  */
+		void
+			computeHessian(Eigen::Matrix<double, 6, 6> &hessian,
+				PointCloudSource &trans_cloud,
+				Eigen::Matrix<double, 6, 1> &p);
+
+		/** \brief Compute individual point contirbutions to hessian of probability function w.r.t. the transformation vector.
+		  * \note Equation 6.13 [Magnusson 2009].
+		  * \param[in,out] hessian the hessian matrix of the probability function w.r.t. the transformation vector
+		  * \param[in] x_trans transformed point minus mean of occupied covariance voxel
+		  * \param[in] c_inv covariance of occupied covariance voxel
+		  */
+		void
+			updateHessian(Eigen::Matrix<double, 6, 6> &hessian,
+				const Eigen::Matrix<double, 3, 6> &point_gradient_,
+				const Eigen::Matrix<double, 18, 6> &point_hessian_,
+				const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv) const;
+
+		/** \brief Compute line search step length and update transform and probability derivatives using More-Thuente method.
+		  * \note Search Algorithm [More, Thuente 1994]
+		  * \param[in] x initial transformation vector, \f$ x \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \vec{p} \f$ in Algorithm 2 [Magnusson 2009]
+		  * \param[in] step_dir descent direction, \f$ p \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ \delta \vec{p} \f$ normalized in Algorithm 2 [Magnusson 2009]
+		  * \param[in] step_init initial step length estimate, \f$ \alpha_0 \f$ in Moore-Thuente (1994) and the noramal of \f$ \delta \vec{p} \f$ in Algorithm 2 [Magnusson 2009]
+		  * \param[in] step_max maximum step length, \f$ \alpha_max \f$ in Moore-Thuente (1994)
+		  * \param[in] step_min minimum step length, \f$ \alpha_min \f$ in Moore-Thuente (1994)
+		  * \param[out] score final score function value, \f$ f(x + \alpha p) \f$ in Equation 1.3 (Moore, Thuente 1994) and \f$ score \f$ in Algorithm 2 [Magnusson 2009]
+		  * \param[in,out] score_gradient gradient of score function w.r.t. transformation vector, \f$ f'(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ \vec{g} \f$ in Algorithm 2 [Magnusson 2009]
+		  * \param[out] hessian hessian of score function w.r.t. transformation vector, \f$ f''(x + \alpha p) \f$ in Moore-Thuente (1994) and \f$ H \f$ in Algorithm 2 [Magnusson 2009]
+		  * \param[in,out] trans_cloud transformed point cloud, \f$ X \f$ transformed by \f$ T(\vec{p},\vec{x}) \f$ in Algorithm 2 [Magnusson 2009]
+		  * \return final step length
+		  */
+		double
+			computeStepLengthMT(const Eigen::Matrix<double, 6, 1> &x,
+				Eigen::Matrix<double, 6, 1> &step_dir,
+				double step_init,
+				double step_max, double step_min,
+				double &score,
+				Eigen::Matrix<double, 6, 1> &score_gradient,
+				Eigen::Matrix<double, 6, 6> &hessian,
+				PointCloudSource &trans_cloud);
+
+		/** \brief Update interval of possible step lengths for More-Thuente method, \f$ I \f$ in More-Thuente (1994)
+		  * \note Updating Algorithm until some value satifies \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$
+		  * and Modified Updating Algorithm from then on [More, Thuente 1994].
+		  * \param[in,out] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994)
+		  * \param[in,out] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_l) \f$ for Update Algorithm and \f$ \phi(\alpha_l) \f$ for Modified Update Algorithm
+		  * \param[in,out] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_l) \f$ for Update Algorithm and \f$ \phi'(\alpha_l) \f$ for Modified Update Algorithm
+		  * \param[in,out] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994)
+		  * \param[in,out] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_u) \f$ for Update Algorithm and \f$ \phi(\alpha_u) \f$ for Modified Update Algorithm
+		  * \param[in,out] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_u) \f$ for Update Algorithm and \f$ \phi'(\alpha_u) \f$ for Modified Update Algorithm
+		  * \param[in] a_t trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994)
+		  * \param[in] f_t value at trial value, \f$ f_t \f$ in Moore-Thuente (1994), \f$ \psi(\alpha_t) \f$ for Update Algorithm and \f$ \phi(\alpha_t) \f$ for Modified Update Algorithm
+		  * \param[in] g_t derivative at trial value, \f$ g_t \f$ in Moore-Thuente (1994), \f$ \psi'(\alpha_t) \f$ for Update Algorithm and \f$ \phi'(\alpha_t) \f$ for Modified Update Algorithm
+		  * \return if interval converges
+		  */
+		bool
+			updateIntervalMT(double &a_l, double &f_l, double &g_l,
+				double &a_u, double &f_u, double &g_u,
+				double a_t, double f_t, double g_t);
+
+		/** \brief Select new trial value for More-Thuente method.
+		  * \note Trial Value Selection [More, Thuente 1994], \f$ \psi(\alpha_k) \f$ is used for \f$ f_k \f$ and \f$ g_k \f$
+		  * until some value satifies the test \f$ \psi(\alpha_k) \leq 0 \f$ and \f$ \phi'(\alpha_k) \geq 0 \f$
+		  * then \f$ \phi(\alpha_k) \f$ is used from then on.
+		  * \note Interpolation Minimizer equations from Optimization Theory and Methods: Nonlinear Programming By Wenyu Sun, Ya-xiang Yuan (89-100).
+		  * \param[in] a_l first endpoint of interval \f$ I \f$, \f$ \alpha_l \f$ in Moore-Thuente (1994)
+		  * \param[in] f_l value at first endpoint, \f$ f_l \f$ in Moore-Thuente (1994)
+		  * \param[in] g_l derivative at first endpoint, \f$ g_l \f$ in Moore-Thuente (1994)
+		  * \param[in] a_u second endpoint of interval \f$ I \f$, \f$ \alpha_u \f$ in Moore-Thuente (1994)
+		  * \param[in] f_u value at second endpoint, \f$ f_u \f$ in Moore-Thuente (1994)
+		  * \param[in] g_u derivative at second endpoint, \f$ g_u \f$ in Moore-Thuente (1994)
+		  * \param[in] a_t previous trial value, \f$ \alpha_t \f$ in Moore-Thuente (1994)
+		  * \param[in] f_t value at previous trial value, \f$ f_t \f$ in Moore-Thuente (1994)
+		  * \param[in] g_t derivative at previous trial value, \f$ g_t \f$ in Moore-Thuente (1994)
+		  * \return new trial value
+		  */
+		double
+			trialValueSelectionMT(double a_l, double f_l, double g_l,
+				double a_u, double f_u, double g_u,
+				double a_t, double f_t, double g_t);
+
+		/** \brief Auxilary function used to determin endpoints of More-Thuente interval.
+		  * \note \f$ \psi(\alpha) \f$ in Equation 1.6 (Moore, Thuente 1994)
+		  * \param[in] a the step length, \f$ \alpha \f$ in More-Thuente (1994)
+		  * \param[in] f_a function value at step length a, \f$ \phi(\alpha) \f$ in More-Thuente (1994)
+		  * \param[in] f_0 initial function value, \f$ \phi(0) \f$ in Moore-Thuente (1994)
+		  * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994)
+		  * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994]
+		  * \return sufficent decrease value
+		  */
+		inline double
+			auxilaryFunction_PsiMT(double a, double f_a, double f_0, double g_0, double mu = 1.e-4)
+		{
+			return (f_a - f_0 - mu * g_0 * a);
+		}
+
+		/** \brief Auxilary function derivative used to determin endpoints of More-Thuente interval.
+		  * \note \f$ \psi'(\alpha) \f$, derivative of Equation 1.6 (Moore, Thuente 1994)
+		  * \param[in] g_a function gradient at step length a, \f$ \phi'(\alpha) \f$ in More-Thuente (1994)
+		  * \param[in] g_0 initial function gradiant, \f$ \phi'(0) \f$ in More-Thuente (1994)
+		  * \param[in] mu the step length, constant \f$ \mu \f$ in Equation 1.1 [More, Thuente 1994]
+		  * \return sufficent decrease derivative
+		  */
+		inline double
+			auxilaryFunction_dPsiMT(double g_a, double g_0, double mu = 1.e-4)
+		{
+			return (g_a - mu * g_0);
+		}
+
+		/** \brief The voxel grid generated from target cloud containing point means and covariances. */
+		TargetGrid target_cells_;
+
+		//double fitness_epsilon_;
+
+		/** \brief The side length of voxels. */
+		float resolution_;
+
+		/** \brief The maximum step length. */
+		double step_size_;
+
+		/** \brief The ratio of outliers of points w.r.t. a normal distribution, Equation 6.7 [Magnusson 2009]. */
+		double outlier_ratio_;
+
+		/** \brief The normalization constants used fit the point distribution to a normal distribution, Equation 6.8 [Magnusson 2009]. */
+		double gauss_d1_, gauss_d2_, gauss_d3_;
+
+		/** \brief The probability score of the transform applied to the input cloud, Equation 6.9 and 6.10 [Magnusson 2009]. */
+		double trans_probability_;
+
+		/** \brief Precomputed Angular Gradient
+		  *
+		  * The precomputed angular derivatives for the jacobian of a transformation vector, Equation 6.19 [Magnusson 2009].
+		  */
+		Eigen::Vector3d j_ang_a_, j_ang_b_, j_ang_c_, j_ang_d_, j_ang_e_, j_ang_f_, j_ang_g_, j_ang_h_;
+
+		Eigen::Matrix<float, 8, 4> j_ang;
+
+		/** \brief Precomputed Angular Hessian
+		  *
+		  * The precomputed angular derivatives for the hessian of a transformation vector, Equation 6.19 [Magnusson 2009].
+		  */
+		Eigen::Vector3d h_ang_a2_, h_ang_a3_,
+			h_ang_b2_, h_ang_b3_,
+			h_ang_c2_, h_ang_c3_,
+			h_ang_d1_, h_ang_d2_, h_ang_d3_,
+			h_ang_e1_, h_ang_e2_, h_ang_e3_,
+			h_ang_f1_, h_ang_f2_, h_ang_f3_;
+
+		Eigen::Matrix<float, 16, 4> h_ang;
+
+		/** \brief The first order derivative of the transformation of a point w.r.t. the transform vector, \f$ J_E \f$ in Equation 6.18 [Magnusson 2009]. */
+  //      Eigen::Matrix<double, 3, 6> point_gradient_;
+
+		/** \brief The second order derivative of the transformation of a point w.r.t. the transform vector, \f$ H_E \f$ in Equation 6.20 [Magnusson 2009]. */
+  //      Eigen::Matrix<double, 18, 6> point_hessian_;
+
+    int num_threads_;
+
+	public:
+		NeighborSearchMethod search_method;
+
+		EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	};
+
+}
+
+#endif // PCL_REGISTRATION_NDT_H_

scan_matcher_ICP/include/pclomp/ndt_omp_impl.hpp

@@ -0,0 +1,955 @@
+#include "ndt_omp.h"
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-2011, Willow Garage, Inc.
+ *  Copyright (c) 2012-, Open Perception, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * $Id$
+ *
+ */
+
+#ifndef PCL_REGISTRATION_NDT_OMP_IMPL_H_
+#define PCL_REGISTRATION_NDT_OMP_IMPL_H_
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget>
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::NormalDistributionsTransform () 
+  : target_cells_ ()
+  , resolution_ (1.0f)
+  , step_size_ (0.1)
+  , outlier_ratio_ (0.55)
+  , gauss_d1_ ()
+  , gauss_d2_ ()
+  , gauss_d3_ ()
+  , trans_probability_ ()
+  , j_ang_a_ (), j_ang_b_ (), j_ang_c_ (), j_ang_d_ (), j_ang_e_ (), j_ang_f_ (), j_ang_g_ (), j_ang_h_ ()
+  , h_ang_a2_ (), h_ang_a3_ (), h_ang_b2_ (), h_ang_b3_ (), h_ang_c2_ (), h_ang_c3_ (), h_ang_d1_ (), h_ang_d2_ ()
+  , h_ang_d3_ (), h_ang_e1_ (), h_ang_e2_ (), h_ang_e3_ (), h_ang_f1_ (), h_ang_f2_ (), h_ang_f3_ ()
+{
+  reg_name_ = "NormalDistributionsTransform";
+
+  double gauss_c1, gauss_c2;
+
+  // Initializes the guassian fitting parameters (eq. 6.8) [Magnusson 2009]
+  gauss_c1 = 10.0 * (1 - outlier_ratio_);
+  gauss_c2 = outlier_ratio_ / pow (resolution_, 3);
+  gauss_d3_ = -log (gauss_c2);
+  gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3_;
+  gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3_) / gauss_d1_);
+
+  transformation_epsilon_ = 0.1;
+  max_iterations_ = 35;
+
+  search_method = DIRECT7;
+  num_threads_ = omp_get_max_threads();
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
+{
+  nr_iterations_ = 0;
+  converged_ = false;
+
+  double gauss_c1, gauss_c2;
+
+  // Initializes the guassian fitting parameters (eq. 6.8) [Magnusson 2009]
+  gauss_c1 = 10 * (1 - outlier_ratio_);
+  gauss_c2 = outlier_ratio_ / pow (resolution_, 3);
+  gauss_d3_ = -log (gauss_c2);
+  gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3_;
+  gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3_) / gauss_d1_);
+
+  if (guess != Eigen::Matrix4f::Identity ())
+  {
+    // Initialise final transformation to the guessed one
+    final_transformation_ = guess;
+    // Apply guessed transformation prior to search for neighbours
+    transformPointCloud (output, output, guess);
+  }
+
+  Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
+  eig_transformation.matrix () = final_transformation_;
+
+  // Convert initial guess matrix to 6 element transformation vector
+  Eigen::Matrix<double, 6, 1> p, delta_p, score_gradient;
+  Eigen::Vector3f init_translation = eig_transformation.translation ();
+  Eigen::Vector3f init_rotation = eig_transformation.rotation ().eulerAngles (0, 1, 2);
+  p << init_translation (0), init_translation (1), init_translation (2),
+  init_rotation (0), init_rotation (1), init_rotation (2);
+
+  Eigen::Matrix<double, 6, 6> hessian;
+
+  double score = 0;
+  double delta_p_norm;
+
+  // Calculate derivates of initial transform vector, subsequent derivative calculations are done in the step length determination.
+  score = computeDerivatives (score_gradient, hessian, output, p);
+
+  while (!converged_)
+  {
+    // Store previous transformation
+    previous_transformation_ = transformation_;
+
+    // Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson 2009]
+    Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6> > sv (hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
+    // Negative for maximization as opposed to minimization
+    delta_p = sv.solve (-score_gradient);
+
+    //Calculate step length with guarnteed sufficient decrease [More, Thuente 1994]
+    delta_p_norm = delta_p.norm ();
+
+    if (delta_p_norm == 0 || delta_p_norm != delta_p_norm)
+    {
+      trans_probability_ = score / static_cast<double> (input_->points.size ());
+      converged_ = delta_p_norm == delta_p_norm;
+      return;
+    }
+
+    delta_p.normalize ();
+    delta_p_norm = computeStepLengthMT (p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, output);
+    delta_p *= delta_p_norm;
+
+
+    transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (delta_p (0)), static_cast<float> (delta_p (1)), static_cast<float> (delta_p (2))) *
+                       Eigen::AngleAxis<float> (static_cast<float> (delta_p (3)), Eigen::Vector3f::UnitX ()) *
+                       Eigen::AngleAxis<float> (static_cast<float> (delta_p (4)), Eigen::Vector3f::UnitY ()) *
+                       Eigen::AngleAxis<float> (static_cast<float> (delta_p (5)), Eigen::Vector3f::UnitZ ())).matrix ();
+
+
+    p = p + delta_p;
+
+    // Update Visualizer (untested)
+    if (update_visualizer_ != 0)
+      update_visualizer_ (output, std::vector<int>(), *target_, std::vector<int>() );
+
+    if (nr_iterations_ > max_iterations_ ||
+        (nr_iterations_ && (std::fabs (delta_p_norm) < transformation_epsilon_)))
+    {
+      converged_ = true;
+    }
+
+    nr_iterations_++;
+
+  }
+
+  // Store transformation probability.  The realtive differences within each scan registration are accurate
+  // but the normalization constants need to be modified for it to be globally accurate
+  trans_probability_ = score / static_cast<double> (input_->points.size ());
+}
+
+#ifndef _OPENMP
+int omp_get_max_threads() { return 1; }
+int omp_get_thread_num() { return 0; }
+#endif
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> double
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient,
+	Eigen::Matrix<double, 6, 6> &hessian,
+	PointCloudSource &trans_cloud,
+	Eigen::Matrix<double, 6, 1> &p,
+	bool compute_hessian)
+{
+	score_gradient.setZero();
+	hessian.setZero();
+	double score = 0;
+
+  std::vector<double> scores(num_threads_);
+  std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1>>> score_gradients(num_threads_);
+  std::vector<Eigen::Matrix<double, 6, 6>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 6>>> hessians(num_threads_);
+  for (int i = 0; i < num_threads_; i++) {
+		scores[i] = 0;
+		score_gradients[i].setZero();
+		hessians[i].setZero();
+	}
+
+	// Precompute Angular Derivatives (eq. 6.19 and 6.21)[Magnusson 2009]
+	computeAngleDerivatives(p);
+
+  std::vector<std::vector<TargetGridLeafConstPtr>> neighborhoods(num_threads_);
+  std::vector<std::vector<float>> distancess(num_threads_);
+
+	// Update gradient and hessian for each point, line 17 in Algorithm 2 [Magnusson 2009]
+#pragma omp parallel for num_threads(num_threads_) schedule(guided, 8)
+	for (int idx = 0; idx < input_->points.size(); idx++)
+	{
+		int thread_n = omp_get_thread_num();
+
+		// Original Point and Transformed Point
+		PointSource x_pt, x_trans_pt;
+		// Original Point and Transformed Point (for math)
+		Eigen::Vector3d x, x_trans;
+		// Occupied Voxel
+		TargetGridLeafConstPtr cell;
+		// Inverse Covariance of Occupied Voxel
+		Eigen::Matrix3d c_inv;
+
+		// Initialize Point Gradient and Hessian
+		Eigen::Matrix<float, 4, 6> point_gradient_;
+		Eigen::Matrix<float, 24, 6> point_hessian_;
+		point_gradient_.setZero();
+		point_gradient_.block<3, 3>(0, 0).setIdentity();
+		point_hessian_.setZero();
+
+		x_trans_pt = trans_cloud.points[idx];
+
+		auto& neighborhood = neighborhoods[thread_n];
+		auto& distances = distancess[thread_n];
+
+		// Find nieghbors (Radius search has been experimentally faster than direct neighbor checking.
+		switch (search_method) {
+		case KDTREE:
+			target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
+			break;
+		case DIRECT26:
+			target_cells_.getNeighborhoodAtPoint(x_trans_pt, neighborhood);
+			break;
+		default:
+		case DIRECT7:
+			target_cells_.getNeighborhoodAtPoint7(x_trans_pt, neighborhood);
+			break;
+		case DIRECT1:
+			target_cells_.getNeighborhoodAtPoint1(x_trans_pt, neighborhood);
+			break;
+		}
+
+		double score_pt = 0;
+		Eigen::Matrix<double, 6, 1> score_gradient_pt = Eigen::Matrix<double, 6, 1>::Zero();
+		Eigen::Matrix<double, 6, 6> hessian_pt = Eigen::Matrix<double, 6, 6>::Zero();
+
+		for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin(); neighborhood_it != neighborhood.end(); neighborhood_it++)
+		{
+			cell = *neighborhood_it;
+			x_pt = input_->points[idx];
+			x = Eigen::Vector3d(x_pt.x, x_pt.y, x_pt.z);
+
+			x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
+
+			// Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
+			x_trans -= cell->getMean();
+			// Uses precomputed covariance for speed.
+			c_inv = cell->getInverseCov();
+
+			// Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009]
+			computePointDerivatives(x, point_gradient_, point_hessian_);
+			// Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009]
+			score_pt += updateDerivatives(score_gradient_pt, hessian_pt, point_gradient_, point_hessian_, x_trans, c_inv, compute_hessian);
+		}
+
+		scores[thread_n] += score_pt;
+		score_gradients[thread_n].noalias() += score_gradient_pt;
+		hessians[thread_n].noalias() += hessian_pt;
+	}
+
+  for (int i = 0; i < num_threads_; i++) {
+		score += scores[i];
+		score_gradient += score_gradients[i];
+		hessian += hessians[i];
+	}
+
+	return (score);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeAngleDerivatives(Eigen::Matrix<double, 6, 1> &p, bool compute_hessian)
+{
+	// Simplified math for near 0 angles
+	double cx, cy, cz, sx, sy, sz;
+	if (fabs(p(3)) < 10e-5)
+	{
+		//p(3) = 0;
+		cx = 1.0;
+		sx = 0.0;
+	}
+	else
+	{
+		cx = cos(p(3));
+		sx = sin(p(3));
+	}
+	if (fabs(p(4)) < 10e-5)
+	{
+		//p(4) = 0;
+		cy = 1.0;
+		sy = 0.0;
+	}
+	else
+	{
+		cy = cos(p(4));
+		sy = sin(p(4));
+	}
+
+	if (fabs(p(5)) < 10e-5)
+	{
+		//p(5) = 0;
+		cz = 1.0;
+		sz = 0.0;
+	}
+	else
+	{
+		cz = cos(p(5));
+		sz = sin(p(5));
+	}
+
+	// Precomputed angular gradiant components. Letters correspond to Equation 6.19 [Magnusson 2009]
+	j_ang_a_ << (-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy);
+	j_ang_b_ << (cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy);
+	j_ang_c_ << (-sy * cz), sy * sz, cy;
+	j_ang_d_ << sx * cy * cz, (-sx * cy * sz), sx * sy;
+	j_ang_e_ << (-cx * cy * cz), cx * cy * sz, (-cx * sy);
+	j_ang_f_ << (-cy * sz), (-cy * cz), 0;
+	j_ang_g_ << (cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0;
+	j_ang_h_ << (sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0;
+
+	j_ang.setZero();
+	j_ang.row(0).noalias() = Eigen::Vector4f((-sx * sz + cx * sy * cz), (-sx * cz - cx * sy * sz), (-cx * cy), 0.0f);
+	j_ang.row(1).noalias() = Eigen::Vector4f((cx * sz + sx * sy * cz), (cx * cz - sx * sy * sz), (-sx * cy), 0.0f);
+	j_ang.row(2).noalias() = Eigen::Vector4f((-sy * cz), sy * sz, cy, 0.0f);
+	j_ang.row(3).noalias() = Eigen::Vector4f(sx * cy * cz, (-sx * cy * sz), sx * sy, 0.0f);
+	j_ang.row(4).noalias() = Eigen::Vector4f((-cx * cy * cz), cx * cy * sz, (-cx * sy), 0.0f);
+	j_ang.row(5).noalias() = Eigen::Vector4f((-cy * sz), (-cy * cz), 0, 0.0f);
+	j_ang.row(6).noalias() = Eigen::Vector4f((cx * cz - sx * sy * sz), (-cx * sz - sx * sy * cz), 0, 0.0f);
+	j_ang.row(7).noalias() = Eigen::Vector4f((sx * cz + cx * sy * sz), (cx * sy * cz - sx * sz), 0, 0.0f);
+
+	if (compute_hessian)
+	{
+		// Precomputed angular hessian components. Letters correspond to Equation 6.21 and numbers correspond to row index [Magnusson 2009]
+		h_ang_a2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy;
+		h_ang_a3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy);
+
+		h_ang_b2_ << (cx * cy * cz), (-cx * cy * sz), (cx * sy);
+		h_ang_b3_ << (sx * cy * cz), (-sx * cy * sz), (sx * sy);
+
+		h_ang_c2_ << (-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0;
+		h_ang_c3_ << (cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0;
+
+		h_ang_d1_ << (-cy * cz), (cy * sz), (sy);
+		h_ang_d2_ << (-sx * sy * cz), (sx * sy * sz), (sx * cy);
+		h_ang_d3_ << (cx * sy * cz), (-cx * sy * sz), (-cx * cy);
+
+		h_ang_e1_ << (sy * sz), (sy * cz), 0;
+		h_ang_e2_ << (-sx * cy * sz), (-sx * cy * cz), 0;
+		h_ang_e3_ << (cx * cy * sz), (cx * cy * cz), 0;
+
+		h_ang_f1_ << (-cy * cz), (cy * sz), 0;
+		h_ang_f2_ << (-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0;
+		h_ang_f3_ << (-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0;
+
+		h_ang.setZero();
+		h_ang.row(0).noalias() = Eigen::Vector4f((-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), sx * cy, 0.0f);		// a2
+		h_ang.row(1).noalias() = Eigen::Vector4f((-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), (-cx * cy), 0.0f);	// a3
+
+		h_ang.row(2).noalias() = Eigen::Vector4f((cx * cy * cz), (-cx * cy * sz), (cx * sy), 0.0f);							// b2
+		h_ang.row(3).noalias() = Eigen::Vector4f((sx * cy * cz), (-sx * cy * sz), (sx * sy), 0.0f);							// b3
+
+		h_ang.row(4).noalias() = Eigen::Vector4f((-sx * cz - cx * sy * sz), (sx * sz - cx * sy * cz), 0, 0.0f);				// c2
+		h_ang.row(5).noalias() = Eigen::Vector4f((cx * cz - sx * sy * sz), (-sx * sy * cz - cx * sz), 0, 0.0f);				// c3
+
+		h_ang.row(6).noalias() = Eigen::Vector4f((-cy * cz), (cy * sz), (sy), 0.0f);										// d1
+		h_ang.row(7).noalias() = Eigen::Vector4f((-sx * sy * cz), (sx * sy * sz), (sx * cy), 0.0f);							// d2
+		h_ang.row(8).noalias() = Eigen::Vector4f((cx * sy * cz), (-cx * sy * sz), (-cx * cy), 0.0f);						// d3
+
+		h_ang.row(9).noalias() = Eigen::Vector4f((sy * sz), (sy * cz), 0, 0.0f);											// e1
+		h_ang.row(10).noalias() = Eigen::Vector4f ((-sx * cy * sz), (-sx * cy * cz), 0, 0.0f);								// e2
+		h_ang.row(11).noalias() = Eigen::Vector4f ((cx * cy * sz), (cx * cy * cz), 0, 0.0f);								// e3
+
+		h_ang.row(12).noalias() = Eigen::Vector4f ((-cy * cz), (cy * sz), 0, 0.0f);											// f1
+		h_ang.row(13).noalias() = Eigen::Vector4f ((-cx * sz - sx * sy * cz), (-cx * cz + sx * sy * sz), 0, 0.0f);			// f2
+		h_ang.row(14).noalias() = Eigen::Vector4f ((-sx * sz + cx * sy * cz), (-cx * sy * sz - sx * cz), 0, 0.0f);			// f3
+	}
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<float, 4, 6>& point_gradient_, Eigen::Matrix<float, 24, 6>& point_hessian_, bool compute_hessian) const
+{
+	Eigen::Vector4f x4(x[0], x[1], x[2], 0.0f);
+
+	// Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p.
+	// Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson 2009]
+	Eigen::Matrix<float, 8, 1> x_j_ang = j_ang * x4;
+
+	point_gradient_(1, 3) = x_j_ang[0];
+	point_gradient_(2, 3) = x_j_ang[1];
+	point_gradient_(0, 4) = x_j_ang[2];
+	point_gradient_(1, 4) = x_j_ang[3];
+	point_gradient_(2, 4) = x_j_ang[4];
+	point_gradient_(0, 5) = x_j_ang[5];
+	point_gradient_(1, 5) = x_j_ang[6];
+	point_gradient_(2, 5) = x_j_ang[7];
+
+	if (compute_hessian)
+	{
+		Eigen::Matrix<float, 16, 1> x_h_ang = h_ang * x4;
+
+		// Vectors from Equation 6.21 [Magnusson 2009]
+		Eigen::Vector4f a (0, x_h_ang[0], x_h_ang[1], 0.0f);
+		Eigen::Vector4f b (0, x_h_ang[2], x_h_ang[3], 0.0f);
+		Eigen::Vector4f c (0, x_h_ang[4], x_h_ang[5], 0.0f);
+		Eigen::Vector4f d (x_h_ang[6], x_h_ang[7], x_h_ang[8], 0.0f);
+		Eigen::Vector4f e (x_h_ang[9], x_h_ang[10], x_h_ang[11], 0.0f);
+		Eigen::Vector4f f (x_h_ang[12], x_h_ang[13], x_h_ang[14], 0.0f);
+
+		// Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p.
+		// Derivative w.r.t. ith and jth elements of transform vector corresponds to the 3x1 block matrix starting at (3i,j), Equation 6.20 and 6.21 [Magnusson 2009]
+		point_hessian_.block<4, 1>((9/3)*4, 3) = a;
+		point_hessian_.block<4, 1>((12/3)*4, 3) = b;
+		point_hessian_.block<4, 1>((15/3)*4, 3) = c;
+		point_hessian_.block<4, 1>((9/3)*4, 4) = b;
+		point_hessian_.block<4, 1>((12/3)*4, 4) = d;
+		point_hessian_.block<4, 1>((15/3)*4, 4) = e;
+		point_hessian_.block<4, 1>((9/3)*4, 5) = c;
+		point_hessian_.block<4, 1>((12/3)*4, 5) = e;
+		point_hessian_.block<4, 1>((15/3)*4, 5) = f;
+	}
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computePointDerivatives(Eigen::Vector3d &x, Eigen::Matrix<double, 3, 6>& point_gradient_, Eigen::Matrix<double, 18, 6>& point_hessian_, bool compute_hessian) const
+{
+	// Calculate first derivative of Transformation Equation 6.17 w.r.t. transform vector p.
+	// Derivative w.r.t. ith element of transform vector corresponds to column i, Equation 6.18 and 6.19 [Magnusson 2009]
+	point_gradient_(1, 3) = x.dot(j_ang_a_);
+	point_gradient_(2, 3) = x.dot(j_ang_b_);
+	point_gradient_(0, 4) = x.dot(j_ang_c_);
+	point_gradient_(1, 4) = x.dot(j_ang_d_);
+	point_gradient_(2, 4) = x.dot(j_ang_e_);
+	point_gradient_(0, 5) = x.dot(j_ang_f_);
+	point_gradient_(1, 5) = x.dot(j_ang_g_);
+	point_gradient_(2, 5) = x.dot(j_ang_h_);
+
+	if (compute_hessian)
+	{
+		// Vectors from Equation 6.21 [Magnusson 2009]
+		Eigen::Vector3d a, b, c, d, e, f;
+
+		a << 0, x.dot(h_ang_a2_), x.dot(h_ang_a3_);
+		b << 0, x.dot(h_ang_b2_), x.dot(h_ang_b3_);
+		c << 0, x.dot(h_ang_c2_), x.dot(h_ang_c3_);
+		d << x.dot(h_ang_d1_), x.dot(h_ang_d2_), x.dot(h_ang_d3_);
+		e << x.dot(h_ang_e1_), x.dot(h_ang_e2_), x.dot(h_ang_e3_);
+		f << x.dot(h_ang_f1_), x.dot(h_ang_f2_), x.dot(h_ang_f3_);
+
+		// Calculate second derivative of Transformation Equation 6.17 w.r.t. transform vector p.
+		// Derivative w.r.t. ith and jth elements of transform vector corresponds to the 3x1 block matrix starting at (3i,j), Equation 6.20 and 6.21 [Magnusson 2009]
+		point_hessian_.block<3, 1>(9, 3) = a;
+		point_hessian_.block<3, 1>(12, 3) = b;
+		point_hessian_.block<3, 1>(15, 3) = c;
+		point_hessian_.block<3, 1>(9, 4) = b;
+		point_hessian_.block<3, 1>(12, 4) = d;
+		point_hessian_.block<3, 1>(15, 4) = e;
+		point_hessian_.block<3, 1>(9, 5) = c;
+		point_hessian_.block<3, 1>(12, 5) = e;
+		point_hessian_.block<3, 1>(15, 5) = f;
+	}
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> double
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::updateDerivatives(Eigen::Matrix<double, 6, 1> &score_gradient,
+	Eigen::Matrix<double, 6, 6> &hessian,
+	const Eigen::Matrix<float, 4, 6> &point_gradient4,
+	const Eigen::Matrix<float, 24, 6> &point_hessian_,
+	const Eigen::Vector3d &x_trans, const Eigen::Matrix3d &c_inv,
+	bool compute_hessian) const
+{
+	Eigen::Matrix<float, 1, 4> x_trans4( x_trans[0], x_trans[1], x_trans[2], 0.0f );
+	Eigen::Matrix4f c_inv4 = Eigen::Matrix4f::Zero();
+	c_inv4.topLeftCorner(3, 3) = c_inv.cast<float>();
+
+	float gauss_d2 = gauss_d2_;
+
+	// e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
+	float e_x_cov_x = exp(-gauss_d2 * x_trans4.dot(x_trans4 * c_inv4) * 0.5f);
+	// Calculate probability of transtormed points existance, Equation 6.9 [Magnusson 2009]
+	float score_inc = -gauss_d1_ * e_x_cov_x;
+
+	e_x_cov_x = gauss_d2 * e_x_cov_x;
+
+	// Error checking for invalid values.
+	if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x)
+		return (0);
+
+	// Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
+	e_x_cov_x *= gauss_d1_;
+
+	Eigen::Matrix<float, 4, 6> c_inv4_x_point_gradient4 = c_inv4 * point_gradient4;
+	Eigen::Matrix<float, 6, 1> x_trans4_dot_c_inv4_x_point_gradient4 = x_trans4 * c_inv4_x_point_gradient4;
+
+	score_gradient.noalias() += (e_x_cov_x * x_trans4_dot_c_inv4_x_point_gradient4).cast<double>();
+
+	if (compute_hessian) {
+		Eigen::Matrix<float, 1, 4> x_trans4_x_c_inv4 = x_trans4 * c_inv4;
+		Eigen::Matrix<float, 6, 6> point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i = point_gradient4.transpose() * c_inv4_x_point_gradient4;
+		Eigen::Matrix<float, 6, 1> x_trans4_dot_c_inv4_x_ext_point_hessian_4ij;
+
+		for (int i = 0; i < 6; i++) {
+			// Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
+			// Update gradient, Equation 6.12 [Magnusson 2009]
+			x_trans4_dot_c_inv4_x_ext_point_hessian_4ij.noalias() = x_trans4_x_c_inv4 * point_hessian_.block<4, 6>(i * 4, 0);
+
+			for (int j = 0; j < hessian.cols(); j++) {
+				// Update hessian, Equation 6.13 [Magnusson 2009]
+				hessian(i, j) += e_x_cov_x * (-gauss_d2 * x_trans4_dot_c_inv4_x_point_gradient4(i) * x_trans4_dot_c_inv4_x_point_gradient4(j) +
+					x_trans4_dot_c_inv4_x_ext_point_hessian_4ij(j) +
+					point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i(j, i));
+			}
+		}
+	}
+
+	return (score_inc);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeHessian (Eigen::Matrix<double, 6, 6> &hessian,
+                                                                             PointCloudSource &trans_cloud, Eigen::Matrix<double, 6, 1> &)
+{
+  // Original Point and Transformed Point
+  PointSource x_pt, x_trans_pt;
+  // Original Point and Transformed Point (for math)
+  Eigen::Vector3d x, x_trans;
+  // Occupied Voxel
+  TargetGridLeafConstPtr cell;
+  // Inverse Covariance of Occupied Voxel
+  Eigen::Matrix3d c_inv;
+
+  // Initialize Point Gradient and Hessian
+  Eigen::Matrix<double, 3, 6> point_gradient_;
+  Eigen::Matrix<double, 18, 6> point_hessian_;
+  point_gradient_.setZero();
+  point_gradient_.block<3, 3>(0, 0).setIdentity();
+  point_hessian_.setZero();
+
+  hessian.setZero ();
+
+  // Precompute Angular Derivatives unessisary because only used after regular derivative calculation
+
+  // Update hessian for each point, line 17 in Algorithm 2 [Magnusson 2009]
+  for (size_t idx = 0; idx < input_->points.size (); idx++)
+  {
+    x_trans_pt = trans_cloud.points[idx];
+
+    // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking.
+    std::vector<TargetGridLeafConstPtr> neighborhood;
+    std::vector<float> distances;
+    target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances);
+
+    for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++)
+    {
+      cell = *neighborhood_it;
+
+      {
+        x_pt = input_->points[idx];
+        x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z);
+
+        x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
+
+        // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
+        x_trans -= cell->getMean ();
+        // Uses precomputed covariance for speed.
+        c_inv = cell->getInverseCov ();
+
+        // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009]
+        computePointDerivatives (x, point_gradient_, point_hessian_);
+        // Update hessian, lines 21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009]
+        updateHessian (hessian, point_gradient_, point_hessian_, x_trans, c_inv);
+      }
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> void
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::updateHessian (Eigen::Matrix<double, 6, 6> &hessian, 
+	const Eigen::Matrix<double, 3, 6> &point_gradient_, 
+	const Eigen::Matrix<double, 18, 6> &point_hessian_, 
+	const Eigen::Vector3d &x_trans, 
+	const Eigen::Matrix3d &c_inv) const
+{
+  Eigen::Vector3d cov_dxd_pi;
+  // e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
+  double e_x_cov_x = gauss_d2_ * exp (-gauss_d2_ * x_trans.dot (c_inv * x_trans) / 2);
+
+  // Error checking for invalid values.
+  if (e_x_cov_x > 1 || e_x_cov_x < 0 || e_x_cov_x != e_x_cov_x)
+    return;
+
+  // Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
+  e_x_cov_x *= gauss_d1_;
+
+  for (int i = 0; i < 6; i++)
+  {
+    // Sigma_k^-1 d(T(x,p))/dpi, Reusable portion of Equation 6.12 and 6.13 [Magnusson 2009]
+    cov_dxd_pi = c_inv * point_gradient_.col (i);
+
+    for (int j = 0; j < hessian.cols (); j++)
+    {
+      // Update hessian, Equation 6.13 [Magnusson 2009]
+      hessian (i, j) += e_x_cov_x * (-gauss_d2_ * x_trans.dot (cov_dxd_pi) * x_trans.dot (c_inv * point_gradient_.col (j)) +
+                                  x_trans.dot (c_inv * point_hessian_.block<3, 1>(3 * i, j)) +
+                                  point_gradient_.col (j).dot (cov_dxd_pi) );
+    }
+  }
+
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> bool
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::updateIntervalMT (double &a_l, double &f_l, double &g_l,
+                                                                               double &a_u, double &f_u, double &g_u,
+                                                                               double a_t, double f_t, double g_t)
+{
+  // Case U1 in Update Algorithm and Case a in Modified Update Algorithm [More, Thuente 1994]
+  if (f_t > f_l)
+  {
+    a_u = a_t;
+    f_u = f_t;
+    g_u = g_t;
+    return (false);
+  }
+  // Case U2 in Update Algorithm and Case b in Modified Update Algorithm [More, Thuente 1994]
+  else
+  if (g_t * (a_l - a_t) > 0)
+  {
+    a_l = a_t;
+    f_l = f_t;
+    g_l = g_t;
+    return (false);
+  }
+  // Case U3 in Update Algorithm and Case c in Modified Update Algorithm [More, Thuente 1994]
+  else
+  if (g_t * (a_l - a_t) < 0)
+  {
+    a_u = a_l;
+    f_u = f_l;
+    g_u = g_l;
+
+    a_l = a_t;
+    f_l = f_t;
+    g_l = g_t;
+    return (false);
+  }
+  // Interval Converged
+  else
+    return (true);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> double
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::trialValueSelectionMT (double a_l, double f_l, double g_l,
+                                                                                    double a_u, double f_u, double g_u,
+                                                                                    double a_t, double f_t, double g_t)
+{
+  // Case 1 in Trial Value Selection [More, Thuente 1994]
+  if (f_t > f_l)
+  {
+    // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
+    // Equation 2.4.52 [Sun, Yuan 2006]
+    double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
+    double w = std::sqrt (z * z - g_t * g_l);
+    // Equation 2.4.56 [Sun, Yuan 2006]
+    double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
+
+    // Calculate the minimizer of the quadratic that interpolates f_l, f_t and g_l
+    // Equation 2.4.2 [Sun, Yuan 2006]
+    double a_q = a_l - 0.5 * (a_l - a_t) * g_l / (g_l - (f_l - f_t) / (a_l - a_t));
+
+    if (std::fabs (a_c - a_l) < std::fabs (a_q - a_l))
+      return (a_c);
+    else
+      return (0.5 * (a_q + a_c));
+  }
+  // Case 2 in Trial Value Selection [More, Thuente 1994]
+  else
+  if (g_t * g_l < 0)
+  {
+    // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
+    // Equation 2.4.52 [Sun, Yuan 2006]
+    double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
+    double w = std::sqrt (z * z - g_t * g_l);
+    // Equation 2.4.56 [Sun, Yuan 2006]
+    double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
+
+    // Calculate the minimizer of the quadratic that interpolates f_l, g_l and g_t
+    // Equation 2.4.5 [Sun, Yuan 2006]
+    double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
+
+    if (std::fabs (a_c - a_t) >= std::fabs (a_s - a_t))
+      return (a_c);
+    else
+      return (a_s);
+  }
+  // Case 3 in Trial Value Selection [More, Thuente 1994]
+  else
+  if (std::fabs (g_t) <= std::fabs (g_l))
+  {
+    // Calculate the minimizer of the cubic that interpolates f_l, f_t, g_l and g_t
+    // Equation 2.4.52 [Sun, Yuan 2006]
+    double z = 3 * (f_t - f_l) / (a_t - a_l) - g_t - g_l;
+    double w = std::sqrt (z * z - g_t * g_l);
+    double a_c = a_l + (a_t - a_l) * (w - g_l - z) / (g_t - g_l + 2 * w);
+
+    // Calculate the minimizer of the quadratic that interpolates g_l and g_t
+    // Equation 2.4.5 [Sun, Yuan 2006]
+    double a_s = a_l - (a_l - a_t) / (g_l - g_t) * g_l;
+
+    double a_t_next;
+
+    if (std::fabs (a_c - a_t) < std::fabs (a_s - a_t))
+      a_t_next = a_c;
+    else
+      a_t_next = a_s;
+
+    if (a_t > a_l)
+      return (std::min (a_t + 0.66 * (a_u - a_t), a_t_next));
+    else
+      return (std::max (a_t + 0.66 * (a_u - a_t), a_t_next));
+  }
+  // Case 4 in Trial Value Selection [More, Thuente 1994]
+  else
+  {
+    // Calculate the minimizer of the cubic that interpolates f_u, f_t, g_u and g_t
+    // Equation 2.4.52 [Sun, Yuan 2006]
+    double z = 3 * (f_t - f_u) / (a_t - a_u) - g_t - g_u;
+    double w = std::sqrt (z * z - g_t * g_u);
+    // Equation 2.4.56 [Sun, Yuan 2006]
+    return (a_u + (a_t - a_u) * (w - g_u - z) / (g_t - g_u + 2 * w));
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointSource, typename PointTarget> double
+pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeStepLengthMT (const Eigen::Matrix<double, 6, 1> &x, Eigen::Matrix<double, 6, 1> &step_dir, double step_init, double step_max,
+                                                                                  double step_min, double &score, Eigen::Matrix<double, 6, 1> &score_gradient, Eigen::Matrix<double, 6, 6> &hessian,
+                                                                                  PointCloudSource &trans_cloud)
+{
+  // Set the value of phi(0), Equation 1.3 [More, Thuente 1994]
+  double phi_0 = -score;
+  // Set the value of phi'(0), Equation 1.3 [More, Thuente 1994]
+  double d_phi_0 = -(score_gradient.dot (step_dir));
+
+  Eigen::Matrix<double, 6, 1>  x_t;
+
+  if (d_phi_0 >= 0)
+  {
+    // Not a decent direction
+    if (d_phi_0 == 0)
+      return 0;
+    else
+    {
+      // Reverse step direction and calculate optimal step.
+      d_phi_0 *= -1;
+      step_dir *= -1;
+
+    }
+  }
+
+  // The Search Algorithm for T(mu) [More, Thuente 1994]
+
+  int max_step_iterations = 10;
+  int step_iterations = 0;
+
+  // Sufficient decreace constant, Equation 1.1 [More, Thuete 1994]
+  double mu = 1.e-4;
+  // Curvature condition constant, Equation 1.2 [More, Thuete 1994]
+  double nu = 0.9;
+
+  // Initial endpoints of Interval I,
+  double a_l = 0, a_u = 0;
+
+  // Auxiliary function psi is used until I is determined ot be a closed interval, Equation 2.1 [More, Thuente 1994]
+  double f_l = auxilaryFunction_PsiMT (a_l, phi_0, phi_0, d_phi_0, mu);
+  double g_l = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
+
+  double f_u = auxilaryFunction_PsiMT (a_u, phi_0, phi_0, d_phi_0, mu);
+  double g_u = auxilaryFunction_dPsiMT (d_phi_0, d_phi_0, mu);
+
+  // Check used to allow More-Thuente step length calculation to be skipped by making step_min == step_max
+  bool interval_converged = (step_max - step_min) > 0, open_interval = true;
+
+  double a_t = step_init;
+  a_t = std::min (a_t, step_max);
+  a_t = std::max (a_t, step_min);
+
+  x_t = x + step_dir * a_t;
+
+  final_transformation_ = (Eigen::Translation<float, 3>(static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
+                           Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
+                           Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
+                           Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
+
+  // New transformed point cloud
+  transformPointCloud (*input_, trans_cloud, final_transformation_);
+
+  // Updates score, gradient and hessian.  Hessian calculation is unessisary but testing showed that most step calculations use the
+  // initial step suggestion and recalculation the reusable portions of the hessian would intail more computation time.
+  score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, true);
+
+  // Calculate phi(alpha_t)
+  double phi_t = -score;
+  // Calculate phi'(alpha_t)
+  double d_phi_t = -(score_gradient.dot (step_dir));
+
+  // Calculate psi(alpha_t)
+  double psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
+  // Calculate psi'(alpha_t)
+  double d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);
+
+  // Iterate until max number of iterations, interval convergance or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994]
+  while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/))
+  {
+    // Use auxilary function if interval I is not closed
+    if (open_interval)
+    {
+      a_t = trialValueSelectionMT (a_l, f_l, g_l,
+                                   a_u, f_u, g_u,
+                                   a_t, psi_t, d_psi_t);
+    }
+    else
+    {
+      a_t = trialValueSelectionMT (a_l, f_l, g_l,
+                                   a_u, f_u, g_u,
+                                   a_t, phi_t, d_phi_t);
+    }
+
+    a_t = std::min (a_t, step_max);
+    a_t = std::max (a_t, step_min);
+
+    x_t = x + step_dir * a_t;
+
+    final_transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
+                             Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
+                             Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
+                             Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();
+
+    // New transformed point cloud
+    // Done on final cloud to prevent wasted computation
+    transformPointCloud (*input_, trans_cloud, final_transformation_);
+
+    // Updates score, gradient. Values stored to prevent wasted computation.
+    score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, false);
+
+    // Calculate phi(alpha_t+)
+    phi_t = -score;
+    // Calculate phi'(alpha_t+)
+    d_phi_t = -(score_gradient.dot (step_dir));
+
+    // Calculate psi(alpha_t+)
+    psi_t = auxilaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
+    // Calculate psi'(alpha_t+)
+    d_psi_t = auxilaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);
+
+    // Check if I is now a closed interval
+    if (open_interval && (psi_t <= 0 && d_psi_t >= 0))
+    {
+      open_interval = false;
+
+      // Converts f_l and g_l from psi to phi
+      f_l = f_l + phi_0 - mu * d_phi_0 * a_l;
+      g_l = g_l + mu * d_phi_0;
+
+      // Converts f_u and g_u from psi to phi
+      f_u = f_u + phi_0 - mu * d_phi_0 * a_u;
+      g_u = g_u + mu * d_phi_0;
+    }
+
+    if (open_interval)
+    {
+      // Update interval end points using Updating Algorithm [More, Thuente 1994]
+      interval_converged = updateIntervalMT (a_l, f_l, g_l,
+                                             a_u, f_u, g_u,
+                                             a_t, psi_t, d_psi_t);
+    }
+    else
+    {
+      // Update interval end points using Modified Updating Algorithm [More, Thuente 1994]
+      interval_converged = updateIntervalMT (a_l, f_l, g_l,
+                                             a_u, f_u, g_u,
+                                             a_t, phi_t, d_phi_t);
+    }
+
+    step_iterations++;
+  }
+
+  // If inner loop was run then hessian needs to be calculated.
+  // Hessian is unnessisary for step length determination but gradients are required
+  // so derivative and transform data is stored for the next iteration.
+  if (step_iterations)
+    computeHessian (hessian, trans_cloud, x_t);
+
+  return (a_t);
+}
+
+
+template<typename PointSource, typename PointTarget>
+double pclomp::NormalDistributionsTransform<PointSource, PointTarget>::calculateScore(const PointCloudSource & trans_cloud) const
+{
+	double score = 0;
+
+	for (int idx = 0; idx < trans_cloud.points.size(); idx++)
+	{
+		PointSource x_trans_pt = trans_cloud.points[idx];
+
+		// Find nieghbors (Radius search has been experimentally faster than direct neighbor checking.
+		std::vector<TargetGridLeafConstPtr> neighborhood;
+		std::vector<float> distances;
+		target_cells_.radiusSearch(x_trans_pt, resolution_, neighborhood, distances);
+
+		for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin(); neighborhood_it != neighborhood.end(); neighborhood_it++)
+		{
+			TargetGridLeafConstPtr cell = *neighborhood_it;
+
+			Eigen::Vector3d x_trans = Eigen::Vector3d(x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);
+
+			// Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
+			x_trans -= cell->getMean();
+			// Uses precomputed covariance for speed.
+			Eigen::Matrix3d c_inv = cell->getInverseCov();
+
+			// e^(-d_2/2 * (x_k - mu_k)^T Sigma_k^-1 (x_k - mu_k)) Equation 6.9 [Magnusson 2009]
+			double e_x_cov_x = exp(-gauss_d2_ * x_trans.dot(c_inv * x_trans) / 2);
+			// Calculate probability of transtormed points existance, Equation 6.9 [Magnusson 2009]
+			double score_inc = -gauss_d1_ * e_x_cov_x - gauss_d3_;
+
+			score += score_inc / neighborhood.size();
+		}
+	}
+	return (score) / static_cast<double> (trans_cloud.size());
+}
+
+#endif // PCL_REGISTRATION_NDT_IMPL_H_

scan_matcher_ICP/include/pclomp/voxel_grid_covariance_omp.h

@@ -0,0 +1,551 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_VOXEL_GRID_COVARIANCE_OMP_H_
+#define PCL_VOXEL_GRID_COVARIANCE_OMP_H_
+
+#include <pcl/filters/boost.h>
+#include <pcl/filters/voxel_grid.h>
+#include <map>
+#include <unordered_map>
+#include <pcl/point_types.h>
+#include <pcl/kdtree/kdtree_flann.h>
+
+namespace pclomp
+{
+  /** \brief A searchable voxel strucure containing the mean and covariance of the data.
+    * \note For more information please see
+    * <b>Magnusson, M. (2009). The Three-Dimensional Normal-Distributions Transform —
+    * an Efficient Representation for Registration, Surface Analysis, and Loop Detection.
+    * PhD thesis, Orebro University. Orebro Studies in Technology 36</b>
+    * \author Brian Okorn (Space and Naval Warfare Systems Center Pacific)
+    */
+  template<typename PointT>
+  class VoxelGridCovariance : public pcl::VoxelGrid<PointT>
+  {
+    protected:
+      using pcl::VoxelGrid<PointT>::filter_name_;
+      using pcl::VoxelGrid<PointT>::getClassName;
+      using pcl::VoxelGrid<PointT>::input_;
+      using pcl::VoxelGrid<PointT>::indices_;
+      using pcl::VoxelGrid<PointT>::filter_limit_negative_;
+      using pcl::VoxelGrid<PointT>::filter_limit_min_;
+      using pcl::VoxelGrid<PointT>::filter_limit_max_;
+      using pcl::VoxelGrid<PointT>::filter_field_name_;
+
+      using pcl::VoxelGrid<PointT>::downsample_all_data_;
+      using pcl::VoxelGrid<PointT>::leaf_layout_;
+      using pcl::VoxelGrid<PointT>::save_leaf_layout_;
+      using pcl::VoxelGrid<PointT>::leaf_size_;
+      using pcl::VoxelGrid<PointT>::min_b_;
+      using pcl::VoxelGrid<PointT>::max_b_;
+      using pcl::VoxelGrid<PointT>::inverse_leaf_size_;
+      using pcl::VoxelGrid<PointT>::div_b_;
+      using pcl::VoxelGrid<PointT>::divb_mul_;
+
+      typedef typename pcl::traits::fieldList<PointT>::type FieldList;
+      typedef typename pcl::Filter<PointT>::PointCloud PointCloud;
+      typedef typename PointCloud::Ptr PointCloudPtr;
+      typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+    public:
+
+      typedef boost::shared_ptr< pcl::VoxelGrid<PointT> > Ptr;
+      typedef boost::shared_ptr< const pcl::VoxelGrid<PointT> > ConstPtr;
+
+      /** \brief Simple structure to hold a centroid, covarince and the number of points in a leaf.
+        * Inverse covariance, eigen vectors and engen values are precomputed. */
+      struct Leaf
+      {
+        /** \brief Constructor.
+         * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref evecs_ to the identity matrix
+         */
+        Leaf () :
+          nr_points (0),
+          mean_ (Eigen::Vector3d::Zero ()),
+          centroid (),
+          cov_ (Eigen::Matrix3d::Identity ()),
+          icov_ (Eigen::Matrix3d::Zero ()),
+          evecs_ (Eigen::Matrix3d::Identity ()),
+          evals_ (Eigen::Vector3d::Zero ())
+        {
+        }
+
+        /** \brief Get the voxel covariance.
+          * \return covariance matrix
+          */
+        Eigen::Matrix3d
+        getCov () const
+        {
+          return (cov_);
+        }
+
+        /** \brief Get the inverse of the voxel covariance.
+          * \return inverse covariance matrix
+          */
+        Eigen::Matrix3d
+        getInverseCov () const
+        {
+          return (icov_);
+        }
+
+        /** \brief Get the voxel centroid.
+          * \return centroid
+          */
+        Eigen::Vector3d
+        getMean () const
+        {
+          return (mean_);
+        }
+
+        /** \brief Get the eigen vectors of the voxel covariance.
+          * \note Order corresponds with \ref getEvals
+          * \return matrix whose columns contain eigen vectors
+          */
+        Eigen::Matrix3d
+        getEvecs () const
+        {
+          return (evecs_);
+        }
+
+        /** \brief Get the eigen values of the voxel covariance.
+          * \note Order corresponds with \ref getEvecs
+          * \return vector of eigen values
+          */
+        Eigen::Vector3d
+        getEvals () const
+        {
+          return (evals_);
+        }
+
+        /** \brief Get the number of points contained by this voxel.
+          * \return number of points
+          */
+        int
+        getPointCount () const
+        {
+          return (nr_points);
+        }
+
+        /** \brief Number of points contained by voxel */
+        int nr_points;
+
+        /** \brief 3D voxel centroid */
+        Eigen::Vector3d mean_;
+
+        /** \brief Nd voxel centroid
+         * \note Differs from \ref mean_ when color data is used
+         */
+        Eigen::VectorXf centroid;
+
+        /** \brief Voxel covariance matrix */
+        Eigen::Matrix3d cov_;
+
+        /** \brief Inverse of voxel covariance matrix */
+        Eigen::Matrix3d icov_;
+
+        /** \brief Eigen vectors of voxel covariance matrix */
+        Eigen::Matrix3d evecs_;
+
+        /** \brief Eigen values of voxel covariance matrix */
+        Eigen::Vector3d evals_;
+
+      };
+
+      /** \brief Pointer to VoxelGridCovariance leaf structure */
+      typedef Leaf* LeafPtr;
+
+      /** \brief Const pointer to VoxelGridCovariance leaf structure */
+      typedef const Leaf* LeafConstPtr;
+
+    typedef std::map<size_t, Leaf> Map;
+
+    public:
+
+      /** \brief Constructor.
+       * Sets \ref leaf_size_ to 0 and \ref searchable_ to false.
+       */
+      VoxelGridCovariance () :
+        searchable_ (true),
+        min_points_per_voxel_ (6),
+        min_covar_eigvalue_mult_ (0.01),
+        leaves_ (),
+        voxel_centroids_ (),
+        voxel_centroids_leaf_indices_ (),
+        kdtree_ ()
+      {
+        downsample_all_data_ = false;
+        save_leaf_layout_ = false;
+        leaf_size_.setZero ();
+        min_b_.setZero ();
+        max_b_.setZero ();
+        filter_name_ = "VoxelGridCovariance";
+      }
+
+      /** \brief Set the minimum number of points required for a cell to be used (must be 3 or greater for covariance calculation).
+        * \param[in] min_points_per_voxel the minimum number of points for required for a voxel to be used
+        */
+      inline void
+      setMinPointPerVoxel (int min_points_per_voxel)
+      {
+        if(min_points_per_voxel > 2)
+        {
+          min_points_per_voxel_ = min_points_per_voxel;
+        }
+        else
+        {
+          PCL_WARN ("%s: Covariance calculation requires at least 3 points, setting Min Point per Voxel to 3 ", this->getClassName ().c_str ());
+          min_points_per_voxel_ = 3;
+        }
+      }
+
+      /** \brief Get the minimum number of points required for a cell to be used.
+        * \return the minimum number of points for required for a voxel to be used
+        */
+      inline int
+      getMinPointPerVoxel ()
+      {
+        return min_points_per_voxel_;
+      }
+
+      /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
+        * \param[in] min_covar_eigvalue_mult the minimum allowable ratio between eigenvalues
+        */
+      inline void
+      setCovEigValueInflationRatio (double min_covar_eigvalue_mult)
+      {
+        min_covar_eigvalue_mult_ = min_covar_eigvalue_mult;
+      }
+
+      /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance matrices.
+        * \return the minimum allowable ratio between eigenvalues
+        */
+      inline double
+      getCovEigValueInflationRatio ()
+      {
+        return min_covar_eigvalue_mult_;
+      }
+
+      /** \brief Filter cloud and initializes voxel structure.
+       * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
+       * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
+       */
+      inline void
+      filter (PointCloud &output, bool searchable = false)
+      {
+        searchable_ = searchable;
+        applyFilter (output);
+
+        voxel_centroids_ = PointCloudPtr (new PointCloud (output));
+
+        if (searchable_ && voxel_centroids_->size() > 0)
+        {
+          // Initiates kdtree of the centroids of voxels containing a sufficient number of points
+          kdtree_.setInputCloud (voxel_centroids_);
+        }
+      }
+
+      /** \brief Initializes voxel structure.
+       * \param[in] searchable flag if voxel structure is searchable, if true then kdtree is built
+       */
+      inline void
+      filter (bool searchable = false)
+      {
+        searchable_ = searchable;
+        voxel_centroids_ = PointCloudPtr (new PointCloud);
+        applyFilter (*voxel_centroids_);
+
+        if (searchable_ && voxel_centroids_->size() > 0)
+        {
+          // Initiates kdtree of the centroids of voxels containing a sufficient number of points
+          kdtree_.setInputCloud (voxel_centroids_);
+        }
+      }
+
+      /** \brief Get the voxel containing point p.
+       * \param[in] index the index of the leaf structure node
+       * \return const pointer to leaf structure
+       */
+      inline LeafConstPtr
+      getLeaf (int index)
+      {
+        auto leaf_iter = leaves_.find (index);
+        if (leaf_iter != leaves_.end ())
+        {
+          LeafConstPtr ret (&(leaf_iter->second));
+          return ret;
+        }
+        else
+          return NULL;
+      }
+
+      /** \brief Get the voxel containing point p.
+       * \param[in] p the point to get the leaf structure at
+       * \return const pointer to leaf structure
+       */
+      inline LeafConstPtr
+      getLeaf (PointT &p)
+      {
+        // Generate index associated with p
+        int ijk0 = static_cast<int> (floor (p.x * inverse_leaf_size_[0]) - min_b_[0]);
+        int ijk1 = static_cast<int> (floor (p.y * inverse_leaf_size_[1]) - min_b_[1]);
+        int ijk2 = static_cast<int> (floor (p.z * inverse_leaf_size_[2]) - min_b_[2]);
+
+        // Compute the centroid leaf index
+        int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
+
+        // Find leaf associated with index
+        auto leaf_iter = leaves_.find (idx);
+        if (leaf_iter != leaves_.end ())
+        {
+          // If such a leaf exists return the pointer to the leaf structure
+          LeafConstPtr ret (&(leaf_iter->second));
+          return ret;
+        }
+        else
+          return NULL;
+      }
+
+      /** \brief Get the voxel containing point p.
+       * \param[in] p the point to get the leaf structure at
+       * \return const pointer to leaf structure
+       */
+      inline LeafConstPtr
+      getLeaf (Eigen::Vector3f &p)
+      {
+        // Generate index associated with p
+        int ijk0 = static_cast<int> (floor (p[0] * inverse_leaf_size_[0]) - min_b_[0]);
+        int ijk1 = static_cast<int> (floor (p[1] * inverse_leaf_size_[1]) - min_b_[1]);
+        int ijk2 = static_cast<int> (floor (p[2] * inverse_leaf_size_[2]) - min_b_[2]);
+
+        // Compute the centroid leaf index
+        int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
+
+        // Find leaf associated with index
+        auto leaf_iter = leaves_.find (idx);
+        if (leaf_iter != leaves_.end ())
+        {
+          // If such a leaf exists return the pointer to the leaf structure
+          LeafConstPtr ret (&(leaf_iter->second));
+          return ret;
+        }
+        else
+          return NULL;
+
+      }
+
+      /** \brief Get the voxels surrounding point p, not including the voxel contating point p.
+       * \note Only voxels containing a sufficient number of points are used (slower than radius search in practice).
+       * \param[in] reference_point the point to get the leaf structure at
+       * \param[out] neighbors
+       * \return number of neighbors found
+       */
+	  int getNeighborhoodAtPoint(const Eigen::MatrixXi&, const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const ;
+	  int getNeighborhoodAtPoint(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const ;
+	  int getNeighborhoodAtPoint7(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const ;
+	  int getNeighborhoodAtPoint1(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const ;
+
+      /** \brief Get the leaf structure map
+       * \return a map contataining all leaves
+       */
+      inline const Map&
+      getLeaves ()
+      {
+        return leaves_;
+      }
+
+      /** \brief Get a pointcloud containing the voxel centroids
+       * \note Only voxels containing a sufficient number of points are used.
+       * \return a map contataining all leaves
+       */
+      inline PointCloudPtr
+      getCentroids ()
+      {
+        return voxel_centroids_;
+      }
+
+
+      /** \brief Get a cloud to visualize each voxels normal distribution.
+       * \param[out] cell_cloud a cloud created by sampling the normal distributions of each voxel
+       */
+      void
+      getDisplayCloud (pcl::PointCloud<pcl::PointXYZ>& cell_cloud);
+
+      /** \brief Search for the k-nearest occupied voxels for the given query point.
+       * \note Only voxels containing a sufficient number of points are used.
+       * \param[in] point the given query point
+       * \param[in] k the number of neighbors to search for
+       * \param[out] k_leaves the resultant leaves of the neighboring points
+       * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
+       * \return number of neighbors found
+       */
+      int
+      nearestKSearch (const PointT &point, int k,
+                      std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
+      {
+        k_leaves.clear ();
+
+        // Check if kdtree has been built
+        if (!searchable_)
+        {
+          PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
+          return 0;
+        }
+
+        // Find k-nearest neighbors in the occupied voxel centroid cloud
+        std::vector<int> k_indices;
+        k = kdtree_.nearestKSearch (point, k, k_indices, k_sqr_distances);
+
+        // Find leaves corresponding to neighbors
+        k_leaves.reserve (k);
+        for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++)
+        {
+          k_leaves.push_back (&leaves_[voxel_centroids_leaf_indices_[*iter]]);
+        }
+        return k;
+      }
+
+      /** \brief Search for the k-nearest occupied voxels for the given query point.
+       * \note Only voxels containing a sufficient number of points are used.
+       * \param[in] cloud the given query point
+       * \param[in] index the index
+       * \param[in] k the number of neighbors to search for
+       * \param[out] k_leaves the resultant leaves of the neighboring points
+       * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
+       * \return number of neighbors found
+       */
+      inline int
+      nearestKSearch (const PointCloud &cloud, int index, int k,
+                      std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances)
+      {
+        if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
+          return (0);
+        return (nearestKSearch (cloud.points[index], k, k_leaves, k_sqr_distances));
+      }
+
+
+      /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
+       * \note Only voxels containing a sufficient number of points are used.
+       * \param[in] point the given query point
+       * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
+       * \param[out] k_leaves the resultant leaves of the neighboring points
+       * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
+       * \param[in] max_nn
+       * \return number of neighbors found
+       */
+      int
+      radiusSearch (const PointT &point, double radius, std::vector<LeafConstPtr> &k_leaves,
+                    std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const
+      {
+        k_leaves.clear ();
+
+        // Check if kdtree has been built
+        if (!searchable_)
+        {
+          PCL_WARN ("%s: Not Searchable", this->getClassName ().c_str ());
+          return 0;
+        }
+
+        // Find neighbors within radius in the occupied voxel centroid cloud
+        std::vector<int> k_indices;
+        int k = kdtree_.radiusSearch (point, radius, k_indices, k_sqr_distances, max_nn);
+
+        // Find leaves corresponding to neighbors
+        k_leaves.reserve (k);
+        for (std::vector<int>::iterator iter = k_indices.begin (); iter != k_indices.end (); iter++)
+        {
+		  auto leaf = leaves_.find(voxel_centroids_leaf_indices_[*iter]);
+		  if (leaf == leaves_.end()) {
+			  std::cerr << "error : could not find the leaf corresponding to the voxel" << std::endl;
+			  std::cin.ignore(1);
+		  }
+          k_leaves.push_back (&(leaf->second));
+        }
+        return k;
+      }
+
+      /** \brief Search for all the nearest occupied voxels of the query point in a given radius.
+       * \note Only voxels containing a sufficient number of points are used.
+       * \param[in] cloud the given query point
+       * \param[in] index a valid index in cloud representing a valid (i.e., finite) query point
+       * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
+       * \param[out] k_leaves the resultant leaves of the neighboring points
+       * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
+       * \param[in] max_nn
+       * \return number of neighbors found
+       */
+      inline int
+      radiusSearch (const PointCloud &cloud, int index, double radius,
+                    std::vector<LeafConstPtr> &k_leaves, std::vector<float> &k_sqr_distances,
+                    unsigned int max_nn = 0) const
+      {
+        if (index >= static_cast<int> (cloud.points.size ()) || index < 0)
+          return (0);
+        return (radiusSearch (cloud.points[index], radius, k_leaves, k_sqr_distances, max_nn));
+      }
+
+    protected:
+
+      /** \brief Filter cloud and initializes voxel structure.
+       * \param[out] output cloud containing centroids of voxels containing a sufficient number of points
+       */
+      void applyFilter (PointCloud &output);
+
+      /** \brief Flag to determine if voxel structure is searchable. */
+      bool searchable_;
+
+      /** \brief Minimum points contained with in a voxel to allow it to be useable. */
+      int min_points_per_voxel_;
+
+      /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance matrices. */
+      double min_covar_eigvalue_mult_;
+
+      /** \brief Voxel structure containing all leaf nodes (includes voxels with less than a sufficient number of points). */
+	  Map leaves_;
+
+      /** \brief Point cloud containing centroids of voxels containing atleast minimum number of points. */
+      PointCloudPtr voxel_centroids_;
+
+      /** \brief Indices of leaf structurs associated with each point in \ref voxel_centroids_ (used for searching). */
+      std::vector<int> voxel_centroids_leaf_indices_;
+
+      /** \brief KdTree generated using \ref voxel_centroids_ (used for searching). */
+      pcl::KdTreeFLANN<PointT> kdtree_;
+  };
+}
+
+#endif  //#ifndef PCL_VOXEL_GRID_COVARIANCE_H_

scan_matcher_ICP/include/pclomp/voxel_grid_covariance_omp_impl.hpp

@@ -0,0 +1,487 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Point Cloud Library (PCL) - www.pointclouds.org
+ *  Copyright (c) 2010-2011, Willow Garage, Inc.
+ *
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder(s) nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#ifndef PCL_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_
+#define PCL_VOXEL_GRID_COVARIANCE_IMPL_OMP_H_
+
+#include <pcl/common/common.h>
+#include <pcl/filters/boost.h>
+#include "voxel_grid_covariance_omp.h"
+#include <Eigen/Dense>
+#include <Eigen/Cholesky>
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> void
+pclomp::VoxelGridCovariance<PointT>::applyFilter (PointCloud &output)
+{
+  voxel_centroids_leaf_indices_.clear ();
+
+  // Has the input dataset been set already?
+  if (!input_)
+  {
+    PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
+    output.width = output.height = 0;
+    output.points.clear ();
+    return;
+  }
+
+  // Copy the header (and thus the frame_id) + allocate enough space for points
+  output.height = 1;                          // downsampling breaks the organized structure
+  output.is_dense = true;                     // we filter out invalid points
+  output.points.clear ();
+
+  Eigen::Vector4f min_p, max_p;
+  // Get the minimum and maximum dimensions
+  if (!filter_field_name_.empty ()) // If we don't want to process the entire cloud...
+      pcl::getMinMax3D<PointT> (input_, filter_field_name_, static_cast<float> (filter_limit_min_), static_cast<float> (filter_limit_max_), min_p, max_p, filter_limit_negative_);
+  else
+	  pcl::getMinMax3D<PointT> (*input_, min_p, max_p);
+
+  // Check that the leaf size is not too small, given the size of the data
+  int64_t dx = static_cast<int64_t>((max_p[0] - min_p[0]) * inverse_leaf_size_[0])+1;
+  int64_t dy = static_cast<int64_t>((max_p[1] - min_p[1]) * inverse_leaf_size_[1])+1;
+  int64_t dz = static_cast<int64_t>((max_p[2] - min_p[2]) * inverse_leaf_size_[2])+1;
+
+  if((dx*dy*dz) > std::numeric_limits<int32_t>::max())
+  {
+    PCL_WARN("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.", getClassName().c_str());
+    output.clear();
+    return;
+  }
+
+  // Compute the minimum and maximum bounding box values
+  min_b_[0] = static_cast<int> (floor (min_p[0] * inverse_leaf_size_[0]));
+  max_b_[0] = static_cast<int> (floor (max_p[0] * inverse_leaf_size_[0]));
+  min_b_[1] = static_cast<int> (floor (min_p[1] * inverse_leaf_size_[1]));
+  max_b_[1] = static_cast<int> (floor (max_p[1] * inverse_leaf_size_[1]));
+  min_b_[2] = static_cast<int> (floor (min_p[2] * inverse_leaf_size_[2]));
+  max_b_[2] = static_cast<int> (floor (max_p[2] * inverse_leaf_size_[2]));
+
+  // Compute the number of divisions needed along all axis
+  div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones ();
+  div_b_[3] = 0;
+
+  // Clear the leaves
+  leaves_.clear ();
+//  leaves_.reserve(8192);
+
+  // Set up the division multiplier
+  divb_mul_ = Eigen::Vector4i (1, div_b_[0], div_b_[0] * div_b_[1], 0);
+
+  int centroid_size = 4;
+
+  if (downsample_all_data_)
+    centroid_size = boost::mpl::size<FieldList>::value;
+
+  // ---[ RGB special case
+  std::vector<pcl::PCLPointField> fields;
+  int rgba_index = -1;
+  rgba_index = pcl::getFieldIndex (*input_, "rgb", fields);
+  if (rgba_index == -1)
+    rgba_index = pcl::getFieldIndex (*input_, "rgba", fields);
+  if (rgba_index >= 0)
+  {
+    rgba_index = fields[rgba_index].offset;
+    centroid_size += 3;
+  }
+
+  // If we don't want to process the entire cloud, but rather filter points far away from the viewpoint first...
+  if (!filter_field_name_.empty ())
+  {
+    // Get the distance field index
+    std::vector<pcl::PCLPointField> fields;
+    int distance_idx = pcl::getFieldIndex (*input_, filter_field_name_, fields);
+    if (distance_idx == -1)
+      PCL_WARN ("[pcl::%s::applyFilter] Invalid filter field name. Index is %d.\n", getClassName ().c_str (), distance_idx);
+
+    // First pass: go over all points and insert them into the right leaf
+    for (size_t cp = 0; cp < input_->points.size (); ++cp)
+    {
+      if (!input_->is_dense)
+        // Check if the point is invalid
+        if (!pcl_isfinite (input_->points[cp].x) ||
+            !pcl_isfinite (input_->points[cp].y) ||
+            !pcl_isfinite (input_->points[cp].z))
+          continue;
+
+      // Get the distance value
+      const uint8_t* pt_data = reinterpret_cast<const uint8_t*> (&input_->points[cp]);
+      float distance_value = 0;
+      memcpy (&distance_value, pt_data + fields[distance_idx].offset, sizeof (float));
+
+      if (filter_limit_negative_)
+      {
+        // Use a threshold for cutting out points which inside the interval
+        if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_))
+          continue;
+      }
+      else
+      {
+        // Use a threshold for cutting out points which are too close/far away
+        if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_))
+          continue;
+      }
+
+      int ijk0 = static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
+      int ijk1 = static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
+      int ijk2 = static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
+
+      // Compute the centroid leaf index
+      int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
+
+      Leaf& leaf = leaves_[idx];
+      if (leaf.nr_points == 0)
+      {
+        leaf.centroid.resize (centroid_size);
+        leaf.centroid.setZero ();
+      }
+
+      Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z);
+      // Accumulate point sum for centroid calculation
+      leaf.mean_ += pt3d;
+      // Accumulate x*xT for single pass covariance calculation
+      leaf.cov_ += pt3d * pt3d.transpose ();
+
+      // Do we need to process all the fields?
+      if (!downsample_all_data_)
+      {
+        Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0);
+        leaf.centroid.template head<4> () += pt;
+      }
+      else
+      {
+        // Copy all the fields
+        Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
+        // ---[ RGB special case
+        if (rgba_index >= 0)
+        {
+          // fill r/g/b data
+          int rgb;
+          memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (int));
+          centroid[centroid_size - 3] = static_cast<float> ((rgb >> 16) & 0x0000ff);
+          centroid[centroid_size - 2] = static_cast<float> ((rgb >> 8) & 0x0000ff);
+          centroid[centroid_size - 1] = static_cast<float> ((rgb) & 0x0000ff);
+        }
+        pcl::for_each_type<FieldList> (pcl::NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
+        leaf.centroid += centroid;
+      }
+      ++leaf.nr_points;
+    }
+  }
+  // No distance filtering, process all data
+  else
+  {
+    // First pass: go over all points and insert them into the right leaf
+    for (size_t cp = 0; cp < input_->points.size (); ++cp)
+    {
+      if (!input_->is_dense)
+        // Check if the point is invalid
+        if (!pcl_isfinite (input_->points[cp].x) ||
+            !pcl_isfinite (input_->points[cp].y) ||
+            !pcl_isfinite (input_->points[cp].z))
+          continue;
+
+      int ijk0 = static_cast<int> (floor (input_->points[cp].x * inverse_leaf_size_[0]) - static_cast<float> (min_b_[0]));
+      int ijk1 = static_cast<int> (floor (input_->points[cp].y * inverse_leaf_size_[1]) - static_cast<float> (min_b_[1]));
+      int ijk2 = static_cast<int> (floor (input_->points[cp].z * inverse_leaf_size_[2]) - static_cast<float> (min_b_[2]));
+
+      // Compute the centroid leaf index
+      int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];
+
+      //int idx = (((input_->points[cp].getArray4fmap () * inverse_leaf_size_).template cast<int> ()).matrix () - min_b_).dot (divb_mul_);
+      Leaf& leaf = leaves_[idx];
+      if (leaf.nr_points == 0)
+      {
+        leaf.centroid.resize (centroid_size);
+        leaf.centroid.setZero ();
+      }
+
+      Eigen::Vector3d pt3d (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z);
+      // Accumulate point sum for centroid calculation
+      leaf.mean_ += pt3d;
+      // Accumulate x*xT for single pass covariance calculation
+      leaf.cov_ += pt3d * pt3d.transpose ();
+
+      // Do we need to process all the fields?
+      if (!downsample_all_data_)
+      {
+        Eigen::Vector4f pt (input_->points[cp].x, input_->points[cp].y, input_->points[cp].z, 0);
+        leaf.centroid.template head<4> () += pt;
+      }
+      else
+      {
+        // Copy all the fields
+        Eigen::VectorXf centroid = Eigen::VectorXf::Zero (centroid_size);
+        // ---[ RGB special case
+        if (rgba_index >= 0)
+        {
+          // Fill r/g/b data, assuming that the order is BGRA
+          int rgb;
+          memcpy (&rgb, reinterpret_cast<const char*> (&input_->points[cp]) + rgba_index, sizeof (int));
+          centroid[centroid_size - 3] = static_cast<float> ((rgb >> 16) & 0x0000ff);
+          centroid[centroid_size - 2] = static_cast<float> ((rgb >> 8) & 0x0000ff);
+          centroid[centroid_size - 1] = static_cast<float> ((rgb) & 0x0000ff);
+        }
+        pcl::for_each_type<FieldList> (pcl::NdCopyPointEigenFunctor<PointT> (input_->points[cp], centroid));
+        leaf.centroid += centroid;
+      }
+      ++leaf.nr_points;
+    }
+  }
+
+  // Second pass: go over all leaves and compute centroids and covariance matrices
+  output.points.reserve (leaves_.size ());
+  if (searchable_)
+    voxel_centroids_leaf_indices_.reserve (leaves_.size ());
+  int cp = 0;
+  if (save_leaf_layout_)
+    leaf_layout_.resize (div_b_[0] * div_b_[1] * div_b_[2], -1);
+
+  // Eigen values and vectors calculated to prevent near singluar matrices
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver;
+  Eigen::Matrix3d eigen_val;
+  Eigen::Vector3d pt_sum;
+
+  // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the max eigen value.
+  double min_covar_eigvalue;
+
+  for (auto it = leaves_.begin (); it != leaves_.end (); ++it)
+  {
+
+    // Normalize the centroid
+    Leaf& leaf = it->second;
+
+    // Normalize the centroid
+    leaf.centroid /= static_cast<float> (leaf.nr_points);
+    // Point sum used for single pass covariance calculation
+    pt_sum = leaf.mean_;
+    // Normalize mean
+    leaf.mean_ /= leaf.nr_points;
+
+    // If the voxel contains sufficient points, its covariance is calculated and is added to the voxel centroids and output clouds.
+    // Points with less than the minimum points will have a can not be accuratly approximated using a normal distribution.
+    if (leaf.nr_points >= min_points_per_voxel_)
+    {
+      if (save_leaf_layout_)
+        leaf_layout_[it->first] = cp++;
+
+      output.push_back (PointT ());
+
+      // Do we need to process all the fields?
+      if (!downsample_all_data_)
+      {
+        output.points.back ().x = leaf.centroid[0];
+        output.points.back ().y = leaf.centroid[1];
+        output.points.back ().z = leaf.centroid[2];
+      }
+      else
+      {
+        pcl::for_each_type<FieldList> (pcl::NdCopyEigenPointFunctor<PointT> (leaf.centroid, output.back ()));
+        // ---[ RGB special case
+        if (rgba_index >= 0)
+        {
+          // pack r/g/b into rgb
+          float r = leaf.centroid[centroid_size - 3], g = leaf.centroid[centroid_size - 2], b = leaf.centroid[centroid_size - 1];
+          int rgb = (static_cast<int> (r)) << 16 | (static_cast<int> (g)) << 8 | (static_cast<int> (b));
+          memcpy (reinterpret_cast<char*> (&output.points.back ()) + rgba_index, &rgb, sizeof (float));
+        }
+      }
+
+      // Stores the voxel indice for fast access searching
+      if (searchable_)
+        voxel_centroids_leaf_indices_.push_back (static_cast<int> (it->first));
+
+      // Single pass covariance calculation
+      leaf.cov_ = (leaf.cov_ - 2 * (pt_sum * leaf.mean_.transpose ())) / leaf.nr_points + leaf.mean_ * leaf.mean_.transpose ();
+      leaf.cov_ *= (leaf.nr_points - 1.0) / leaf.nr_points;
+
+      //Normalize Eigen Val such that max no more than 100x min.
+      eigensolver.compute (leaf.cov_);
+      eigen_val = eigensolver.eigenvalues ().asDiagonal ();
+      leaf.evecs_ = eigensolver.eigenvectors ();
+
+      if (eigen_val (0, 0) < 0 || eigen_val (1, 1) < 0 || eigen_val (2, 2) <= 0)
+      {
+        leaf.nr_points = -1;
+        continue;
+      }
+
+      // Avoids matrices near singularities (eq 6.11)[Magnusson 2009]
+
+      min_covar_eigvalue = min_covar_eigvalue_mult_ * eigen_val (2, 2);
+      if (eigen_val (0, 0) < min_covar_eigvalue)
+      {
+        eigen_val (0, 0) = min_covar_eigvalue;
+
+        if (eigen_val (1, 1) < min_covar_eigvalue)
+        {
+          eigen_val (1, 1) = min_covar_eigvalue;
+        }
+
+        leaf.cov_ = leaf.evecs_ * eigen_val * leaf.evecs_.inverse ();
+      }
+      leaf.evals_ = eigen_val.diagonal ();
+
+      leaf.icov_ = leaf.cov_.inverse ();
+      if (leaf.icov_.maxCoeff () == std::numeric_limits<float>::infinity ( )
+          || leaf.icov_.minCoeff () == -std::numeric_limits<float>::infinity ( ) )
+      {
+        leaf.nr_points = -1;
+      }
+
+    }
+  }
+
+  output.width = static_cast<uint32_t> (output.points.size ());
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> int
+pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint(const Eigen::MatrixXi& relative_coordinates, const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
+{
+	neighbors.clear();
+
+	// Find displacement coordinates
+	Eigen::Vector4i ijk(static_cast<int> (floor(reference_point.x / leaf_size_[0])),
+		static_cast<int> (floor(reference_point.y / leaf_size_[1])),
+		static_cast<int> (floor(reference_point.z / leaf_size_[2])), 0);
+	Eigen::Array4i diff2min = min_b_ - ijk;
+	Eigen::Array4i diff2max = max_b_ - ijk;
+	neighbors.reserve(relative_coordinates.cols());
+
+	// Check each neighbor to see if it is occupied and contains sufficient points
+	// Slower than radius search because needs to check 26 indices
+	for (int ni = 0; ni < relative_coordinates.cols(); ni++)
+	{
+		Eigen::Vector4i displacement = (Eigen::Vector4i() << relative_coordinates.col(ni), 0).finished();
+		// Checking if the specified cell is in the grid
+		if ((diff2min <= displacement.array()).all() && (diff2max >= displacement.array()).all())
+		{
+			auto leaf_iter = leaves_.find(((ijk + displacement - min_b_).dot(divb_mul_)));
+			if (leaf_iter != leaves_.end() && leaf_iter->second.nr_points >= min_points_per_voxel_)
+			{
+				LeafConstPtr leaf = &(leaf_iter->second);
+				neighbors.push_back(leaf);
+			}
+		}
+	}
+
+	return (static_cast<int> (neighbors.size()));
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> int
+pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
+{
+	neighbors.clear();
+
+	// Find displacement coordinates
+	Eigen::MatrixXi relative_coordinates = pcl::getAllNeighborCellIndices();
+	return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> int
+pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint7(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
+{
+	neighbors.clear();
+
+	Eigen::MatrixXi relative_coordinates(3, 7);
+	relative_coordinates.setZero();
+	relative_coordinates(0, 1) = 1;
+	relative_coordinates(0, 2) = -1;
+	relative_coordinates(1, 3) = 1;
+	relative_coordinates(1, 4) = -1;
+	relative_coordinates(2, 5) = 1;
+	relative_coordinates(2, 6) = -1;
+
+	return getNeighborhoodAtPoint(relative_coordinates, reference_point, neighbors);
+}
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> int
+pclomp::VoxelGridCovariance<PointT>::getNeighborhoodAtPoint1(const PointT& reference_point, std::vector<LeafConstPtr> &neighbors) const
+{
+	neighbors.clear();
+	return getNeighborhoodAtPoint(Eigen::MatrixXi::Zero(3,1), reference_point, neighbors);
+}
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////
+template<typename PointT> void
+pclomp::VoxelGridCovariance<PointT>::getDisplayCloud (pcl::PointCloud<pcl::PointXYZ>& cell_cloud)
+{
+  cell_cloud.clear ();
+
+  int pnt_per_cell = 1000;
+  boost::mt19937 rng;
+  boost::normal_distribution<> nd (0.0, leaf_size_.head (3).norm ());
+  boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
+
+  Eigen::LLT<Eigen::Matrix3d> llt_of_cov;
+  Eigen::Matrix3d cholesky_decomp;
+  Eigen::Vector3d cell_mean;
+  Eigen::Vector3d rand_point;
+  Eigen::Vector3d dist_point;
+
+  // Generate points for each occupied voxel with sufficient points.
+  for (auto it = leaves_.begin (); it != leaves_.end (); ++it)
+  {
+    Leaf& leaf = it->second;
+
+    if (leaf.nr_points >= min_points_per_voxel_)
+    {
+      cell_mean = leaf.mean_;
+      llt_of_cov.compute (leaf.cov_);
+      cholesky_decomp = llt_of_cov.matrixL ();
+
+      // Random points generated by sampling the normal distribution given by voxel mean and covariance matrix
+      for (int i = 0; i < pnt_per_cell; i++)
+      {
+        rand_point = Eigen::Vector3d (var_nor (), var_nor (), var_nor ());
+        dist_point = cell_mean + cholesky_decomp * rand_point;
+        cell_cloud.push_back (pcl::PointXYZ (static_cast<float> (dist_point (0)), static_cast<float> (dist_point (1)), static_cast<float> (dist_point (2))));
+      }
+    }
+  }
+}
+
+#define PCL_INSTANTIATE_VoxelGridCovariance(T) template class PCL_EXPORTS pcl::VoxelGridCovariance<T>;
+
+#endif    // PCL_VOXEL_GRID_COVARIANCE_IMPL_H_

scan_matcher_ICP/package.xml

@@ -0,0 +1,80 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>scan_matcher_ICP</name>
+  <version>0.0.0</version>
+  <description>The scan_matcher_ICP package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="youchen@todo.todo">youchen</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/scan_matcher_ICP</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>csm</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>libpcl-all-dev</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>nodelet</build_depend>
+  <build_depend>pcl_conversions</build_depend>
+  <build_depend>pcl_ros</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>tf</build_depend>
+
+  <exec_depend>csm</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>libpcl-all</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>nodelet</exec_depend>
+  <exec_depend>pcl_ros</exec_depend>
+  <exec_depend>pcl_conversions</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>tf</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

scan_matcher_ICP/scripts/demo_sick.launch

@@ -0,0 +1,41 @@
+<!--
+Example launch file: launches the scan matcher with pre-recorded data
+-->
+
+<launch>
+  <!-- <arg name="IS_TWISTSTAMPED" default="true" /> -->
+  <arg name="use_rviz" default="true" />
+  <arg name="publish_covariance" default="false"/>
+  #### set up data playback from bag #############################
+
+  <param name="/use_sim_time" value="true"/>
+  <!-- <param name="/stamped_vel" value="$(arg IS_TWISTSTAMPED)"/> -->
+
+  <group if="$(arg use_rviz)">
+    <node pkg="rviz" type="rviz" name="rviz"
+          args="-d $(find scan_matcher_ICP)/scripts/demo.rviz"/>
+  </group>
+
+  <!-- <node pkg="rosbag" type="play" name="play"
+  args="$(find laser_scan_matcher)/demo/scan1.bag"/> -->
+
+  #### publish an example base_link -> laser transform ###########
+
+  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser"
+    args="0.0 0.0 0.0 0.0 0.0 0.0 1.0 /base_link /laser 40" />
+
+  #### start the laser scan_matcher ##############################
+
+  <group if="$(arg publish_covariance)">
+    <param name="laser_scan_matcher_node/do_compute_covariance" value="1"/>
+    <param name="laser_scan_matcher_node/publish_pose_with_covariance" value="true"/>
+    <param name="laser_scan_matcher_node/publish_pose_with_covariance_stamped" value="true"/>
+  </group>
+  <node pkg="scan_matcher_ICP" type="scan_matcher_ICP_node"
+    name="scan_matcher_ICP_node" output="screen">
+    <param name="fixed_frame" value="map"/>
+    <param name="base_frame" value="base_link"/>
+    <param name="max_iterations" value="10"/>
+  </node>
+
+</launch>

scan_matcher_ICP/src/laser_scan_matcher.cpp

@@ -0,0 +1,689 @@
+/*
+ * Copyright (c) 2011, Ivan Dryanovski, William Morris
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the CCNY Robotics Lab nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*  This package uses Canonical Scan Matcher [1], written by
+ *  Andrea Censi
+ *
+ *  [1] A. Censi, "An ICP variant using a point-to-line metric"
+ *  Proceedings of the IEEE International Conference
+ *  on Robotics and Automation (ICRA), 2008
+ */
+
+#include <laser_scan_matcher.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/common/transforms.h>
+#include <pcl/registration/icp.h>
+
+#include <pcl/registration/ndt.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/approximate_voxel_grid.h>
+
+#include <../include/pclomp/ndt_omp.h>
+
+#include <boost/assign.hpp>
+
+namespace scan_tools
+{
+
+LaserScanMatcher::LaserScanMatcher(ros::NodeHandle nh, ros::NodeHandle nh_private):
+  nh_(nh),
+  nh_private_(nh_private),
+  initialized_(false)
+{
+  ROS_INFO("Starting LaserScanMatcher");
+
+  // **** init parameters
+
+  initParams();
+
+  // **** state variables
+
+  f2b_.setIdentity();
+  f2b_kf_.setIdentity();
+  input_.laser[0] = 0.0;
+  input_.laser[1] = 0.0;
+  input_.laser[2] = 0.0;
+
+  // Initialize output_ vectors as Null for error-checking
+  output_.cov_x_m = 0;
+  output_.dx_dy1_m = 0;
+  output_.dx_dy2_m = 0;
+
+  // *** subscribers
+  scan_subscriber_ = nh_.subscribe(
+      "scan", 1, &LaserScanMatcher::scanCallback, this);
+
+  // *** publishers
+  origin_cloud_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>("origin_cloud", 10);
+  current_cloud_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>("current_cloud", 10);
+  current_pcl_cloud_publisher_ = nh_.advertise<sensor_msgs::PointCloud2>("current_cloud_pcl", 10);
+}
+
+LaserScanMatcher::~LaserScanMatcher()
+{
+  ROS_INFO("Destroying LaserScanMatcher");
+}
+
+void LaserScanMatcher::initParams()
+{
+  if (!nh_private_.getParam ("base_frame", base_frame_))
+    base_frame_ = "base_link";
+  if (!nh_private_.getParam ("fixed_frame", fixed_frame_))
+    fixed_frame_ = "world";
+
+  // **** keyframe params: when to generate the keyframe scan
+  // if either is set to 0, reduces to frame-to-frame matching
+  // 更新关键帧粒度，超过10cm或10度
+  if (!nh_private_.getParam ("kf_dist_linear", kf_dist_linear_))
+    kf_dist_linear_ = 5.10;
+  if (!nh_private_.getParam ("kf_dist_angular", kf_dist_angular_))
+    kf_dist_angular_ = 310.0 * (M_PI / 180.0);
+
+  kf_dist_linear_sq_ = kf_dist_linear_ * kf_dist_linear_;
+
+  // **** How to publish the output?
+  // tf (fixed_frame->base_frame),
+  // pose message (pose of base frame in the fixed frame)
+
+  if (!nh_private_.getParam ("publish_tf", publish_tf_))
+    publish_tf_ = true;
+
+  // **** CSM parameters - comments copied from algos.h (by Andrea Censi)
+
+  // Maximum angular displacement between scans
+  if (!nh_private_.getParam ("max_angular_correction_deg", input_.max_angular_correction_deg))
+    input_.max_angular_correction_deg = 45.0;
+
+  // Maximum translation between scans (m)
+  if (!nh_private_.getParam ("max_linear_correction", input_.max_linear_correction))
+    input_.max_linear_correction = 0.50;
+
+  // Maximum ICP cycle iterations
+  if (!nh_private_.getParam ("max_iterations", input_.max_iterations))
+    input_.max_iterations = 10;
+
+  // A threshold for stopping (m)
+  if (!nh_private_.getParam ("epsilon_xy", input_.epsilon_xy))
+    input_.epsilon_xy = 0.000001;
+
+  // A threshold for stopping (rad)
+  if (!nh_private_.getParam ("epsilon_theta", input_.epsilon_theta))
+    input_.epsilon_theta = 0.000001;
+
+  // Maximum distance for a correspondence to be valid
+  if (!nh_private_.getParam ("max_correspondence_dist", input_.max_correspondence_dist))
+    input_.max_correspondence_dist = 0.3;
+
+  // Noise in the scan (m)
+  if (!nh_private_.getParam ("sigma", input_.sigma))
+    input_.sigma = 0.010;
+
+  // Use smart tricks for finding correspondences.
+  if (!nh_private_.getParam ("use_corr_tricks", input_.use_corr_tricks))
+    input_.use_corr_tricks = 1;
+
+  // Restart: Restart if error is over threshold
+  if (!nh_private_.getParam ("restart", input_.restart))
+    input_.restart = 0;
+
+  // Restart: Threshold for restarting
+  if (!nh_private_.getParam ("restart_threshold_mean_error", input_.restart_threshold_mean_error))
+    input_.restart_threshold_mean_error = 0.01;
+
+  // Restart: displacement for restarting. (m)
+  if (!nh_private_.getParam ("restart_dt", input_.restart_dt))
+    input_.restart_dt = 1.0;
+
+  // Restart: displacement for restarting. (rad)
+  if (!nh_private_.getParam ("restart_dtheta", input_.restart_dtheta))
+    input_.restart_dtheta = 0.1;
+
+  // Max distance for staying in the same clustering
+  if (!nh_private_.getParam ("clustering_threshold", input_.clustering_threshold))
+    input_.clustering_threshold = 0.25;
+
+  // Number of neighbour rays used to estimate the orientation
+  if (!nh_private_.getParam ("orientation_neighbourhood", input_.orientation_neighbourhood))
+    input_.orientation_neighbourhood = 20;
+
+  // If 0, it's vanilla ICP
+  if (!nh_private_.getParam ("use_point_to_line_distance", input_.use_point_to_line_distance))
+    input_.use_point_to_line_distance = 1;
+
+  // Discard correspondences based on the angles
+  if (!nh_private_.getParam ("do_alpha_test", input_.do_alpha_test))
+    input_.do_alpha_test = 0;
+
+  // Discard correspondences based on the angles - threshold angle, in degrees
+  if (!nh_private_.getParam ("do_alpha_test_thresholdDeg", input_.do_alpha_test_thresholdDeg))
+    input_.do_alpha_test_thresholdDeg = 20.0;
+
+  // Percentage of correspondences to consider: if 0.9,
+  // always discard the top 10% of correspondences with more error
+  if (!nh_private_.getParam ("outliers_maxPerc", input_.outliers_maxPerc))
+    input_.outliers_maxPerc = 0.90;
+
+  // Parameters describing a simple adaptive algorithm for discarding.
+  //  1) Order the errors.
+  //  2) Choose the percentile according to outliers_adaptive_order.
+  //     (if it is 0.7, get the 70% percentile)
+  //  3) Define an adaptive threshold multiplying outliers_adaptive_mult
+  //     with the value of the error at the chosen percentile.
+  //  4) Discard correspondences over the threshold.
+  //  This is useful to be conservative; yet remove the biggest errors.
+  if (!nh_private_.getParam ("outliers_adaptive_order", input_.outliers_adaptive_order))
+    input_.outliers_adaptive_order = 0.7;
+
+  if (!nh_private_.getParam ("outliers_adaptive_mult", input_.outliers_adaptive_mult))
+    input_.outliers_adaptive_mult = 2.0;
+
+  // If you already have a guess of the solution, you can compute the polar angle
+  // of the points of one scan in the new position. If the polar angle is not a monotone
+  // function of the readings index, it means that the surface is not visible in the
+  // next position. If it is not visible, then we don't use it for matching.
+  if (!nh_private_.getParam ("do_visibility_test", input_.do_visibility_test))
+    input_.do_visibility_test = 0;
+
+  // no two points in laser_sens can have the same corr.
+  if (!nh_private_.getParam ("outliers_remove_doubles", input_.outliers_remove_doubles))
+    input_.outliers_remove_doubles = 1;
+
+  // If 1, computes the covariance of ICP using the method http://purl.org/censi/2006/icpcov
+  if (!nh_private_.getParam ("do_compute_covariance", input_.do_compute_covariance))
+    input_.do_compute_covariance = 0;
+
+  // Checks that find_correspondences_tricks gives the right answer
+  if (!nh_private_.getParam ("debug_verify_tricks", input_.debug_verify_tricks))
+    input_.debug_verify_tricks = 0;
+
+  // If 1, the field 'true_alpha' (or 'alpha') in the first scan is used to compute the
+  // incidence beta, and the factor (1/cos^2(beta)) used to weight the correspondence.");
+  if (!nh_private_.getParam ("use_ml_weights", input_.use_ml_weights))
+    input_.use_ml_weights = 0;
+
+  // If 1, the field 'readings_sigma' in the second scan is used to weight the
+  // correspondence by 1/sigma^2
+  if (!nh_private_.getParam ("use_sigma_weights", input_.use_sigma_weights))
+    input_.use_sigma_weights = 0;
+}
+
+void LaserScanMatcher::scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
+{
+  double dist = 0, theta = 0;
+  // **** if first scan, cache the tf from base to the scanner
+  // std::cout<<"received scan msgs: "<<scan_msg->ranges.size()<<std::endl;
+  if (!initialized_)
+  {
+    createCache(scan_msg);    // caches the sin and cos of all angles
+
+    // cache the static tf from base to laser
+    if (!getBaseToLaserTf(scan_msg->header.frame_id))
+    {
+      ROS_WARN("Skipping scan");
+      return;
+    }
+
+    laserScanToLDP(scan_msg, prev_ldp_scan_, scan_msg->ranges.size());
+    last_icp_time_ = scan_msg->header.stamp;
+    initialized_ = true;
+
+    //for cloud display
+    pc_origin_.width = prev_ldp_scan_->nrays;
+    pc_origin_.height = 1;
+    pc_origin_.resize(prev_ldp_scan_->nrays);
+
+    // pc_origin_.header.stamp = ros::Time::now();
+    // pc_origin_.header.frame_id = base_frame_;
+    // pc_origin_.points.resize(prev_ldp_scan_->nrays);
+    // std::cout<<base_frame_<<std::endl;
+    for (int i = 0; i < prev_ldp_scan_->nrays; i++)
+    {
+      // printf("prev i, valid[i]: %d, %d\n",i,prev_ldp_scan_->valid[i]);
+      if (prev_ldp_scan_->valid[i])
+      {
+        // std::cout<<"000"<<std::endl;
+        dist = prev_ldp_scan_->readings[i];
+        theta = prev_ldp_scan_->theta[i];
+        // std::cout<<"dist, theta: "<<dist<<", "<<theta<<std::endl;
+        // std::cout<<"cos, sin: "<<cos(theta)<<", "<<sin(theta)<<std::endl;
+        pc_origin_.points[i].x = dist * cos(theta);
+        pc_origin_.points[i].y = dist * sin(theta);
+        pc_origin_.points[i].z = 0;
+        
+      }else{
+        pc_origin_.points[i].x = 0;
+        pc_origin_.points[i].y = 0;
+        pc_origin_.points[i].z = 0;
+      }
+      // printf("curr x,y,z: %.3f, %.3f, %.3f\n", pc_origin_.points[i].x, pc_origin_.points[i].y, pc_origin_.points[i].z);
+    }
+
+    //printf("min_ang, max_ang, ang_inc: %.3f %.3f %.3f\n",scan_msg->angle_min, scan_msg->angle_max, scan_msg->angle_increment);
+  }
+  LDP curr_ldp_scan;
+  laserScanToLDP(scan_msg, curr_ldp_scan, scan_msg->ranges.size()/2);
+ 
+  //for cloud display
+  pc_curr_.width = curr_ldp_scan->nrays;
+  pc_curr_.height = 1;
+  pc_curr_.resize(curr_ldp_scan->nrays);
+  for (int i = 0; i < curr_ldp_scan->nrays; i++)
+  {
+    // printf("curr i, valid[i]: %d, %d\n",i,curr_ldp_scan->valid[i]);
+    if (curr_ldp_scan->valid[i])
+    {
+      dist = curr_ldp_scan->readings[i];
+      theta = curr_ldp_scan->theta[i];
+      pc_curr_.points[i].x = dist * cos(theta);
+      pc_curr_.points[i].y = dist * sin(theta);
+      pc_curr_.points[i].z = 0;
+    }
+    else
+    {
+      pc_curr_.points[i].x = 0;
+      pc_curr_.points[i].y = 0;
+      pc_curr_.points[i].z = 0;
+    }
+    // printf("curr x,y,z: %.3f, %.3f, %.3f\n", pc_curr_.points[i].x, pc_curr_.points[i].y, pc_curr_.points[i].z);
+  }
+  processScan(curr_ldp_scan, scan_msg->header.stamp);
+}
+
+void LaserScanMatcher::processScan(LDP& curr_ldp_scan, const ros::Time& time)
+{
+  // 发布tf模板
+  // tf::TransformBroadcaster tf_broadcaster_;
+  // tf::Transform transform;
+  // transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0));
+  // transform.setRotation( tf::Quaternion(0, 0, 0, 1));
+  // tf::StampedTransform transform_msg (transform, ros::Time::now(), "map", "livox_frame");
+  // tf_broadcaster_.sendTransform(transform_msg);
+
+  ros::WallTime start = ros::WallTime::now();
+
+  prev_ldp_scan_->odometry[0] = 0.0;
+  prev_ldp_scan_->odometry[1] = 0.0;
+  prev_ldp_scan_->odometry[2] = 0.0;
+
+  prev_ldp_scan_->estimate[0] = 0.0;
+  prev_ldp_scan_->estimate[1] = 0.0;
+  prev_ldp_scan_->estimate[2] = 0.0;
+
+  prev_ldp_scan_->true_pose[0] = 0.0;
+  prev_ldp_scan_->true_pose[1] = 0.0;
+  prev_ldp_scan_->true_pose[2] = 0.0;
+
+  input_.laser_ref  = prev_ldp_scan_;
+  input_.laser_sens = curr_ldp_scan;
+
+  input_.first_guess[0] = f2b_.getOrigin().getX();
+  input_.first_guess[1] = f2b_.getOrigin().getY();
+  input_.first_guess[2] = tf::getYaw(f2b_.getRotation());
+
+  // If they are non-Null, free covariance gsl matrices to avoid leaking memory
+  //清空GNU科学计算库协方差矩阵
+  if (output_.cov_x_m)
+  {
+    gsl_matrix_free(output_.cov_x_m);
+    output_.cov_x_m = 0;
+  }
+  if (output_.dx_dy1_m)
+  {
+    gsl_matrix_free(output_.dx_dy1_m);
+    output_.dx_dy1_m = 0;
+  }
+  if (output_.dx_dy2_m)
+  {
+    gsl_matrix_free(output_.dx_dy2_m);
+    output_.dx_dy2_m = 0;
+  }
+
+  // *** scan match - using point to line icp from CSM， CSM is C(anonical) Scan Matcher
+    // printf("first_guess: %.4f, %.4f, %.4f.\n",input_.first_guess[0],input_.first_guess[1],input_.first_guess[2]);
+    // printf("max_ang, max_lin, max_iter: %.4f, %.4f %d\n",input_.max_angular_correction_deg, input_.max_linear_correction, input_.max_iterations);
+    // printf("ep_xy, ep_the: %.4f, %.4f\n",input_.epsilon_xy, input_.epsilon_theta);
+    // printf("max_corr, use_corr, restart, re_thre, re_dt, re_dth: %.4f, %d, %d, %.4f, %.4f %.4f\n",input_.max_correspondence_dist, input_.use_corr_tricks, input_.restart, input_.restart_threshold_mean_error, input_.restart_dt, input_.restart_dtheta);
+    // printf("out_maxP, out_ada_or, out_ada_mu, out_rem_do: %.4f, %.4f, %.4f %d\n",input_.outliers_maxPerc, input_.outliers_adaptive_order, input_.outliers_adaptive_mult, input_.outliers_remove_doubles);
+    // printf("clust_thre, orien_nei, do_al_te, do_al_te_thr: %.4f, %d, %d, %.4f\n",input_.clustering_threshold, input_.orientation_neighbourhood, input_.do_alpha_test, input_.do_alpha_test_thresholdDeg);
+    // printf("do_visi, use_point, use_ml, use_sig, do_com_conv, debug_veri: %d, %d, %d, %d, %d, %d\n",input_.do_visibility_test, input_.use_point_to_line_distance, input_.use_ml_weights, input_.use_sigma_weights, input_.do_compute_covariance, input_.debug_verify_tricks);
+    // printf("laser[3]: %.4f, %.4f, %.4f\n",input_.laser[0],input_.laser[1],input_.laser[2]);
+    // printf("sigma, min_r, max_r: %.4f, %.4f, %.4f\n",input_.sigma, input_.min_reading, input_.max_reading);
+    // printf("gpm_the, gpm_ex, gpm_int: %.4f, %.4f, %d\n",input_.gpm_theta_bin_size_deg, input_.gpm_extend_range_deg, input_.gpm_interval);
+    // printf("--------------- hsm -------------\n");
+    // printf("max_norm, lin_cel, ang_cel, num_ang, ang_hyp: %.4f, %.4f, %.4f, %d, %.4f\n", input_.hsm.max_norm, input_.hsm.linear_cell_size, input_.hsm.angular_cell_size_deg, input_.hsm.num_angular_hypotheses, input_.hsm.angular_hyp_min_distance_deg);
+    // printf("xc_ndi, xc_dir_min, lin_xc_max, lin_xc_pea, max_trans, debug_true: %d, %.4f, %d, %.4f, %.4f, %d\n",input_.hsm.xc_ndirections,input_.hsm.xc_directions_min_distance_deg, input_.hsm.linear_xc_max_npeaks,input_.hsm.linear_xc_peaks_min_distance, input_.hsm.max_translation, input_.hsm.debug_true_x_valid);
+    // printf("debug_true_x[3]: %.4f, %.4f, %.4f\n",input_.hsm.debug_true_x[0],input_.hsm.debug_true_x[1],input_.hsm.debug_true_x[2]);
+  
+  ros::WallTime end0 = ros::WallTime::now();
+  // ******************** csm ICP配准 ********************
+  sm_icp(&input_, &output_);
+
+  ros::WallTime end1 = ros::WallTime::now();
+  double dur1 = (end1 - end0).toSec() * 1e3;
+  //ROS_INFO("Scan matcher total duration: %.2f ms", dur1);
+
+  // // ******************** pcl ICP配准 ********************
+	// pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
+  // pcl::PointCloud<pcl::PointXYZ>::Ptr ori_ptr = pc_origin_.makeShared();
+  // pcl::PointCloud<pcl::PointXYZ>::Ptr cur_ptr = pc_curr_.makeShared();
+	// icp.setInputSource(ori_ptr);
+	// icp.setInputTarget(cur_ptr);
+	// pcl::PointCloud<pcl::PointXYZ>::Ptr Final(new pcl::PointCloud<pcl::PointXYZ>);
+  // ros::WallTime end1_1 = ros::WallTime::now();
+  // double dur1_1 = (end1_1 - end1).toSec() * 1e3;
+	// icp.align(*Final);
+  // ros::WallTime end1_2 = ros::WallTime::now();
+  // double dur1_2 = (end1_2 - end1_1).toSec() * 1e3;
+  // Eigen::Matrix4f pcl_trans= icp.getFinalTransformation();
+	// //输出最终的变换矩阵（4x4）
+	// printf("pcl transform x,y,theta: %.3f, %.3f, %.3f\n",pcl_trans(0,3),pcl_trans(1,3),acos(pcl_trans(0,0)));
+  // // std::cout << pcl_trans << std::endl;
+  // ros::WallTime end2 = ros::WallTime::now();
+  // double dur2 = (end2 - end1).toSec() * 1e3;
+
+  // ******************** NDT配准 ********************
+  pcl::PointCloud<pcl::PointXYZ>::Ptr ori_ptr = pc_origin_.makeShared();
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cur_ptr = pc_curr_.makeShared();
+  // // Filtering input scan to roughly 10% of original size to increase speed of registration.
+  // pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+
+  pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
+  approximate_voxel_filter.setLeafSize (0.05, 0.05, 0.05);
+  approximate_voxel_filter.setInputCloud (ori_ptr);
+  approximate_voxel_filter.filter (*ori_ptr);
+
+  // std::cout << "Filtered cloud contains " << filtered_cloud->size ()
+  //           << " data points from room_scan2.pcd" << std::endl;
+
+  // Initializing Normal Distributions Transform (NDT).
+  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
+  pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+  // Setting scale dependent NDT parameters
+  // Setting minimum transformation difference for termination condition.
+  ndt.setTransformationEpsilon (0.00001);
+  // Setting maximum step size for More-Thuente line search.
+  ndt.setStepSize (0.1);
+  //Setting Resolution of NDT grid structure (VoxelGridCovariance).
+  ndt.setResolution (0.5);
+  // Setting max number of registration iterations.
+  ndt.setMaximumIterations (60);
+  // Setting point cloud to be aligned.
+  ndt.setInputSource (cur_ptr);
+  // Setting point cloud to be aligned to.
+  ndt.setInputTarget (ori_ptr);
+
+  int n = 3;
+  ndt_omp->setInputTarget(ori_ptr);
+  ndt_omp->setInputSource(cur_ptr);
+  ndt_omp->setNumThreads(n);
+  ndt_omp->setTransformationEpsilon(0.0001);
+  ndt_omp->setStepSize(0.05);
+  ndt_omp->setResolution(0.5);
+  ndt_omp->setMaximumIterations(35);
+  ndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7);
+
+  // Set initial alignment estimate found using robot odometry.
+  Eigen::AngleAxisf init_rotation (trans[2], Eigen::Vector3f::UnitZ ());
+  Eigen::Translation3f init_translation (trans[0], trans[1], 0);
+  Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();
+
+  ros::WallTime end1_1 = ros::WallTime::now();
+  double dur1_1 = (end1_1 - end1).toSec() * 1e3;
+
+  // Calculating required rigid transform to align the input cloud to the target cloud.
+  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
+  ndt_omp->align (*output_cloud, init_guess);
+  // std::cout << "Normal Distributions Transform has converged:" << ndt_omp->hasConverged ()
+  //           << " score: " << ndt_omp->getFitnessScore () << std::endl;
+
+  ros::WallTime end1_2 = ros::WallTime::now();
+  double dur1_2 = (end1_2 - end1_1).toSec() * 1e3;
+
+  Eigen::Matrix4f pcl_trans= ndt_omp->getFinalTransformation();
+	//输出最终的变换矩阵（4x4）
+	printf("pcl transform x,y,theta: %.3f, %.3f, %.3f\n",pcl_trans(0,3),pcl_trans(1,3),atan2(-pcl_trans(0,1), pcl_trans(0,0)));
+  // std::cout << pcl_trans << std::endl;
+  ros::WallTime end2 = ros::WallTime::now();
+  double dur2 = (end2 - end1).toSec() * 1e3;
+
+  // // Saving transformed input cloud.
+  // pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan1.pcd", *target_cloud)
+  // pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);
+
+  tf::Transform corr_ch;
+  // int valid = output_.valid;
+  int valid = 1;
+  if (valid)
+  {
+
+    // the correction of the laser's position, in the laser frame
+    tf::Transform corr_ch_l;
+    createTfFromXYTheta(pcl_trans(0,3),pcl_trans(1,3), atan2(-pcl_trans(0,1), pcl_trans(0,0)), corr_ch_l);
+    // trans[0] = pcl_trans(0,3);
+    // trans[1] = pcl_trans(1,3);
+    // trans[2] = atan2(-pcl_trans(0,1), pcl_trans(0,0));
+    // createTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l);
+    trans[0] = output_.x[0];
+    trans[1] = output_.x[1];
+    trans[2] = output_.x[2];
+    // printf("csm transform x,y,theta: %.3f, %.3f, %.3f\n",trans[0],trans[1],trans[2]);
+
+    //theta [-pi,pi]
+    //std::cout<<"theta: "<<trans[2]<<std::endl;
+    // the correction of the base's position, in the base frame
+    //corr_ch = base_to_laser_ * corr_ch_l * laser_to_base_;
+    corr_ch = laser_to_base_ * corr_ch_l * base_to_laser_;
+
+    // update the pose in the world frame
+    f2b_ = f2b_kf_ * corr_ch;
+
+    if (publish_tf_)
+    {
+      tf::StampedTransform transform_msg (f2b_, time, fixed_frame_, base_frame_);
+      tf_broadcaster_.sendTransform(transform_msg);
+
+      //定义发布的消息
+      sensor_msgs::PointCloud2 output_origin;
+      //把点云转化为ros消息
+      pcl::toROSMsg(pc_origin_, output_origin);
+      output_origin.header.frame_id = fixed_frame_;
+      origin_cloud_publisher_.publish(output_origin);
+
+      //定义发布的消息
+      sensor_msgs::PointCloud2 output_curr;
+      // //点云旋转
+      // Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity(); //单位矩阵
+      // float theta = M_PI / 20;
+      // //给矩阵赋值
+      // transform_1(0, 0) = cos(theta);
+      // transform_1(0, 1) = -sin(theta);
+      // transform_1(1, 0) = sin(theta);
+      // transform_1(1, 1) = cos(theta);
+      // //沿x轴平移0.025
+      // transform_1(0, 3) = 0.01;
+      // printf("Method #1: using a Matrix4f\n");
+      // std::cout << transform_1 << std::endl;
+      pcl::PointCloud<pcl::PointXYZ> curr1;
+      Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
+      transform_1.translation() << trans[0], trans[1], 0.0;
+      transform_1.rotate (Eigen::AngleAxisf (trans[2], Eigen::Vector3f::UnitZ()));
+      pcl::transformPointCloud(pc_curr_, curr1, transform_1);
+      //把点云转化为ros消息
+      pcl::toROSMsg(curr1, output_curr);
+      output_curr.header.frame_id = fixed_frame_;
+      current_cloud_publisher_.publish(output_curr);
+      // std::cout<<"----"<<std::endl;
+
+      pcl::PointCloud<pcl::PointXYZ> curr2;
+      sensor_msgs::PointCloud2 output_curr2;
+      Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
+      transform_2.translation() << pcl_trans(0,3),pcl_trans(1,3), 0.0;
+      transform_2.rotate (Eigen::AngleAxisf (atan2(-pcl_trans(0,1), pcl_trans(0,0)), Eigen::Vector3f::UnitZ()));
+      pcl::transformPointCloud(pc_curr_, curr2, transform_2);
+      //把点云转化为ros消息
+      pcl::toROSMsg(curr2, output_curr2);
+      output_curr2.header.frame_id = fixed_frame_;
+      current_pcl_cloud_publisher_.publish(output_curr2);
+      // std::cout<<"****"<<std::endl;
+    }
+  }
+  else
+  {
+    corr_ch.setIdentity();
+    ROS_WARN("Error in scan matching");
+  }
+
+  // **** swap old and new
+
+  if (newKeyframeNeeded(corr_ch))
+  {
+    // generate a keyframe
+    ld_free(prev_ldp_scan_);
+    prev_ldp_scan_ = curr_ldp_scan;
+    f2b_kf_ = f2b_;
+    std::cout<<"new key generated.............."<<std::endl;
+  }
+  else
+  {
+    ld_free(curr_ldp_scan);
+    // ld_free(ldp_prev);
+  }
+
+  last_icp_time_ = time;
+
+  // **** statistics
+  ros::WallTime end3 = ros::WallTime::now();
+  double dur3 = (end3 - start).toSec() * 1e3;
+  std::cout<<"time consumption(csm, p1 p2 pcl and total): "<<dur1<<", "/* <<dur1_1<<", "<<dur1_2<<", "*/<<dur2<<", "<<dur3<<std::endl;
+}
+
+bool LaserScanMatcher::newKeyframeNeeded(const tf::Transform& d)
+{
+  if (fabs(tf::getYaw(d.getRotation())) > kf_dist_angular_) return true;
+
+  double x = d.getOrigin().getX();
+  double y = d.getOrigin().getY();
+  if (x*x + y*y > kf_dist_linear_sq_) return true;
+
+  return false;
+}
+
+void LaserScanMatcher::laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg,
+                                            LDP& ldp, unsigned int count)
+{
+  unsigned int n = scan_msg->ranges.size();
+  if(count>n || count<0)
+    return;
+  ldp = ld_alloc_new(n);
+  //printf("n, ang_lim: %d, %.3f\n",n,scan_msg->angle_min + n * scan_msg->angle_increment);
+  for (unsigned int i = 0; i < n; i++)
+  {
+    // calculate position in laser frame
+
+    double r = scan_msg->ranges[i];
+
+    if (r > scan_msg->range_min && r < scan_msg->range_max && i<=count)
+    {
+      // fill in laser scan data
+
+      ldp->valid[i] = 1;
+      ldp->readings[i] = r;
+    }
+    else
+    {
+      ldp->valid[i] = 0;
+      ldp->readings[i] = -1;  // for invalid range -1
+    }
+
+    ldp->theta[i]    = scan_msg->angle_min + i * scan_msg->angle_increment;
+
+    ldp->cluster[i]  = -1;
+  }
+
+  ldp->min_theta = ldp->theta[0];
+  ldp->max_theta = ldp->theta[n-1];
+
+  ldp->odometry[0] = 0.0;
+  ldp->odometry[1] = 0.0;
+  ldp->odometry[2] = 0.0;
+
+  ldp->true_pose[0] = 0.0;
+  ldp->true_pose[1] = 0.0;
+  ldp->true_pose[2] = 0.0;
+}
+
+void LaserScanMatcher::createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
+{
+  a_cos_.clear();
+  a_sin_.clear();
+
+  for (unsigned int i = 0; i < scan_msg->ranges.size(); ++i)
+  {
+    double angle = scan_msg->angle_min + i * scan_msg->angle_increment;
+    a_cos_.push_back(cos(angle));
+    a_sin_.push_back(sin(angle));
+  }
+
+  input_.min_reading = scan_msg->range_min;
+  input_.max_reading = scan_msg->range_max;
+}
+
+bool LaserScanMatcher::getBaseToLaserTf (const std::string& frame_id)
+{
+  ros::Time t = ros::Time::now();
+
+  tf::StampedTransform base_to_laser_tf;
+  try
+  {
+    tf_listener_.waitForTransform(
+      base_frame_, frame_id, t, ros::Duration(1.0));
+    tf_listener_.lookupTransform (
+      base_frame_, frame_id, t, base_to_laser_tf);
+  }
+  catch (tf::TransformException ex)
+  {
+    ROS_WARN("Could not get initial transform from base to laser frame, %s", ex.what());
+    return false;
+  }
+  base_to_laser_ = base_to_laser_tf;
+  laser_to_base_ = base_to_laser_.inverse();
+
+  return true;
+}
+
+void LaserScanMatcher::createTfFromXYTheta(
+  double x, double y, double theta, tf::Transform& t)
+{
+  t.setOrigin(tf::Vector3(x, y, 0.0));
+  tf::Quaternion q;
+  q.setRPY(0.0, 0.0, theta);
+  t.setRotation(q);
+}
+
+} // namespace scan_tools

scan_matcher_ICP/src/laser_scan_matcher.h

@@ -0,0 +1,154 @@
+/*
+ * Copyright (c) 2011, Ivan Dryanovski, William Morris
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the CCNY Robotics Lab nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*  This package uses Canonical Scan Matcher [1], written by
+ *  Andrea Censi
+ *
+ *  [1] A. Censi, "An ICP variant using a point-to-line metric"
+ *  Proceedings of the IEEE International Conference
+ *  on Robotics and Automation (ICRA), 2008
+ */
+
+#ifndef LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H
+#define LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H
+
+#include <ros/ros.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/LaserScan.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/PoseWithCovariance.h>
+#include <geometry_msgs/TwistStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl_ros/point_cloud.h>
+
+#include <csm/csm_all.h>  // csm defines min and max, but Eigen complains
+#undef min
+#undef max
+
+namespace scan_tools
+{
+
+class LaserScanMatcher
+{
+  public:
+
+    LaserScanMatcher(ros::NodeHandle nh, ros::NodeHandle nh_private);
+    ~LaserScanMatcher();
+
+  private:
+
+    // **** ros
+
+    ros::NodeHandle nh_;
+    ros::NodeHandle nh_private_;
+
+    ros::Subscriber scan_subscriber_;
+
+    tf::TransformListener    tf_listener_;
+    tf::TransformBroadcaster tf_broadcaster_;
+
+    tf::Transform base_to_laser_; // static, cached
+    tf::Transform laser_to_base_; // static, cached, calculated from base_to_laser_
+
+    // **** parameters
+
+    std::string base_frame_;
+    std::string fixed_frame_;
+    bool publish_tf_;
+    std::vector<double> position_covariance_;
+    std::vector<double> orientation_covariance_;
+
+    double kf_dist_linear_;
+    double kf_dist_linear_sq_;
+    double kf_dist_angular_;
+
+    // **** state variables
+
+    boost::mutex mutex_;
+
+    bool initialized_;
+
+    tf::Transform f2b_;    // fixed-to-base tf (pose of base frame in fixed frame)
+    tf::Transform f2b_kf_; // pose of the last keyframe scan in fixed frame
+
+    ros::Time last_icp_time_;
+
+    sensor_msgs::Imu latest_imu_msg_;
+    sensor_msgs::Imu last_used_imu_msg_;
+    nav_msgs::Odometry latest_odom_msg_;
+    nav_msgs::Odometry last_used_odom_msg_;
+
+    geometry_msgs::Twist latest_vel_msg_;
+
+    std::vector<double> a_cos_;
+    std::vector<double> a_sin_;
+
+    sm_params input_;
+    sm_result output_;
+    LDP prev_ldp_scan_;
+
+    ros::Publisher  origin_cloud_publisher_;
+    ros::Publisher  current_cloud_publisher_;
+    ros::Publisher  current_pcl_cloud_publisher_;
+    pcl::PointCloud<pcl::PointXYZ> pc_origin_;
+    pcl::PointCloud<pcl::PointXYZ> pc_curr_;
+
+    double trans[3]={0};
+
+    // **** methods
+
+    void initParams();
+
+    void processScan(LDP& curr_ldp_scan, const ros::Time& time);
+
+    void laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& scan_msg,
+                              LDP& ldp, unsigned int count);
+
+    void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg);
+
+    void createCache (const sensor_msgs::LaserScan::ConstPtr& scan_msg);
+
+    bool getBaseToLaserTf (const std::string& frame_id);
+
+    bool newKeyframeNeeded(const tf::Transform& d);
+
+    void createTfFromXYTheta(double x, double y, double theta, tf::Transform& t);
+};
+
+} // namespace scan_tools
+
+#endif // LASER_SCAN_MATCHER_LASER_SCAN_MATCHER_H

scan_matcher_ICP/src/laser_scan_matcher_node.cpp

@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2011, Ivan Dryanovski, William Morris
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the CCNY Robotics Lab nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*  This package uses Canonical Scan Matcher [1], written by
+ *  Andrea Censi
+ *
+ *  [1] A. Censi, "An ICP variant using a point-to-line metric"
+ *  Proceedings of the IEEE International Conference
+ *  on Robotics and Automation (ICRA), 2008
+ */
+
+#include <laser_scan_matcher.h>
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "LaserScanMatcher");
+  ros::NodeHandle nh;
+  ros::NodeHandle nh_private("~");
+  scan_tools::LaserScanMatcher laser_scan_matcher(nh, nh_private);
+  ros::spin();
+  return 0;
+}

scan_matcher_ICP/src/ndtomp_test.cpp

@@ -0,0 +1,69 @@
+#include <cmath>
+
+#include <loam_velodyne/common.h>
+#include <nav_msgs/Odometry.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/voxel_grid.h>
+//#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/LaserScan.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_broadcaster.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/registration/ndt.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <ndt_omp.h>
+
+using std::sin;
+using std::cos;
+using std::atan2;
+bool first = true;
+ros::Publisher pub1, pub2;
+sensor_msgs::PointCloud2 first_cloud;
+pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+void ndtcallback(const sensor_msgs::PointCloud2ConstPtr& msg)
+{
+    pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>::Ptr ndt_omp(new pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>());
+    sensor_msgs::PointCloud2 res_cloud;
+    if(first)
+    {
+        first_cloud = *msg;
+        first_cloud.header.frame_id = "/laser";
+        pcl::fromROSMsg(first_cloud, *target_cloud);
+        first = false;
+        return;
+    }
+    pcl::fromROSMsg(*msg, *input_cloud);
+    int n = 3;
+    ndt_omp->setInputTarget(target_cloud);
+    ndt_omp->setInputCloud(input_cloud);
+    ndt_omp->setNumThreads(n);
+    ndt_omp->setTransformationEpsilon(0.002);
+    ndt_omp->setStepSize(0.05);
+    ndt_omp->setResolution(0.3);
+    ndt_omp->setMaximumIterations(100);
+    ndt_omp->setNeighborhoodSearchMethod(pclomp::DIRECT7);
+    Eigen::AngleAxisf init_rotation(0, Eigen::Vector3f::UnitZ()); 
+    Eigen::Translation3f init_translation(0, 0, 0); 
+    Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    ndt_omp->align(*output_cloud, init_guess);
+    Eigen::Matrix4f globalPose=ndt_omp->getFinalTransformation();
+    std::cout<<globalPose<<std::endl;
+}
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "ndtomp_test");
+    ros::NodeHandle nh;
+    ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("cloud2", 1000, ndtcallback);
+    pub1 = nh.advertise<sensor_msgs::PointCloud2>("cloud_1",1000);
+    pub2 = nh.advertise<sensor_msgs::PointCloud2>("cloud_2",1000);
+    ros::spin();
+    return 0;
+}

scan_matcher_ICP/src/pclomp/gicp_omp.cpp

@@ -0,0 +1,6 @@
+#include <pclomp/gicp_omp.h>
+#include <pclomp/gicp_omp_impl.hpp>
+
+template class pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ>;
+template class pclomp::GeneralizedIterativeClosestPoint<pcl::PointXYZI, pcl::PointXYZI>;
+

scan_matcher_ICP/src/pclomp/ndt_omp.cpp

@@ -0,0 +1,5 @@
+#include <pclomp/ndt_omp.h>
+#include <pclomp/ndt_omp_impl.hpp>
+
+template class pclomp::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>;
+template class pclomp::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI>;

scan_matcher_ICP/src/pclomp/voxel_grid_covariance_omp.cpp

@@ -0,0 +1,5 @@
+#include <pclomp/voxel_grid_covariance_omp.h>
+#include <pclomp/voxel_grid_covariance_omp_impl.hpp>
+
+template class pclomp::VoxelGridCovariance<pcl::PointXYZ>;
+template class pclomp::VoxelGridCovariance<pcl::PointXYZI>;

scan_tools

@@ -0,0 +1 @@
+Subproject commit 219c31965ff904c59072389d1873a21bcb433e81