first version of slam

src/localization_module_lio_locus/CMakeLists.txt

@@ -0,0 +1,97 @@
+cmake_minimum_required(VERSION 3.8)
+project(lio_locus_humble)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+#add_compile_options(-std=c++17 -O3 -w)
+add_compile_options(-std=c++17 -O3 -w -Wno-dev)
+#add_compile_options(-std=c++11)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(pcl_conversions REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(std_srvs REQUIRED)
+
+find_package(Boost)
+find_package(yaml-cpp 0.6 REQUIRED)
+find_package(OpenMP)
+find_package(Eigen3)
+
+
+set(CMAKE_PREFIX_PATH "/home/demo/deps_install" ${CMAKE_PREFIX_PATH})
+set(CMAKE_INCLUDE_PATH "/home/demo/deps_install/include" ${CMAKE_INCLUDE_PATH})
+set(CMAKE_LIBRARY_PATH "/home/demo/deps_install/lib" ${CMAKE_LIBRARY_PATH})
+
+set(PCL_DIR /home/demo/deps_install/share/pcl-1.10)
+
+set(PCL_ROOT /home/demo/deps_install)
+
+find_package(PCL REQUIRED)
+
+message("##################" ${PCL_INCLUDE_DIRS})
+message("##################" ${PCL_LIBRARIES})
+message("---------------------------"${PCL_INCLUDE_DIRS})
+
+if (OPENMP_FOUND)
+    set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+    set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+    set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
+endif()
+
+include_directories(
+    include/${PROJECT_NAME}
+    ${pcl_conversions_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+    ${YAML_CPP_INCLUDE_DIR}
+    /user/include
+    /home/demo/deps_install/include/pcl-1.10
+    )
+
+link_directories(
+        /home/demo/deps_install/lib
+)
+
+
+
+add_subdirectory(src)
+#add_subdirectory(apps/nodelets)
+
+install(
+    DIRECTORY launch
+	DESTINATION share/${PROJECT_NAME}
+)
+
+install(
+    DIRECTORY config 
+	DESTINATION share/${PROJECT_NAME}
+)
+
+install(
+    DIRECTORY rviz 
+	DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
+
+

src/localization_module_lio_locus/README.md

@@ -0,0 +1,260 @@
+# LIO-Locus (Lidar Inertial Odometry)
+
+[TOC]
+
+## 1. Abstract
+
+​	This package is a CONCISE implementation for Loosely-Coupled LIO(LIDAR Inertial Odometry) based on open-source project LOCUS. 
+
+* LIDAR Centric Sensor Sources: LIDAR sensor is centric in this approach, and IMU orientation value can be used as a prior for scan matching. However, IMU is not a necessity. Pure scan-only localization mode is also supported;
+* Working Mode: this package can work in SLAM mode starting from scratch, and it can also work in PURE LOCALIZATION mode with prior map loaded from user specified path. 
+
+Note: The PCL Library used in this code is PCL-1.10.
+
+## 2. ROS Wrapper
+
+​	ROS wrapper interfaces with ROS environment, subscribing raw sensor data and pass them to algorithm, then the estimation results from algorithm are returned to ROS environment:
+
+- Raw cloud data is pre-processed in way of voxel filtering and normal vector computation, and later fed to  pose estimation algorithm for cloud registration;
+- Raw IMU data is pre-processed with complementary filter for orientation estimation, and then can be optionally used as pose prior input for cloud registration;
+- During cloud registration, robot pose could be estimated in SLAM mode with online-built cloud map and in PURE-LOCALIZATION model with previously uploaded cloud map.
+
+## 3. API (Application Programming Interfaces)
+
+### 3.0 Data Flow Graph
+
+​	The codes in app folder take the following forms:
+
+* Two independent pre-processing ROS nodes for cloud and IMU works together with state estimator ROS node, as illustrated in figure below:
+
+![](./figs/rosgraph_lio.png)
+
+### 3.1 Cloud Pre-Processor
+
+#### Parameter Loading
+
+* header file inclusion
+
+```
+#include "lio_locus/parameter_loading.h"
+```
+
+* parameter structure loading
+
+```
+bool loadParametersFromFileForCloudPreprocessing(std::string path_to_file, ParameterCloudPreprocessing & params_cloud_preproc);
+```
+
+#### Functionalities
+
+* header file inclusion
+
+```
+#include "lio_locus/cloud_pre_processor.h"
+```
+
+* set basic parameters for cloud pre-processor;
+
+```
+void CloudPreProcessor::setCloudPreProcessingParameters
+	(double leaf_voxel_filtering, double filter_limit, size_cores, int size_k_search, double radius_search)
+```
+
+* to transform cloud to target frame and compute associated normal vectors:
+
+```
+bool CloudPreProcessor::transformCloudWithNormalCalculation
+	(pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_input, tf2::Transform T,
+	pcl::PointCloud<pcl::PointXYZINormal>::Ptr &cloud_normals);
+```
+
+### 3.2 IMU Pre-Processor
+
+#### Parameter Loading
+
+* header file inclusion
+
+```
+#include "lio_locus/parameter_loading.h"
+```
+
+* parameter structure loading
+
+```
+bool loadParametersFromFileForIMUPreprocessing(std::string path_to_file, ParameterImuPreprocessing &params_imu_preproc)
+```
+
+#### Functionalities
+
+* parameter setting
+
+```
+void ComplementaryFilter::setDoBiasEstimation(bool do_bias_estimation)
+```
+
+```
+void ComplementaryFilter::setDoAdaptiveGain(bool do_adaptive_gain)
+```
+
+* observation update
+
+```
+void ComplementaryFilter::update(double ax, double ay, double az, double wx, double wy, double wz, double dt)
+```
+
+* orientation retrieval
+
+```
+void ComplementaryFilter::getOrientation(double& q0, double& q1, double& q2, double& q3) const
+```
+
+### 3.3 LIO-Locus
+
+#### Parameter Loading
+
+* header file inclusion
+
+```
+#include "lio_locus/lio_locus.h"
+```
+
+* parameter structure loading
+
+```
+bool loadParametersFromFileForLioLocus(std::string path_to_file, ParameterLioLocus& params_lio_locus)
+```
+
+#### Initialization
+
+```
+bool LioLocus::initialize(string path_to_param);
+```
+
+#### Parameter Setting
+
+* set flag to use IMU data or not;
+
+```
+void LioLocus::setUseIMU(bool flag) 
+```
+
+* set flag to use consecutive scan matching or not;
+
+```
+void LioLocus::setUseScan2ScanMatching(bool flag) 
+```
+
+* set threshold values as input arguments for translation and rotation to insert new localized cloud to cloud map:
+
+```
+void LioLocus::setThresholds(double trans, double rot)
+```
+
+* set relative rotation from IMU frame id to lidar cloud frame id:
+
+```
+void LioLocus::setIMU2CloudExtrinsic(Eigen::Quaterniond quat) 
+```
+
+* set flag to use flat ground assumption or not;
+
+```
+void LioLocus::setFlatGroundAssumption(bool flag)
+```
+
+#### Functionalities:
+
+##### SLAM Mode with Online Built Map
+
+* start from scratch without any cloud map in memory, and cloud map is updated online:
+
+```
+void LioLocus::trackCurrentCloudWithOnlineBuiltMap(PointCloudType::Ptr msg_cloud);	
+```
+
+##### Pure Localization Mode with Loaded Cloud Map
+
+* to fresh memory cloud with data storage:
+
+```
+bool LioLocus::uploadPriorCloudMap(string fn_cloud_map, double leaf_size);
+```
+
+*  for re-localization when prior cloud map is loaded in advance:
+
+```
+void LioLocus::setPosePrior(geometry_utils::Transform3 pose_prior);
+```
+
+* cloud map in memory is not updated;
+
+```
+bool LioLocus::trackCurrentCloudWithPriorCloudMap(PointCloudType::Ptr msg_cloud);
+```
+
+* clear map stored in memory:
+
+```
+void LioLocus::clearCloudMapInMemory();
+```
+
+#### Data Retrieval
+
+##### Time Stamped Pose
+
+* get time-stamped robot pose:
+
+```
+inline TransformStamped getTimestampedLatestPose(){
+	return m_pose_latest;
+}
+```
+
+##### Localized Cloud
+
+* get latest localized cloud registered against cloud map in memory:
+
+```
+PointCloudType::Ptr getLatestLocalizedCloud()
+```
+
+##### Cloud Map
+
+* get cloud map stored in memory in form of Point Cloud:
+
+```
+bool getCloudMapInMemory(PointCloudType::Ptr cloud, double voxel_size = 0.5)
+```
+
+## 4. Algorithm Structure
+
+The logics for data flow within structure of LIO-Locus is illustrated in figure below. The blue blocks are cloud map construction steps, and the magenta blocks are cloud registration steps, including scan-to-scan matching and scan-to-map matching.
+
+![](./figs/further_dev_flowchart_logic_locus.jpg)
+
+## 5. Summary
+
+​	This package constructs algorithm stuff as stand-alone cpp class, feeding LIDAR cloud data and optionally IMU data  into algorithm for data processing.
+
+​	The 6 DOF (Degree of Freedom) pose of the robot together with online localized cloud, the cloud map in form of PCL data format, can be retrieved for further application such as navigation and so on.
+
+## 6. Acknowledgement
+
+​	The developer of the code shows great thanks and respect to the original open-source code LOCUS.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

src/localization_module_lio_locus/apps/nodelets/CMakeLists.txt

@@ -0,0 +1,98 @@
+
+add_executable(lio_locus_node lio_locus_node.cpp)
+
+target_link_libraries(lio_locus_node
+	${PROJECT_NAME} 
+	point_cloud_odometry
+	point_cloud_localization
+	point_cloud_mapper
+	boost_filesystem
+)
+
+target_include_directories(lio_locus_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+target_compile_features( lio_locus_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+
+ament_target_dependencies(
+  lio_locus_node
+  "rclcpp"
+  #  "std_msgs"
+  "sensor_msgs"
+  "nav_msgs"
+  "pcl_conversions"
+  "std_srvs"
+  "tf2"
+  #"tf2_ros"
+)
+
+# install exe to folder within lib;
+install(TARGETS lio_locus_node
+  DESTINATION lib/${PROJECT_NAME}
+  )
+
+
+
+# install exe to folder within lib;
+install(TARGETS lio_locus_node 
+  DESTINATION lib/${PROJECT_NAME}
+  )
+#--------------------------------------------------------
+add_executable(imu_preprocessing_node imu_preprocessing_node.cpp)
+ament_target_dependencies(imu_preprocessing_node rclcpp sensor_msgs)
+
+target_link_libraries(imu_preprocessing_node 
+    complementary_filter
+	${YAML_CPP_LIBRARIES}
+    boost_filesystem
+)
+
+target_include_directories(imu_preprocessing_node PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+target_compile_features(imu_preprocessing_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+
+ament_target_dependencies(
+  imu_preprocessing_node
+  "rclcpp"
+  #  "std_msgs"
+  "sensor_msgs"
+  "nav_msgs"
+  #"pcl_conversions"
+  #"tf2"
+  "tf2_ros"
+)
+
+install(TARGETS imu_preprocessing_node 
+  DESTINATION lib/${PROJECT_NAME})
+#-------------------------------------
+add_executable(cloud_preprocessing_node cloud_preprocessing_node.cpp)
+
+target_link_libraries(cloud_preprocessing_node 
+	cloud_pre_processor
+	boost_filesystem
+)
+
+#target_include_directories(cloud_preprocessing_node PUBLIC
+#  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+#  $<INSTALL_INTERFACE:include>)
+
+target_compile_features(cloud_preprocessing_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+
+ament_target_dependencies(
+    cloud_preprocessing_node
+  "rclcpp"
+  #  "std_msgs"
+  "sensor_msgs"
+  #"nav_msgs"
+  #"pcl_conversions"
+  "tf2"
+  #"tf2_ros"
+)
+
+install(TARGETS cloud_preprocessing_node 
+  DESTINATION lib/${PROJECT_NAME}
+  )
+#

src/localization_module_lio_locus/apps/nodelets/cloud_preprocessing_node.cpp

@@ -0,0 +1,125 @@
+
+/*
+ * Author: Solomon
+ * Date: Oct. 20, 2025,
+ *
+ */
+
+/*
+ * functionality: pre-processing of cloud for more sparse and normal-vectorized cloud;
+ * inputs: lidar cloud, transformation from lidar frame id to target frame id, and parameters;
+ * outputs: pre-processing cloud after voxel grid filtering and normal vector calculation;
+ *
+ * note: transformation can also be retrieved from IMU message;
+ */
+
+//================================================================================
+// algorithm stuff;
+#include "lio_locus/cloud_pre_processor.h"
+ParameterCloudPreprocessing params_cloud_preproc;
+tf2::Transform T_base2laser;
+CloudPreProcessor::Ptr ptr_cloud_pre_processor;
+//================================================================================
+// ROS stuff;
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/executors/multi_threaded_executor.hpp>
+#include <tf2/LinearMath/Transform.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
+
+using std::string;
+
+void callbackLidarRaw(const sensor_msgs::msg::PointCloud2 &msg);
+rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cloud_preprocessed;
+rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr pub_dt_cloud_preproc;
+
+int main(int argc, char **argv)
+{
+	// node for subscription of cloud;
+	rclcpp::init(argc, argv);
+	rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("cloud_preprocessing_node");
+
+    // loading parameters for LIO-Locus;    
+    string path_to_param_file;
+    node->declare_parameter("file_params", "/home/solomon/ws_humble_pcl_10/src/lio_locus_humble/config/parameters_lio_locus.yaml");
+    path_to_param_file = node->get_parameter("file_params").as_string();
+
+    RCLCPP_INFO_STREAM(rclcpp::get_logger(params_cloud_preproc.log_tag), "... file params -> #["    
+        << path_to_param_file << "]");
+	//================================================================================
+	// algorithm instantiation;
+	ParameterCloudPreprocessing params_cloud_preproc;
+	loadParametersFromFileForCloudPreprocessing(path_to_param_file, params_cloud_preproc);
+	T_base2laser.setOrigin(tf2::Vector3(params_cloud_preproc.ext_tx, params_cloud_preproc.ext_ty, params_cloud_preproc.ext_tz));
+	tf2::Quaternion q;
+	q.setRPY(params_cloud_preproc.ext_roll, params_cloud_preproc.ext_pitch, params_cloud_preproc.ext_yaw);
+	T_base2laser.setRotation(q);
+	ptr_cloud_pre_processor = CloudPreProcessor::Ptr(new CloudPreProcessor());
+	ptr_cloud_pre_processor->setCloudPreProcessingParameters(params_cloud_preproc.leaf_voxel_filtering, params_cloud_preproc.filter_limit,
+															 params_cloud_preproc.size_cores,
+															 params_cloud_preproc.size_k_search_knn, params_cloud_preproc.radius_search_knn);
+	ptr_cloud_pre_processor->setLogPath(params_cloud_preproc.path_log);															 
+	//================================================================================
+
+	rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_cloud_raw =
+		 node->create_subscription<sensor_msgs::msg::PointCloud2>(params_cloud_preproc.topic_cloud_raw, 9, callbackLidarRaw);
+	pub_cloud_preprocessed = node->create_publisher<sensor_msgs::msg::PointCloud2>(params_cloud_preproc.topic_cloud, 9);
+	pub_dt_cloud_preproc = node->create_publisher<geometry_msgs::msg::Vector3Stamped>("/dt_cloud_preprocessing", 9);
+
+	rclcpp::spin(node);
+
+	return 0;
+}
+
+void callbackLidarRaw(const sensor_msgs::msg::PointCloud2 &msg)
+{
+
+	rclcpp::Time current_time;
+	current_time = rclcpp::Clock().now();
+	double seconds_start = current_time.seconds();
+
+	//================================================================================
+	// transform cloud from ROS format to PCL format;
+	pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_raw(
+		new pcl::PointCloud<pcl::PointXYZINormal>());
+
+	if(params_cloud_preproc.topic_cloud_raw == "/rsm1_1")
+	{
+		cloud_raw = CloudPreProcessor::adaptCloudForm(msg);
+	}else if (params_cloud_preproc.topic_cloud_raw == "/livox/lidar"){
+		cloud_raw = CloudPreProcessor::adaptCloudFormFromLivoxCloud(msg);
+	}	
+
+	pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_normals(
+		new pcl::PointCloud<pcl::PointXYZINormal>());
+	ptr_cloud_pre_processor->transformCloudWithNormalCalculation(cloud_raw,
+																 T_base2laser, cloud_normals);
+	//================================================================================
+
+	current_time = rclcpp::Clock().now();
+	double seconds_end = current_time.seconds();
+	double dt = seconds_end - seconds_start;
+	dt *= 1e3;
+	RCLCPP_INFO_STREAM(rclcpp::get_logger(params_cloud_preproc.log_tag),
+					   "... time passed for cloud pre-processing: [" << dt << "] ms");
+
+	// // publish time elapsed;
+	// geometry_msgs::msg::Vector3Stamped msg_dt;
+	// msg_dt.header.stamp = msg.header.stamp;
+	// msg_dt.vector.x = dt;
+	// pub_dt_cloud_preproc->publish(msg_dt);
+
+	sensor_msgs::msg::PointCloud2 msg_cloud_ros2;
+	pcl::toROSMsg(*cloud_normals, msg_cloud_ros2);
+	msg_cloud_ros2.header.stamp = msg.header.stamp;
+	msg_cloud_ros2.header.frame_id = params_cloud_preproc.frame_id_base;
+	pub_cloud_preprocessed->publish(msg_cloud_ros2);
+
+	// current_time = rclcpp::Clock().now();
+	// double seconds_end2 = current_time.seconds();
+	// dt = seconds_end2 - seconds_end;
+	// RCLCPP_INFO_STREAM(rclcpp::get_logger(params_cloud_preproc.log_tag),
+	// 				   "... time passed for cloud format conversion: [" << dt * 1e3 << "] ms");
+}

src/localization_module_lio_locus/apps/nodelets/imu_preprocessing_node.cpp

@@ -0,0 +1,219 @@
+
+/*
+ * Author: Solomon
+ * Date: Oct. 20, 2025,
+ *
+ */
+
+/*
+ * functionality: pre-processing IMU data for orientation;
+ * input: IMU content with linear accelerator values and gyroscope values;
+ * output: compensated IMU data with orientation from filter;
+ */
+
+//================================================================================
+// algorithm stuff;
+#include "lio_locus/complementary_filter.h"
+ParameterImuPreprocessing params_imu_preproc;
+imu_tools::ComplementaryFilter filter_;
+//================================================================================
+// ROS stuff;
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/executors/multi_threaded_executor.hpp>
+
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
+
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2_ros/transform_broadcaster.h>
+
+#include <yaml-cpp/yaml.h>
+#include <boost/shared_ptr.hpp>
+#include <boost/filesystem.hpp>
+
+using std::string;
+
+typedef sensor_msgs::msg::Imu ImuMsg;
+void callbackImuRaw(const sensor_msgs::msg::Imu &msg);
+void publish(ImuMsg imu_msg_raw);
+rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr rpy_publisher_;
+rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr state_publisher_;
+rclcpp::Publisher<ImuMsg>::SharedPtr imu_publisher_;
+rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr dt_publisher_;
+double orientation_variance_;
+bool reverse_tf_;
+tf2::Quaternion hamiltonToTFQuaternion(double q0, double q1, double q2,
+                                       double q3);
+boost::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;                                       
+
+int main(int argc, char **argv)
+{
+    // ROS parameter stuff;
+    rclcpp::init(argc, argv);
+    rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("imu_preprocessing_node");        
+    tf_broadcaster_ = boost::shared_ptr<tf2_ros::TransformBroadcaster>(new tf2_ros::TransformBroadcaster(node));
+
+    string path_to_param_file;
+    node->declare_parameter("file_params", "/home/solomon/ws_humble_pcl_10/src/lio_locus_humble/config/parameters_lio_locus.yaml");
+    path_to_param_file = node->get_parameter("file_params").as_string();
+    RCLCPP_INFO_STREAM(rclcpp::get_logger(params_imu_preproc.log_tag), "... parameter path -> [" << path_to_param_file << "]");
+
+    //================================================================================
+    // algorithm stuff for initialization;
+    loadParametersFromFileForIMUPreprocessing(path_to_param_file, params_imu_preproc);
+    filter_.setDoBiasEstimation(params_imu_preproc.do_bias_estimation);
+    filter_.setDoAdaptiveGain(params_imu_preproc.do_adaptive_gain);
+    if (params_imu_preproc.do_bias_estimation)
+    {
+        if (!filter_.setBiasAlpha(params_imu_preproc.bias_alpha))
+            RCLCPP_WARN(node->get_logger(),
+                        "Invalid bias_alpha passed to ComplementaryFilter.");
+    }
+    orientation_variance_ = params_imu_preproc.orientation_stddev * params_imu_preproc.orientation_stddev;  
+    filter_.setLogPath(params_imu_preproc.path_log);      
+    //================================================================================    
+    // ROS stuff;
+    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu =
+        node->create_subscription<sensor_msgs::msg::Imu>(params_imu_preproc.topic_imu_raw, 9, callbackImuRaw);
+    rpy_publisher_ = node->create_publisher<geometry_msgs::msg::Vector3Stamped>("imu/rpy/filtered", 9);
+    state_publisher_ = node->create_publisher<std_msgs::msg::Bool>("imu/steady_state", 9);
+    imu_publisher_ = node->create_publisher<ImuMsg>("/imu_filtered", 9);
+    dt_publisher_ = node->create_publisher<geometry_msgs::msg::Vector3Stamped>("/dt_imu_preprocessing",9);
+
+    rclcpp::spin(node);
+
+    return 0;
+}
+
+// feeding raw data for orientation estimation;
+void callbackImuRaw(const sensor_msgs::msg::Imu &imu_msg_raw)
+{
+
+    
+    // compute time difference;
+    static auto initialized_filter_ = false;
+    static double t_secs_prev_;
+    const rclcpp::Time &time = imu_msg_raw.header.stamp;
+    // Initialize.
+    if (!initialized_filter_)
+    {
+        t_secs_prev_ = time.seconds();
+        initialized_filter_ = true;
+        return;
+    }
+    
+    double dt = (time.seconds() - t_secs_prev_);
+    t_secs_prev_ = time.seconds();
+    
+    rclcpp::Time current_time;
+    current_time = rclcpp::Clock().now();
+    double seconds_start = current_time.seconds();
+    //================================================================================    
+    const geometry_msgs::msg::Vector3 &a = imu_msg_raw.linear_acceleration;
+    const geometry_msgs::msg::Vector3 &w = imu_msg_raw.angular_velocity;
+    filter_.update(a.x, a.y, a.z, w.x, w.y, w.z, dt);
+    //================================================================================
+    current_time = rclcpp::Clock().now();
+    double seconds_end = current_time.seconds();
+    dt = seconds_end - seconds_start;
+
+    dt *= 1e3;
+    RCLCPP_INFO_STREAM(rclcpp::get_logger(params_imu_preproc.log_tag),
+                       "... time passed for imu pre-processing: [" << dt  << "] ms");
+
+    geometry_msgs::msg::Vector3Stamped msg_dt;
+    msg_dt.header.stamp = imu_msg_raw.header.stamp;
+    msg_dt.vector.x = dt;    
+    dt_publisher_->publish(msg_dt);  
+
+    publish(imu_msg_raw);
+}
+
+tf2::Quaternion hamiltonToTFQuaternion(double q0, double q1, double q2,
+                                       double q3)
+{
+    // ROS uses the Hamilton quaternion convention (q0 is the scalar). However,
+    // the ROS quaternion is in the form [x, y, z, w], with w as the scalar.
+    return tf2::Quaternion(q1, q2, q3, q0);
+}
+
+void publish(ImuMsg imu_msg_raw)
+{
+    // Get the orientation:
+    double q0, q1, q2, q3;
+    filter_.getOrientation(q0, q1, q2, q3);
+
+    // formulate IMU msg with raw data and estimated orientation;
+    ImuMsg::SharedPtr imu_msg = std::make_shared<ImuMsg>(imu_msg_raw);
+    imu_msg->orientation.x = q1;
+    imu_msg->orientation.y = q2;
+    imu_msg->orientation.z = q3;
+    imu_msg->orientation.w = q0;
+
+    imu_msg->orientation_covariance[0] = orientation_variance_;
+    imu_msg->orientation_covariance[1] = 0.0;
+    imu_msg->orientation_covariance[2] = 0.0;
+    imu_msg->orientation_covariance[3] = 0.0;
+    imu_msg->orientation_covariance[4] = orientation_variance_;
+    imu_msg->orientation_covariance[5] = 0.0;
+    imu_msg->orientation_covariance[6] = 0.0;
+    imu_msg->orientation_covariance[7] = 0.0;
+    imu_msg->orientation_covariance[8] = orientation_variance_;
+
+    // Account for biases.
+    if (filter_.getDoBiasEstimation())
+    {
+        imu_msg->angular_velocity.x -= filter_.getAngularVelocityBiasX();
+        imu_msg->angular_velocity.y -= filter_.getAngularVelocityBiasY();
+        imu_msg->angular_velocity.z -= filter_.getAngularVelocityBiasZ();
+    }
+
+    imu_publisher_->publish(*imu_msg);
+
+    // Create and publish roll, pitch, yaw angles
+    geometry_msgs::msg::Vector3Stamped rpy;
+    rpy.header = imu_msg_raw.header;
+    tf2::Quaternion q = hamiltonToTFQuaternion(q0, q1, q2, q3);
+    tf2::Matrix3x3 M;
+    M.setRotation(q);
+    M.getRPY(rpy.vector.x, rpy.vector.y, rpy.vector.z);
+    rpy_publisher_->publish(rpy);
+
+    // Publish whether we are in the steady state, when doing bias
+    // estimation
+    if (filter_.getDoBiasEstimation())
+    {
+        std_msgs::msg::Bool state_msg;
+        state_msg.data = filter_.getSteadyState();
+        state_publisher_->publish(state_msg);
+    }
+
+    if (params_imu_preproc.flag_publish_tf)
+    {
+        // Create and publish the ROS tf.
+        geometry_msgs::msg::TransformStamped transform;
+        transform.header.stamp = imu_msg_raw.header.stamp;
+        transform.transform.rotation.x = q1;
+        transform.transform.rotation.y = q2;
+        transform.transform.rotation.z = q3;
+        transform.transform.rotation.w = q0;
+
+        if (reverse_tf_)
+        {
+            transform.header.frame_id = imu_msg_raw.header.frame_id;
+            transform.child_frame_id = params_imu_preproc.frame_fixed;
+            tf_broadcaster_->sendTransform(transform);
+        }
+        else
+        {
+            transform.child_frame_id = imu_msg_raw.header.frame_id;
+            transform.header.frame_id = params_imu_preproc.frame_fixed;
+            tf_broadcaster_->sendTransform(transform);
+        }
+    }
+}

src/localization_module_lio_locus/apps/nodelets/lio_locus_node.cpp

@@ -0,0 +1,514 @@
+
+/*
+ * Author: Solomon
+ * Date: Oct. 20, 2025,
+ *
+ */
+
+/*
+ * functionality: loosely-couple lidar inertial odometry;
+ * input: pre-processed cloud with normal vector information and pre-processed IMU with orientations;
+ * output: estimated robot pose, recorded trajectory, localized cloud and cloud map;
+ */
+
+//==============================================================================
+#include "lio_locus/lio_locus.h"
+ParameterLioLocus params_lio_locus;
+using lio_locus::LioLocus;
+LioLocus::Ptr ptr_lio_locus;
+//==============================================================================
+
+// ROS2 stuff;
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/executors/multi_threaded_executor.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <nav_msgs/msg/path.hpp>
+#include <geometry_msgs/msg/pose_array.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <geometry_msgs/msg/vector3_stamped.hpp>
+#include <tf2/LinearMath/Quaternion.h>
+
+using std::string;
+
+bool flag_init_pose_set;
+typedef sensor_msgs::msg::Imu ImuMsg;
+void callbackLidar(const sensor_msgs::msg::PointCloud2 &msg);
+void callbackInitialGuess(const geometry_msgs::msg::PoseWithCovarianceStamped &msg);
+void callbackPublishIncrementals();
+void callbackPublishMap();
+void callbackProcessData();
+void callbackImu(const ImuMsg &msg);
+rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_localized_cloud;
+rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cloud_map;
+rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr pub_path;
+rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pub_pose_array;
+
+rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_pose;
+rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr dt_publisher_;
+void publishRobotPose();
+void publishLocalizedCloud();
+void publishCloudMap(rclcpp::Time ts);
+void processCloud4Localization(pcl::PointCloud<pcl::PointXYZINormal>::Ptr p_cloud);
+
+
+
+void recordRobotTrajectory(rclcpp::Time ts,
+                                          const geometry_utils::Transform3 &current_pose, double threshold_trans, double threshold_rot,
+                                          nav_msgs::msg::Path& path , geometry_msgs::msg::PoseArray& pose_array);
+
+// for cloud data storage;
+typedef pcl::PointCloud<pcl::PointXYZINormal> PointCloudNormal;
+#include <deque>
+std::deque<PointCloudNormal::Ptr> deq_clouds;
+
+#include <boost/thread/mutex.hpp>
+
+#include <std_srvs/srv/empty.hpp>
+
+bool flag_invoke_saving_map;
+
+void serviceInvoking(const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+                     std::shared_ptr<std_srvs::srv::Empty::Response> response)
+{
+    flag_invoke_saving_map = true;
+
+    ptr_lio_locus->saveCloudMapInMemory(params_lio_locus.path_save_map, params_lio_locus.flag_voxel_filtering, params_lio_locus.leaf_size);
+    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "################## service invoked: !!! ");
+}
+
+int main(int argc, char **argv)
+{
+
+    flag_invoke_saving_map = false;
+
+    RCLCPP_INFO_STREAM(rclcpp::get_logger(params_lio_locus.log_tag), "###########!!!!!!######################");
+
+    rclcpp::Node::SharedPtr node;
+
+    // node for subscription of cloud and IMU;
+    rclcpp::init(argc, argv);
+
+    node = std::make_shared<rclcpp::Node>("lio_locus_node");
+
+    // algorithm initialization stuff;
+    flag_init_pose_set = false;
+
+    string path_to_param_file;
+    node->declare_parameter("file_params", "/home/solomon/ws_humble_pcl_10/src/lio_locus_humble/config/parameters_lio_locus.yaml");
+    path_to_param_file = node->get_parameter("file_params").as_string();
+
+    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr service =
+        node->create_service<std_srvs::srv::Empty>("invoke_saving_map", &serviceInvoking);
+
+    RCLCPP_INFO_STREAM(rclcpp::get_logger(params_lio_locus.log_tag), "... file params -> #["
+                                                                         << path_to_param_file << "]");
+    //==============================================================================
+    loadParametersFromFileForLioLocus(path_to_param_file, params_lio_locus);
+
+    // algorithm initialization;
+    ptr_lio_locus = LioLocus::Ptr(new LioLocus());
+    ptr_lio_locus->initialize(path_to_param_file);
+    ptr_lio_locus->setUseIMU(params_lio_locus.flag_use_imu);
+    // set extrinsic for imu2lidar;
+    tf2::Quaternion q_imu2lidar_ext;
+    q_imu2lidar_ext.setRPY(params_lio_locus.ext_imu2lidar_droll, params_lio_locus.ext_imu2lidar_dpitch, params_lio_locus.ext_imu2lidar_dyaw);
+    Eigen::Quaterniond q_imu2lidar(q_imu2lidar_ext.w(), q_imu2lidar_ext.x(), q_imu2lidar_ext.y(), q_imu2lidar_ext.z());
+    ptr_lio_locus->setIMU2CloudExtrinsic(q_imu2lidar);
+    ptr_lio_locus->printIMU2CloudExtrinsic();
+
+    if (params_lio_locus.flag_use_scan2scan_matching)
+        ptr_lio_locus->setUseScan2ScanMatching(params_lio_locus.flag_use_scan2scan_matching);
+    ptr_lio_locus->setThresholds(params_lio_locus.threshold_trans_map, params_lio_locus.threshold_rot_map);
+
+    // when use prior map,
+    if (params_lio_locus.flag_use_prior_map)
+    {
+        ptr_lio_locus->uploadPriorCloudMap(params_lio_locus.fn_cloud_map, params_lio_locus.leaf_size_load_map);
+
+        // set initial pose;
+        geometry_utils::Transform3 pose_prior = geometry_utils::Transform3::Identity();
+        ptr_lio_locus->setPosePrior(pose_prior);
+        RCLCPP_INFO_STREAM(rclcpp::get_logger(params_lio_locus.log_tag), "... setting initial pose -> ["
+                                                                             << pose_prior.translation.X() << "," << pose_prior.translation.Y() << "," << pose_prior.translation.Z() << "],["
+                                                                             << pose_prior.rotation.Roll() << "," << pose_prior.rotation.Pitch() << "," << pose_prior.rotation.Yaw() << "]");
+    }
+    ptr_lio_locus->setPathLog(params_lio_locus.path_log);
+
+    //===============================================================================
+    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_cloud;
+    sub_cloud = node->create_subscription<sensor_msgs::msg::PointCloud2>(params_lio_locus.topic_cloud, 9, callbackLidar);
+
+    rclcpp::Subscription<ImuMsg>::SharedPtr sub_imu;
+    sub_imu = node->create_subscription<ImuMsg>(params_lio_locus.topic_imu, 9, callbackImu);
+
+    // for initial guess of pose prior;
+    rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_initial_guess =
+        node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>("/initialpose", 9, callbackInitialGuess);
+
+    pub_pose = node->create_publisher<nav_msgs::msg::Odometry>("pose_lo", 9);
+    pub_localized_cloud =
+        node->create_publisher<sensor_msgs::msg::PointCloud2>("/dbg_localized_cloud", 9);
+
+
+    
+
+
+    pub_path = node->create_publisher<nav_msgs::msg::Path>("/dbg_lio_path", 9);
+    pub_pose_array = node->create_publisher<geometry_msgs::msg::PoseArray>("/dbg_lio_pose_array", 9);
+
+    pub_cloud_map =
+        node->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_map", 9);
+
+    dt_publisher_ = node->create_publisher<geometry_msgs::msg::Vector3Stamped>("/dt_lio_locus", 9);
+
+    // independent threads for data processing, incrementals publication, map publication;
+    // rclcpp::TimerBase::SharedPtr p_process_data = node->create_wall_timer(std::chrono::milliseconds(100), callbackProcessData);
+    rclcpp::TimerBase::SharedPtr p_publish_incrementals = node->create_wall_timer(std::chrono::milliseconds(200), callbackPublishIncrementals);
+    rclcpp::TimerBase::SharedPtr p_publish_map = node->create_wall_timer(std::chrono::milliseconds(5000), callbackPublishMap);
+
+    // multi-threaded spinners;
+    rclcpp::executors::MultiThreadedExecutor::SharedPtr threaded = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
+    threaded->add_node(node);
+    threaded->spin();
+
+    rclcpp::shutdown();
+    return 0;
+}
+
+// store input clouds to memory for later processing;
+void callbackLidar(const sensor_msgs::msg::PointCloud2 &msg)
+{
+    PointCloudType::Ptr p_cloud(new PointCloudType());
+    pcl::fromROSMsg(msg, *p_cloud);
+
+    processCloud4Localization(p_cloud);
+
+    // deq_clouds.push_back(p_cloud);
+}
+
+void callbackProcessData()
+{
+
+    boost::mutex mutex_;
+    boost::mutex::scoped_lock lock(mutex_);
+
+    if (!deq_clouds.size())
+        return;
+
+    PointCloudNormal::Ptr p_cloud = deq_clouds.front();
+    processCloud4Localization(p_cloud);
+
+    deq_clouds.pop_front();
+}
+
+// do data processing, publish estimated pose and localized cloud;
+void callbackPublishIncrementals()
+{
+
+    // publish algorithm result for visualization;
+    publishRobotPose();
+    publishLocalizedCloud();
+}
+
+void callbackPublishMap()
+{
+    publishCloudMap(rclcpp::Clock().now());
+}
+
+void callbackInitialGuess(const geometry_msgs::msg::PoseWithCovarianceStamped &msg)
+{
+
+    RCLCPP_INFO_STREAM(rclcpp::get_logger(params_lio_locus.log_tag), "... initial guess for pose prior is given!");
+
+    if (!params_lio_locus.flag_use_prior_map)
+        return;
+
+    auto p = msg.pose.pose.position;
+    auto q = msg.pose.pose.orientation;
+
+    geometry_utils::Transform3 pose_prior(geometry_utils::Vec3d(p.x, p.y, p.z),
+                                          geometry_utils::Quaterniond(q.w, q.x, q.y, q.z));
+    ptr_lio_locus->setPosePrior(pose_prior);
+    RCLCPP_INFO_STREAM(rclcpp::get_logger(params_lio_locus.log_tag), "... setting initial pose -> ["
+                                                                         << pose_prior.translation.X() << "," << pose_prior.translation.Y() << "," << pose_prior.translation.Z() << "],["
+                                                                         << pose_prior.rotation.Roll() << "," << pose_prior.rotation.Pitch() << "," << pose_prior.rotation.Yaw() << "]");
+
+    flag_init_pose_set = true;
+}
+
+// imu subscription callback;
+void callbackImu(const ImuMsg &msg)
+{
+    lio_locus::IMUData imu_data(msg.header.stamp.sec,
+                                msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w);
+
+    ptr_lio_locus->feedIMUData(imu_data);
+}
+
+void processCloud4Localization(PointCloudNormal::Ptr p_cloud)
+{
+
+    RCLCPP_INFO_STREAM(rclcpp::get_logger(params_lio_locus.log_tag), "##################################################################");
+    RCLCPP_INFO_STREAM(rclcpp::get_logger(params_lio_locus.log_tag), "... start processing cloud with normal vector obtained: #["
+                                                                         << p_cloud->size() << "] points");
+
+    //==============================================================================
+    double seconds_start = rclcpp::Clock().now().seconds();
+    // cloud feeding to algorithm;
+    if (params_lio_locus.flag_use_prior_map)
+    {
+
+        if (flag_init_pose_set)
+        {
+            ptr_lio_locus->trackCurrentCloudWithPriorCloudMap(p_cloud);
+        }
+    }
+    else
+
+    {
+        ptr_lio_locus->trackCurrentCloudWithOnlineBuiltMap(p_cloud);
+    }
+
+    double seconds_end = rclcpp::Clock().now().seconds();
+    double dt = seconds_end - seconds_start;
+    dt *= 1e3;
+    RCLCPP_INFO_STREAM(rclcpp::get_logger(params_lio_locus.log_tag),
+                       "############## time passed for LIO processing: [" << dt << "] ms ##############");
+
+    //==============================================================================
+    // publish results;
+    rclcpp::Time time_now;
+    pcl_conversions::fromPCL(p_cloud->header.stamp, time_now);
+
+    geometry_msgs::msg::Vector3Stamped msg_dt;
+    msg_dt.header.stamp = time_now;
+    msg_dt.vector.x = dt;
+    dt_publisher_->publish(msg_dt);
+}
+
+// ROS function to publish cloud map at times;
+void publishCloudMap(rclcpp::Time ts)
+{
+
+    // do voxel grid filtering at times;
+    double seconds_start = rclcpp::Clock().now().seconds();
+
+    PointCloudType::Ptr ptr_cloud_map(new PointCloudType());
+    if (!ptr_lio_locus->getCloudMapInMemory(ptr_cloud_map))
+        return;
+
+
+    if(!ptr_cloud_map->size())
+        return ;
+
+    // check: normal vector information ;
+
+
+//     if(flag_invoke_saving_map)
+// {
+//     pcl::io::savePCDFileASCII(params_lio_locus.path_save_map, *ptr_cloud_map);
+//     flag_invoke_saving_map = false;
+// }
+
+    // // for first time and for time when size doubles;
+    // static bool flag_recorded = false;
+    // static int size_cloud_map;
+    // int size_now = ptr_cloud_map->size();
+
+    // bool flag_publish = false;
+    // if (!flag_recorded)
+    // {
+    //     flag_publish = true;
+    //     size_cloud_map = size_now;
+    // }
+    // else
+    // {
+    //     if (size_now > 1.6 * size_cloud_map)
+    //     {
+    //         size_cloud_map = size_now;
+    //         flag_publish = true;
+    //     }
+    // }
+
+    // flag_recorded = true;
+
+    // if (!flag_publish)
+    //     return;
+
+    // voxel grid filtering for cloud map;
+    PointCloudType::Ptr cloud_downsampled = LioLocus::downsampleCloud(ptr_cloud_map, params_lio_locus.leaf_size_vis_map);
+
+    sensor_msgs::msg::PointCloud2 msg_cloud_map;
+    pcl::toROSMsg(*cloud_downsampled, msg_cloud_map);
+    msg_cloud_map.header.stamp = ts;
+    msg_cloud_map.header.frame_id = params_lio_locus.frame_id_world;
+
+    // publish to ROS2 env.
+    pub_cloud_map->publish(msg_cloud_map);
+
+    double seconds_end = rclcpp::Clock().now().seconds();
+    double dt = seconds_end - seconds_start;
+    dt *= 1e3;
+    RCLCPP_INFO_STREAM(rclcpp::get_logger(params_lio_locus.log_tag),
+                       "############## time passed for cloud map publication: [" << dt << "] ms ##############");
+}
+
+void publishRobotPose()
+{
+    auto pose = ptr_lio_locus->getTimestampedLatestPose().pose;
+
+    nav_msgs::msg::Odometry msg_pose;
+    auto &p = msg_pose.pose.pose.position;
+    p.x = pose.translation.X();
+    p.y = pose.translation.Y();
+    p.z = pose.translation.Z();
+
+    auto &q = msg_pose.pose.pose.orientation;
+    tf2::Quaternion q_tf2;
+    q_tf2.setRPY(pose.rotation.Roll(), pose.rotation.Pitch(), pose.rotation.Yaw());
+    q.x = q_tf2.x();
+    q.y = q_tf2.y();
+    q.z = q_tf2.z();
+    q.w = q_tf2.w();
+
+    rclcpp::Time time_now;
+    pcl_conversions::fromPCL(ptr_lio_locus->getTimestampedLatestPose().timestamp, time_now);
+
+    msg_pose.header.stamp = time_now;
+    msg_pose.header.frame_id = params_lio_locus.frame_id_world;
+    pub_pose->publish(msg_pose);
+
+    bool flag_record_traj = false;
+    if (!params_lio_locus.flag_use_prior_map)
+    {
+        flag_record_traj = true;
+    }
+
+    // publish path travelled so far;
+    if (flag_record_traj)
+    {
+        nav_msgs::msg::Path msg_path;
+        geometry_msgs::msg::PoseArray msg_pose_array;
+         recordRobotTrajectory(time_now, ptr_lio_locus->getTimestampedLatestPose().pose,
+         params_lio_locus.threshold_trans_traj, params_lio_locus.threshold_rot_traj, msg_path, msg_pose_array);
+
+        RCLCPP_INFO_STREAM(rclcpp::get_logger(params_lio_locus.log_tag), "#[" << msg_path.poses.size()
+                                                                              << "] poses recored for trajecotry");
+
+        msg_path.header.frame_id = params_lio_locus.frame_id_world;
+        msg_path.header.stamp = time_now;
+        pub_path->publish(msg_path);
+
+        msg_pose_array.header = msg_path.header;
+        pub_pose_array->publish(msg_pose_array);
+
+    }
+}
+
+void publishLocalizedCloud()
+{
+    // publish localized cloud;
+    auto cloud_incre = ptr_lio_locus->getLatestLocalizedCloud();
+
+    if (cloud_incre != nullptr)
+    {
+        sensor_msgs::msg::PointCloud2 msg_loc_cloud;
+        pcl::toROSMsg(*cloud_incre, msg_loc_cloud);
+
+        rclcpp::Time time_now;
+        pcl_conversions::fromPCL(ptr_lio_locus->getTimestampedLatestPose().timestamp, time_now);
+
+        msg_loc_cloud.header.stamp = time_now;
+        msg_loc_cloud.header.frame_id = params_lio_locus.frame_id_world;
+        pub_localized_cloud->publish(msg_loc_cloud);
+        RCLCPP_INFO_STREAM(rclcpp::get_logger(params_lio_locus.log_tag),
+                           "... publishing localized cloud with points #[" << cloud_incre->size() << "]!");
+    }
+    else
+    {
+
+        RCLCPP_INFO_STREAM(rclcpp::get_logger(params_lio_locus.log_tag),
+                           "... localized cloud is NULL!");
+    }
+}
+
+void recordRobotTrajectory(rclcpp::Time ts,
+                                          const geometry_utils::Transform3 &current_pose, double threshold_trans, double threshold_rot,
+                                          nav_msgs::msg::Path& path_get , geometry_msgs::msg::PoseArray& pose_array_get)
+{
+
+    static nav_msgs::msg::Path path;
+    static geometry_msgs::msg::PoseArray poses_array;
+
+    path.header.frame_id = params_lio_locus.frame_id_world;
+    path.header.stamp = ts;
+
+    poses_array.header = path.header;
+
+    // milestone pose;
+    static bool flag_pose_recorded = false;
+    static geometry_utils::Transform3 pose_milestone;
+
+    // check displacement over thresholds;
+    bool flag_milestone = false;
+    if (flag_pose_recorded)
+    {
+
+        geometry_utils::Transform3 delta_pose = geometry_utils::PoseUpdate(
+            geometry_utils::PoseInverse(pose_milestone), current_pose);
+
+        // for distance more than threshold;
+        auto translation = delta_pose.translation;
+        if (std::pow(translation.X(), 2) + std::pow(translation.Y(), 2) + std::pow(translation.Z(), 2) > threshold_trans * threshold_trans or std::fabs(delta_pose.rotation.Yaw()) > threshold_rot)
+        {
+
+            flag_milestone = true;
+            pose_milestone = current_pose;
+        }
+    }
+    else
+    { // for first time;
+        pose_milestone = current_pose;
+        flag_milestone = true;
+    }
+    flag_pose_recorded = true;
+
+    // elementary item for path;
+    if (flag_milestone)
+    {
+
+        // data to store milestone pose for path;
+        geometry_msgs::msg::PoseStamped aux;
+        aux.header.frame_id = params_lio_locus.frame_id_world;
+        aux.header.stamp = ts;
+
+        // assign translation part of current pose;
+        auto &p = aux.pose.position;
+        p.x = current_pose.translation.X();
+        p.y = current_pose.translation.Y();
+        p.z = current_pose.translation.Z();
+
+        // assign orientation part of current pose;
+        auto rotation = current_pose.rotation;
+        tf2::Quaternion quat;
+        quat.setEuler(rotation.Yaw(), rotation.Pitch(), rotation.Roll());
+
+        auto &q = aux.pose.orientation;
+        q.x = quat.x();
+        q.y = quat.y();
+        q.z = quat.z();
+        q.w = quat.w();
+        path.poses.push_back(aux);
+
+        geometry_msgs::msg::Pose pose_aux;
+        pose_aux.position = aux.pose.position;
+        pose_aux.orientation = aux.pose.orientation;
+        poses_array.poses.push_back(pose_aux);
+    }
+
+    path_get = path;
+    pose_array_get = poses_array;
+}

src/localization_module_lio_locus/config/parameters_lio_locus.yaml

@@ -0,0 +1,150 @@
+ros2_wrapper: 
+    frame_id_world: fixed
+    frame_id_ego: base_link
+
+    topic_cloud_raw: /livox/lidar
+    topic_imu_raw: /livox/imu
+
+    topic_cloud: /cloud_input
+    topic_imu: /imu_filtered
+
+    flag_use_imu: true 
+    flag_use_scan2scan_matching: true
+
+    flag_use_prior_map: false 
+    path_cloud_map: /home/solomon/cloud_map
+    fn_cloud_map: cloud_map.pcd
+    leaf_size_load_map: 0.35
+
+    ratio_fresh_map: 1.2
+    leaf_size_vis_map: 0.2 
+
+    flag_save_map_in_memory: true 
+    path_save_map: /home/demo/cloud_map
+    fn_save_cloud_map: cloud_map.pcd
+    flag_voxel_filtering: true 
+    leaf_size: 0.1
+
+    path_log: no_log
+
+    threshold_trans_map: 0.5
+    threshold_rot_map: 0.35 
+
+    threshold_trans_traj: 0.2
+    threshold_rot_traj: 0.088 
+
+    flag_arranged_proc: false 
+    freq_cloud_proc: 10
+
+    imu_to_lidar:
+        tx: -0.1 # m
+        ty: 1.498 # m
+        tz: 1.227 # m
+        r: 1.889 # deg
+        p: 12.220 # deg
+        y: 89.331 # deg
+
+    baselink_to_lidar:
+        tx: 0
+        ty: 0
+        tz: 0 # m
+        r: 0
+        p: 0 
+        y: 0 # deg
+
+
+cloud_preprocessing:
+    leaf_voxel_filtering: 0.5
+    filter_limit: 120
+
+    size_cores: 6
+    size_k_search_knn: 20
+    radius_search_knn: 0
+
+    pub_cloud_preproc: true 
+
+imu_complementary_filtering:
+    do_bias_estimation: true
+    do_adaptive_gain: true
+    bias_alpha: 0.01
+    frame_fixed_: fixed_imu
+    flag_publish_tf: false
+    orientation_stddev: 0.0
+    pub_imu_preproc: true 
+
+   
+
+b_is_flat_ground_assumption: false 
+
+icp:
+  registration_method: gicp #ndt #gicp
+
+  corr_dist: 1.0
+
+  tf_epsilon: 0.001
+  iterations: 50
+
+  num_threads: 3
+  enable_timing_output: false
+
+  transform_thresholding: true
+  max_translation: 1.0
+  max_rotation: 1.0
+
+  recompute_covariances: false
+  b_verbose: true
+
+localization:
+  # Registration method
+  registration_method: gicp
+
+  # Compute ICP covariance and condition number
+  compute_icp_covariance: true 
+  # 0: point to point 1: point to plane
+  icp_covariance_method: 1
+
+  icp_max_covariance: 0.01
+
+  # Compute ICP observability 
+  compute_icp_observability: true
+
+  # Stop ICP if the transformation from the last iteration was this small.
+  tf_epsilon: 0.00001
+
+  # During ICP, two points won't be considered a correspondence if they are
+  # at least this far from one another.
+  corr_dist: 0.6
+
+  # Iterate ICP this many times.
+  iterations: 50 #25 # Suggest 25 for NeBula Spot datasets
+
+  # Maximum acceptable incremental rotation and translation.
+  transform_thresholding: true
+  max_translation: 1.0 # 0.05
+  max_rotation: 1.0 # Radians - max 60 deg rotation
+
+  # Number of threads GICP can use (3 threads ~ 3x speedup)
+  num_threads: 6 # - defined in launch files to adapt to robot
+
+  # Enable GICP timing output
+  enable_timing_output: false
+  
+  # Radius for normal search
+  normal_search_radius: 10
+
+  # scan-to-map, scan has normals, map has normals as well, but the data comes
+  # from scan sparse data therefore may be beneficial to recompute the covariance matrix
+  recompute_covariance_local_map: false 
+
+# in general if there is no particular reason it should always be false,
+# since it recomputes the covariances from scratch but we calculate them from normals
+  recompute_covariance_scan: false
+
+
+map:
+    octree_resolution: 0.01 # 0.001
+    frame_id_fixed: fixed
+    num_threads: 6
+
+
+

src/localization_module_lio_locus/config/parameters_lio_locus_reloc.yaml

@@ -0,0 +1,150 @@
+ros2_wrapper: 
+    frame_id_world: fixed
+    frame_id_ego: base_link
+
+    topic_cloud_raw: /livox/lidar
+    topic_imu_raw: /livox/imu
+
+    topic_cloud: /cloud_input
+    topic_imu: /imu_filtered
+
+    flag_use_imu: true 
+    flag_use_scan2scan_matching: true
+
+    flag_use_prior_map: true 
+    path_cloud_map: /home/demo/cloud_map
+    fn_cloud_map: cloud_map.pcd
+    leaf_size_load_map: 0.35
+
+    ratio_fresh_map: 1.2
+    leaf_size_vis_map: 0.2 
+
+    flag_save_map_in_memory: true 
+    path_save_map: /home/solomon/cloud_map_bkup
+    fn_save_cloud_map: cloud_map.pcd
+    flag_voxel_filtering: true 
+    leaf_size: 0.1
+
+    path_log: no_log
+
+    threshold_trans_map: 0.5
+    threshold_rot_map: 0.35 
+
+    threshold_trans_traj: 0.2
+    threshold_rot_traj: 0.088 
+
+    flag_arranged_proc: false 
+    freq_cloud_proc: 10
+
+    imu_to_lidar:
+        tx: -0.1 # m
+        ty: 1.498 # m
+        tz: 1.227 # m
+        r: 1.889 # deg
+        p: 12.220 # deg
+        y: 89.331 # deg
+
+    baselink_to_lidar:
+        tx: 0
+        ty: 0
+        tz: 0 # m
+        r: 0
+        p: 0 
+        y: 0 # deg
+
+
+cloud_preprocessing:
+    leaf_voxel_filtering: 0.5
+    filter_limit: 120
+
+    size_cores: 6
+    size_k_search_knn: 20
+    radius_search_knn: 0
+
+    pub_cloud_preproc: true 
+
+imu_complementary_filtering:
+    do_bias_estimation: true
+    do_adaptive_gain: true
+    bias_alpha: 0.01
+    frame_fixed_: fixed_imu
+    flag_publish_tf: false
+    orientation_stddev: 0.0
+    pub_imu_preproc: true 
+
+   
+
+b_is_flat_ground_assumption: false 
+
+icp:
+  registration_method: gicp #ndt #gicp
+
+  corr_dist: 1.0
+
+  tf_epsilon: 0.001
+  iterations: 50
+
+  num_threads: 3
+  enable_timing_output: false
+
+  transform_thresholding: true
+  max_translation: 1.0
+  max_rotation: 1.0
+
+  recompute_covariances: false
+  b_verbose: true
+
+localization:
+  # Registration method
+  registration_method: gicp
+
+  # Compute ICP covariance and condition number
+  compute_icp_covariance: true 
+  # 0: point to point 1: point to plane
+  icp_covariance_method: 1
+
+  icp_max_covariance: 0.01
+
+  # Compute ICP observability 
+  compute_icp_observability: true
+
+  # Stop ICP if the transformation from the last iteration was this small.
+  tf_epsilon: 0.00001
+
+  # During ICP, two points won't be considered a correspondence if they are
+  # at least this far from one another.
+  corr_dist: 0.6
+
+  # Iterate ICP this many times.
+  iterations: 50 #25 # Suggest 25 for NeBula Spot datasets
+
+  # Maximum acceptable incremental rotation and translation.
+  transform_thresholding: true
+  max_translation: 1.0 # 0.05
+  max_rotation: 1.0 # Radians - max 60 deg rotation
+
+  # Number of threads GICP can use (3 threads ~ 3x speedup)
+  num_threads: 6 # - defined in launch files to adapt to robot
+
+  # Enable GICP timing output
+  enable_timing_output: false
+  
+  # Radius for normal search
+  normal_search_radius: 10
+
+  # scan-to-map, scan has normals, map has normals as well, but the data comes
+  # from scan sparse data therefore may be beneficial to recompute the covariance matrix
+  recompute_covariance_local_map: false 
+
+# in general if there is no particular reason it should always be false,
+# since it recomputes the covariances from scratch but we calculate them from normals
+  recompute_covariance_scan: false
+
+
+map:
+    octree_resolution: 0.01 # 0.001
+    frame_id_fixed: fixed
+    num_threads: 6
+
+
+

src/localization_module_lio_locus/include/lio_locus_humble/frontend_utils/CommonFunctions.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include <frontend_utils/CommonStructs.h>
+#include <omp.h>
+template <typename T>
+Eigen::Matrix<T, 3, 3> PCLTwoPlaneVectorsFromNormal(const PointF& normal) {
+  Eigen::Matrix<T, 3, 1> vec1, vec2;
+  Eigen::Matrix<T, 3, 1> normal_eig(normal._PointXYZINormal::normal_x,
+                                    normal._PointXYZINormal::normal_y,
+                                    normal._PointXYZINormal::normal_z);
+  normal_eig.normalize();
+  vec1[0] = 1.0;
+  vec1[1] = 0.0;
+  if (std::abs(normal._PointXYZINormal::normal_z) < 0.0000001 or
+      std::abs(normal_eig[2]) < 0.0000001) {
+    normal_eig[2] = 0.0000001;
+  }
+  vec1[2] = -normal_eig[0] / normal_eig[2];
+  vec1.normalize();
+  vec2 = normal_eig.cross(vec1);
+  vec2.normalize();
+
+  return 0.001 * normal_eig * normal_eig.transpose() + vec1 * vec1.transpose() +
+      vec2 * vec2.transpose();
+}
+
+template <typename T>
+void CalculateCovarianceFromNormals(const PointCloudF::ConstPtr& point_cloud,
+                                    MatricesVectorT<T>& cloud_covariances,
+                                    int k_num_threads = 1) {
+  Eigen::Matrix<T, 3, 1> vec1, vec2;
+  cloud_covariances.resize(point_cloud->size());
+  omp_set_num_threads(k_num_threads);
+#pragma omp parallel for schedule(dynamic, 1)
+  for (int i = 0; i < point_cloud->points.size(); ++i) {
+    cloud_covariances[i] =
+        PCLTwoPlaneVectorsFromNormal<T>(point_cloud->points[i]);
+  }
+}

src/localization_module_lio_locus/include/lio_locus_humble/frontend_utils/CommonStructs.h

@@ -0,0 +1,28 @@
+#pragma once
+//#include <pcl_ros/point_cloud.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+
+#include <vector>
+
+typedef pcl::PointXYZINormal PointF;
+typedef pcl::PointCloud<PointF> PointCloudF;
+typedef pcl::PointCloud<PointF>::ConstPtr pclConstPtr;
+typedef pcl::PointCloud<PointF>::Ptr pclPtr;
+
+typedef std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>
+    MatricesVector;
+typedef boost::shared_ptr<MatricesVector> MatricesVectorPtr;
+
+typedef std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f>>
+    MatricesVectorf;
+typedef boost::shared_ptr<MatricesVectorf> MatricesVectorfPtr;
+
+template <typename T>
+using MatricesVectorT =
+    std::vector<Eigen::Matrix<T, 3, 3>,
+                Eigen::aligned_allocator<Eigen::Matrix<T, 3, 3>>>;
+
+template <typename T>
+using MatricesVectorTPtr = boost::shared_ptr<MatricesVectorT<T>>;

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/GeometryUtils.h

@@ -0,0 +1,160 @@
+/*
+  geometry_utils: Utility library to provide common geometry types and
+  transformations Copyright (C) 2014  Nathan Michael 2016  Erik Nelson
+
+  This program is free software; you can redistribute it and/or
+  modify it under the terms of the GNU General Public License
+  as published by the Free Software Foundation; either version 2
+  of the License, or (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301,
+  USA.
+*/
+
+#ifndef GEOMETRY_UTILS_H
+#define GEOMETRY_UTILS_H
+
+#include "Matrix2x2.h"
+#include "Matrix3x3.h"
+#include "Matrix4x4.h"
+#include "Quaternion.h"
+#include "Rotation2.h"
+#include "Rotation3.h"
+#include "Transform2.h"
+#include "Transform3.h"
+#include "Vector2.h"
+#include "Vector3.h"
+#include "Vector4.h"
+
+namespace geometry_utils
+{
+inline double Unroll(double x)
+{
+  x = fmod(x, 2.0 * M_PI);
+  if (x < 0)
+    x += 2.0 * M_PI;
+  return x;
+}
+
+inline double Normalize(double x)
+{
+  x = fmod(x + M_PI, 2.0 * M_PI);
+  if (x < 0)
+    x += 2.0 * M_PI;
+  return x - M_PI;
+}
+
+inline double S1Distance(double from, double to)
+{
+  double result = Unroll(Unroll(to) - Unroll(from));
+  if (result > M_PI)
+    result = -(2.0 * M_PI - result);
+  return Normalize(result);
+}
+
+inline double Rad2Deg(double angle)
+{
+  return angle * 180.0 * M_1_PI;
+}
+
+inline double Deg2Rad(double angle)
+{
+  return angle * M_PI / 180.0;
+}
+
+inline Vec3 Rad2Deg(const Vec3& angles)
+{
+  return Vec3(Rad2Deg(angles(0)), Rad2Deg(angles(1)), Rad2Deg(angles(2)));
+}
+
+inline Vec3 Deg2Rad(const Vec3& angles)
+{
+  return Vec3(Deg2Rad(angles(0)), Deg2Rad(angles(1)), Deg2Rad(angles(2)));
+}
+
+inline Vec3 RToZYX(const Rot3& rot)
+{
+  return rot.GetEulerZYX();
+}
+
+inline Rot3 ZYXToR(const Vec3& angles)
+{
+  return Rot3(angles);
+}
+
+inline Rot3 QuatToR(const Quat& quat)
+{
+  return Rot3(quat);
+}
+
+inline Quat RToQuat(const Rot3& rot)
+{
+  return Quat(Eigen::Quaterniond(rot.Eigen()));
+}
+
+inline double GetRoll(const Rot3& r)
+{
+  return r.Roll();
+}
+
+inline double GetRoll(const Quat& q)
+{
+  return Rot3(q).Roll();
+}
+
+inline double GetPitch(const Rot3& r)
+{
+  return r.Pitch();
+}
+
+inline double GetPitch(const Quat& q)
+{
+  return Rot3(q).Pitch();
+}
+
+inline double GetYaw(const Rot3& r)
+{
+  return r.Yaw();
+}
+
+inline double GetYaw(const Quat& q)
+{
+  return Rot3(q).Yaw();
+}
+
+inline double SO3Error(const Quat& q1, const Quat& q2)
+{
+  return Rot3(q1).Error(Rot3(q2));
+}
+
+inline double SO3Error(const Rot3& r1, const Rot3& r2)
+{
+  return r1.Error(r2);
+}
+
+inline Vec3 CartesianToSpherical(const Vec3& v)
+{
+  double rho = v.Norm();
+  return Vec3(rho, acos(v.Z() / rho), atan2(v.Y(), v.X()));
+}
+
+inline Vec3 SphericalToCartesian(const Vec3& v)
+{
+  return Vec3(v(0) * sin(v(1)) * cos(v(2)), v(0) * sin(v(1)) * sin(v(2)), v(0) * cos(v(1)));
+}
+
+inline Vec3 NEDCartesian(const Vec3& v)
+{
+  return Vec3(v(0), -v(1), -v(2));
+}
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/GeometryUtilsMath.h

@@ -0,0 +1,201 @@
+/*
+  geometry_utils: Utility library to provide common geometry types and
+  transformations Copyright (C) 2014  Nathan Michael 2016  Erik Nelson
+
+  This program is free software; you can redistribute it and/or
+  modify it under the terms of the GNU General Public License
+  as published by the Free Software Foundation; either version 2
+  of the License, or (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301,
+  USA.
+*/
+
+#ifndef GEOMETRY_UTILS_MATH_H
+#define GEOMETRY_UTILS_MATH_H
+
+#include <cmath>
+
+namespace geometry_utils
+{
+namespace math
+{
+template <typename T>
+inline T cos(T in);
+template <>
+inline float cos(float in)
+{
+  return ::cosf(in);
+}
+template <>
+inline double cos(double in)
+{
+  return ::cos(in);
+}
+
+template <typename T>
+inline T acos(T in);
+template <>
+inline float acos(float in)
+{
+  return ::acosf(in);
+}
+template <>
+inline double acos(double in)
+{
+  return ::acos(in);
+}
+
+template <typename T>
+inline T sin(T in);
+template <>
+inline float sin(float in)
+{
+  return ::sinf(in);
+}
+template <>
+inline double sin(double in)
+{
+  return ::sin(in);
+}
+
+template <typename T>
+inline T asin(T in);
+template <>
+inline float asin(float in)
+{
+  return ::asinf(in);
+}
+template <>
+inline double asin(double in)
+{
+  return ::asin(in);
+}
+
+template <typename T>
+inline T tan(T in);
+template <>
+inline float tan(float in)
+{
+  return ::tanf(in);
+}
+template <>
+inline double tan(double in)
+{
+  return ::tan(in);
+}
+
+template <typename T>
+inline T atan(T in);
+template <>
+inline float atan(float in)
+{
+  return ::atanf(in);
+}
+template <>
+inline double atan(double in)
+{
+  return ::atan(in);
+}
+
+template <typename T>
+inline T fabs(T in);
+template <>
+inline float fabs(float in)
+{
+  return ::fabsf(in);
+}
+template <>
+inline double fabs(double in)
+{
+  return ::fabs(in);
+}
+
+template <typename T>
+inline T fmin(T v1, T v2);
+template <>
+inline float fmin(float v1, float v2)
+{
+  return ::fminf(v1, v2);
+}
+template <>
+inline double fmin(double v1, double v2)
+{
+  return ::fmin(v1, v2);
+}
+
+template <typename T>
+inline T fmax(T v1, T v2);
+template <>
+inline float fmax(float v1, float v2)
+{
+  return ::fmaxf(v1, v2);
+}
+template <>
+inline double fmax(double v1, double v2)
+{
+  return ::fmax(v1, v2);
+}
+
+template <typename T>
+inline T sqrt(T in);
+template <>
+inline float sqrt(float in)
+{
+  return ::sqrtf(in);
+}
+template <>
+inline double sqrt(double in)
+{
+  return ::sqrt(in);
+}
+
+template <typename T>
+inline T pow(T in, T exp);
+template <>
+inline float pow(float in, float exp)
+{
+  return ::powf(in, exp);
+}
+template <>
+inline double pow(double in, double exp)
+{
+  return ::pow(in, exp);
+}
+
+template <typename T>
+inline T atan2(T y, T x);
+template <>
+inline float atan2(float y, float x)
+{
+  return ::atan2f(y, x);
+}
+template <>
+inline double atan2(double y, double x)
+{
+  return ::atan2(y, x);
+}
+
+template <typename T>
+inline T hypot(T x, T y);
+template <>
+inline float hypot(float x, float y)
+{
+  return ::hypotf(x, y);
+}
+template <>
+inline double hypot(double x, double y)
+{
+  return ::hypot(x, y);
+}
+
+}  // namespace math
+}  // namespace geometry_utils
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/GeometryUtilsROS.h

@@ -0,0 +1,211 @@
+/*
+geometry_utils: Utility library to provide common geometry types and
+transformations Copyright (C) 2013  Nathan Michael 2015  Erik Nelson
+
+This program is free software; you can redistribute it and/or
+modify it under the terms of the GNU General Public License
+as published by the Free Software Foundation; either version 2
+of the License, or (at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+*/
+
+#ifndef GEOMETRY_UTILS_ROS_H
+#define GEOMETRY_UTILS_ROS_H
+
+#include "GeometryUtils.h"
+
+#include <geometry_msgs/msg/point.h>
+#include <geometry_msgs/msg/point32.h>
+#include <geometry_msgs/msg/pose.h>
+#include <geometry_msgs/msg/quaternion.h>
+#include <geometry_msgs/msg/transform.h>
+#include <nav_msgs/msg/odometry.h>
+
+// check headers for ros messages;
+
+namespace geometry_utils
+{
+namespace ros
+{
+ 
+inline void test(){
+
+  // geometry_msgs__msg__Point32
+
+}
+
+inline Vec3 FromROS( const geometry_msgs::msg::Point& p)
+{
+  
+  
+  return Vec3(p.x, p.y, p.z);
+}
+
+inline Vec3 FromROS(const geometry_msgs::msg::Point32& p)
+{
+  return Vec3(p.x, p.y, p.z);
+}
+
+inline Vec3 FromROS(const geometry_msgs::msg::Vector3& p)
+{
+  return Vec3(p.x, p.y, p.z);
+}
+
+inline Quat FromROS(const geometry_msgs::msg::Quaternion& msg)
+{
+  return Quat(msg.w, msg.x, msg.y, msg.z);
+}
+
+inline Transform3 FromROS(const geometry_msgs::msg::Pose& msg)
+{
+  return Transform3(FromROS(msg.position), QuatToR(FromROS(msg.orientation)));
+}
+
+inline Transform3 FromROS(const geometry_msgs::msg::Transform& msg)
+{
+  return Transform3(FromROS(msg.translation), QuatToR(FromROS(msg.rotation)));
+}
+
+inline geometry_msgs::msg::Point ToRosPoint(const Vec2& v)
+{
+  geometry_msgs::msg::Point msg;
+  msg.x = v(0);
+  msg.y = v(1);
+  msg.z = 0.0;
+
+  return msg;
+}
+
+inline geometry_msgs::msg::Point ToRosPoint(const Vec3& v)
+{
+  geometry_msgs::Point msg;
+  msg.x = v(0);
+  msg.y = v(1);
+  msg.z = v(2);
+
+  return msg;
+}
+
+inline geometry_msgs::msg::Point32 ToRosPoint32(const Vec2& v)
+{
+  geometry_msgs::msg::Point32 msg;
+  msg.x = v(0);
+  msg.y = v(1);
+  msg.z = 0.0f;
+
+  return msg;
+}
+
+inline geometry_msgs::msg::Point32 ToRosPoint32(const Vec3& v)
+{
+  geometry_msgs::msg::Point32 msg;
+  msg.x = v(0);
+  msg.y = v(1);
+  msg.z = v(2);
+
+  return msg;
+}
+
+inline geometry_msgs::msg::Vector3 ToRosVec(const Vec2& v)
+{
+  geometry_msgs::msg::Vector3 msg;
+  msg.x = v(0);
+  msg.y = v(1);
+  msg.z = 0.0;
+
+  return msg;
+}
+
+inline geometry_msgs::msg::Vector3 ToRosVec(const Vec3& v)
+{
+  geometry_msgs::msg::Vector3 msg;
+  msg.x = v(0);
+  msg.y = v(1);
+  msg.z = v(2);
+
+  return msg;
+}
+
+inline geometry_msgs::msg::Quaternion ToRosQuat(const Quat& quat)
+{
+  geometry_msgs::msg::Quaternion msg;
+  msg.w = quat.W();
+  msg.x = quat.X();
+  msg.y = quat.Y();
+  msg.z = quat.Z();
+
+  return msg;
+}
+
+inline geometry_msgs::msg::Pose ToRosPose(const Transform2& trans)
+{
+  geometry_msgs::msg::Pose msg;
+  msg.position = ToRosPoint(trans.translation);
+  msg.orientation = ToRosQuat(RToQuat(Rot3(trans.rotation)));
+
+  return msg;
+}
+
+inline geometry_msgs::msg::Pose ToRosPose(const Transform3& trans)
+{
+  geometry_msgs::msg::Pose msg;
+  msg.position = ToRosPoint(trans.translation);
+  msg.orientation = ToRosQuat(RToQuat(trans.rotation));
+
+  return msg;
+}
+
+inline geometry_msgs::msg::Transform ToRosTransform(const Transform2& trans)
+{
+  geometry_msgs::msg::Transform msg;
+  msg.translation = ToRosVec(trans.translation);
+  msg.rotation = ToRosQuat(RToQuat(Rot3(trans.rotation)));
+
+  return msg;
+}
+
+inline geometry_msgs::msg::Transform ToRosTransform(const Transform3& trans)
+{
+  geometry_msgs::msg::Transform msg;
+  msg.translation = ToRosVec(trans.translation);
+  msg.rotation = ToRosQuat(RToQuat(trans.rotation));
+
+  return msg;
+}
+
+inline geometry_msgs::msg::Quaternion ToRosQuat(const Vec3& angles)
+{
+  return ToRosQuat(RToQuat(ZYXToR(angles)));
+}
+
+inline Vec3 RosQuatToZYX(const geometry_msgs::msg::Quaternion& msg)
+{
+  return RToZYX(QuatToR(FromROS(msg)));
+}
+
+inline double GetRoll(const geometry_msgs::msg::Quaternion& q)
+{
+  return Rot3(FromROS(q)).Roll();
+}
+
+inline double GetPitch(const geometry_msgs::msg::Quaternion& q)
+{
+  return Rot3(FromROS(q)).Pitch();
+}
+
+inline double GetYaw(const geometry_msgs::msg::Quaternion& q)
+{
+  return Rot3(FromROS(q)).Yaw();
+}
+
+}  // namespace ros
+}  // namespace geometry_utils
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/GeometryUtilsSerialization.h

@@ -0,0 +1,73 @@
+/*
+geometry_utils: Utility library to provide common geometry types and
+transformations Copyright (C) 2013  Nathan Michael 2016  Erik Nelson
+
+This program is free software; you can redistribute it and/or
+modify it under the terms of the GNU General Public License
+as published by the Free Software Foundation; either version 2
+of the License, or (at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+*/
+
+#ifndef GEOMETRY_UTILS_SERIALIZATION_H
+#define GEOMETRY_UTILS_SERIALIZATION_H
+
+#include "GeometryUtils.h"
+
+#include <boost/archive/text_iarchive.hpp>
+#include <boost/archive/text_oarchive.hpp>
+
+namespace boost
+{
+namespace serialization
+{
+template <class Archive>
+void Serialize(Archive& ar, geometry_utils::Vector2& v, const unsigned int version)
+{
+  ar& v(0);
+  ar& v(1);
+}
+
+template <class Archive>
+void Serialize(Archive& ar, geometry_utils::Vector3& v, const unsigned int version)
+{
+  ar& v(0);
+  ar& v(1);
+  ar& v(2);
+}
+
+template <class Archive>
+void Serialize(Archive& ar, geometry_utils::Vector4& v, const unsigned int version)
+{
+  ar& v(0);
+  ar& v(1);
+  ar& v(2);
+  ar& v(3);
+}
+
+template <class Archive>
+void Serialize(Archive& ar, geometry_utils::Matrix3x3& m, const unsigned int version)
+{
+  for (unsigned int i = 0; i < 3; i++)
+    for (unsigned int j = 0; j < 3; j++)
+      ar& m(i, j);
+}
+
+template <class Archive>
+void Serialize(Archive& ar, geometry_utils::Transform& t, const unsigned int version)
+{
+  ar& t.translation;
+  ar& t.rotation;
+}
+
+}  // namespace serialization
+}  // namespace boost
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/Matrix2x2.h

@@ -0,0 +1,117 @@
+#ifndef GEOMETRY_UTILS_MATRIX2X2_H
+#define GEOMETRY_UTILS_MATRIX2X2_H
+
+#include "MatrixNxNBase.h"
+#include "Vector2.h"
+
+namespace geometry_utils
+{
+template <typename T>
+struct Matrix2x2Base : MatrixNxNBase<T, 2>
+{
+  Matrix2x2Base() : MatrixNxNBase<T, 2>()
+  {
+  }
+  Matrix2x2Base(T val) : MatrixNxNBase<T, 2>(val)
+  {
+  }
+  Matrix2x2Base(const Matrix2x2Base& in) : MatrixNxNBase<T, 2>(in.data)
+  {
+  }
+  Matrix2x2Base(const boost::array<T, 4>& in) : MatrixNxNBase<T, 2>(in)
+  {
+  }
+  Matrix2x2Base(T (&in)[2 * 2]) : MatrixNxNBase<T, 2>(in)
+  {
+  }
+  Matrix2x2Base(const Eigen::Matrix<T, 2, 2>& in) : MatrixNxNBase<T, 2>(in)
+  {
+  }
+  Matrix2x2Base(const MatrixNxNBase<T, 2>& in) : MatrixNxNBase<T, 2>(in)
+  {
+  }
+  Matrix2x2Base(const MatrixNxMBase<T, 2, 2>& in) : MatrixNxNBase<T, 2>(in)
+  {
+  }
+
+  Matrix2x2Base(T R11, T R12, T R21, T R22)
+  {
+    this->data[0] = R11;
+    this->data[1] = R12;
+    this->data[2] = R21;
+    this->data[3] = R22;
+  }
+
+  virtual inline T Det() const
+  {
+    T a = this->data[0];
+    T b = this->data[1];
+    T c = this->data[2];
+    T d = this->data[3];
+    return (-(b * c) + a * d);
+  }
+
+  virtual inline MatrixNxNBase<T, 2> Inv() const
+  {
+    Vector2Base<T> e(SingularValues());
+
+    T emax = e(0);
+    T emin = e(1);
+
+    if (emin < std::numeric_limits<T>::denorm_min())
+      throw std::runtime_error("Matrix2x2Base: appears singular");
+
+    if (emax / emin > std::numeric_limits<T>::epsilon())
+    {
+      T a = this->data[0];
+      T b = this->data[1];
+      T c = this->data[2];
+      T d = this->data[3];
+
+      T tmp[4] = { d / (-b * c + a * d), b / (b * c - a * d), c / (b * c - a * d), a / (-b * c + a * d) };
+      return MatrixNxNBase<T, 2>(tmp);
+    }
+    else
+      throw std::runtime_error("Matrix2x2Base: appears singular");
+  }
+
+  virtual inline Vector2Base<T> SingularValues() const
+  {
+    T a = this->data[0];
+    T b = this->data[1];
+    T c = this->data[2];
+    T d = this->data[3];
+
+    T tmp1 = a * a + b * b + c * c + d * d;
+    T tmp2 = math::sqrt((math::pow(b + c, static_cast<T>(2)) + math::pow(a - d, static_cast<T>(2))) *
+                        (math::pow(b - c, static_cast<T>(2)) + math::pow(a + d, static_cast<T>(2))));
+
+    T e1 = math::sqrt(tmp1 - tmp2) * M_SQRT1_2;
+    T e2 = math::sqrt(tmp1 + tmp2) * M_SQRT1_2;
+
+    return Vector2Base<T>(e1 > e2 ? e1 : e2, e1 < e2 ? e1 : e2);
+  }
+};
+
+inline Matrix2x2Base<float> operator*(const float& lhs, const Matrix2x2Base<float>& rhs)
+{
+  return Matrix2x2Base<float>(rhs * lhs);
+}
+
+inline Matrix2x2Base<double> operator*(const double& lhs, const Matrix2x2Base<double>& rhs)
+{
+  return Matrix2x2Base<double>(rhs * lhs);
+}
+
+typedef Matrix2x2Base<float> Matrix2x2f;
+typedef Matrix2x2Base<float> Mat22f;
+
+typedef Matrix2x2Base<double> Matrix2x2d;
+typedef Matrix2x2Base<double> Mat22d;
+
+typedef Matrix2x2Base<double> Matrix2x2;
+typedef Matrix2x2Base<double> Mat22;
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/Matrix3x3.h

@@ -0,0 +1,111 @@
+#ifndef GEOMETRY_UTILS_MATRIX3X3_H
+#define GEOMETRY_UTILS_MATRIX3X3_H
+
+#include "MatrixNxNBase.h"
+
+namespace geometry_utils
+{
+template <typename T>
+struct Matrix3x3Base : MatrixNxNBase<T, 3>
+{
+  Matrix3x3Base() : MatrixNxNBase<T, 3>()
+  {
+  }
+  Matrix3x3Base(T val) : MatrixNxNBase<T, 3>(val)
+  {
+  }
+  Matrix3x3Base(const Matrix3x3Base& in) : MatrixNxNBase<T, 3>(in.data)
+  {
+  }
+  Matrix3x3Base(const boost::array<T, 9>& in) : MatrixNxNBase<T, 3>(in)
+  {
+  }
+  Matrix3x3Base(T (&in)[9]) : MatrixNxNBase<T, 3>(in)
+  {
+  }
+  Matrix3x3Base(const Eigen::Matrix<T, 3, 3>& in) : MatrixNxNBase<T, 3>(in)
+  {
+  }
+  Matrix3x3Base(const MatrixNxNBase<T, 3>& in) : MatrixNxNBase<T, 3>(in)
+  {
+  }
+  Matrix3x3Base(const MatrixNxMBase<T, 3, 3>& in) : MatrixNxNBase<T, 3>(in)
+  {
+  }
+
+  Matrix3x3Base(T R11, T R12, T R13, T R21, T R22, T R23, T R31, T R32, T R33)
+  {
+    this->data[0] = R11;
+    this->data[1] = R12;
+    this->data[2] = R13;
+    this->data[3] = R21;
+    this->data[4] = R22;
+    this->data[5] = R23;
+    this->data[6] = R31;
+    this->data[7] = R32;
+    this->data[8] = R33;
+  }
+
+  inline T Det() const
+  {
+    T a = this->data[0];
+    T b = this->data[1];
+    T c = this->data[2];
+    T d = this->data[3];
+    T e = this->data[4];
+    T f = this->data[5];
+    T g = this->data[6];
+    T h = this->data[7];
+    T i = this->data[8];
+    return (-(c * e * g) + b * f * g + c * d * h - a * f * h - b * d * i + a * e * i);
+  }
+
+  virtual inline MatrixNxNBase<T, 3> Inv() const
+  {
+    if (math::fabs(Det()) < std::numeric_limits<T>::denorm_min())
+      throw std::runtime_error("Matrix3x3Base: appears singular");
+
+    T a = this->data[0];
+    T b = this->data[1];
+    T c = this->data[2];
+    T d = this->data[3];
+    T e = this->data[4];
+    T f = this->data[5];
+    T g = this->data[6];
+    T h = this->data[7];
+    T i = this->data[8];
+    T tmp[9] = { (f * h - e * i) / (c * e * g - b * f * g - c * d * h + a * f * h + b * d * i - a * e * i),
+                 (c * h - b * i) / (-(c * e * g) + b * f * g + c * d * h - a * f * h - b * d * i + a * e * i),
+                 (c * e - b * f) / (c * e * g - b * f * g - c * d * h + a * f * h + b * d * i - a * e * i),
+                 (f * g - d * i) / (-(c * e * g) + b * f * g + c * d * h - a * f * h - b * d * i + a * e * i),
+                 (c * g - a * i) / (c * e * g - b * f * g - c * d * h + a * f * h + b * d * i - a * e * i),
+                 (c * d - a * f) / (-(c * e * g) + b * f * g + c * d * h - a * f * h - b * d * i + a * e * i),
+                 (e * g - d * h) / (c * e * g - b * f * g - c * d * h + a * f * h + b * d * i - a * e * i),
+                 (b * g - a * h) / (-(c * e * g) + b * f * g + c * d * h - a * f * h - b * d * i + a * e * i),
+                 (b * d - a * e) / (c * e * g - b * f * g - c * d * h + a * f * h + b * d * i - a * e * i) };
+    return MatrixNxNBase<T, 3>(tmp);
+  }
+};
+
+inline Matrix3x3Base<float> operator*(const float& lhs, const Matrix3x3Base<float>& rhs)
+{
+  return Matrix3x3Base<float>(rhs.Scale(lhs));
+}
+
+inline Matrix3x3Base<double> operator*(const double& lhs, const Matrix3x3Base<double>& rhs)
+{
+  return Matrix3x3Base<double>(rhs.Scale(lhs));
+}
+
+typedef Matrix3x3Base<float> Matrix3x3f;
+typedef Matrix3x3Base<float> Mat33f;
+
+typedef Matrix3x3Base<double> Matrix3x3d;
+typedef Matrix3x3Base<double> Mat33d;
+
+typedef Matrix3x3Base<double> Matrix3x3;
+typedef Matrix3x3Base<double> Mat33;
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/Matrix4x4.h

@@ -0,0 +1,209 @@
+#ifndef GEOMETRY_UTILS_MATRIX4X4_H
+#define GEOMETRY_UTILS_MATRIX4X4_H
+
+#include "MatrixNxNBase.h"
+
+namespace geometry_utils
+{
+template <typename T>
+struct Matrix4x4Base : MatrixNxNBase<T, 4>
+{
+  Matrix4x4Base() : MatrixNxNBase<T, 4>()
+  {
+  }
+  Matrix4x4Base(T val) : MatrixNxNBase<T, 4>(val)
+  {
+  }
+  Matrix4x4Base(const Matrix4x4Base& in) : MatrixNxNBase<T, 4>(in.data)
+  {
+  }
+  Matrix4x4Base(const boost::array<T, 16>& in) : MatrixNxNBase<T, 4>(in)
+  {
+  }
+  Matrix4x4Base(T (&in)[16]) : MatrixNxNBase<T, 4>(in)
+  {
+  }
+  Matrix4x4Base(const Eigen::Matrix<T, 4, 4>& in) : MatrixNxNBase<T, 4>(in)
+  {
+  }
+  Matrix4x4Base(const MatrixNxNBase<T, 4>& in) : MatrixNxNBase<T, 4>(in)
+  {
+  }
+  Matrix4x4Base(const MatrixNxMBase<T, 4, 4>& in) : MatrixNxNBase<T, 4>(in)
+  {
+  }
+
+  Matrix4x4Base(T R11, T R12, T R13, T R14, T R21, T R22, T R23, T R24, T R31, T R32, T R33, T R34, T R41, T R42, T R43,
+                T R44)
+  {
+    this->data[0] = R11;
+    this->data[1] = R12;
+    this->data[2] = R13;
+    this->data[3] = R14;
+    this->data[4] = R21;
+    this->data[5] = R22;
+    this->data[6] = R23;
+    this->data[7] = R24;
+    this->data[8] = R31;
+    this->data[9] = R32;
+    this->data[10] = R33;
+    this->data[11] = R34;
+    this->data[12] = R41;
+    this->data[13] = R42;
+    this->data[14] = R43;
+    this->data[15] = R44;
+  }
+
+  inline T Det() const
+  {
+    T a = this->data[0];
+    T b = this->data[1];
+    T c = this->data[2];
+    T d = this->data[3];
+    T e = this->data[4];
+    T f = this->data[5];
+    T g = this->data[6];
+    T h = this->data[7];
+    T i = this->data[8];
+    T j = this->data[9];
+    T k = this->data[10];
+    T l = this->data[11];
+    T m = this->data[12];
+    T n = this->data[13];
+    T o = this->data[14];
+    T p = this->data[15];
+
+    return d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+           d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+           d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+           c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p;
+  }
+
+  virtual inline MatrixNxNBase<T, 4> Inv() const
+  {
+    if (math::fabs(Det()) < std::numeric_limits<T>::denorm_min())
+      throw std::runtime_error("Matrix4x4Base: appears singular");
+
+    T a = this->data[0];
+    T b = this->data[1];
+    T c = this->data[2];
+    T d = this->data[3];
+    T e = this->data[4];
+    T f = this->data[5];
+    T g = this->data[6];
+    T h = this->data[7];
+    T i = this->data[8];
+    T j = this->data[9];
+    T k = this->data[10];
+    T l = this->data[11];
+    T m = this->data[12];
+    T n = this->data[13];
+    T o = this->data[14];
+    T p = this->data[15];
+
+    T tmp[16] = { (-(h * k * n) + g * l * n + h * j * o - f * l * o - g * j * p + f * k * p) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
+                  (d * k * n - c * l * n - d * j * o + b * l * o + c * j * p - b * k * p) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
+                  (-(d * g * n) + c * h * n + d * f * o - b * h * o - c * f * p + b * g * p) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
+                  (d * g * j - c * h * j - d * f * k + b * h * k + c * f * l - b * g * l) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
+                  (h * k * m - g * l * m - h * i * o + e * l * o + g * i * p - e * k * p) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
+                  (-(d * k * m) + c * l * m + d * i * o - a * l * o - c * i * p + a * k * p) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
+                  (d * g * m - c * h * m - d * e * o + a * h * o + c * e * p - a * g * p) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
+                  (-(d * g * i) + c * h * i + d * e * k - a * h * k - c * e * l + a * g * l) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
+                  (-(h * j * m) + f * l * m + h * i * n - e * l * n - f * i * p + e * j * p) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
+                  (d * j * m - b * l * m - d * i * n + a * l * n + b * i * p - a * j * p) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
+                  (-(d * f * m) + b * h * m + d * e * n - a * h * n - b * e * p + a * f * p) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
+                  (d * f * i - b * h * i - d * e * j + a * h * j + b * e * l - a * f * l) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
+                  (g * j * m - f * k * m - g * i * n + e * k * n + f * i * o - e * j * o) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
+                  (-(c * j * m) + b * k * m + c * i * n - a * k * n - b * i * o + a * j * o) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
+                  (c * f * m - b * g * m - c * e * n + a * g * n + b * e * o - a * f * o) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p),
+                  (-(c * f * i) + b * g * i + c * e * j - a * g * j - b * e * k + a * f * k) /
+                      (d * g * j * m - c * h * j * m - d * f * k * m + b * h * k * m + c * f * l * m - b * g * l * m -
+                       d * g * i * n + c * h * i * n + d * e * k * n - a * h * k * n - c * e * l * n + a * g * l * n +
+                       d * f * i * o - b * h * i * o - d * e * j * o + a * h * j * o + b * e * l * o - a * f * l * o -
+                       c * f * i * p + b * g * i * p + c * e * j * p - a * g * j * p - b * e * k * p + a * f * k * p) };
+    return MatrixNxNBase<T, 4>(tmp);
+  }
+};
+
+inline Matrix4x4Base<float> operator*(const float& lhs, const Matrix4x4Base<float>& rhs)
+{
+  return Matrix4x4Base<float>(rhs.Scale(lhs));
+}
+
+inline Matrix4x4Base<double> operator*(const double& lhs, const Matrix4x4Base<double>& rhs)
+{
+  return Matrix4x4Base<double>(rhs.Scale(lhs));
+}
+
+typedef Matrix4x4Base<float> Matrix4x4f;
+typedef Matrix4x4Base<float> Mat44f;
+
+typedef Matrix4x4Base<double> Matrix4x4d;
+typedef Matrix4x4Base<double> Mat44d;
+
+typedef Matrix4x4Base<double> Matrix4x4;
+typedef Matrix4x4Base<double> Mat44;
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/MatrixNxMBase.h

@@ -0,0 +1,335 @@
+#ifndef GEOMETRY_UTILS_MATRIXNXM_H
+#define GEOMETRY_UTILS_MATRIXNXM_H
+
+#include "GeometryUtilsMath.h"
+#include "VectorNBase.h"
+#include <ostream>
+
+namespace geometry_utils
+{
+template <typename T, size_t N, size_t M>
+struct MatrixNxMBase
+{
+  typedef typename boost::shared_ptr<MatrixNxMBase<T, N, M>> Ptr;
+  typedef typename boost::shared_ptr<const MatrixNxMBase<T, N, M>> ConstPtr;
+
+  static const size_t size = N * M;
+  static const size_t nrows = N;
+  static const size_t ncols = M;
+
+  boost::array<T, size> data;
+
+  MatrixNxMBase()
+  {
+    data.fill(0);
+  }
+
+  MatrixNxMBase(T val)
+  {
+    data.fill(val);
+  }
+
+  MatrixNxMBase(const MatrixNxMBase& in) : data(in.data)
+  {
+  }
+
+  MatrixNxMBase(const boost::array<T, size>& in) : data(in)
+  {
+  }
+
+  MatrixNxMBase(T (&in)[size])
+  {
+    for (unsigned int i = 0; i < size; i++)
+      data[i] = in[i];
+  }
+
+  MatrixNxMBase(const Eigen::Matrix<T, N, M>& in)
+  {
+    for (size_t i = 0; i < nrows; i++)
+      for (size_t j = 0; j < ncols; j++)
+        data[ncols * i + j] = in(i, j);
+  }
+
+  inline T& operator()(unsigned int i)
+  {
+    return data[i];
+  }
+
+  inline const T& operator()(unsigned int i) const
+  {
+    return data[i];
+  }
+
+  inline T& Get(unsigned int i)
+  {
+    return data[i];
+  }
+
+  inline const T& Get(unsigned int i) const
+  {
+    return data[i];
+  }
+
+  inline T& operator()(unsigned int i, unsigned int j)
+  {
+    return data[ncols * i + j];
+  }
+
+  inline const T& operator()(unsigned int i, unsigned int j) const
+  {
+    return data[ncols * i + j];
+  }
+
+  inline T& Get(unsigned int i, unsigned int j)
+  {
+    return data[ncols * i + j];
+  }
+
+  inline const T& Get(unsigned int i, unsigned int j) const
+  {
+    return data[ncols * i + j];
+  }
+
+  inline std::ostream& operator<<(std::ostream& out)
+  {
+    out << data;
+    return out;
+  }
+
+  inline MatrixNxMBase& operator=(const MatrixNxMBase& rhs)
+  {
+    if (this == &rhs)
+      return *this;
+    data = rhs.data;
+    return *this;
+  }
+
+  inline MatrixNxMBase<T, N, M> operator*(T rhs) const
+  {
+    T d[size];
+    for (size_t i = 0; i < size; i++)
+      d[i] = data[i] * rhs;
+    return MatrixNxMBase<T, N, M>(d);
+  }
+
+  inline MatrixNxMBase<T, N, M> operator+(const MatrixNxMBase& rhs) const
+  {
+    T d[size];
+    for (size_t i = 0; i < size; i++)
+      d[i] = data[i] + rhs(i);
+    return MatrixNxMBase<T, N, M>(d);
+  }
+
+  inline MatrixNxMBase<T, N, M> operator-() const
+  {
+    T d[size];
+    for (size_t i = 0; i < size; i++)
+      d[i] = -data[i];
+    return MatrixNxMBase<T, N, M>(d);
+  }
+
+  inline MatrixNxMBase<T, N, M> operator-(const MatrixNxMBase& rhs) const
+  {
+    T d[size];
+    for (size_t i = 0; i < size; i++)
+      d[i] = data[i] - rhs(i);
+    return MatrixNxMBase<T, N, M>(d);
+  }
+
+  inline MatrixNxMBase<T, N, N> operator*(const MatrixNxMBase<T, M, N>& rhs) const
+  {
+    T d[N * N];
+    for (size_t i = 0; i < N; i++)
+      for (size_t j = 0; j < N; j++)
+        d[N * i + j] = this->Row(i) ^ rhs.Col(j);
+    return MatrixNxMBase<T, N, N>(d);
+  }
+
+  inline VectorNBase<T, N> operator*(const VectorNBase<T, M>& rhs) const
+  {
+    T d[nrows];
+    for (size_t i = 0; i < nrows; i++)
+      d[i] = this->Row(i) ^ rhs;
+    return VectorNBase<T, N>(d);
+  }
+
+  inline MatrixNxMBase<T, N, M> operator%(const MatrixNxMBase& rhs) const
+  {
+    T d[size];
+    for (size_t i = 0; i < size; i++)
+      d[i] = data[i] * rhs(i);
+    return MatrixNxMBase<T, N, M>(d);
+  }
+
+  inline MatrixNxMBase<T, N, M> operator/(const MatrixNxMBase& rhs) const
+  {
+    T d[size];
+    for (size_t i = 0; i < size; i++)
+      d[i] = data[i] / rhs(i);
+    return MatrixNxMBase<T, N, M>(d);
+  }
+
+  inline MatrixNxMBase<T, N, M> operator+=(const MatrixNxMBase& rhs)
+  {
+    for (size_t i = 0; i < size; i++)
+      data[i] += rhs.data[i];
+    return *this;
+  }
+
+  inline MatrixNxMBase<T, N, M> operator-=(const MatrixNxMBase& rhs)
+  {
+    for (size_t i = 0; i < size; i++)
+      data[i] -= rhs.data[i];
+    return *this;
+  }
+
+  inline MatrixNxMBase<T, N, M> operator%=(const MatrixNxMBase& rhs)
+  {
+    for (size_t i = 0; i < size; i++)
+      data[i] *= rhs.data[i];
+    return *this;
+  }
+
+  inline MatrixNxMBase<T, N, M> operator/=(const MatrixNxMBase& rhs)
+  {
+    for (size_t i = 0; i < size; i++)
+      data[i] /= rhs.data[i];
+    return *this;
+  }
+
+  inline MatrixNxMBase<T, M, N> Trans() const
+  {
+    T d[size];
+    for (size_t i = 0; i < nrows; i++)
+      for (size_t j = 0; j < ncols; j++)
+        d[nrows * j + i] = data[ncols * i + j];
+    return MatrixNxMBase<T, M, N>(d);
+  }
+
+  inline MatrixNxMBase<T, M, N> t() const
+  {
+    return this->Trans();
+  }
+
+  inline VectorNBase<T, M> Row(unsigned int r) const
+  {
+    T d[ncols];
+    for (size_t i = 0; i < ncols; i++)
+      d[i] = data[ncols * r + i];
+    return VectorNBase<T, M>(d);
+  }
+
+  inline VectorNBase<T, N> Col(unsigned int c) const
+  {
+    T d[nrows];
+    for (size_t i = 0; i < nrows; i++)
+      d[i] = data[ncols * i + c];
+    return VectorNBase<T, N>(d);
+  }
+
+  inline void Ones()
+  {
+    data.fill(1);
+  }
+
+  inline void Zeros()
+  {
+    data.fill(0);
+  }
+
+  inline MatrixNxMBase<T, N, M> Scale(T s) const
+  {
+    return (*this) * s;
+  }
+
+  inline void Print(const std::string& prefix = std::string()) const
+  {
+    if (!prefix.empty())
+      std::cout << prefix << std::endl;
+    std::cout << (*this) << std::endl;
+  }
+
+  inline Eigen::Matrix<T, N, M> Eigen() const
+  {
+    return Eigen::Matrix<T, M, N>(data.data()).transpose();
+  }
+
+  inline bool operator==(const MatrixNxMBase& that) const
+  {
+    return this->Equals(that);
+  }
+
+  inline bool operator!=(const MatrixNxMBase& that) const
+  {
+    return !this->Equals(that);
+  }
+
+  virtual inline bool Equals(const MatrixNxMBase& that, const T ptol = 1e-8) const
+  {
+    return (*this - that).Norm() < ptol;
+  }
+
+  inline T Norm() const
+  {
+    return math::sqrt((this->Trans() * (*this)).Trace());
+  }
+
+  inline T Trace()
+  {
+    size_t count = nrows <= ncols ? nrows : ncols;
+    T tr = 0;
+    for (size_t i = 0; i < count; i++)
+      tr += data[ncols * i + i];
+    return tr;
+  }
+};
+
+template <size_t N, size_t M>
+inline MatrixNxMBase<float, N, M> operator*(const float& lhs, const MatrixNxMBase<float, N, M>& rhs)
+{
+  return rhs * lhs;
+}
+
+template <size_t N, size_t M>
+inline MatrixNxMBase<double, N, M> operator*(const double& lhs, const MatrixNxMBase<double, N, M>& rhs)
+{
+  return rhs * lhs;
+}
+
+template <typename T, size_t N, size_t M>
+inline std::ostream& operator<<(std::ostream& out, const MatrixNxMBase<T, N, M>& m)
+{
+  for (size_t i = 0; i < N; i++)
+  {
+    for (size_t j = 0; j < M; j++)
+      out << m.data[M * i + j] << " ";
+    out << std::endl;
+  }
+  return out;
+}
+
+template <typename T, size_t N, size_t M>
+inline Eigen::Matrix<T, N, M> Eigen(const MatrixNxMBase<T, N, M>& in)
+{
+  return in.Eigen();
+}
+
+template <typename T, size_t N, size_t M>
+inline MatrixNxMBase<T, M, N> Trans(const MatrixNxMBase<T, N, M>& m)
+{
+  return m.Trans();
+}
+
+template <typename T, size_t N, size_t M>
+inline MatrixNxMBase<T, N, M> Outer(const VectorNBase<T, N>& v1, const VectorNBase<T, M>& v2)
+{
+  T d[N * M];
+  for (size_t i = 0; i < N; i++)
+    for (size_t j = 0; j < M; j++)
+      d[M * i + j] = v1(i) * v2(j);
+  return MatrixNxMBase<T, N, M>(d);
+}
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/MatrixNxNBase.h

@@ -0,0 +1,84 @@
+#ifndef GEOMETRY_UTILS_MATRIXNXN_H
+#define GEOMETRY_UTILS_MATRIXNXN_H
+
+#include "GeometryUtilsMath.h"
+#include "MatrixNxMBase.h"
+#include "VectorNBase.h"
+#include <ostream>
+
+namespace geometry_utils
+{
+template <typename T, size_t N>
+struct MatrixNxNBase : MatrixNxMBase<T, N, N>
+{
+  MatrixNxNBase() : MatrixNxMBase<T, N, N>()
+  {
+  }
+  MatrixNxNBase(T val) : MatrixNxMBase<T, N, N>(val)
+  {
+  }
+  MatrixNxNBase(const MatrixNxNBase& in) : MatrixNxMBase<T, N, N>(in.data)
+  {
+  }
+  MatrixNxNBase(const boost::array<T, N * N>& in) : MatrixNxMBase<T, N, N>(in)
+  {
+  }
+  MatrixNxNBase(T (&in)[N * N]) : MatrixNxMBase<T, N, N>(in)
+  {
+  }
+  MatrixNxNBase(const Eigen::Matrix<T, N, N>& in) : MatrixNxMBase<T, N, N>(in)
+  {
+  }
+  MatrixNxNBase(const MatrixNxMBase<T, N, N>& in) : MatrixNxMBase<T, N, N>(in)
+  {
+  }
+
+  inline void Eye()
+  {
+    this->data.fill(0);
+    for (size_t i = 0; i < this->nrows; i++)
+      this->data[this->nrows * i + i] = 1;
+  }
+
+  virtual inline T Det() const
+  {
+    std::cerr << "MatrixNxMBase::det not implemented" << std::endl;
+    return T();
+  }
+
+  virtual inline MatrixNxNBase<T, N> Inv() const
+  {
+    std::cerr << "MatrixNxMBase::inv not implemented" << std::endl;
+    return MatrixNxNBase<T, N>();
+  }
+
+  static inline MatrixNxNBase<T, N> Diag(const VectorNBase<T, N>& in)
+  {
+    T d[N * N] = { 0 };
+    for (size_t i = 0; i < N; i++)
+      d[N * i + i] = in(i);
+    return MatrixNxNBase<T, N>(d);
+  }
+};
+
+template <size_t N>
+inline MatrixNxNBase<float, N> operator*(const float& lhs, const MatrixNxNBase<float, N>& rhs)
+{
+  return MatrixNxNBase<float, N>(rhs * lhs);
+}
+
+template <size_t N>
+inline MatrixNxNBase<double, N> operator*(const double& lhs, const MatrixNxNBase<double, N>& rhs)
+{
+  return MatrixNxNBase<double, N>(rhs * lhs);
+}
+
+template <typename T, size_t N>
+inline MatrixNxNBase<T, N> Inv(const MatrixNxNBase<T, N>& m)
+{
+  return m.Inv();
+}
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/Quaternion.h

@@ -0,0 +1,134 @@
+#ifndef GEOMETRY_UTILS_QUATERNION_H
+#define GEOMETRY_UTILS_QUATERNION_H
+
+#include "GeometryUtilsMath.h"
+#include <Eigen/Geometry>
+#include <boost/array.hpp>
+#include <string>
+
+namespace geometry_utils
+{
+template <typename T>
+struct QuaternionBase : VectorNBase<T, 4>
+{
+  QuaternionBase() : VectorNBase<T, 4>()
+  {
+    this->data.assign(0);
+    this->data[0] = 1;
+  }
+
+  QuaternionBase(T val) : VectorNBase<T, 4>(val)
+  {
+  }
+  QuaternionBase(const QuaternionBase& in) : VectorNBase<T, 4>(in.data)
+  {
+  }
+  QuaternionBase(const boost::array<T, 4>& in) : VectorNBase<T, 4>(in)
+  {
+  }
+  QuaternionBase(T (&in)[4]) : VectorNBase<T, 4>(in)
+  {
+  }
+  QuaternionBase(const Eigen::Quaternion<T>& in)
+  {
+    this->data[0] = in.w();
+    this->data[1] = in.x();
+    this->data[2] = in.y();
+    this->data[3] = in.z();
+  }
+  QuaternionBase(const VectorNBase<T, 4>& in) : VectorNBase<T, 4>(in)
+  {
+  }
+
+  QuaternionBase(T w, T x, T y, T z)
+  {
+    this->data[0] = w;
+    this->data[1] = x;
+    this->data[2] = y;
+    this->data[3] = z;
+  }
+
+  inline QuaternionBase operator*(const QuaternionBase& rhs) const
+  {
+    T a1 = this->data[0];
+    T b1 = this->data[1];
+    T c1 = this->data[2];
+    T d1 = this->data[3];
+    T a2 = rhs.data[0];
+    T b2 = rhs.data[1];
+    T c2 = rhs.data[2];
+    T d2 = rhs.data[3];
+    return QuaternionBase(a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2, a2 * b1 + a1 * b2 - c2 * d1 + c1 * d2,
+                          a2 * c1 + a1 * c2 + b2 * d1 - b1 * d2, -(b2 * c1) + b1 * c2 + a2 * d1 + a1 * d2);
+  }
+
+  inline T& W()
+  {
+    return this->data[0];
+  }
+  inline const T& W() const
+  {
+    return this->data[0];
+  }
+
+  inline T& X()
+  {
+    return this->data[1];
+  }
+  inline const T& X() const
+  {
+    return this->data[1];
+  }
+
+  inline T& Y()
+  {
+    return this->data[2];
+  }
+  inline const T& Y() const
+  {
+    return this->data[2];
+  }
+
+  inline T& Z()
+  {
+    return this->data[3];
+  }
+  inline const T& Z() const
+  {
+    return this->data[3];
+  }
+
+  inline QuaternionBase Conj() const
+  {
+    return QuaternionBase(this->data[0], -this->data[1], -this->data[2], -this->data[3]);
+  }
+
+  inline QuaternionBase Error(const QuaternionBase& q) const
+  {
+    return q * (*this).Conj();
+  }
+
+  inline QuaternionBase AxisAngle() const
+  {
+    QuaternionBase q((*this));
+    if (q.W() > 1)
+      q = q.Normalize();
+    T den = math::sqrt(1 - q.W() * q.W());
+    if (den < 1e-6)
+      den = 1;
+    return QuaternionBase(2 * math::acos(q.W()), q.X() / den, q.Y() / den, q.Z() / den);
+  }
+};
+
+typedef QuaternionBase<float> Quaternionf;
+typedef QuaternionBase<float> Quatf;
+
+typedef QuaternionBase<double> Quaterniond;
+typedef QuaternionBase<double> Quatd;
+
+typedef QuaternionBase<double> Quaternion;
+typedef QuaternionBase<double> Quat;
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/Rotation2.h

@@ -0,0 +1,111 @@
+#ifndef GEOMETRY_UTILS_ROTATION2_H
+#define GEOMETRY_UTILS_ROTATION2_H
+
+#include "GeometryUtilsMath.h"
+#include "Matrix2x2.h"
+#include "RotationNBase.h"
+#include <Eigen/Geometry>
+
+namespace geometry_utils
+{
+template <typename T>
+struct Rotation2Base : RotationNBase<T, 2>
+{
+  Rotation2Base() : RotationNBase<T, 2>()
+  {
+  }
+  Rotation2Base(const Rotation2Base& in) : RotationNBase<T, 2>(in.data)
+  {
+  }
+  Rotation2Base(const boost::array<T, 4>& in) : RotationNBase<T, 2>(in)
+  {
+  }
+  Rotation2Base(T (&in)[2 * 2]) : RotationNBase<T, 2>(in)
+  {
+  }
+  Rotation2Base(const Eigen::Matrix<T, 2, 2>& in) : RotationNBase<T, 2>(in)
+  {
+  }
+  Rotation2Base(const Eigen::Rotation2D<T>& in) : RotationNBase<T, 2>(in.toRotationMatrix())
+  {
+  }
+  Rotation2Base(const RotationNBase<T, 2>& in) : RotationNBase<T, 2>(in)
+  {
+  }
+  Rotation2Base(const Matrix2x2Base<T>& in) : RotationNBase<T, 2>(in)
+  {
+  }
+  Rotation2Base(const MatrixNxMBase<T, 2, 2>& in) : RotationNBase<T, 2>(in)
+  {
+  }
+
+  Rotation2Base(T val)
+  {
+    FromAngle(val);
+  }
+
+  Rotation2Base(T R11, T R12, T R21, T R22)
+  {
+    this->data[0] = R11;
+    this->data[1] = R12;
+    this->data[2] = R21;
+    this->data[3] = R22;
+  }
+
+  virtual inline bool Equals(const Rotation2Base& that, const T ptol = 1e-8) const
+  {
+    return Error(that) < ptol;
+  }
+
+  inline T Error(const Rotation2Base& r) const
+  {
+    return math::sin(Angle() - r.Angle());
+  }
+
+  inline T Angle() const
+  {
+    return math::atan2(this->data[2], this->data[0]);
+  }
+
+  inline void FromAngle(T val)
+  {
+    this->data[0] = math::cos(val);
+    this->data[1] = -math::sin(val);
+    this->data[2] = math::sin(val);
+    this->data[3] = math::cos(val);
+  }
+
+  inline Eigen::Rotation2D<T> Eigen()
+  {
+    return Eigen::Rotation2D<T>(Angle());
+  }
+};
+
+inline Rotation2Base<float> operator*(const float& lhs, const Rotation2Base<float>& rhs)
+{
+  return Rotation2Base<float>(rhs.Scale(lhs));
+}
+
+inline Rotation2Base<double> operator*(const double& lhs, const Rotation2Base<double>& rhs)
+{
+  return Rotation2Base<double>(rhs.Scale(lhs));
+}
+
+template <typename T>
+inline Eigen::Rotation2D<T> Eigen(const Rotation2Base<T>& in)
+{
+  return in.Eigen();
+}
+
+typedef Rotation2Base<float> Rotation2f;
+typedef Rotation2Base<float> Rot2f;
+
+typedef Rotation2Base<double> Rotation2d;
+typedef Rotation2Base<double> Rot2d;
+
+typedef Rotation2Base<double> Rotation2;
+typedef Rotation2Base<double> Rot2;
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/Rotation3.h

@@ -0,0 +1,223 @@
+#ifndef GEOMETRY_UTILS_ROTATION3_H
+#define GEOMETRY_UTILS_ROTATION3_H
+
+#include "GeometryUtilsMath.h"
+#include "Matrix2x2.h"
+#include "Quaternion.h"
+#include "Rotation2.h"
+#include "RotationNBase.h"
+#include "Vector3.h"
+#include <Eigen/Geometry>
+
+namespace geometry_utils
+{
+template <typename T>
+struct Rotation3Base : RotationNBase<T, 3>
+{
+  Rotation3Base() : RotationNBase<T, 3>()
+  {
+  }
+  Rotation3Base(const Rotation3Base& in) : RotationNBase<T, 3>(in.data)
+  {
+  }
+  Rotation3Base(const boost::array<T, 9>& in) : RotationNBase<T, 3>(in)
+  {
+  }
+  Rotation3Base(T (&in)[9]) : RotationNBase<T, 3>(in)
+  {
+  }
+  Rotation3Base(const Eigen::Matrix<T, 3, 3>& in) : RotationNBase<T, 3>(in)
+  {
+  }
+  Rotation3Base(const Eigen::AngleAxis<T>& in) : RotationNBase<T, 3>(in.toRotationMatrix())
+  {
+  }
+  Rotation3Base(const RotationNBase<T, 3>& in) : RotationNBase<T, 3>(in)
+  {
+  }
+  Rotation3Base(const Matrix2x2Base<T>& in) : RotationNBase<T, 3>(in)
+  {
+  }
+  Rotation3Base(const MatrixNxMBase<T, 3, 3>& in) : RotationNBase<T, 3>(in)
+  {
+  }
+
+  Rotation3Base(T R11, T R12, T R13, T R21, T R22, T R23, T R31, T R32, T R33)
+  {
+    this->data[0] = R11;
+    this->data[1] = R12;
+    this->data[2] = R13;
+    this->data[3] = R21;
+    this->data[4] = R22;
+    this->data[5] = R23;
+    this->data[6] = R31;
+    this->data[7] = R32;
+    this->data[8] = R33;
+  }
+
+  Rotation3Base(const Vector3Base<T>& in)
+  {
+    FromEulerZYX(in(0), in(1), in(2));
+  }
+
+  Rotation3Base(T euler_x, T euler_y, T euler_z)
+  {
+    FromEulerZYX(euler_x, euler_y, euler_z);
+  }
+
+  Rotation3Base(const Rotation2Base<T>& rot)
+  {
+    this->Zeros();
+    this->data[0] = rot(0);
+    this->data[1] = rot(1);
+    this->data[3] = rot(2);
+    this->data[4] = rot(3);
+    this->data[8] = static_cast<T>(1);
+  }
+
+  Rotation3Base(const QuaternionBase<T>& quat)
+  {
+    QuaternionBase<T> q(quat);
+    if (math::fabs(1 - q.Norm()) > static_cast<T>(1e-6))
+      q = q.Normalize();
+
+    T a = q.W();
+    T b = q.X();
+    T c = q.Y();
+    T d = q.Z();
+
+    T a2 = a * a;
+    T b2 = b * b;
+    T c2 = c * c;
+    T d2 = d * d;
+
+    T ab = a * b;
+    T ac = a * c;
+    T ad = a * d;
+
+    T bc = b * c;
+    T bd = b * d;
+
+    T cd = c * d;
+
+    (*this)(0, 0) = a2 + b2 - c2 - d2;
+    (*this)(0, 1) = static_cast<T>(2) * (bc - ad);
+    (*this)(0, 2) = static_cast<T>(2) * (bd + ac);
+    (*this)(1, 0) = static_cast<T>(2) * (bc + ad);
+    (*this)(1, 1) = a2 - b2 + c2 - d2;
+    (*this)(1, 2) = static_cast<T>(2) * (cd - ab);
+    (*this)(2, 0) = static_cast<T>(2) * (bd - ac);
+    (*this)(2, 1) = static_cast<T>(2) * (cd + ab);
+    (*this)(2, 2) = a2 - b2 - c2 + d2;
+  }
+
+  virtual inline bool Equals(const Rotation3Base& that, const T ptol = 1e-8) const
+  {
+    return Error(that) < ptol;
+  }
+
+  inline T Error(const Rotation3Base& r) const
+  {
+    return Norm(static_cast<T>(0.5) * Rotation3Base<T>(this->Trans() * r - r.Trans() * (*this)).Vee());
+  }
+
+  inline Vector3Base<T> Vee() const
+  {
+    return Vector3Base<T>((*this)(2, 1), (*this)(0, 2), (*this)(1, 0));
+  }
+
+  inline void FromEulerZYX(T euler_x, T euler_y, T euler_z)
+  {
+    T cph = math::cos(euler_x);
+    T sph = math::sin(euler_x);
+
+    T ct = math::cos(euler_y);
+    T st = math::sin(euler_y);
+
+    T cps = math::cos(euler_z);
+    T sps = math::sin(euler_z);
+
+    (*this)(0, 0) = ct * cps;
+    (*this)(0, 1) = cps * st * sph - cph * sps;
+    (*this)(0, 2) = cph * cps * st + sph * sps;
+
+    (*this)(1, 0) = ct * sps;
+    (*this)(1, 1) = cph * cps + st * sph * sps;
+    (*this)(1, 2) = -cps * sph + cph * st * sps;
+
+    (*this)(2, 0) = -st;
+    (*this)(2, 1) = ct * sph;
+    (*this)(2, 2) = ct * cph;
+  }
+
+  inline Vector3Base<T> GetEulerZYX() const
+  {
+    return ToEulerZYX();
+  }
+
+  inline Vector3Base<T> ToEulerZYX() const
+  {
+    T theta = -math::asin((*this)(2, 0));
+    T phi = 0;
+    T psi = 0;
+    if (math::fabs(cos(theta)) > static_cast<T>(1e-6))
+    {
+      phi = math::atan2((*this)(2, 1), (*this)(2, 2));
+      psi = math::atan2((*this)(1, 0), (*this)(0, 0));
+    }
+
+    return Vector3Base<T>(phi, theta, psi);
+  }
+
+  inline T Roll() const
+  {
+    T theta = -math::asin((*this)(2, 0));
+    return math::fabs(cos(theta)) > static_cast<T>(1e-6) ? math::atan2((*this)(2, 1), (*this)(2, 2)) : 0;
+  }
+
+  inline T Pitch() const
+  {
+    return -math::asin((*this)(2, 0));
+  }
+
+  inline T Yaw() const
+  {
+    T theta = -math::asin((*this)(2, 0));
+    return math::fabs(cos(theta)) > static_cast<T>(1e-6) ? math::atan2((*this)(1, 0), (*this)(0, 0)) : 0;
+  }
+};
+
+inline Rotation3Base<float> operator*(const float& lhs, const Rotation3Base<float>& rhs)
+{
+  return Rotation3Base<float>(rhs.Scale(lhs));
+}
+
+inline Rotation3Base<double> operator*(const double& lhs, const Rotation3Base<double>& rhs)
+{
+  return Rotation3Base<double>(rhs.Scale(lhs));
+}
+
+template <typename T>
+inline Rotation3Base<T> Hat(const Vector3Base<T>& v)
+{
+  return Rotation3Base<T>(0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0);
+}
+
+template <typename T>
+inline Vector3Base<T> Vee(const MatrixNxMBase<T, 3, 3>& m)
+{
+  return Rotation3Base<T>(m).Vee();
+}
+
+typedef Rotation3Base<float> Rotation3f;
+typedef Rotation3Base<float> Rot3f;
+
+typedef Rotation3Base<double> Rotation3d;
+typedef Rotation3Base<double> Rot3d;
+
+typedef Rotation3Base<double> Rotation3;
+typedef Rotation3Base<double> Rot3;
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/RotationNBase.h

@@ -0,0 +1,61 @@
+#ifndef GEOMETRY_UTILS_ROTATIONN_H
+#define GEOMETRY_UTILS_ROTATIONN_H
+
+#include "MatrixNxNBase.h"
+#include "VectorNBase.h"
+#include <Eigen/Core>
+#include <ostream>
+
+namespace geometry_utils
+{
+template <typename T, size_t N>
+struct RotationNBase : MatrixNxNBase<T, N>
+{
+  RotationNBase() : MatrixNxNBase<T, N>()
+  {
+    this->Eye();
+  }
+
+  RotationNBase(const RotationNBase& in) : MatrixNxNBase<T, N>(in.data)
+  {
+  }
+  RotationNBase(const boost::array<T, N * N>& in) : MatrixNxNBase<T, N>(in)
+  {
+  }
+  RotationNBase(T (&in)[N * N]) : MatrixNxNBase<T, N>(in)
+  {
+  }
+  RotationNBase(const Eigen::Matrix<T, N, N>& in) : MatrixNxNBase<T, N>(in)
+  {
+  }
+  RotationNBase(const MatrixNxNBase<T, N>& in) : MatrixNxNBase<T, N>(in)
+  {
+  }
+
+  virtual inline MatrixNxNBase<T, N> Inv() const
+  {
+    return this->Trans();
+  }
+};
+
+template <size_t N>
+inline RotationNBase<float, N> operator*(const float& lhs, const RotationNBase<float, N>& rhs)
+{
+  return RotationNBase<float, N>(rhs * lhs);
+}
+
+template <size_t N>
+inline RotationNBase<double, N> operator*(const double& lhs, const RotationNBase<double, N>& rhs)
+{
+  return RotationNBase<double, N>(rhs * lhs);
+}
+
+template <typename T, size_t N>
+inline RotationNBase<T, N> Inv(const RotationNBase<T, N>& m)
+{
+  return m.Inv();
+}
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/Transform2.h

@@ -0,0 +1,117 @@
+#ifndef GEOMETRY_UTILS_TRANSFORM2_H
+#define GEOMETRY_UTILS_TRANSFORM2_H
+
+#include "Rotation2.h"
+#include "Vector2.h"
+
+namespace geometry_utils
+{
+template <typename T>
+struct Transform2Base
+{
+  typedef boost::shared_ptr<Transform2Base> Ptr;
+  typedef boost::shared_ptr<const Transform2Base> ConstPtr;
+
+  Vector2Base<T> translation;
+  Rotation2Base<T> rotation;
+
+  Transform2Base()
+  {
+    translation.Zeros();
+    rotation.Eye();
+  }
+
+  Transform2Base(const Vector2Base<T>& translation_, const Rotation2Base<T>& rotation_)
+    : translation(translation_), rotation(rotation_)
+  {
+  }
+
+  Transform2Base(const Transform2Base<T>& in) : translation(in.translation), rotation(in.rotation)
+  {
+  }
+
+  Transform2Base(T x, T y, T th) : translation(x, y), rotation(th)
+  {
+  }
+
+  Transform2Base& operator=(const Transform2Base& rhs)
+  {
+    if (this == &rhs)
+      return *this;
+    translation = rhs.translation;
+    rotation = rhs.rotation;
+    return *this;
+  }
+
+  Vector2Base<T> operator*(const Vector2Base<T>& p) const
+  {
+    return rotation * p + translation;
+  }
+
+  Transform2Base<T> operator+(const Transform2Base<T>& t) const
+  {
+    return Transform2Base<T>(translation + rotation * t.translation, rotation * t.rotation);
+  }
+
+  bool operator==(const Transform2Base& that) const
+  {
+    return this->Equals(that);
+  }
+
+  bool operator!=(const Transform2Base& that) const
+  {
+    return !this->Equals(that);
+  }
+
+  bool Equals(const Transform2Base& that, const T ptol = 1e-5, const T rtol = 1e-5) const
+  {
+    return (translation.Equals(that.translation, ptol) && rotation.Equals(that.rotation, rtol));
+  }
+
+  void Print(const std::string& prefix = std::string()) const
+  {
+    if (!prefix.empty())
+      std::cout << prefix << std::endl;
+    std::cout << (*this) << std::endl;
+  }
+
+  static Transform2Base Identity()
+  {
+    return Transform2Base();
+  }
+};
+
+template <typename T>
+std::ostream& operator<<(std::ostream& out, const Transform2Base<T>& m)
+{
+  out << "Translation:" << std::endl << m.translation << std::endl;
+  out << "Rotation:" << std::endl << m.rotation;
+  return out;
+}
+
+template <typename T>
+Transform2Base<T> PoseUpdate(const Transform2Base<T>& t1, const Transform2Base<T>& t2)
+{
+  return Transform2Base<T>(t1.translation + t1.rotation * t2.translation, t1.rotation * t2.rotation);
+}
+
+template <typename T>
+Transform2Base<T> PoseInverse(const Transform2Base<T>& t)
+{
+  return Transform2Base<T>(-1.0 * t.rotation.Trans() * t.translation, t.rotation.Trans());
+}
+
+template <typename T>
+Transform2Base<T> PoseDelta(const Transform2Base<T>& t1, const Transform2Base<T>& t2)
+{
+  return Transform2Base<T>(t1.rotation.Trans() * (t2.translation - t1.translation), t1.rotation.Trans() * t2.rotation);
+}
+
+typedef Transform2Base<float> Transform2f;
+typedef Transform2Base<double> Transform2d;
+typedef Transform2d Transform2;
+typedef Transform2 Tr2;
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/Transform3.h

@@ -0,0 +1,125 @@
+#ifndef GEOMETRY_UTILS_TRANSFORM3_H
+#define GEOMETRY_UTILS_TRANSFORM3_H
+
+#include "Rotation3.h"
+#include "Transform2.h"
+#include "Vector3.h"
+
+namespace geometry_utils
+{
+template <typename T>
+struct Transform3Base
+{
+  typedef boost::shared_ptr<Transform3Base> Ptr;
+  typedef boost::shared_ptr<const Transform3Base> ConstPtr;
+
+  Vector3Base<T> translation;
+  Rotation3Base<T> rotation;
+
+  Transform3Base()
+  {
+    translation.Zeros();
+    rotation.Eye();
+  }
+
+  Transform3Base(const Vector3Base<T>& translation_, const Rotation3Base<T>& rotation_)
+    : translation(translation_), rotation(rotation_)
+  {
+  }
+
+  Transform3Base(const Transform3Base<T>& in) : translation(in.translation), rotation(in.rotation)
+  {
+  }
+
+  Transform3Base(const Transform2Base<T>& in)
+  {
+    translation(0) = in.translation(0);
+    translation(1) = in.translation(1);
+    translation(2) = 0;
+    rotation.Eye();
+    for (unsigned int i = 0; i < 2; i++)
+      for (unsigned int j = 0; j < 2; j++)
+        rotation(i, j) = in.Rotation(i, j);
+  }
+
+  Transform3Base& operator=(const Transform3Base& rhs)
+  {
+    if (this == &rhs)
+      return *this;
+    translation = rhs.translation;
+    rotation = rhs.rotation;
+    return *this;
+  }
+
+  Vector3Base<T> operator*(const Vector3Base<T>& p) const
+  {
+    return rotation * p + translation;
+  }
+
+  Transform3Base<T> operator+(const Transform3Base<T>& t) const
+  {
+    return Transform3Base<T>(translation + rotation * t.translation, rotation * t.rotation);
+  }
+
+  bool operator==(const Transform3Base& that) const
+  {
+    return this->Equals(that);
+  }
+
+  bool operator!=(const Transform3Base& that) const
+  {
+    return !this->Equals(that);
+  }
+
+  bool Equals(const Transform3Base& that, const T ptol = 1e-5, const T rtol = 1e-5) const
+  {
+    return (translation.Equals(that.translation, ptol) && rotation.Equals(that.rotation, rtol));
+  }
+
+  void Print(const std::string& prefix = std::string()) const
+  {
+    if (!prefix.empty())
+      std::cout << prefix << std::endl;
+    std::cout << (*this) << std::endl;
+  }
+
+  static Transform3Base Identity()
+  {
+    return Transform3Base();
+  }
+};
+
+template <typename T>
+std::ostream& operator<<(std::ostream& out, const Transform3Base<T>& m)
+{
+  out << "Translation:" << std::endl << m.translation << std::endl;
+  out << "Rotation:" << std::endl << m.rotation;
+  return out;
+}
+
+template <typename T>
+Transform3Base<T> PoseUpdate(const Transform3Base<T>& t1, const Transform3Base<T>& t2)
+{
+  return Transform3Base<T>(t1.translation + t1.rotation * t2.translation, t1.rotation * t2.rotation);
+}
+
+template <typename T>
+Transform3Base<T> PoseInverse(const Transform3Base<T>& t)
+{
+  return Transform3Base<T>(-1.0 * t.rotation.Trans() * t.translation, t.rotation.Trans());
+}
+
+template <typename T>
+Transform3Base<T> PoseDelta(const Transform3Base<T>& t1, const Transform3Base<T>& t2)
+{
+  return Transform3Base<T>(t1.rotation.Trans() * (t2.translation - t1.translation), t1.rotation.Trans() * t2.rotation);
+}
+
+typedef Transform3Base<float> Transform3f;
+typedef Transform3Base<double> Transform3d;
+typedef Transform3d Transform3;
+typedef Transform3 Tr3;
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/Vector2.h

@@ -0,0 +1,70 @@
+#ifndef GEOMETRY_UTILS_VECTOR2_H
+#define GEOMETRY_UTILS_VECTOR2_H
+
+#include "VectorNBase.h"
+
+namespace geometry_utils
+{
+template <typename T>
+struct Vector2Base : VectorNBase<T, 2>
+{
+  Vector2Base() : VectorNBase<T, 2>()
+  {
+  }
+  Vector2Base(T val) : VectorNBase<T, 2>(val)
+  {
+  }
+  Vector2Base(const Vector2Base& in) : VectorNBase<T, 2>(in.data)
+  {
+  }
+  Vector2Base(const boost::array<T, 2>& in) : VectorNBase<T, 2>(in)
+  {
+  }
+  Vector2Base(T (&in)[2]) : VectorNBase<T, 2>(in)
+  {
+  }
+  Vector2Base(const Eigen::Matrix<T, 2, 1>& in) : VectorNBase<T, 2>(in)
+  {
+  }
+  Vector2Base(const VectorNBase<T, 2>& in) : VectorNBase<T, 2>(in)
+  {
+  }
+
+  Vector2Base(T v1, T v2)
+  {
+    this->data[0] = v1;
+    this->data[1] = v2;
+  }
+
+  T X() const
+  {
+    return this->data[0];
+  }
+  T Y() const
+  {
+    return this->data[1];
+  }
+};
+
+inline Vector2Base<float> operator*(const float& lhs, const Vector2Base<float>& rhs)
+{
+  return Vector2Base<float>(rhs * lhs);
+}
+
+inline Vector2Base<double> operator*(const double& lhs, const Vector2Base<double>& rhs)
+{
+  return Vector2Base<double>(rhs * lhs);
+}
+
+typedef Vector2Base<float> Vector2f;
+typedef Vector2Base<float> Vec2f;
+
+typedef Vector2Base<double> Vector2d;
+typedef Vector2Base<double> Vec2d;
+
+typedef Vector2Base<double> Vector2;
+typedef Vector2Base<double> Vec2;
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/Vector3.h

@@ -0,0 +1,87 @@
+#ifndef GEOMETRY_UTILS_VECTOR3_H
+#define GEOMETRY_UTILS_VECTOR3_H
+
+#include "VectorNBase.h"
+
+namespace geometry_utils
+{
+template <typename T>
+struct Vector3Base : VectorNBase<T, 3>
+{
+  Vector3Base() : VectorNBase<T, 3>()
+  {
+  }
+  Vector3Base(T val) : VectorNBase<T, 3>(val)
+  {
+  }
+  Vector3Base(const Vector3Base& in) : VectorNBase<T, 3>(in.data)
+  {
+  }
+  Vector3Base(const boost::array<T, 3>& in) : VectorNBase<T, 3>(in)
+  {
+  }
+  Vector3Base(T (&in)[3]) : VectorNBase<T, 3>(in)
+  {
+  }
+  Vector3Base(const Eigen::Matrix<T, 3, 1>& in) : VectorNBase<T, 3>(in)
+  {
+  }
+  Vector3Base(const VectorNBase<T, 3>& in) : VectorNBase<T, 3>(in)
+  {
+  }
+
+  Vector3Base(T v1, T v2, T v3)
+  {
+    this->data[0] = v1;
+    this->data[1] = v2;
+    this->data[2] = v3;
+  }
+
+  T X() const
+  {
+    return this->data[0];
+  }
+  T Y() const
+  {
+    return this->data[1];
+  }
+  T Z() const
+  {
+    return this->data[2];
+  }
+
+  inline Vector3Base<T> Cross(const Vector3Base<T>& v) const
+  {
+    return Vector3Base<T>(-(*this)(2) * v(1) + (*this)(1) * v(2), (*this)(2) * v(0) - (*this)(0) * v(2),
+                          -(*this)(1) * v(0) + (*this)(0) * v(1));
+  }
+};
+
+inline Vector3Base<float> operator*(const float& lhs, const Vector3Base<float>& rhs)
+{
+  return Vector3Base<float>(rhs * lhs);
+}
+
+inline Vector3Base<double> operator*(const double& lhs, const Vector3Base<double>& rhs)
+{
+  return Vector3Base<double>(rhs * lhs);
+}
+
+template <typename T>
+inline VectorNBase<T, 3> Cross(const VectorNBase<T, 3>& v1, const VectorNBase<T, 3>& v2)
+{
+  return Vector3Base<T>(v1).Cross(v2);
+}
+
+typedef Vector3Base<float> Vector3f;
+typedef Vector3Base<float> Vec3f;
+
+typedef Vector3Base<double> Vector3d;
+typedef Vector3Base<double> Vec3d;
+
+typedef Vector3Base<double> Vector3;
+typedef Vector3Base<double> Vec3;
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/Vector4.h

@@ -0,0 +1,63 @@
+#ifndef GEOMETRY_UTILS_VECTOR4_H
+#define GEOMETRY_UTILS_VECTOR4_H
+
+#include "VectorNBase.h"
+
+namespace geometry_utils
+{
+template <typename T>
+struct Vector4Base : VectorNBase<T, 4>
+{
+  Vector4Base() : VectorNBase<T, 4>()
+  {
+  }
+  Vector4Base(T val) : VectorNBase<T, 4>(val)
+  {
+  }
+  Vector4Base(const Vector4Base& in) : VectorNBase<T, 4>(in.data)
+  {
+  }
+  Vector4Base(const boost::array<T, 4>& in) : VectorNBase<T, 4>(in)
+  {
+  }
+  Vector4Base(T (&in)[4]) : VectorNBase<T, 4>(in)
+  {
+  }
+  Vector4Base(const Eigen::Matrix<T, 4, 1>& in) : VectorNBase<T, 4>(in)
+  {
+  }
+  Vector4Base(const VectorNBase<T, 4>& in) : VectorNBase<T, 4>(in)
+  {
+  }
+
+  Vector4Base(T v1, T v2, T v3, T v4)
+  {
+    this->data[0] = v1;
+    this->data[1] = v2;
+    this->data[2] = v3;
+    this->data[3] = v4;
+  }
+};
+
+inline Vector4Base<float> operator*(const float& lhs, const Vector4Base<float>& rhs)
+{
+  return Vector4Base<float>(rhs * lhs);
+}
+
+inline Vector4Base<double> operator*(const double& lhs, const Vector4Base<double>& rhs)
+{
+  return Vector4Base<double>(rhs * lhs);
+}
+
+typedef Vector4Base<float> Vector4f;
+typedef Vector4Base<float> Vec4f;
+
+typedef Vector4Base<double> Vector4d;
+typedef Vector4Base<double> Vec4d;
+
+typedef Vector4Base<double> Vector4;
+typedef Vector4Base<double> Vec4;
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/VectorNBase.h

@@ -0,0 +1,280 @@
+#ifndef GEOMETRY_UTILS_VECTORN_H
+#define GEOMETRY_UTILS_VECTORN_H
+
+#include "GeometryUtilsMath.h"
+#include <Eigen/Core>
+#include <boost/array.hpp>
+#include <boost/shared_ptr.hpp>
+#include <iostream>
+#include <string>
+
+namespace geometry_utils
+{
+template <typename T, size_t N>
+struct VectorNBase
+{
+  typedef typename boost::shared_ptr<VectorNBase<T, N>> Ptr;
+  typedef typename boost::shared_ptr<const VectorNBase<T, N>> ConstPtr;
+
+  static const size_t length = N;
+
+  boost::array<T, N> data;
+
+  VectorNBase()
+  {
+    data.fill(0);
+  }
+
+  VectorNBase(T val)
+  {
+    data.fill(val);
+  }
+
+  VectorNBase(const VectorNBase& in) : data(in.data)
+  {
+  }
+
+  VectorNBase(const boost::array<T, N>& in) : data(in)
+  {
+  }
+
+  VectorNBase(T (&in)[N])
+  {
+    for (size_t i = 0; i < N; i++)
+      data[i] = in[i];
+  }
+
+  VectorNBase(const Eigen::Matrix<T, N, 1>& in)
+  {
+    for (size_t i = 0; i < N; i++)
+      data[i] = in(i, 0);
+  }
+
+  inline T& operator()(unsigned int i)
+  {
+    return data[i];
+  }
+
+  inline const T& operator()(unsigned int i) const
+  {
+    return data[i];
+  }
+
+  inline T& Get(unsigned int i)
+  {
+    return data[i];
+  }
+
+  inline const T& Get(unsigned int i) const
+  {
+    return data[i];
+  }
+
+  inline VectorNBase& operator=(const VectorNBase& rhs)
+  {
+    if (this == &rhs)
+      return *this;
+    data = rhs.data;
+    return *this;
+  }
+
+  inline VectorNBase operator*(T rhs) const
+  {
+    T d[N];
+    for (size_t i = 0; i < N; i++)
+      d[i] = data[i] * rhs;
+    return VectorNBase<T, N>(d);
+  }
+
+  inline VectorNBase operator+(const VectorNBase& rhs) const
+  {
+    T d[N];
+    for (size_t i = 0; i < N; i++)
+      d[i] = data[i] + rhs.data[i];
+    return VectorNBase<T, N>(d);
+  }
+
+  inline VectorNBase operator-() const
+  {
+    T d[N];
+    for (size_t i = 0; i < N; i++)
+      d[i] = -data[i];
+    return VectorNBase<T, N>(d);
+  }
+
+  inline VectorNBase operator-(const VectorNBase& rhs) const
+  {
+    T d[N];
+    for (size_t i = 0; i < N; i++)
+      d[i] = data[i] - rhs.data[i];
+    return VectorNBase<T, N>(d);
+  }
+
+  inline VectorNBase operator%(const VectorNBase& rhs) const
+  {
+    T d[N];
+    for (size_t i = 0; i < N; i++)
+      d[i] = data[i] * rhs.data[i];
+    return VectorNBase<T, N>(d);
+  }
+
+  inline T operator^(const VectorNBase& rhs) const
+  {
+    T dot = 0;
+    for (size_t i = 0; i < N; i++)
+      dot += data[i] * rhs.data[i];
+    return dot;
+  }
+
+  inline VectorNBase operator/(const VectorNBase& rhs) const
+  {
+    T d[N];
+    for (size_t i = 0; i < N; i++)
+      d[i] = data[i] / rhs.data[i];
+    return VectorNBase<T, N>(d);
+  }
+
+  inline VectorNBase operator+=(const VectorNBase& rhs)
+  {
+    for (size_t i = 0; i < N; i++)
+      data[i] += rhs.data[i];
+    return *this;
+  }
+
+  inline VectorNBase operator-=(const VectorNBase& rhs)
+  {
+    for (size_t i = 0; i < N; i++)
+      data[i] -= rhs.data[i];
+    return *this;
+  }
+
+  inline VectorNBase operator%=(const VectorNBase& rhs)
+  {
+    for (size_t i = 0; i < N; i++)
+      data[i] *= rhs.data[i];
+    return *this;
+  }
+
+  inline VectorNBase operator/=(const VectorNBase& rhs)
+  {
+    for (size_t i = 0; i < N; i++)
+      data[i] /= rhs.data[i];
+    return *this;
+  }
+
+  inline VectorNBase operator*=(const T& rhs)
+  {
+    for (size_t i = 0; i < N; i++)
+      data[i] *= rhs;
+    return *this;
+  }
+
+  inline VectorNBase operator/=(const T& rhs)
+  {
+    for (size_t i = 0; i < N; i++)
+      data[i] /= rhs;
+    return *this;
+  }
+
+  inline bool operator==(const VectorNBase& that) const
+  {
+    return this->Equals(that);
+  }
+
+  inline bool operator!=(const VectorNBase& that) const
+  {
+    return !this->Equals(that);
+  }
+
+  inline bool Equals(const VectorNBase& that, const T ptol = 1e-8) const
+  {
+    return (*this - that).Norm() < ptol;
+  }
+
+  inline T Norm() const
+  {
+    return math::sqrt((*this) ^ (*this));
+  }
+
+  inline VectorNBase Normalize() const
+  {
+    return (*this) / Norm();
+  }
+
+  inline VectorNBase Abs() const
+  {
+    T d[N];
+    for (size_t i = 0; i < N; i++)
+      d[i] = std::abs(data[i]);
+    return VectorNBase<T, N>(d);
+  }
+
+  inline void Ones()
+  {
+    data.fill(1);
+  }
+
+  inline void Zeros()
+  {
+    data.fill(0);
+  }
+
+  inline T Dot(const VectorNBase& v) const
+  {
+    return (*this) ^ v;
+  }
+
+  inline VectorNBase Scale(T s) const
+  {
+    return (*this) * s;
+  }
+
+  inline void Print(const std::string& prefix = std::string()) const
+  {
+    if (!prefix.empty())
+      std::cout << prefix << std::endl;
+    std::cout << (*this) << std::endl;
+  }
+
+  inline Eigen::Matrix<T, N, 1> Eigen() const
+  {
+    return Eigen::Matrix<T, N, 1>(data.data());
+  }
+};
+
+template <typename T, size_t N>
+inline VectorNBase<T, N> operator*(const T& lhs, const VectorNBase<T, N>& rhs)
+{
+  return rhs * lhs;
+}
+
+template <typename T, size_t N>
+inline std::ostream& operator<<(std::ostream& out, const VectorNBase<T, N>& m)
+{
+  for (size_t i = 0; i < N - 1; i++)
+    out << m.data[i] << " ";
+  out << m.data[N - 1];
+  return out;
+}
+
+template <typename T, size_t N>
+inline Eigen::Matrix<T, N, 1> Eigen(const VectorNBase<T, N>& in)
+{
+  return in.Eigen();
+}
+
+template <typename T, size_t N>
+inline T Norm(const VectorNBase<T, N>& v)
+{
+  return v.Norm();
+}
+
+template <typename T, size_t N>
+inline T Dot(const VectorNBase<T, N>& v1, const VectorNBase<T, N>& v2)
+{
+  return v1.Dot(v2);
+}
+
+}  // namespace geometry_utils
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/geometry_utils/YAMLLoader.h

@@ -0,0 +1,106 @@
+/*
+geometry_utils: Utility library to provide common geometry types and
+transformations Copyright (C) 2013  Nathan Michael 2016  Erik Nelson
+
+This program is free software; you can redistribute it and/or
+modify it under the terms of the GNU General Public License
+as published by the Free Software Foundation; either version 2
+of the License, or (at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+*/
+
+#ifndef GEOMETRY_UTILS_YAML_LOADER_H
+#define GEOMETRY_UTILS_YAML_LOADER_H
+
+#ifdef __YAML_UTILS_H__
+#error "YAMLUtils must be included last"
+#endif
+
+#include <geometry_utils/GeometryUtils.h>
+#include <yaml-cpp/yaml.h>
+
+namespace YAML
+{
+template <>
+struct convert<geometry_utils::Vector2>
+{
+  static Node Encode(const geometry_utils::Vector2& rhs)
+  {
+    Node node;
+    node.push_back(rhs[0]);
+    node.push_back(rhs[1]);
+    return node;
+  }
+
+  static bool Decode(const Node& node, geometry_utils::Vector2& rhs)
+  {
+    if (!node.IsSequence() || node.size() != 2)
+      return false;
+
+    rhs[0] = node[0].as<double>();
+    rhs[1] = node[1].as<double>();
+    return true;
+  }
+};
+
+template <>
+struct convert<geometry_utils::Vector3>
+{
+  static Node Encode(const geometry_utils::Vector3& rhs)
+  {
+    Node node;
+    node.push_back(rhs[0]);
+    node.push_back(rhs[1]);
+    node.push_back(rhs[2]);
+    return node;
+  }
+
+  static bool Decode(const Node& node, geometry_utils::Vector3& rhs)
+  {
+    if (!node.IsSequence() || node.size() != 3)
+      return false;
+
+    rhs[0] = node[0].as<double>();
+    rhs[1] = node[1].as<double>();
+    rhs[2] = node[2].as<double>();
+    return true;
+  }
+};
+
+template <>
+struct convert<geometry_utils::Vector4>
+{
+  static Node Encode(const geometry_utils::Vector4& rhs)
+  {
+    Node node;
+    node.push_back(rhs[0]);
+    node.push_back(rhs[1]);
+    node.push_back(rhs[2]);
+    node.push_back(rhs[3]);
+    return node;
+  }
+
+  static bool Decode(const Node& node, geometry_utils::Vector4& rhs)
+  {
+    if (!node.IsSequence() || node.size() != 4)
+      return false;
+
+    rhs[0] = node[0].as<double>();
+    rhs[1] = node[1].as<double>();
+    rhs[2] = node[2].as<double>();
+    rhs[3] = node[3].as<double>();
+    return true;
+  }
+};
+
+}  // namespace YAML
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/ikd_tree/ikd_Tree.h

@@ -0,0 +1,240 @@
+#pragma once
+#include <algorithm>
+#include <chrono>
+#include <math.h>
+#include <memory.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pthread.h>
+#include <queue>
+#include <stdio.h>
+#include <time.h>
+#include <unistd.h>
+
+#define EPSS 1e-6
+#define Minimal_Unbalanced_Tree_Size 10
+#define Multi_Thread_Rebuild_Point_Num 1500
+#define DOWNSAMPLE_SWITCH true
+#define ForceRebuildPercentage 0.2
+#define Q_LEN 1000000
+
+using namespace std;
+
+typedef pcl::PointXYZINormal PointType;
+// typedef vector<PointType, Eigen::aligned_allocator<PointType>>  PointVector;
+typedef pcl::PointCloud<PointType> PointVector;
+
+// struct PointType {
+//  float x, y, z;
+//  PointType(float px = 0.0f, float py = 0.0f, float pz = 0.0f) {
+//    x = px;
+//    y = py;
+//    z = pz;
+//  }
+//};
+
+// typedef vector<PointType> PointVector;
+
+const PointType ZeroP;
+
+struct KD_TREE_NODE {
+  PointType point;
+  int division_axis;
+  int TreeSize = 1;
+  int invalid_point_num = 0;
+  int down_del_num = 0;
+  bool point_deleted = false;
+  bool tree_deleted = false;
+  bool point_downsample_deleted = false;
+  bool tree_downsample_deleted = false;
+  bool need_push_down_to_left = false;
+  bool need_push_down_to_right = false;
+  bool working_flag = false;
+  pthread_mutex_t push_down_mutex_lock;
+  float node_range_x[2], node_range_y[2], node_range_z[2];
+  KD_TREE_NODE* left_son_ptr = nullptr;
+  KD_TREE_NODE* right_son_ptr = nullptr;
+  KD_TREE_NODE* father_ptr = nullptr;
+  // For paper data record
+  float alpha_del;
+  float alpha_bal;
+};
+
+struct PointType_CMP {
+  PointType point;
+  float dist = 0.0;
+  PointType_CMP(PointType p = ZeroP, float d = INFINITY) {
+    this->point = p;
+    this->dist = d;
+  };
+  bool operator<(const PointType_CMP& a) const {
+    if (fabs(dist - a.dist) < 1e-10)
+      return point.x < a.point.x;
+    else
+      return dist < a.dist;
+  }
+};
+
+// struct BoxPointType {
+//  float vertex_min[3];
+//  float vertex_max[3];
+//};
+struct BoxPointType {
+  Eigen::Vector3f vertex_min;
+  Eigen::Vector3f vertex_max;
+};
+enum operation_set {
+  ADD_POINT,
+  DELETE_POINT,
+  DELETE_BOX,
+  ADD_BOX,
+  DOWNSAMPLE_DELETE,
+  PUSH_DOWN
+};
+
+enum delete_point_storage_set {
+  NOT_RECORD,
+  DELETE_POINTS_REC,
+  MULTI_THREAD_REC
+};
+
+struct Operation_Logger_Type {
+  PointType point;
+  BoxPointType boxpoint;
+  bool tree_deleted, tree_downsample_deleted;
+  operation_set op;
+};
+
+class MANUAL_Q {
+private:
+  int head = 0, tail = 0, counter = 0;
+  Operation_Logger_Type q[Q_LEN];
+  bool is_empty;
+
+public:
+  void pop();
+  Operation_Logger_Type front();
+  Operation_Logger_Type back();
+  void clear();
+  void push(Operation_Logger_Type op);
+  bool empty();
+  int size();
+};
+
+class MANUAL_HEAP {
+public:
+  MANUAL_HEAP(int max_capacity = 100);
+  ~MANUAL_HEAP();
+  void pop();
+  PointType_CMP top();
+  void push(PointType_CMP point);
+  int size();
+  void clear();
+
+private:
+  PointType_CMP* heap;
+  void MoveDown(int heap_index);
+  void FloatUp(int heap_index);
+  int heap_size = 0;
+  int cap = 0;
+};
+
+class KD_TREE {
+private:
+  // Multi-thread Tree Rebuild
+  bool termination_flag = false;
+  bool rebuild_flag = false;
+  pthread_t rebuild_thread;
+  pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock,
+      working_flag_mutex, search_flag_mutex;
+  pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock;
+  // queue<Operation_Logger_Type> Rebuild_Logger;
+  MANUAL_Q Rebuild_Logger;
+  PointVector Rebuild_PCL_Storage;
+  KD_TREE_NODE** Rebuild_Ptr = nullptr;
+  int search_mutex_counter = 0;
+  static void* multi_thread_ptr(void* arg);
+  void multi_thread_rebuild();
+  void start_thread();
+  void stop_thread();
+  void run_operation(KD_TREE_NODE** root, Operation_Logger_Type operation);
+  // KD Tree Functions and augmented variables
+  int Treesize_tmp = 0, Validnum_tmp = 0;
+  float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0;
+  float delete_criterion_param = 0.5f;
+  float balance_criterion_param = 0.7f;
+  float downsample_size = 0.2f;
+  bool Delete_Storage_Disabled = false;
+  KD_TREE_NODE* STATIC_ROOT_NODE = nullptr;
+  PointVector Points_deleted;
+  PointVector Downsample_Storage;
+  PointVector Multithread_Points_deleted;
+  void InitTreeNode(KD_TREE_NODE* root);
+  void Test_Lock_States(KD_TREE_NODE* root);
+  void BuildTree(KD_TREE_NODE** root, int l, int r, PointVector& Storage);
+  void Rebuild(KD_TREE_NODE** root);
+  int Delete_by_range(KD_TREE_NODE** root,
+                      BoxPointType boxpoint,
+                      bool allow_rebuild,
+                      bool is_downsample);
+  void
+  Delete_by_point(KD_TREE_NODE** root, PointType point, bool allow_rebuild);
+  void Add_by_point(KD_TREE_NODE** root,
+                    PointType point,
+                    bool allow_rebuild,
+                    int father_axis);
+  void
+  Add_by_range(KD_TREE_NODE** root, BoxPointType boxpoint, bool allow_rebuild);
+  void Search(KD_TREE_NODE* root,
+              int k_nearest,
+              PointType point,
+              MANUAL_HEAP& q,
+              double max_dist); // priority_queue<PointType_CMP>
+  void Search_by_range(KD_TREE_NODE* root,
+                       BoxPointType boxpoint,
+                       PointVector& Storage);
+  bool Criterion_Check(KD_TREE_NODE* root);
+  void Push_Down(KD_TREE_NODE* root);
+  void Update(KD_TREE_NODE* root);
+  void delete_tree_nodes(KD_TREE_NODE** root);
+  void downsample(KD_TREE_NODE** root);
+  bool same_point(PointType a, PointType b);
+  float calc_dist(PointType a, PointType b);
+  float calc_box_dist(KD_TREE_NODE* node, PointType point);
+  static bool point_cmp_x(PointType a, PointType b);
+  static bool point_cmp_y(PointType a, PointType b);
+  static bool point_cmp_z(PointType a, PointType b);
+
+public:
+  KD_TREE(float delete_param = 0.5,
+          float balance_param = 0.6,
+          float box_length = 0.2);
+  ~KD_TREE();
+  void Set_delete_criterion_param(float delete_param);
+  void Set_balance_criterion_param(float balance_param);
+  void set_downsample_param(float box_length);
+  void InitializeKDTree(float delete_param = 0.5,
+                        float balance_param = 0.7,
+                        float box_length = 0.2);
+  int size();
+  int validnum();
+  void root_alpha(float& alpha_bal, float& alpha_del);
+  void Build(PointVector point_cloud);
+  void Nearest_Search(PointType point,
+                      int k_nearest,
+                      PointVector& Nearest_Points,
+                      vector<float>& Point_Distance,
+                      double max_dist = INFINITY);
+  int Add_Points(PointVector& PointToAdd, bool downsample_on);
+  void Add_Point_Boxes(vector<BoxPointType>& BoxPoints);
+  void Delete_Points(PointVector& PointToDel);
+  int Delete_Point_Boxes(vector<BoxPointType>& BoxPoints);
+  void flatten(KD_TREE_NODE* root,
+               PointVector& Storage,
+               delete_point_storage_set storage_type);
+  void acquire_removed_points(PointVector& removed_points);
+  BoxPointType tree_range();
+  PointVector PCL_Storage;
+  KD_TREE_NODE* Root_Node = nullptr;
+  int max_queue_size = 0;
+};

src/localization_module_lio_locus/include/lio_locus_humble/lio_locus/cloud_pre_processor.h

@@ -0,0 +1,127 @@
+/*
+ * cloud_pre_processor.h
+ *
+ *  Created on: Oct 17, 2025
+ *      Author: solomon
+ */
+
+#ifndef PACKAGES_LIO_LOCUS_INCLUDE_LIO_LOCUS_CLOUD_PRE_PROCESSOR_H_
+#define PACKAGES_LIO_LOCUS_INCLUDE_LIO_LOCUS_CLOUD_PRE_PROCESSOR_H_
+
+// headers for ROS;
+#include <pcl_conversions/pcl_conversions.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <tf2/LinearMath/Transform.h>
+
+// headers for PCL;
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/PCLPointCloud2.h>
+#include <pcl/conversions.h>
+#include <pcl/pcl_base.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/features/normal_3d_omp.h>
+
+// headers for convenience;
+#include <boost/shared_ptr.hpp>
+#include <boost/thread/mutex.hpp>
+
+#include <glog/logging.h>
+
+#include "lio_locus/parameter_loading.h"
+#include <chrono>
+
+
+// define data type to store customized lidar data;
+namespace robosense_ros {
+struct EIGEN_ALIGN16 Point {
+	PCL_ADD_POINT4D
+	;
+	float intensity;
+	double timestamp;
+	uint16_t ring;EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace velodyne_ros
+POINT_CLOUD_REGISTER_POINT_STRUCT(robosense_ros::Point,
+		(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(double, timestamp, timestamp) // of 64-bits
+		(uint16_t, ring, ring))
+
+namespace livox_lidar {
+struct EIGEN_ALIGN16 Point {
+	PCL_ADD_POINT4D
+	;
+	float intensity;
+	uint8_t line;
+	uint8_t tag;
+	double timestamp;EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+} // namespace velodyne_ros
+POINT_CLOUD_REGISTER_POINT_STRUCT(livox_lidar::Point,
+		(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(double, timestamp, timestamp) (uint8_t, tag, tag)(uint8_t, line, line))
+
+class CloudPreProcessor {
+public:
+	CloudPreProcessor();
+	virtual ~CloudPreProcessor();
+	typedef boost::shared_ptr<CloudPreProcessor> Ptr;
+
+	// convert ROS cloud message to PCL cloud format;
+	static pcl::PointCloud<pcl::PointXYZINormal>::Ptr adaptCloudForm(
+			const sensor_msgs::msg::PointCloud2 &cloud_input);
+
+	static pcl::PointCloud<pcl::PointXYZINormal>::Ptr adaptCloudFormFromLivoxCloud(
+			const sensor_msgs::msg::PointCloud2 &cloud_input);
+
+	// attention: curvature segment to store time difference for each laser beam;
+	static pcl::PointCloud<pcl::PointXYZINormal>::Ptr transformCloudCartesian(
+			pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_input,
+			tf2::Transform T);
+
+	// combo unique interface for transformation and normal vector calculation;
+	bool transformCloudWithNormalCalculation(
+			pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_input,
+			tf2::Transform T,
+			pcl::PointCloud<pcl::PointXYZINormal>::Ptr &cloud_normals);
+
+	inline void setCloudPreProcessingParameters(double leaf_voxel_filtering,
+			double filter_limit, int size_cores, int size_k_search,
+			double radius_search) {
+		leaf_voxel_filtering_ = leaf_voxel_filtering;
+		filter_limit_ = filter_limit;
+
+		size_cores_ = size_cores;
+		size_k_search_ = size_k_search;
+		radius_search_ = radius_search;
+
+		LOG(INFO) << "... leaf_voxel_filtering_ -> ["
+				<< leaf_voxel_filtering_ << "]";
+		LOG(INFO) << "... filter_limit_ -> " << filter_limit_ << "]";
+		LOG(INFO) << "... size_k_search_ -> [" << size_k_search_ << "]";
+		LOG(INFO) << "... radius_search_ -> [" << radius_search_ << "]";
+		LOG(INFO) << "... size_cores_ -> [" << size_cores_ << "]";
+	}
+
+	inline void setLogPath(std::string path) {
+		path_log = path;
+	}
+
+private:
+	bool doVoxelGridFilteringAndComputeNormals(
+			pcl::PointCloud<pcl::PointXYZINormal>::Ptr input,
+			pcl::PointCloud<pcl::PointXYZINormal>::Ptr &cloud_normals);
+
+	boost::mutex mutex_;
+	int size_cores_ { 3 };
+	double leaf_voxel_filtering_ { 0.6 };
+	int size_k_search_ { 20 };
+	double radius_search_ { 0 };
+	double filter_limit_ { 100 };
+
+	std::string path_log { "" };
+
+	std::string log_tag { "cloud pre-processing" };
+};
+
+#endif /* PACKAGES_LIO_LOCUS_INCLUDE_LIO_LOCUS_CLOUD_PRE_PROCESSOR_H_ */

src/localization_module_lio_locus/include/lio_locus_humble/lio_locus/complementary_filter.h

@@ -0,0 +1,184 @@
+/*
+ @author Roberto G. Valenti <robertogl.valenti@gmail.com>
+
+ @section LICENSE
+ Copyright (c) 2015, City University of New York
+ CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
+ 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 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 IMU_TOOLS_COMPLEMENTARY_FILTER_H
+#define IMU_TOOLS_COMPLEMENTARY_FILTER_H
+
+#include "lio_locus/parameter_loading.h"
+#include <chrono>
+#include <cmath>
+
+using std::string;
+
+namespace imu_tools {
+
+class ComplementaryFilter {
+public:
+	ComplementaryFilter();
+	virtual ~ComplementaryFilter();
+
+	bool setGainAcc(double gain);
+	bool setGainMag(double gain);
+	double getGainAcc() const;
+	double getGainMag() const;
+
+	bool setBiasAlpha(double bias_alpha);
+	double getBiasAlpha() const;
+
+	// When the filter is in the steady state, bias estimation will occur (if
+	// the parameter is enabled).
+	bool getSteadyState() const;
+
+	void setDoBiasEstimation(bool do_bias_estimation);
+	bool getDoBiasEstimation() const;
+
+	void setDoAdaptiveGain(bool do_adaptive_gain);
+	bool getDoAdaptiveGain() const;
+
+	double getAngularVelocityBiasX() const;
+	double getAngularVelocityBiasY() const;
+	double getAngularVelocityBiasZ() const;
+
+	// Set the orientation, as a Hamilton Quaternion, of the body frame wrt the
+	// fixed frame.
+	void setOrientation(double q0, double q1, double q2, double q3);
+
+	// Get the orientation, as a Hamilton Quaternion, of the body frame wrt the
+	// fixed frame.
+	void getOrientation(double &q0, double &q1, double &q2, double &q3) const;
+
+	// Update from accelerometer and gyroscope data.
+	// [ax, ay, az]: Normalized gravity vector.
+	// [wx, wy, wz]: Angular veloctiy, in rad / s.
+	// dt: time delta, in seconds.
+	void update(double ax, double ay, double az, double wx, double wy,
+			double wz, double dt);
+
+	// Update from accelerometer, gyroscope, and magnetometer data.
+	// [ax, ay, az]: Normalized gravity vector.
+	// [wx, wy, wz]: Angular veloctiy, in rad / s.
+	// [mx, my, mz]: Magnetic field, units irrelevant.
+	// dt: time delta, in seconds.
+	void update(double ax, double ay, double az, double wx, double wy,
+			double wz, double mx, double my, double mz, double dt);
+
+	inline void setLogPath(string path) {
+		path_log = path;
+	}
+
+private:
+	static const double kGravity;
+	static const double gamma_;
+	// Bias estimation steady state thresholds
+	static const double kAngularVelocityThreshold;
+	static const double kAccelerationThreshold;
+	static const double kDeltaAngularVelocityThreshold;
+
+	string path_log { "" };
+
+	// Gain parameter for the complementary filter, belongs in [0, 1].
+	double gain_acc_;
+	double gain_mag_;
+
+	// Bias estimation gain parameter, belongs in [0, 1].
+	double bias_alpha_;
+
+	// Parameter whether to do bias estimation or not.
+	bool do_bias_estimation_;
+
+	// Parameter whether to do adaptive gain or not.
+	bool do_adaptive_gain_;
+
+	bool initialized_;
+	bool steady_state_;
+
+	// The orientation as a Hamilton quaternion (q0 is the scalar). Represents
+	// the orientation of the fixed frame wrt the body frame.
+	double q0_, q1_, q2_, q3_;
+
+	// Bias in angular velocities;
+	double wx_prev_, wy_prev_, wz_prev_;
+
+	// Bias in angular velocities;
+	double wx_bias_, wy_bias_, wz_bias_;
+
+	void updateBiases(double ax, double ay, double az, double wx, double wy,
+			double wz);
+
+	bool checkState(double ax, double ay, double az, double wx, double wy,
+			double wz) const;
+
+	void getPrediction(double wx, double wy, double wz, double dt,
+			double &q0_pred, double &q1_pred, double &q2_pred,
+			double &q3_pred) const;
+
+	void getMeasurement(double ax, double ay, double az, double &q0_meas,
+			double &q1_meas, double &q2_meas, double &q3_meas);
+
+	void getMeasurement(double ax, double ay, double az, double mx, double my,
+			double mz, double &q0_meas, double &q1_meas, double &q2_meas,
+			double &q3_meas);
+
+	void getAccCorrection(double ax, double ay, double az, double p0, double p1,
+			double p2, double p3, double &dq0, double &dq1, double &dq2,
+			double &dq3);
+
+	void getMagCorrection(double mx, double my, double mz, double p0, double p1,
+			double p2, double p3, double &dq0, double &dq1, double &dq2,
+			double &dq3);
+
+	double getAdaptiveGain(double alpha, double ax, double ay, double az);
+};
+
+// Utility math functions:
+
+void normalizeVector(double &x, double &y, double &z);
+
+void normalizeQuaternion(double &q0, double &q1, double &q2, double &q3);
+
+void scaleQuaternion(double gain, double &dq0, double &dq1, double &dq2,
+		double &dq3);
+
+void invertQuaternion(double q0, double q1, double q2, double q3,
+		double &q0_inv, double &q1_inv, double &q2_inv, double &q3_inv);
+
+void quaternionMultiplication(double p0, double p1, double p2, double p3,
+		double q0, double q1, double q2, double q3, double &r0, double &r1,
+		double &r2, double &r3);
+
+void rotateVectorByQuaternion(double x, double y, double z, double q0,
+		double q1, double q2, double q3, double &vx, double &vy, double &vz);
+
+}  // namespace imu_tools
+
+#endif  // IMU_TOOLS_COMPLEMENTARY_FILTER_H

src/localization_module_lio_locus/include/lio_locus_humble/lio_locus/lio_locus.h

@@ -0,0 +1,231 @@
+/*
+ * lio_locus.h
+ *
+ *  Created on: Sep 11, 2025
+ *      Author: solomon
+ */
+
+#ifndef PACKAGES_TEST_LOCUS_SRC_LIO_LOCUS_H_
+#define PACKAGES_TEST_LOCUS_SRC_LIO_LOCUS_H_
+
+// dependency headers;
+#include <point_cloud_odometry/PointCloudOdometry.h>
+#include <point_cloud_localization/PointCloudLocalization.h>
+
+//#include <point_cloud_mapper/PointCloudMapper.h>
+//#include <point_cloud_mapper/PointCloudMultiThreadedMapper.h>
+#include <point_cloud_mapper/PointCloudIkdTreeMapper.h>
+
+#include <geometry_utils/GeometryUtils.h>
+
+#include <Eigen/Geometry>
+#include <boost/filesystem.hpp>
+#include <boost/shared_ptr.hpp>
+#include <map>
+#include <math.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl_conversions/pcl_conversions.h>
+
+#include "lio_locus/parameter_loading.h"
+
+#include <glog/logging.h>
+
+using std::string;
+using std::map;
+
+using point_cloud_odometry::PointCloudOdometry;
+using point_cloud_localization::PointCloudLocalization;
+
+typedef pcl::PointCloud<pcl::PointXYZINormal> PointCloudType;
+typedef boost::shared_ptr<PointCloudOdometry> PointCloudOdometryPtr;
+typedef boost::shared_ptr<PointCloudLocalization> PointCloudLocalizationPtr;
+typedef boost::shared_ptr<PointCloudIkdTreeMapper> PointCloudIkdTreeMapperPtr;
+// typedef boost::shared_ptr<PointCloudMultiThreadedMapper> PointCloudMultiThreadedMapperPtr;
+// typedef boost::shared_ptr<PointCloudMapper> PointCloudMapperPtr;
+
+#include <Eigen/Geometry>
+#include <Eigen/Core>
+
+namespace lio_locus {
+
+struct IMUData {
+	IMUData() {
+	}
+
+	IMUData(double ts, double qx, double qy, double qz, double qw) {
+
+		timestamp_ = ts;
+
+		orientation.x() = qx;
+		orientation.y() = qy;
+		orientation.z() = qz;
+		orientation.w() = qw;
+	}
+
+	Eigen::Quaterniond orientation { Eigen::Quaterniond::Identity() };
+	double timestamp_;
+};
+
+struct TransformStamped {
+
+	TransformStamped() {
+	}
+
+	TransformStamped(geometry_utils::Transform3 T, std::uint64_t timestamp_) {
+		pose = T;
+		timestamp = timestamp_;
+	}
+
+	geometry_utils::Transform3 pose { geometry_utils::Transform3::Identity() };
+	std::uint64_t timestamp { 0 }; // PCL time stamp;
+};
+
+class LioLocus {
+public:
+	LioLocus();
+	virtual ~LioLocus();
+	typedef boost::shared_ptr<LioLocus> Ptr;
+
+	//------------------------------------------------------------------
+	// initialization;
+	bool initialize(string path_to_param);
+	// data feeding;
+	void feedIMUData(IMUData imu_data);
+	//------------------------------------------------------------------
+	// cloud map in memory is updated;
+	void trackCurrentCloudWithOnlineBuiltMap(PointCloudType::Ptr msg_cloud);
+	//------------------------------------------------------------------
+	// to fresh memory cloud with data storage;
+	bool uploadPriorCloudMap(string fn_cloud_map, double leaf_size);
+	// for re-localization;
+	void setPosePrior(geometry_utils::Transform3 pose_prior);
+	// cloud map in memory is not updated;
+	bool trackCurrentCloudWithPriorCloudMap(PointCloudType::Ptr msg_cloud);
+
+	void clearCloudMapInMemory();
+
+	bool saveCloudMapInMemory(string path, bool flag_voxel_filtering = false,
+			double leaf_size = 0.5);
+
+	//===============================================================================
+	// data retrieval;
+
+	bool getCloudMapInMemory(PointCloudType::Ptr cloud);
+
+	static PointCloudType::Ptr downsampleCloud(PointCloudType::Ptr input_cloud,
+			double leaf_size);
+
+	inline TransformStamped getTimestampedLatestPose() {
+		return m_pose_latest;
+	}
+
+	// Pose6DOF getCurrentPose6DOF();
+
+	inline void printCurrentPose() {
+
+		auto loc = getTimestampedLatestPose().pose;
+
+		LOG(INFO) << "... current pose -> #[" << loc.translation.X() << ","
+				<< loc.translation.Y() << "," << loc.rotation.Yaw() << "]";
+	}
+	inline int showImuBufferSize() {
+		return m_imus.size();
+	}
+
+	inline PointCloudType::Ptr getLatestLocalizedCloud() {
+		return mp_cloud_incremental;
+	}
+
+	//------------------------------------------------------------------
+	// to use flat ground assumption or not;
+	inline void setFlatGroundAssumption(bool flag) {
+		mb_set_flat_ground = flag;
+		mp_cloud_odometry->SetFlatGroundAssumptionValue(mb_set_flat_ground);
+		mp_cloud_localization->SetFlatGroundAssumptionValue(mb_set_flat_ground);
+	}
+
+	// to use imu prior for delta transform updating or not;
+	inline void setUseIMU(bool flag) {
+		mb_use_imu = flag;
+
+	}
+
+	// to use scan-to-scan matching for delta transform updating or not;
+	inline void setUseScan2ScanMatching(bool flag) {
+		mb_use_scan2scan_matching = flag;
+	}
+
+	inline void setThresholds(double trans, double rot) {
+		m_threshold_trans_map = trans;
+		m_threshold_rot_map = rot;
+	}
+
+	inline void setIMU2CloudExtrinsic(Eigen::Quaterniond quat) {
+		m_dq_imu2cloud = quat;
+	}
+
+	inline void printIMU2CloudExtrinsic() {
+		Eigen::Matrix3d T = m_dq_imu2cloud.toRotationMatrix();
+		geometry_utils::Rot3 rot_gu = geometry_utils::Rot3(T(0, 0), T(0, 1),
+				T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1), T(2, 2));
+
+		LOG(INFO) << "... orientation for extrinsic -> RPY ["
+				<< rot_gu.Roll() << "," << rot_gu.Pitch() << "," << rot_gu.Yaw()
+				<< "]";
+	}
+
+	inline void setPathLog(string path) {
+		path_log = path;
+	}
+
+	//===============================================================================
+
+private:
+	void setPath2Parameters(string path_to_param);
+	bool getRelativeRotationFromIMU(double ts,
+			geometry_utils::Quaterniond &quat_imu);
+	Eigen::Matrix3d toRotationMatrix(geometry_utils::Quaterniond quat_imu);
+	bool getMessageAtTimeStamp(double stamp, IMUData &msg,
+			map<double, IMUData> buffer);
+	void transformCloudAndInsertToMap(PointCloudType::Ptr msg_cloud);
+
+	// to store input data;	
+	map<double, IMUData> m_imus;
+	int m_size_imus { 20000 };
+	Eigen::Quaterniond m_dq_imu2cloud { Eigen::Quaterniond::Identity() };
+	string m_path_to_file { "" };
+
+	// flags;
+	bool mb_use_scan2scan_matching { true };
+	bool mb_use_imu { false };
+	bool mb_tracker_reset { false };
+	bool mb_set_flat_ground { false };
+
+	// cloud map update thresholds;
+	double m_threshold_trans_map { 0.5 };
+	double m_threshold_rot_map { 20.0 / 180.0 * M_PI };
+
+	string log_tag { "lio-locus" };
+	string path_log { "" };
+
+	// incrementally localized cloud;
+	PointCloudType::Ptr mp_cloud_incremental;
+
+	TransformStamped m_pose_latest;
+
+	// dependency processors;
+	PointCloudOdometryPtr mp_cloud_odometry;
+	PointCloudLocalizationPtr mp_cloud_localization;
+	PointCloudIkdTreeMapperPtr mp_cloud_mapper;
+
+//	shared_ptr<PointCloudMapper> mp_cloud_mapper;
+
+//	PointCloudMapperPtr mp_cloud_mapper;
+//	PointCloudMultiThreadedMapperPtr mp_cloud_mapper;
+
+};
+
+}
+
+#endif /* PACKAGES_TEST_LOCUS_SRC_LIO_LOCUS_H_ */

src/localization_module_lio_locus/include/lio_locus_humble/lio_locus/parameter_loading.h

@@ -0,0 +1,430 @@
+#include <boost/filesystem.hpp>
+
+#include <yaml-cpp/yaml.h>
+
+#include <string>
+#include <cmath>
+
+#include <glog/logging.h>
+
+// for time consumption logging;
+#include <fstream>
+
+using std::string;
+
+struct ParameterLioLocus {
+
+	ParameterLioLocus() {
+	}
+
+	string log_tag { "pure-lo-locus-ros2" };
+
+	string topic_cloud { "/cloud_input" };
+	string topic_imu { "/imu_filtered" };
+	string frame_id_world;
+	string frame_id_ego;
+
+	bool flag_save_map_in_memory { false };
+	string path_save_map { "" };
+	string fn_save_cloud_map { "" };
+	bool flag_voxel_filtering { false };
+	double leaf_size { 0.5 };
+
+	int freq_cloud_proc { 10 };
+
+	bool flag_arranged_proc { true };
+
+	// thresholds for mapping pose recording;
+	double threshold_trans_map { 0.5 };
+	double threshold_rot_map { 10.0 / 180.0 * M_PI };
+
+	// thresholds for trajectory pose recording;
+	double threshold_trans_traj { 0.2 };
+	double threshold_rot_traj { 5.0 / 180.0 * M_PI };
+
+	bool flag_use_imu { false };
+	bool flag_use_scan2scan_matching { true };
+
+	bool flag_use_prior_map { false };
+	string fn_cloud_map { "" };
+	double ratio_fresh_map { 1.2 };
+
+	string path_log { "" };
+
+	double leaf_size_load_map { 0.5 };
+	double leaf_size_vis_map { 0.5 };
+
+	double ext_imu2lidar_dx { 0.0 };
+	double ext_imu2lidar_dy { 0.0 };
+	double ext_imu2lidar_dz { 0.0 };
+	double ext_imu2lidar_droll { 0.0 };
+	double ext_imu2lidar_dpitch { 0.0 };
+	double ext_imu2lidar_dyaw { 0.0 };
+};
+
+bool loadParametersFromFileForLioLocus(std::string path_to_file,
+		ParameterLioLocus &params_lio_locus);
+
+using std::string;
+
+struct ParameterCloudPreprocessing {
+	ParameterCloudPreprocessing() {
+	}
+
+	std::string log_tag { "cloud_preprocessor" };
+
+	bool pub_cloud_preproc { false };
+
+	string topic_cloud_raw { "/rsm1_1" };
+	string topic_cloud { "/cloud_input" };
+	string frame_id_base { "base_link" };
+
+	string path_log { "" };
+
+	double leaf_voxel_filtering { 0.6 };
+	double filter_limit { 100 };
+
+	int size_cores { 6 };
+
+	int size_k_search_knn { 20 };
+	double radius_search_knn { 0 };
+
+	double ext_tx;
+	double ext_ty;
+	double ext_tz;
+
+	double ext_roll;
+	double ext_pitch;
+	double ext_yaw;
+
+};
+
+bool loadParametersFromFileForCloudPreprocessing(std::string path_to_file,
+		ParameterCloudPreprocessing &params_cloud_preproc);
+
+struct ParameterImuPreprocessing {
+
+	ParameterImuPreprocessing() {
+	}
+
+	std::string log_tag { "imu_preprocessor" };
+
+	string topic_imu_raw { "/imu" };
+	string path_log { "" };
+
+	bool pub_imu_preproc { true };
+
+	bool do_bias_estimation { true };
+	bool do_adaptive_gain { true };
+	double bias_alpha { 0.01 };
+	double orientation_stddev { 0.0 };
+	bool flag_publish_tf { false };
+	std::string frame_fixed { "fixed_imu" };
+};
+
+bool loadParametersFromFileForIMUPreprocessing(std::string path_to_file,
+		ParameterImuPreprocessing &params_imu_preproc);
+
+using std::string;
+void writeDt2LogFile(string path, string fn, double dt);
+//-------------------------------------------------------------------------------------------
+
+bool loadParametersFromFileForLioLocus(std::string path_to_file,
+		ParameterLioLocus &params_lio_locus) {
+	if (!boost::filesystem::exists(path_to_file)) {
+		LOG(INFO) << "... parameter file NOT exists -> [" << path_to_file
+				<< "]";
+		return false;
+	} else {
+		LOG(INFO) << "... reading parameter from file -> [" << path_to_file
+				<< "]";
+	}
+
+	LOG(INFO)
+			<< "............................. parameters for lio locus  .............................";
+
+	LOG(INFO) << "... reading parameter for lidar odometry from ["
+			<< path_to_file << "]";
+
+	YAML::Node yaml_node = YAML::LoadFile(path_to_file);
+	auto yn = yaml_node["ros2_wrapper"];
+
+	params_lio_locus.path_log = yn["path_log"].as<string>();
+	LOG(INFO) << "... path_log -> ["
+			<< params_lio_locus.path_log << "]";
+
+	params_lio_locus.flag_save_map_in_memory = yn["flag_save_map_in_memory"].as<
+			bool>();
+	LOG(INFO) << "... flag_save_map_in_memory -> ["
+			<< params_lio_locus.flag_save_map_in_memory << "]";
+
+	params_lio_locus.path_save_map = yn["path_save_map"].as<string>();
+	LOG(INFO) << "... path_save_map -> [" << params_lio_locus.path_save_map
+			<< "]";
+
+	params_lio_locus.fn_save_cloud_map = yn["fn_save_cloud_map"].as<string>();
+	LOG(INFO) << "... fn_save_cloud_map -> ["
+			<< params_lio_locus.fn_save_cloud_map << "]";
+
+	params_lio_locus.flag_voxel_filtering =
+			yn["flag_voxel_filtering"].as<bool>();
+	LOG(INFO) << "... flag_voxel_filtering -> ["
+			<< params_lio_locus.flag_voxel_filtering << "]";
+
+	params_lio_locus.leaf_size = yn["leaf_size"].as<double>();
+	LOG(INFO) << "... leaf_size -> [" << params_lio_locus.leaf_size << "]";
+
+	params_lio_locus.ratio_fresh_map = yn["ratio_fresh_map"].as<double>();
+	LOG(INFO) << "... ratio_fresh_map -> ["
+			<< params_lio_locus.ratio_fresh_map << "]";
+
+	params_lio_locus.freq_cloud_proc = yn["freq_cloud_proc"].as<int>();
+	LOG(INFO) << "... freq_cloud_proc -> ["
+			<< params_lio_locus.freq_cloud_proc << "]";
+
+	params_lio_locus.topic_cloud = yn["topic_cloud"].as<string>();
+	LOG(INFO) << "... topic_cloud -> [" << params_lio_locus.topic_cloud
+			<< "]";
+
+	params_lio_locus.topic_imu = yn["topic_imu"].as<string>();
+	LOG(INFO) << "... topic_imu -> [" << params_lio_locus.topic_imu << "]";
+
+	params_lio_locus.frame_id_world = yn["frame_id_world"].as<string>();
+	LOG(INFO) << "... params_lio_locus.frame_id_world -> ["
+			<< params_lio_locus.frame_id_world << "]";
+
+	params_lio_locus.frame_id_ego = yn["frame_id_ego"].as<string>();
+	LOG(INFO) << "... frame_id_ego -> [" << params_lio_locus.frame_id_ego
+			<< "]";
+
+	// parameter for certain flags;
+	params_lio_locus.flag_use_imu = yn["flag_use_imu"].as<bool>();
+	LOG(INFO) << "... flag_use_imu -> [" << params_lio_locus.flag_use_imu
+			<< "]";
+
+	params_lio_locus.flag_use_scan2scan_matching =
+			yn["flag_use_scan2scan_matching"].as<bool>();
+	LOG(INFO) << "... flag_use_scan2scan_matching -> ["
+			<< params_lio_locus.flag_use_scan2scan_matching << "]";
+
+	params_lio_locus.flag_arranged_proc = yn["flag_arranged_proc"].as<bool>();
+	LOG(INFO) << "... flag_arranged_proc -> ["
+			<< params_lio_locus.flag_arranged_proc << "]";
+
+	// parameter for cloud map loading;
+	params_lio_locus.flag_use_prior_map = yn["flag_use_prior_map"].as<bool>();
+	LOG(INFO) << "... flag_use_prior_map -> ["
+			<< params_lio_locus.flag_use_prior_map << "]";
+
+	string path_map, nm_map;
+	path_map = yn["path_cloud_map"].as<string>();
+	nm_map = yn["fn_cloud_map"].as<string>();
+	params_lio_locus.fn_cloud_map = path_map + "/" + nm_map;
+	LOG(INFO) << "... path_to_map -> [" << params_lio_locus.fn_cloud_map
+			<< "]";
+
+	params_lio_locus.leaf_size_vis_map = yn["leaf_size_vis_map"].as<double>();
+	LOG(INFO) << "... leaf size load map -> ["
+			<< params_lio_locus.leaf_size_vis_map << "]";
+
+	params_lio_locus.leaf_size_load_map = yn["leaf_size_load_map"].as<double>();
+	LOG(INFO) << "... leaf size load map -> ["
+			<< params_lio_locus.leaf_size_load_map << "]";
+
+	// parameters for cloud map generation;
+	params_lio_locus.threshold_trans_map =
+			yn["threshold_trans_map"].as<double>();
+	LOG(INFO) << "... threshold_trans_map -> ["
+			<< params_lio_locus.threshold_trans_map << "]";
+
+	params_lio_locus.threshold_rot_map = yn["threshold_rot_map"].as<double>();
+	LOG(INFO) << "... threshold_rot_map -> ["
+			<< params_lio_locus.threshold_rot_map << "]";
+
+	params_lio_locus.threshold_trans_traj =
+			yn["threshold_trans_traj"].as<double>();
+	LOG(INFO) << "... threshold_trans_traj -> ["
+			<< params_lio_locus.threshold_trans_traj << "]";
+
+	params_lio_locus.threshold_rot_traj = yn["threshold_rot_traj"].as<double>();
+	LOG(INFO) << "... threshold_rot_traj -> ["
+			<< params_lio_locus.threshold_rot_traj << "]";
+
+	auto imu2lidar = yn["imu_to_lidar"];
+
+	params_lio_locus.ext_imu2lidar_dx = imu2lidar["tx"].as<double>();
+	params_lio_locus.ext_imu2lidar_dy = imu2lidar["ty"].as<double>();
+	params_lio_locus.ext_imu2lidar_dz = imu2lidar["tz"].as<double>();
+	params_lio_locus.ext_imu2lidar_droll = imu2lidar["r"].as<double>();
+	params_lio_locus.ext_imu2lidar_dpitch = imu2lidar["p"].as<double>();
+	params_lio_locus.ext_imu2lidar_dyaw = imu2lidar["y"].as<double>();
+	LOG(INFO) << "... imu2lidar translation extrinsic -> ["
+			<< params_lio_locus.ext_imu2lidar_dx << ","
+			<< params_lio_locus.ext_imu2lidar_dy << ","
+			<< params_lio_locus.ext_imu2lidar_dz << "]";
+
+	LOG(INFO) << "... imu2lidar orientation extrinsic -> ["
+			<< params_lio_locus.ext_imu2lidar_droll << ","
+			<< params_lio_locus.ext_imu2lidar_dpitch << ","
+			<< params_lio_locus.ext_imu2lidar_dyaw << "]";
+
+	return true;
+}
+
+bool loadParametersFromFileForCloudPreprocessing(std::string path_to_file,
+		ParameterCloudPreprocessing &params_cloud_preproc) {
+
+	if (!boost::filesystem::exists(path_to_file)) {
+
+		LOG(INFO) << "... parameter file NOT exists -> [" << path_to_file
+				<< "]";
+
+		return false;
+	} else {
+		LOG(INFO) << "... reading parameter from file -> [" << path_to_file
+				<< "]";
+	}
+
+	LOG(INFO)
+			<< "............................. parameters for cloud preprocessing .............................";
+
+	// loading parameters;
+	YAML::Node yaml_node = YAML::LoadFile(path_to_file);
+
+	//---------------------------------------------------------------------------
+	// for cloud pre-processing parameters;
+	auto yn_ros = yaml_node["ros2_wrapper"];
+
+	params_cloud_preproc.path_log = yn_ros["path_log"].as<string>();
+	LOG(INFO) << "... path log -> [" << params_cloud_preproc.path_log
+			<< "]";
+
+	params_cloud_preproc.topic_cloud_raw =
+			yn_ros["topic_cloud_raw"].as<string>();
+	LOG(INFO) << "... topic_cloud_raw -> ["
+			<< params_cloud_preproc.topic_cloud_raw << "]";
+
+	params_cloud_preproc.topic_cloud = yn_ros["topic_cloud"].as<string>();
+	LOG(INFO) << "... topic_cloud -> [" << params_cloud_preproc.topic_cloud
+			<< "]";
+
+	auto yn = yaml_node["cloud_preprocessing"];
+
+	params_cloud_preproc.pub_cloud_preproc = yn["pub_cloud_preproc"].as<bool>();
+
+	params_cloud_preproc.size_cores = yn["size_cores"].as<int>();
+	LOG(INFO) << "... size_cores -> [" << params_cloud_preproc.size_cores
+			<< "]";
+
+	params_cloud_preproc.filter_limit = yn["filter_limit"].as<double>();
+
+	params_cloud_preproc.leaf_voxel_filtering = yn["leaf_voxel_filtering"].as<
+			double>();
+	LOG(INFO) << "... leaf_voxel_filtering -> ["
+			<< params_cloud_preproc.leaf_voxel_filtering << "]";
+
+	params_cloud_preproc.size_k_search_knn = yn["size_k_search_knn"].as<int>();
+	LOG(INFO) << "... size_k_search_knn -> ["
+			<< params_cloud_preproc.size_k_search_knn << "]";
+
+	params_cloud_preproc.radius_search_knn =
+			yn["radius_search_knn"].as<double>();
+	LOG(INFO) << "... radius_search_knn -> ["
+			<< params_cloud_preproc.radius_search_knn << "]";
+
+	params_cloud_preproc.frame_id_base =
+			yaml_node["ros2_wrapper"]["frame_id_ego"].as<string>();
+	LOG(INFO) << "... frame_id_base -> ["
+			<< params_cloud_preproc.frame_id_base << "]";
+	//---------------------------------------------------------------------------
+	// for extrinsic parameters;
+	auto extrinsic_lidar = yaml_node["ros2_wrapper"]["baselink_to_lidar"];
+	params_cloud_preproc.ext_tx = extrinsic_lidar["tx"].as<double>();
+	params_cloud_preproc.ext_ty = extrinsic_lidar["ty"].as<double>();
+	params_cloud_preproc.ext_tz = extrinsic_lidar["tz"].as<double>();
+
+	params_cloud_preproc.ext_pitch = extrinsic_lidar["p"].as<double>();
+	params_cloud_preproc.ext_roll = extrinsic_lidar["r"].as<double>();
+	params_cloud_preproc.ext_yaw = extrinsic_lidar["y"].as<double>();
+
+	double ratio = M_PI / 180.0;
+	params_cloud_preproc.ext_roll *= ratio;
+	params_cloud_preproc.ext_pitch *= ratio;
+	params_cloud_preproc.ext_yaw *= ratio;
+
+	//---------------------------------------------------------------------------
+
+	return true;
+}
+
+bool loadParametersFromFileForIMUPreprocessing(std::string path_to_file,
+		ParameterImuPreprocessing &params_imu_preproc) {
+
+	if (!boost::filesystem::exists(path_to_file)) {
+
+		LOG(INFO) << "... parameter file NOT exists -> [" << path_to_file
+				<< "]";
+
+		return false;
+	} else {
+		LOG(INFO) << "... reading parameter from file -> [" << path_to_file
+				<< "]";
+	}
+
+	LOG(INFO)
+			<< "............................. parameters for imu preprocessing .............................";
+
+	YAML::Node yaml_node = YAML::LoadFile(path_to_file);
+
+	auto yn_ros = yaml_node["ros2_wrapper"];
+	params_imu_preproc.topic_imu_raw = yn_ros["topic_imu_raw"].as<string>();
+	params_imu_preproc.path_log = yn_ros["path_log"].as<string>();
+
+	auto yn = yaml_node["imu_complementary_filtering"];
+	params_imu_preproc.pub_imu_preproc = yn["pub_imu_preproc"].as<bool>();
+	params_imu_preproc.do_adaptive_gain = yn["do_adaptive_gain"].as<bool>();
+
+	LOG(INFO) << "... do_adaptive_gain -> ["
+			<< params_imu_preproc.do_adaptive_gain << "]";
+
+	params_imu_preproc.do_bias_estimation = yn["do_bias_estimation"].as<bool>();
+	LOG(INFO) << "... do_bias_estimation -> ["
+			<< params_imu_preproc.do_bias_estimation << "]";
+
+	params_imu_preproc.bias_alpha = yn["bias_alpha"].as<double>();
+	LOG(INFO) << "... bias_alpha -> [" << params_imu_preproc.bias_alpha
+			<< "]";
+
+	params_imu_preproc.flag_publish_tf = yn["flag_publish_tf"].as<bool>();
+	LOG(INFO) << "... flag_publish_tf -> ["
+			<< params_imu_preproc.flag_publish_tf << "]";
+
+	params_imu_preproc.frame_fixed = yn["frame_fixed_"].as<std::string>();
+
+	params_imu_preproc.orientation_stddev =
+			yn["orientation_stddev"].as<double>();
+
+	return true;
+}
+
+void writeDt2LogFile(string path, string fn, double dt) {
+	// logging to file;
+	boost::filesystem::path path_cloud(path);
+	if (!boost::filesystem::exists(path)) {
+		boost::filesystem::create_directory(path);
+	}
+
+	if(path=="no_log")
+	{
+		LOG(INFO) << "... disabled dt logging ... turn it on if needed by setting value of [path_log]!";
+		return;
+	}
+
+	std::string fn_full = path + "/" + fn;
+	// first time create, then append;
+	std::ofstream ofs(fn_full.c_str(), std::ios_base::app | std::ios_base::out);
+	ofs << dt << "\n";
+	ofs.close();
+
+}
+

src/localization_module_lio_locus/include/lio_locus_humble/multithreaded_gicp/gicp.h

@@ -0,0 +1,443 @@
+/*
+ * 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 MULTITHREADED_GICP_H_
+#define MULTITHREADED_GICP_H_
+
+#include <omp.h>
+
+#include <frontend_utils/CommonFunctions.h>
+#include <frontend_utils/CommonStructs.h>
+#include <pcl/registration/bfgs.h>
+#include <pcl/registration/gicp.h>
+#include <pcl/registration/icp.h>
+
+namespace pcl
+{
+/** \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 MultithreadedGeneralizedIterativeClosestPoint : public GeneralizedIterativeClosestPoint<PointSource, PointTarget>
+{
+public:
+  using IterativeClosestPoint<PointSource, PointTarget>::reg_name_;
+  using IterativeClosestPoint<PointSource, PointTarget>::getClassName;
+  using IterativeClosestPoint<PointSource, PointTarget>::indices_;
+  using IterativeClosestPoint<PointSource, PointTarget>::target_;
+  using IterativeClosestPoint<PointSource, PointTarget>::input_;
+  using IterativeClosestPoint<PointSource, PointTarget>::tree_;
+  using IterativeClosestPoint<PointSource, PointTarget>::tree_reciprocal_;
+  using IterativeClosestPoint<PointSource, PointTarget>::nr_iterations_;
+  using IterativeClosestPoint<PointSource, PointTarget>::max_iterations_;
+  using IterativeClosestPoint<PointSource, PointTarget>::previous_transformation_;
+  using IterativeClosestPoint<PointSource, PointTarget>::final_transformation_;
+  using IterativeClosestPoint<PointSource, PointTarget>::transformation_;
+  using IterativeClosestPoint<PointSource, PointTarget>::transformation_epsilon_;
+  using IterativeClosestPoint<PointSource, PointTarget>::converged_;
+  using IterativeClosestPoint<PointSource, PointTarget>::corr_dist_threshold_;
+  using IterativeClosestPoint<PointSource, PointTarget>::inlier_threshold_;
+  using IterativeClosestPoint<PointSource, PointTarget>::min_number_correspondences_;
+  using 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 PointIndices::Ptr PointIndicesPtr;
+  typedef 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 Registration<PointSource, PointTarget>::KdTree InputKdTree;
+  typedef typename Registration<PointSource, PointTarget>::KdTreePtr InputKdTreePtr;
+
+  typedef boost::shared_ptr<MultithreadedGeneralizedIterativeClosestPoint<PointSource, PointTarget>> Ptr;
+  typedef boost::shared_ptr<const MultithreadedGeneralizedIterativeClosestPoint<PointSource, PointTarget>> ConstPtr;
+
+  typedef Eigen::Matrix<double, 6, 1> Vector6d;
+
+  /** \brief Empty constructor. */
+  MultithreadedGeneralizedIterativeClosestPoint()
+    : k_correspondences_(20)
+    , k_enable_timing_output_(false)
+    , reclculate_cov_normal_point_clouds_(false)
+    , recompute_target_cov_(false)
+    , recompute_source_cov(false)
+    , k_num_threads_(1)
+    , gicp_epsilon_(0.001)
+    , rotation_epsilon_(2e-3)
+    , mahalanobis_(0)
+    , max_inner_iterations_(20)
+  {
+    min_number_correspondences_ = 4;
+    reg_name_ = "MultithreadedGeneralizedIterativeClosestPoint";
+    max_iterations_ = 200;
+    transformation_epsilon_ = 5e-4;
+    corr_dist_threshold_ = 5.;
+    // rigid_transformation_estimation_ = boost::bind(
+    //     &MultithreadedGeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationBFGS, this,
+    //     _1, _2, _3, _4, _5);
+
+        rigid_transformation_estimation_ = [this] (const PointCloudSource& cloud_src,
+                                                   const std::vector<int>& indices_src,
+                                                   const PointCloudTarget& cloud_tgt,
+                                                   const std::vector<int>& indices_tgt,
+                                                   Eigen::Matrix4f& transformation_matrix)
+        {
+          estimateRigidTransformationBFGS (cloud_src, indices_src, cloud_tgt, indices_tgt, transformation_matrix);
+        };
+
+
+
+    setNumThreads(k_num_threads_);
+  }
+
+  void setNumThreads(int num_threads)
+  {
+    assert(num_threads > 0);
+    k_num_threads_ = num_threads;
+    omp_set_num_threads(k_num_threads_);
+  }
+
+  void enableTimingOutput(bool enable)
+  {
+    k_enable_timing_output_ = enable;
+  }
+
+  // template <typename PointSource, typename PointTarget>
+  // static GeneralizedIterativeClosestPoint<PointSource, PointTarget>
+  // MultithreadedGeneralizedIterativeClosestPoint ()
+
+  /** \brief Provide a pointer to the input dataset
+   * \param cloud the const boost shared pointer to a PointCloud message
+   */
+  //  PCL_DEPRECATED(
+  //      "[pcl::registration::MultithreadedGeneralizedIterativeClosestPoint::"
+  //      "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_);
+  }
+
+  void RecomputeTargetCovariance(bool recalculate)
+  {
+    recompute_target_cov_ = recalculate;
+  }
+  void RecomputeSourceCovariance(bool recalculate)
+  {
+    recompute_source_cov = recalculate;
+  }
+
+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
+   * \param recompute recompute covariance matrices based on the inner algorithm
+   * cloud
+   */
+  template <typename PointT>
+  void computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
+                          const typename pcl::search::KdTree<PointT>::Ptr tree, MatricesVector& cloud_covariances,
+                          bool recompute = false);
+
+  /** \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)
+  {
+    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 MultithreadedGeneralizedIterativeClosestPoint* 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 MultithreadedGeneralizedIterativeClosestPoint* gicp_;
+  };
+
+// boost::function
+  std::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_;
+
+private:
+  /** \brief Number of threads that GICP is allowed to use. */
+  int k_num_threads_;
+
+  /** \brief Enables log print statements with GICP timing information. */
+  bool k_enable_timing_output_;
+
+  bool reclculate_cov_normal_point_clouds_;
+
+  bool recompute_target_cov_;
+  bool recompute_source_cov;
+};
+}  // namespace pcl
+
+#include <multithreaded_gicp/gicp.hpp>
+
+#endif  //#ifndef MULTITHREADED_GICP_H_

src/localization_module_lio_locus/include/lio_locus_humble/multithreaded_gicp/gicp.hpp

@@ -0,0 +1,635 @@
+/*
+ * 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 IMPL_MULTITHREADED_GICP_HPP_
+#define IMPL_MULTITHREADEd_GICP_HPP_
+
+#include <algorithm>
+#include <chrono>
+#include <fstream>
+#include <omp.h>
+#include <pcl/features/feature.h>
+#include <pcl/registration/boost.h>
+#include <pcl/registration/exceptions.h>
+
+///////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget>
+void pcl::MultithreadedGeneralizedIterativeClosestPoint
+	<PointSource,PointTarget>::
+    setInputCloud(
+        const typename pcl::MultithreadedGeneralizedIterativeClosestPoint<
+            PointSource,
+            PointTarget>::PointCloudSourceConstPtr& cloud) {
+  setInputSource(cloud);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget>
+template <typename PointT>
+void pcl::MultithreadedGeneralizedIterativeClosestPoint<PointSource,
+                                                        PointTarget>::
+    computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud,
+                       const typename pcl::search::KdTree<PointT>::Ptr kdtree,
+                       MatricesVector& cloud_covariances,
+                       bool recompute) {
+  if (k_correspondences_ > int(cloud->size())) {
+    PCL_ERROR("[pcl::MultithreadedGeneralizedIterativeClosestPoint::"
+              "computeCovariances] Number or points in cloud (%lu) is less "
+              "than k_correspondences_ (%lu)!\n",
+              cloud->size(),
+              k_correspondences_);
+    return;
+  }
+
+  if (std::is_same<PointSource, PointF>::value and !recompute) {
+    CalculateCovarianceFromNormals(cloud, cloud_covariances, k_num_threads_);
+  } else {
+    // TODO cloud->points.size() vs cloud->size() ? any difference?
+    if (cloud_covariances.size() < cloud->size()) {
+      cloud_covariances.resize(cloud->size());
+    }
+
+    int enable_omp = (1 < k_num_threads_);
+#pragma omp parallel for schedule(dynamic, 1) if (enable_omp)
+    for (int i = 0; i < cloud->points.size(); i++) {
+      if (k_enable_timing_output_) {
+        if (0 == i) {
+          // ROS_INFO_STREAM("Using " << omp_get_num_threads() << " threads");
+        }
+      }
+
+      Eigen::Vector3d mean;
+      std::vector<int> nn_indices(k_correspondences_);
+      std::vector<float> nn_dist_sq(k_correspondences_);
+      const PointT& query_point = cloud->points[i];
+      Eigen::Matrix3d& cov = cloud_covariances[i];
+      // Zero out the cov and mean
+      cov.setZero();
+      mean.setZero();
+
+      // Search for the K nearest neighbours
+      kdtree->nearestKSearch(
+          query_point, k_correspondences_, nn_indices, nn_dist_sq);
+
+      // Find the covariance matrix
+      for (int j = 0; j < k_correspondences_; j++) {
+        const PointT& pt = (*cloud)[nn_indices[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 pcl::MultithreadedGeneralizedIterativeClosestPoint<
+    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 pcl::MultithreadedGeneralizedIterativeClosestPoint<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(
+        NotEnoughPointsException,
+        "[pcl::MultithreadedGeneralizedIterativeClosestPoint::"
+        "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(
+        SolverDidntConvergeException,
+        "[pcl::"
+            << getClassName()
+            << "::TransformationEstimationBFGS::estimateRigidTransformation] "
+               "BFGS solver didn't converge!");
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget>
+inline double
+pcl::MultithreadedGeneralizedIterativeClosestPoint<PointSource, PointTarget>::
+    OptimizationFunctorWithIndices::operator()(const Vector6d& x) {
+  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
+  gicp_->applyState(transformation_matrix, x);
+  double f = 0;
+  int m = static_cast<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
+    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
+    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::Vector3d res(pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
+    Eigen::Vector3d temp(gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
+    // increment= res'*temp/num_matches = temp'*M*temp/num_matches (we postpone
+    // 1/num_matches after the loop closes)
+    f += double(res.transpose() * temp);
+  }
+  return f / m;
+}
+
+////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget>
+inline void
+pcl::MultithreadedGeneralizedIterativeClosestPoint<PointSource, PointTarget>::
+    OptimizationFunctorWithIndices::df(const Vector6d& x, Vector6d& g) {
+  Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
+  gicp_->applyState(transformation_matrix, x);
+  // Zero out g
+  g.setZero();
+  // Eigen::Vector3d g_t = g.head<3> ();
+  Eigen::Matrix3d R = Eigen::Matrix3d::Zero();
+  int m = static_cast<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
+    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
+    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 translation gradient
+    // g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the
+    // loop closes)
+    g.head<3>() += temp;
+    // Increment rotation gradient
+    pp = gicp_->base_transformation_ * p_src;
+    Eigen::Vector3d p_src3(pp[0], pp[1], pp[2]);
+    R += p_src3 * temp.transpose();
+  }
+  g.head<3>() *= 2.0 / m;
+  R *= 2.0 / m;
+  gicp_->computeRDerivative(x, R, g);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget>
+inline void
+pcl::MultithreadedGeneralizedIterativeClosestPoint<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
+    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
+    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
+pcl::MultithreadedGeneralizedIterativeClosestPoint<PointSource, PointTarget>::
+    computeTransformation(PointCloudSource& output,
+                          const Eigen::Matrix4f& guess) {
+  auto start_gicp = std::chrono::steady_clock::now();
+
+  pcl::IterativeClosestPoint<PointSource, PointTarget>::initComputeReciprocal();
+  using namespace std;
+  // Difference between consecutive transforms
+  // 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
+  auto start_covariances = std::chrono::steady_clock::now();
+  if ((!target_covariances_) || (target_covariances_->empty())) {
+    target_covariances_.reset(new MatricesVector);
+    computeCovariances<PointTarget>(
+        target_, tree_, *target_covariances_, recompute_target_cov_);
+  }
+  // Compute input cloud covariance matrices
+  if ((!input_covariances_) || (input_covariances_->empty())) {
+    input_covariances_.reset(new MatricesVector);
+    computeCovariances<PointSource>(
+        input_, tree_reciprocal_, *input_covariances_, recompute_source_cov);
+  }
+  auto end_covariances = std::chrono::steady_clock::now();
+
+  base_transformation_ = Eigen::Matrix4f::Identity();
+  nr_iterations_ = 0;
+  converged_ = false;
+  double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_;
+
+  pcl::transformPointCloud(output, output, guess);
+
+  double delta = 0.;
+
+  auto start_iterations = std::chrono::steady_clock::now();
+  while (!converged_) {
+    std::vector<int> source_indices(indices_->size(), -1);
+    std::vector<int> target_indices(indices_->size(), -1);
+
+    // 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>();
+    int failure = 0;
+    auto start_lookups = std::chrono::steady_clock::now();
+    int enable_omp = (1 < k_num_threads_);
+#pragma omp parallel for schedule(dynamic, 1) if (enable_omp)
+    for (size_t i = 0; i < N; i++) {
+      std::vector<int> nn_indices(1);
+      std::vector<float> nn_dists(1);
+      PointSource query = output[i];
+      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]);
+        failure = 1;
+        continue;
+      }
+
+      // Check if the distance to the nearest neighbor is smaller than the user
+      // imposed threshold
+      if (nn_dists[0] < dist_threshold) {
+        Eigen::Matrix3d& C1 = (*input_covariances_)[i];
+        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();
+
+        source_indices[i] = static_cast<int>(i);
+        target_indices[i] = nn_indices[0];
+      }
+    }
+    auto end_lookups = std::chrono::steady_clock::now();
+
+    // There used to be a return statement inside of the loop that was
+    // incompatible with OpenMP Moving the failure and return logic outside of
+    // the loop
+    if (failure) {
+      return;
+    }
+
+    // Resize to the actual number of valid correspondences
+    source_indices.erase(
+        std::remove(source_indices.begin(), source_indices.end(), -1),
+        source_indices.end());
+    target_indices.erase(
+        std::remove(target_indices.begin(), target_indices.end(), -1),
+        target_indices.end());
+
+    /* optimize transformation using the current assignment and Mahalanobis
+     * metrics*/
+    previous_transformation_ = transformation_;
+
+    // optimization right here
+    auto start_optimization = std::chrono::steady_clock::now();
+    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 (PCLException& e) {
+      PCL_DEBUG("[pcl::%s::computeTransformation] Optimization issue %s\n",
+                getClassName().c_str(),
+                e.what());
+      break;
+    }
+    auto end_optimization = std::chrono::steady_clock::now();
+    double lookups_time =
+        static_cast<double>(
+            std::chrono::duration_cast<std::chrono::microseconds>(end_lookups -
+                                                                  start_lookups)
+                .count()) *
+        1.0e-6;
+    double optimization_time =
+        static_cast<double>(
+            std::chrono::duration_cast<std::chrono::microseconds>(
+                end_optimization - start_optimization)
+                .count()) *
+        1.0e-6;
+    // ROS_INFO_STREAM("Lookups: " << lookups_time << " Optimization: " <<
+    // optimization_time);
+
+    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());
+    }
+  }
+  auto end_iterations = std::chrono::steady_clock::now();
+
+  final_transformation_ = previous_transformation_ * guess;
+
+  // Transform the point cloud
+  pcl::transformPointCloud(*input_, output, final_transformation_);
+
+  auto end_gicp = std::chrono::steady_clock::now();
+  if (k_enable_timing_output_) {
+    double iteration_time =
+        static_cast<double>(
+            std::chrono::duration_cast<std::chrono::microseconds>(
+                end_iterations - start_iterations)
+                .count()) *
+        1.0e-6;
+    double covariance_time =
+        static_cast<double>(
+            std::chrono::duration_cast<std::chrono::microseconds>(
+                end_covariances - start_covariances)
+                .count()) *
+        1.0e-6;
+    double total_time =
+        static_cast<double>(
+            std::chrono::duration_cast<std::chrono::microseconds>(end_gicp -
+                                                                  start_gicp)
+                .count()) *
+        1.0e-6;
+    // TODO(JN) Structured logging? ros topic? add info about the clouds? Remove
+    // ROS logs from GICP?
+    // ROS_DEBUG_STREAM("NrThreads: " << k_num_threads_ << " Total: " << total_time
+    //                                << " "
+    //                                << " Iteration: " << iteration_time
+    //                                << " Covariances: " << covariance_time
+    //                                << " NrIterations: " << nr_iterations_
+    //                                << " Delta " << delta);
+  }
+}
+
+template <typename PointSource, typename PointTarget>
+void pcl::MultithreadedGeneralizedIterativeClosestPoint<
+    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 // IMPL_MULTITHREADED_GICP_HPP_

src/localization_module_lio_locus/include/lio_locus_humble/multithreaded_ndt/ndt_omp.h

@@ -0,0 +1,537 @@
+/*
+ * 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 <multithreaded_ndt/voxel_grid_covariance_omp.h>
+#include <pcl/registration/registration.h>
+#include <pcl/search/impl/search.hpp>
+
+#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;
+
+  // TODO: the functionality it's not yet implemented, it's just for the sake of
+  // interface
+  void enableTimingOutput(bool enable) {
+    k_enable_timing_output_ = enable;
+  }
+
+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_;
+
+  /** \brief Enables log print statements with GICP timing information. */
+  bool k_enable_timing_output_;
+
+public:
+  NeighborSearchMethod search_method;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+} // namespace pclomp
+
+#include <multithreaded_ndt/ndt_omp_impl.hpp>
+#include <multithreaded_ndt/voxel_grid_covariance_omp_impl.hpp>
+#endif // PCL_REGISTRATION_NDT_H_

src/localization_module_lio_locus/include/lio_locus_humble/multithreaded_ndt/voxel_grid_covariance_omp.h

@@ -0,0 +1,526 @@
+/*
+ * 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 <map>
+#include <pcl/filters/boost.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/point_types.h>
+#include <unordered_map>
+
+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_;
+};
+} // namespace pclomp
+
+#endif //#ifndef PCL_VOXEL_GRID_COVARIANCE_H_

src/localization_module_lio_locus/include/lio_locus_humble/multithreaded_ndt/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 (!std::isfinite (input_->points[cp].x) ||
+            !std::isfinite (input_->points[cp].y) ||
+            !std::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 (!std::isfinite (input_->points[cp].x) ||
+            !std::isfinite (input_->points[cp].y) ||
+            !std::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_

src/localization_module_lio_locus/include/lio_locus_humble/point_cloud_localization/PointCloudLocalization.h

@@ -0,0 +1,181 @@
+/*
+ * Copyright (c) 2016, The Regents of the University of California (Regents).
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ *
+ *    1. Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *
+ *    2. 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.
+ *
+ *    3. Neither the name of the copyright holder 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 HOLDER 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.
+ *
+ * Please contact the author(s) of this library if you have any questions.
+ * Authors: Erik Nelson            ( eanelson@eecs.berkeley.edu )
+ */
+
+#ifndef POINT_CLOUD_LOCALIZATION_H
+#define POINT_CLOUD_LOCALIZATION_H
+
+//#include <pcl_ros/point_cloud.h>
+
+#include <frontend_utils/CommonFunctions.h>
+#include <frontend_utils/CommonStructs.h>
+
+
+#include <geometry_utils/GeometryUtils.h>
+#include <geometry_utils/Transform3.h>
+
+#include <multithreaded_gicp/gicp.h>
+#include <multithreaded_ndt/ndt_omp.h>
+#include <registration_settings.h>
+
+#include <mutex>
+#include <chrono>
+
+#include <yaml-cpp/yaml.h>
+#include <boost/filesystem.hpp>
+#include <glog/logging.h>
+
+using pcl::transformPointCloud;
+
+namespace point_cloud_localization{
+
+//enum RegistrationMethod { GICP, NDT };
+
+
+class PointCloudLocalization {
+public:
+	typedef pcl::PointCloud<pcl::PointNormal> PointNormal;
+	typedef pcl::search::KdTree<PointF> KdTree;
+
+	PointCloudLocalization();
+	~PointCloudLocalization();
+
+	bool Initialize(std::string path_to_params);
+
+	// Transform a point cloud from the sensor frame into the fixed frame using
+	// the current best position estimate
+	bool TransformPointsToFixedFrame(const PointCloudF &points,
+			PointCloudF *points_transformed) const;
+
+	// Transform a point cloud from the fixed frame into the sensor frame using
+	// the current best position estimate
+	bool TransformPointsToSensorFrame(const PointCloudF &points,
+			PointCloudF *points_transformed) const;
+
+	// Store incremental estimate from odometry
+	bool MotionUpdate(const geometry_utils::Transform3 &incremental_odom);
+
+	// Align incoming point cloud with a reference point cloud from the map.
+	// Output the query scan aligned in the localization frame
+	bool MeasurementUpdate(const PointCloudF::Ptr &query,
+			const PointCloudF::Ptr &reference, PointCloudF *aligned_query);
+
+	bool MeasurementUpdateRelocalization(const PointCloudF::Ptr &query,
+			const PointCloudF::Ptr &reference, PointCloudF *aligned_query);
+
+	// Get pose estimates.
+	const geometry_utils::Transform3& GetIncrementalEstimate() const;
+	const geometry_utils::Transform3& GetIntegratedEstimate() const;
+
+	// Set integrated estimate. Useful for graph SLAM whenever the pose graph is
+	// updated and the map is regenerated
+	void
+	SetIntegratedEstimate(
+			const geometry_utils::Transform3 &integrated_estimate);
+
+	// Pose estimate
+	// TODO: why do we need an interface if we have them as  a public?
+	geometry_utils::Transform3 incremental_estimate_;
+	geometry_utils::Transform3 integrated_estimate_;
+
+	// ICP fitness score
+	double icpFitnessScore_;
+
+	// Aligned point cloud returned by ICP
+	PointCloudF icpAlignedPointsLocalization_;
+
+	void SetFlatGroundAssumptionValue(const bool &value);
+
+	inline double getFitnessScore() {
+		return score_registration_;
+	}
+
+private:
+	bool loadParametersFromFile(std::string path_to_file);
+	void applyParameters();
+
+	// when scan is processed;
+	double timestamp_; // nano-sec;
+
+	// Parameters for filtering and ICP
+	struct Parameters {
+		// What registration method should be used: GICP, NDT
+		std::string registration_method;
+		// Compute ICP covariance and condition number
+		bool compute_icp_covariance;
+		// Point-to-point or Point-to-plane
+		int icp_covariance_method;
+		// Max boundd for icp covariance
+		double icp_max_covariance;
+		// Compute ICP observability
+		bool compute_icp_observability;
+		// Stop ICP if the transformation from the last iteration was this small
+		double tf_epsilon;
+		// During ICP, two points won't be considered a correspondence if they are
+		// at least this far from one another
+		double corr_dist;
+		// Iterate ICP this many times
+		unsigned int iterations;
+		// Number of threads GICP is allowed to use
+		int num_threads;
+		// Enable GICP timing information print logs
+		bool enable_timing_output;
+		// Radius used when computing ptcld normals
+		//    double normal_radius_;
+		int k_nearest_neighbours_;
+	} params_;
+
+	// ICP
+	pcl::Registration<PointF, PointF>::Ptr icp_;
+	double score_registration_ { 0.0 };
+	bool recompute_covariance_local_map_;
+	bool recompute_covariance_scan_;
+
+	// Maximum acceptable translation and rotation tolerances.
+	// If transform_thresholding_ is set to false,
+	// neither of these thresholds are considered
+	bool transform_thresholding_;
+	double max_translation_;
+	double max_rotation_;
+	bool b_is_flat_ground_assumption_;
+
+	bool SetupICP();
+
+	std::string log_tag{"component-localizer"};
+};
+
+}
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/point_cloud_localization/utils.h

@@ -0,0 +1,40 @@
+/*
+point_cloud_localization/utils.h
+author: Yun Chang yunchang@mit.edu
+
+Utilitiy functions for point cloud localization and ICP
+*/
+
+#ifndef UTILS_H
+#define UTILS_H
+
+#include <frontend_utils/CommonStructs.h>
+#include <pcl/common/transforms.h>
+#include <pcl/features/normal_3d_omp.h>
+#include <pcl/filters/filter.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+void addNormal(const PointCloudF& cloud,
+               pcl::PointCloud<pcl::PointNormal>::Ptr& cloud_with_normals,
+               const int k_nearest_neighbours);
+
+void addNormal(const PointCloudF& cloud,
+               PointCloudF::Ptr& cloud_with_normals,
+               const int k_nearest_neighbours);
+
+// returns a point cloud whose centroid is the origin, and that the mean of the
+// distances to the origin is 1
+void normalizePCloud(const pcl::PointCloud<pcl::PointXYZI>& cloud,
+                     pcl::PointCloud<pcl::PointXYZI>::Ptr pclptr_normalized);
+void normalizePCloud(const PointCloudF& cloud,
+                     PointCloudF::Ptr pclptr_normalized);
+
+void doEigenDecomp6x6(const Eigen::Matrix<double, 6, 6>& data,
+                      Eigen::Matrix<double, 6, 1>& eigenvalues,
+                      Eigen::Matrix<double, 6, 6>& eigenvectors);
+
+void doEigenDecomp3x3(const Eigen::Matrix<double, 3, 3>& data,
+                      Eigen::Matrix<double, 3, 1>& eigenvalues,
+                      Eigen::Matrix<double, 3, 3>& eigenvectors);
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/point_cloud_mapper/IPointCloudMapper.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include <mutex>
+#include <thread>
+
+#include <geometry_utils/Transform3.h>
+
+#include <frontend_utils/CommonStructs.h>
+#include <glog/logging.h>
+
+
+class IPointCloudMapper {
+public:
+	using Ptr = std::shared_ptr<IPointCloudMapper>;
+	IPointCloudMapper() {
+	}
+	;
+	virtual ~IPointCloudMapper() {
+	}
+	;
+
+	virtual bool Initialize(std::string path_to_params) = 0;
+
+	// Resets the octree and stored points to an empty map. This is used when
+	// closing loops or otherwise regenerating the map from scratch.
+	virtual void Reset() = 0;
+
+	// Adds a set of points to the datastructure
+	virtual bool InsertPoints(const PointCloudF::ConstPtr &points,
+			PointCloudF *incremental_points) = 0;
+
+	virtual bool ApproxNearestNeighbors(const PointCloudF &points,
+			PointCloudF *neighbors) = 0;
+	// Setting size of the local window map.
+	virtual void SetBoxFilterSize(const int box_filter_size) = 0;
+
+	void SetupNumberThreads(int no_threads) {
+		number_threads_ = no_threads;
+        LOG(INFO) << "Setting up number threads for local mapping: " << number_threads_;
+	}
+
+//	virtual void Refresh(const geometry_utils::Transform3 &current_pose) = 0;
+
+	void UpdateCurrentPose(const geometry_utils::Transform3 &current_pose) {
+		current_pose_estimate_ = current_pose;
+	}
+
+
+private:
+	virtual bool loadParametersFromFile(std::string path_to_file) = 0;
+
+protected:
+
+	PointCloudF::Ptr map_data_;
+	geometry_utils::Transform3 current_pose_estimate_;
+	int number_threads_ { 1 };
+};

src/localization_module_lio_locus/include/lio_locus_humble/point_cloud_mapper/PointCloudIkdTreeMapper.h

@@ -0,0 +1,64 @@
+#pragma once
+
+#include <pcl/octree/octree_search.h>
+
+#include <mutex>
+#include <thread>
+
+#include <geometry_utils/Transform3.h>
+#include <pcl/filters/crop_box.h>
+
+#include "IPointCloudMapper.h"
+#include "ikd_tree/ikd_Tree.h"
+
+//#include <std_msgs/msg/float64.hpp>
+//#include <std_msgs/msg/u_int64.hpp>
+
+#include <frontend_utils/CommonStructs.h>
+
+#include <glog/logging.h>
+
+class PointCloudIkdTreeMapper: public IPointCloudMapper {
+public:
+	// typedef pcl::PointCloud<pcl::PointXYZI> PointCloud;
+
+	PointCloudIkdTreeMapper();
+	~PointCloudIkdTreeMapper();
+
+	bool Initialize(std::string path_to_params) override;
+	// Resets the octree and stored points to an empty map. This is used when
+	// closing loops or otherwise regenerating the map from scratch.
+	void Reset();
+
+	// Adds a set of points to the octree. Only inserts points if one does not
+	// already exist in the corresponding voxel. Output the points from the input
+	// that ended up being inserted into the octree.
+	// Effective C++ item 37
+	bool InsertPoints(const PointCloudF::ConstPtr &points,
+			PointCloudF *incremental_points) override;
+
+	// Returns the approximate nearest neighbor for every point in the input point
+	// cloud. Localization to the map can be performed by doing ICP between the
+	// input and output. Returns true if at least one of the inputs points had a
+	// nearest neighbor.
+	virtual bool ApproxNearestNeighbors(const PointCloudF &points,
+			PointCloudF *neighbors) override;
+
+
+	PointCloudF::Ptr getCloudMapInMem();
+
+	// to drop;
+	void SetBoxFilterSize(const int box_filter_size) override;
+private:
+
+	bool loadParametersFromFile(std::string path_to_file) override;
+
+	std::string fixed_frame_id_;
+	double octree_resolution_;
+	bool initialized_;
+
+	int num_threads_{2};
+
+	mutable std::mutex map_mutex_;
+	KD_TREE ikdtree;
+};

src/localization_module_lio_locus/include/lio_locus_humble/point_cloud_mapper/settings.h

@@ -0,0 +1,70 @@
+#pragma once
+
+#include <point_cloud_mapper/PointCloudIkdTreeMapper.h>
+#include <point_cloud_mapper/PointCloudMapper.h>
+#include <point_cloud_mapper/PointCloudMultiThreadedMapper.h>
+
+enum class MappingMethod {
+  MAPPER,
+  BUFFER_MAPPER,
+  OCTOMAP_MAPPER,
+  VDB_MAPPER,
+  KFLANN,
+  MULTI_THREADED_MAPPER,
+  IKDTREE_MAPPER
+};
+
+using EnumToStringMappingMethods = std::pair<std::string, MappingMethod>;
+
+const std::vector<EnumToStringMappingMethods> EnumToStringMappingMethodsVector =
+    {EnumToStringMappingMethods("mapper", MappingMethod::MAPPER),
+     EnumToStringMappingMethods("buffer_mapper", MappingMethod::BUFFER_MAPPER),
+     EnumToStringMappingMethods("kflann", MappingMethod::KFLANN),
+     EnumToStringMappingMethods("octomap_mapper",
+                                MappingMethod::OCTOMAP_MAPPER),
+     EnumToStringMappingMethods("vdb_mapper", MappingMethod::VDB_MAPPER),
+     EnumToStringMappingMethods("multi_threaded_mapper",
+                                MappingMethod::MULTI_THREADED_MAPPER),
+     EnumToStringMappingMethods("ikdtree_mapper",
+                                MappingMethod::IKDTREE_MAPPER)};
+// TODO: maybe somehow varialbe template, but it's available from cpp17 i think
+MappingMethod getCalibrationMethodFromString(const std::string& mode) {
+  for (const auto& available_vlo : EnumToStringMappingMethodsVector) {
+    if (mode == available_vlo.first) {
+      return available_vlo.second;
+    }
+  }
+  throw std::runtime_error("No such mapping mode!: " + mode);
+}
+
+IPointCloudMapper::Ptr mapperFabric(const std::string& mapping_method) {
+  switch (getCalibrationMethodFromString(mapping_method)) {
+  case MappingMethod::MAPPER: {
+    ROS_INFO_STREAM("MappingMethod::MAPPER activated.");
+    return std::make_shared<PointCloudMapper>();
+  }
+  case MappingMethod::MULTI_THREADED_MAPPER: {
+    ROS_INFO_STREAM("MappingMethod::MULTI_THREADED_MAPPER activated.");
+    return std::make_shared<PointCloudMultiThreadedMapper>();
+  }
+  case MappingMethod::IKDTREE_MAPPER: {
+    ROS_INFO_STREAM("MappingMethod::IKDTREE_MAPPER activated.");
+    return std::make_shared<PointCloudIkdTreeMapper>();
+  }
+  //  case MappingMethod::OCTOMAP_MAPPER: {
+  //    ROS_INFO_STREAM("MappingMethod::OCTOMAP_MAPPER activated.");
+  //    return std::make_shared<PointCloudOctomapMapper>();
+  //  }
+  //  case MappingMethod::VDB_MAPPER: {
+  //    ROS_INFO_STREAM("MappingMethod::VDB_MAPPER activated.");
+  //    return std::make_shared<PointCloudVDBMapper>();
+  //  }
+  //  case MappingMethod::KFLANN: {
+  //    ROS_INFO_STREAM("MappingMethod::KFLANN acticated.");
+  //    return std::make_shared<PointCloudFlannMapper>();
+  //  }
+  default:
+    throw std::runtime_error("No such mapping mode or not implemented yet " +
+                             mapping_method);
+  }
+}

src/localization_module_lio_locus/include/lio_locus_humble/point_cloud_odometry/PointCloudOdometry.h

@@ -0,0 +1,141 @@
+/*
+ * Copyright (c) 2016, The Regents of the University of California (Regents).
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ *
+ *    1. Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *
+ *    2. 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.
+ *
+ *    3. Neither the name of the copyright holder 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 HOLDER 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.
+ *
+ * Please contact the author(s) of this library if you have any questions.
+ * Authors: Erik Nelson            ( eanelson@eecs.berkeley.edu )
+ */
+
+#ifndef POINT_CLOUD_ODOMETRY_H
+#include <geometry_utils/GeometryUtils.h>
+#include <geometry_utils/Transform3.h>
+#include <frontend_utils/CommonStructs.h>
+
+#include <multithreaded_gicp/gicp.h>
+#include <multithreaded_ndt/ndt_omp.h>
+
+#include <glog/logging.h>
+
+
+namespace point_cloud_odometry{
+enum RegistrationMethod { GICP, NDT };
+
+
+class PointCloudOdometry {
+public:
+	PointCloudOdometry();
+	~PointCloudOdometry();
+
+	inline void reset() {
+		initialized_ = false;
+	}
+
+	bool Initialize(std::string path_to_file);
+
+	bool SetLidar(const PointCloudF &points);
+	bool SetImuDelta(const Eigen::Matrix3d &imu_delta);
+	bool UpdateEstimate();
+
+	const geometry_utils::Transform3& GetIncrementalEstimate() const;
+	const geometry_utils::Transform3& GetIntegratedEstimate() const;
+
+	bool GetLastPointCloud(PointCloudF::Ptr &out) const;
+
+	// Aligned point cloud returned by ICP
+	PointCloudF icpAlignedPointsOdometry_;
+
+	void EnableImuIntegration();
+	void EnableOdometryIntegration();
+	void DisableSensorIntegration();
+
+	void SetFlatGroundAssumptionValue(const bool &value);
+
+private:
+
+	geometry_utils::Transform3 incremental_estimate_;
+	geometry_utils::Transform3 integrated_estimate_;
+
+	bool loadParametersFromFile(std::string path_to_file);
+	void applyParameters();
+
+	// Use ICP between a query and reference point cloud to estimate pose
+	bool UpdateICP();
+
+	std::string name_;
+	bool b_verbose_;
+	bool initialized_;
+
+	// Coordinate frames
+	bool b_is_flat_ground_assumption_;
+	bool recompute_covariances_;
+
+	// Point cloud containers
+	PointCloudF points_;
+	PointCloudF::Ptr query_;
+	PointCloudF::Ptr reference_;
+	PointCloudF::Ptr query_trans_;
+
+	// Maximum acceptable translation and rotation tolerances
+	// If transform_thresholding_ is set to false,
+	// neither of these thresholds are considered
+	bool transform_thresholding_;
+	double max_translation_;
+	double max_rotation_;
+
+	// ICP
+	struct Parameters {
+		// Registration method
+		std::string registration_method;
+		double icp_tf_epsilon;
+		double icp_corr_dist;
+		unsigned int icp_iterations;
+		// Number of threads GICP is allowed to use
+		int num_threads;
+		// Enable GICP timing information print logs
+		bool enable_timing_output;
+
+	} params_;
+
+	pcl::Registration<PointF, PointF>::Ptr icp_;
+
+	bool SetupICP();
+
+	// prior storage for relative displacement;
+	bool b_use_imu_integration_;
+	Eigen::Matrix3d imu_delta_;
+	Eigen::Matrix4d imu_prior_;
+
+	std::string log_tag{"component-icp"};
+};
+
+}
+
+#endif

src/localization_module_lio_locus/include/lio_locus_humble/registration_settings.h

@@ -0,0 +1,20 @@
+#pragma once
+
+enum class RegistrationMethod { GICP, NDT };
+
+using EnumToStringRegistrationMethods =
+    std::pair<std::string, RegistrationMethod>;
+
+const std::vector<EnumToStringRegistrationMethods>
+    EnumToStringRegistrationMethodsVector = {
+        EnumToStringRegistrationMethods("gicp", RegistrationMethod::GICP),
+        EnumToStringRegistrationMethods("ndt", RegistrationMethod::NDT)};
+// TODO: maybe somehow varialbe template, but it's available from cpp17 i think
+RegistrationMethod getRegistrationMethodFromString(const std::string& mode) {
+  for (const auto& available_vlo : EnumToStringRegistrationMethodsVector) {
+    if (mode == available_vlo.first) {
+      return available_vlo.second;
+    }
+  }
+  throw std::runtime_error("No such Registration mode!: " + mode);
+}

src/localization_module_lio_locus/launch/nodelets/cloud_preprocessing.launch.py

@@ -0,0 +1,34 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch.conditions import IfCondition
+from launch.substitutions import (
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    PythonExpression,
+)
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    current_pkg = FindPackageShare("lio_locus_humble")
+    return LaunchDescription(
+        [
+            # ROS 2 parameters
+            DeclareLaunchArgument("params", default_value=PathJoinSubstitution([current_pkg, "config", "parameters_lio_locus.yaml"])),
+          # ROS2 Nodes
+            Node(
+                package="lio_locus_humble",
+                executable="cloud_preprocessing_node",
+                name="cloud_preprocessing",
+                output="screen",
+                parameters=[
+                    {
+                        "file_params": LaunchConfiguration("params"),
+                    }
+                ],
+            ),
+        ]
+    )
+
+

src/localization_module_lio_locus/launch/nodelets/imu_preprocessing_livox.launch.py

@@ -0,0 +1,36 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch.conditions import IfCondition
+from launch.substitutions import (
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    PythonExpression,
+)
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    current_pkg = FindPackageShare("lio_locus_humble")
+    return LaunchDescription(
+        [
+            # ROS 2 parameters
+            DeclareLaunchArgument("params", default_value=PathJoinSubstitution([current_pkg, "config", "parameters_lio_locus.yaml"])),
+          # ROS2 Nodes
+            Node(
+                package="lio_locus_humble",
+                executable="imu_preprocessing_node",
+                name="lio_locus",
+                output="screen",
+                parameters=[
+                    {
+                        "file_params": LaunchConfiguration("params"),
+                    }
+                ],
+            ),
+        ]
+    )
+
+
+
+

src/localization_module_lio_locus/launch/nodelets/lio_locus.launch.py

@@ -0,0 +1,36 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess
+from launch.conditions import IfCondition
+from launch.substitutions import (
+    LaunchConfiguration,
+    PathJoinSubstitution,
+    PythonExpression,
+)
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    current_pkg = FindPackageShare("lio_locus_humble")
+    return LaunchDescription(
+        [
+            # ROS 2 parameters
+            DeclareLaunchArgument("params", default_value=PathJoinSubstitution([current_pkg, "config", "parameters_lio_locus.yaml"])),
+          # ROS2 Nodes
+            Node(
+                package="lio_locus_humble",
+                executable="lio_locus_node",
+                prefix="gdb -ex run --args",
+                name="lio_locus",
+                output="screen",
+                parameters=[
+                    {
+                        "file_params": LaunchConfiguration("params"),
+                    }
+                ],
+            ),
+        ]
+    )
+
+
+

src/localization_module_lio_locus/launch/nodelets/vis_ego_viewpoint.launch.py

@@ -0,0 +1,22 @@
+from launch import LaunchDescription
+from launch.substitutions import (
+    PathJoinSubstitution,
+)
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    current_pkg = FindPackageShare("lio_locus_humble")
+    return LaunchDescription(
+        [
+            Node(
+                package="rviz2",
+                executable="rviz2",
+                output={"both": "log"},
+                arguments=["-d", PathJoinSubstitution([current_pkg, "rviz", "ego_view_titan.rviz"])],
+            ),
+        ]
+    )
+
+

src/localization_module_lio_locus/launch/nodelets/vis_world_viewpoint.launch.py

@@ -0,0 +1,21 @@
+from launch import LaunchDescription
+from launch.substitutions import (
+    PathJoinSubstitution,
+)
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    current_pkg = FindPackageShare("lio_locus_humble")
+    return LaunchDescription(
+        [
+            Node(
+                package="rviz2",
+                executable="rviz2",
+                output={"both": "log"},
+                arguments=["-d", PathJoinSubstitution([current_pkg, "rviz", "world_view_titan.rviz"])],
+            ),
+        ]
+    )
+

src/localization_module_lio_locus/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>lio_locus_humble</name>
+  <version>1.0.0</version>
+  <description>loosely-coupled-lidar-inertial-odometry</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>pcl_conversions</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/localization_module_lio_locus/rviz/ego_view_titan.rviz

@@ -0,0 +1,323 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Grid1
+        - /LioLocus1/Odometry1/Shape1
+        - /LioLocus1/cloud_map1
+        - /CloudRaw1
+        - /CloudRaw1/cloud_input1
+        - /Axes1
+      Splitter Ratio: 0.5
+    Tree Height: 1079
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: cloud_input
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 5
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 60
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_common/Group
+      Displays:
+        - Angle Tolerance: 0.10000000149011612
+          Class: rviz_default_plugins/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
+          Enabled: true
+          Keep: 1
+          Name: Odometry
+          Position Tolerance: 0.10000000149011612
+          Shape:
+            Alpha: 1
+            Axes Length: 6
+            Axes Radius: 0.10000000149011612
+            Color: 255; 25; 0
+            Head Length: 0.30000001192092896
+            Head Radius: 0.10000000149011612
+            Shaft Length: 6
+            Shaft Radius: 0.10000000149011612
+            Value: Axes
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /pose_lo
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 255
+          Min Color: 0; 0; 0
+          Min Intensity: 1
+          Name: cloud_map
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.11999999731779099
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /cloud_map
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 223
+          Min Color: 0; 0; 0
+          Min Intensity: 1
+          Name: localized_cloud
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.20000000298023224
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /dbg_localized_cloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz_default_plugins/Path
+          Color: 25; 255; 0
+          Enabled: true
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.30000001192092896
+          Line Style: Lines
+          Line Width: 0.029999999329447746
+          Name: Path
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Radius: 0.029999999329447746
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /dbg_lio_path
+          Value: true
+      Enabled: true
+      Name: LioLocus
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 123.83333587646484
+          Min Color: 0; 0; 0
+          Min Intensity: 0.19392558932304382
+          Name: cloud_input
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.10000000149011612
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /cloud_input
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+      Enabled: true
+      Name: CloudRaw
+    - Class: rviz_default_plugins/Axes
+      Enabled: true
+      Length: 6
+      Name: Axes
+      Radius: 0.10000000149011612
+      Reference Frame: base_link
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: base_link
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/XYOrbit
+      Distance: 38.536495208740234
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -2.5817513465881348
+        Y: -5.34882926940918
+        Z: 1.5393958165077493e-05
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.6497955918312073
+      Target Frame: <Fixed Frame>
+      Value: XYOrbit (rviz_default_plugins)
+      Yaw: 2.5491929054260254
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1376
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000305fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000305000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004dc0000003efc0100000002fb0000000800540069006d00650100000000000004dc000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000380000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1244
+  X: 1316
+  Y: 27

src/localization_module_lio_locus/rviz/world_view_titan.rviz

@@ -0,0 +1,412 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Grid1
+        - /TF1/Frames1
+        - /LioLocus1
+        - /LioLocus1/Odometry1/Shape1
+        - /LioLocus1/cloud_map1
+        - /LioLocus1/OdometryLidar1/Shape1
+        - /Axes1
+        - /CloudRaw1
+      Splitter Ratio: 0.5
+    Tree Height: 719
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz_common/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: localized_cloud
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 300
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz_default_plugins/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+      Marker Scale: 6
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz_common/Group
+      Displays:
+        - Angle Tolerance: 0.10000000149011612
+          Class: rviz_default_plugins/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
+          Enabled: true
+          Keep: 1
+          Name: Odometry
+          Position Tolerance: 0.10000000149011612
+          Shape:
+            Alpha: 1
+            Axes Length: 6
+            Axes Radius: 0.10000000149011612
+            Color: 255; 25; 0
+            Head Length: 0.30000001192092896
+            Head Radius: 0.10000000149011612
+            Shaft Length: 6
+            Shaft Radius: 0.10000000149011612
+            Value: Axes
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /pose_lo
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: FlatColor
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 255
+          Min Color: 0; 0; 0
+          Min Intensity: 1
+          Name: cloud_map
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.12999999523162842
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /cloud_map
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 144.3333282470703
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: localized_cloud
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.10000000149011612
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /dbg_localized_cloud
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz_default_plugins/Path
+          Color: 25; 255; 0
+          Enabled: true
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.30000001192092896
+          Line Style: Lines
+          Line Width: 0.029999999329447746
+          Name: Path
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Radius: 0.029999999329447746
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /dbg_lio_path
+          Value: true
+        - Angle Tolerance: 0.10000000149011612
+          Class: rviz_default_plugins/Odometry
+          Covariance:
+            Orientation:
+              Alpha: 0.5
+              Color: 255; 255; 127
+              Color Style: Unique
+              Frame: Local
+              Offset: 1
+              Scale: 1
+              Value: true
+            Position:
+              Alpha: 0.30000001192092896
+              Color: 204; 51; 204
+              Scale: 1
+              Value: true
+            Value: true
+          Enabled: true
+          Keep: 1
+          Name: OdometryLidar
+          Position Tolerance: 0.10000000149011612
+          Shape:
+            Alpha: 1
+            Axes Length: 6
+            Axes Radius: 0.10000000149011612
+            Color: 255; 25; 0
+            Head Length: 0.30000001192092896
+            Head Radius: 0.10000000149011612
+            Shaft Length: 1
+            Shaft Radius: 0.05000000074505806
+            Value: Axes
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /pose_lidar
+          Value: true
+      Enabled: true
+      Name: LioLocus
+    - Class: rviz_default_plugins/Axes
+      Enabled: false
+      Length: 6
+      Name: Axes
+      Radius: 0.10000000149011612
+      Reference Frame: fixed
+      Value: false
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 255
+          Min Color: 0; 0; 0
+          Min Intensity: 1
+          Name: cloud_raw
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.05999999865889549
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /rsm1_1
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz_default_plugins/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: Intensity
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 209
+          Min Color: 0; 0; 0
+          Min Intensity: 1
+          Name: cloud_input
+          Position Transformer: XYZ
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.30000001192092896
+          Style: Flat Squares
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /cloud_input
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+      Enabled: true
+      Name: CloudRaw
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: fixed
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Covariance x: 0.25
+      Covariance y: 0.25
+      Covariance yaw: 0.06853891909122467
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/XYOrbit
+      Distance: 28.173295974731445
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.9926908612251282
+        Y: 1.7843061685562134
+        Z: -5.0866601668531075e-05
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.43979689478874207
+      Target Frame: <Fixed Frame>
+      Value: XYOrbit (rviz_default_plugins)
+      Yaw: 4.894167423248291
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1016
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000305fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000305000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005dc0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1848
+  X: 463
+  Y: 143

src/localization_module_lio_locus/src/CMakeLists.txt

@@ -0,0 +1,5 @@
+add_subdirectory(point_cloud_odometry)
+add_subdirectory(point_cloud_localization)
+add_subdirectory(point_cloud_mapper)
+#add_subdirectory(lio_locus)
+

src/localization_module_lio_locus/src/lio_locus/CMakeLists.txt

@@ -0,0 +1,58 @@
+add_library(${PROJECT_NAME} SHARED	lio_locus.cpp)
+
+target_link_libraries(${PROJECT_NAME} 
+    point_cloud_odometry
+    point_cloud_localization
+    point_cloud_mapper
+    boost_filesystem
+    glog
+      ${PCL_LIBRARIES}
+    pcl_common
+  ${YAML_CPP_LIBRARIES}
+)
+
+ament_target_dependencies(${PROJECT_NAME} 
+  pcl_conversions
+)
+
+install(TARGETS ${PROJECT_NAME} 
+  DESTINATION lib
+)
+
+#-------------------------------------------------------
+
+add_library(cloud_pre_processor SHARED cloud_pre_processor.cpp)
+target_link_libraries(cloud_pre_processor
+	pcl_filters
+	pcl_features
+	boost_filesystem
+    glog
+	${YAML_CPP_LIBRARIES})
+
+ament_target_dependencies(cloud_pre_processor 
+  sensor_msgs
+  pcl_conversions
+  tf2
+)
+
+
+install(TARGETS cloud_pre_processor
+  DESTINATION lib
+)
+
+#-------------------------------------------------------
+add_library(complementary_filter SHARED
+  complementary_filter.cpp
+)
+
+target_link_libraries(complementary_filter 
+    glog
+)
+
+install(TARGETS complementary_filter
+  DESTINATION lib
+)
+
+
+
+

src/localization_module_lio_locus/src/lio_locus/cloud_pre_processor.cpp

@@ -0,0 +1,234 @@
+/*
+ * cloud_pre_processor.cpp
+ *
+ *  Created on: Oct 17, 2025
+ *      Author: solomon
+ */
+
+/*
+ * test for trials
+ *
+ */
+
+#include "lio_locus/cloud_pre_processor.h"
+
+CloudPreProcessor::CloudPreProcessor() {
+	google::SetStderrLogging(google::GLOG_WARNING);
+	FLAGS_colorlogtostderr = true;
+	FLAGS_alsologtostderr = true;
+}
+
+CloudPreProcessor::~CloudPreProcessor() {
+}
+
+// cloud format adaption: only xyzi curvature for time difference;
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr CloudPreProcessor::adaptCloudForm(
+		const sensor_msgs::msg::PointCloud2 &cloud_input) {
+
+	// container to store all points transformed;
+	pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_raw(
+			new pcl::PointCloud<pcl::PointXYZINormal>());
+
+	// raw ROS msg for data conversion;
+	pcl::PointCloud<robosense_ros::Point> cloud_rs;
+	pcl::fromROSMsg(cloud_input, cloud_rs);
+
+	// timestamp update for original cloud;
+	for (int k = 0; k < cloud_rs.size(); k++) {
+		auto pnt = cloud_rs[k];
+		pcl::PointXYZINormal p_xyzi_n;
+		p_xyzi_n.x = pnt.x;
+		p_xyzi_n.y = pnt.y;
+		p_xyzi_n.z = pnt.z;
+		p_xyzi_n.intensity = pnt.intensity;
+		p_xyzi_n.curvature = pnt.timestamp - cloud_input.header.stamp.sec;
+		cloud_raw->points.push_back(p_xyzi_n);
+	}
+
+	LOG(INFO) << "... cloud size after format adaption -> #[" << cloud_raw->points.size() << "]";
+
+	// convert ROS message time stamp to pcl timestamp;
+	pcl_conversions::toPCL(cloud_input.header.stamp, cloud_raw->header.stamp);
+
+	return cloud_raw;
+}
+
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr CloudPreProcessor::adaptCloudFormFromLivoxCloud(
+		const sensor_msgs::msg::PointCloud2 &cloud_input) {
+
+	// ROS cloud to PCL format;
+	pcl::PointCloud<livox_lidar::Point> cloud_livox;
+	pcl::fromROSMsg(cloud_input, cloud_livox);
+
+	// to PCL standard format: {x,y,z,intensity, line, tag, timestamp};
+	pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_raw(
+			new pcl::PointCloud<pcl::PointXYZINormal>());
+	for (int k = 0; k < cloud_livox.size(); k++) {
+		livox_lidar::Point pnt = cloud_livox[k];
+		pcl::PointXYZINormal p_xyzi_n;
+		p_xyzi_n.x = pnt.x;
+		p_xyzi_n.y = pnt.y;
+		p_xyzi_n.z = pnt.z;
+		p_xyzi_n.intensity = pnt.intensity;
+		p_xyzi_n.curvature = pnt.timestamp;
+		cloud_raw->points.push_back(p_xyzi_n);
+	}
+	pcl_conversions::toPCL(cloud_input.header.stamp, cloud_raw->header.stamp);
+
+	LOG(INFO) << "... cloud size after format adaption -> #[" << cloud_raw->points.size() << "]";
+
+	return cloud_raw;
+}
+
+// transform coordinates of cloud based on input static transformation matrix;
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr CloudPreProcessor::transformCloudCartesian(
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_input,
+		tf2::Transform T) {
+
+	pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_out(
+			new pcl::PointCloud<pcl::PointXYZINormal>());
+
+	cloud_out->points.reserve(cloud_input->points.size());
+	for (int k = 0; k < cloud_input->points.size(); k++) {
+
+		pcl::PointXYZINormal pnt_raw = cloud_input->points[k];
+		tf2::Vector3 pnt = T * tf2::Vector3(pnt_raw.x, pnt_raw.y, pnt_raw.z);
+
+		pcl::PointXYZINormal p_xyzi_n;
+		p_xyzi_n.x = pnt.x();
+		p_xyzi_n.y = pnt.y();
+		p_xyzi_n.z = pnt.z();
+		p_xyzi_n.intensity = pnt_raw.intensity;
+		p_xyzi_n.curvature = pnt_raw.curvature;
+
+		cloud_out->points.push_back(p_xyzi_n);
+	}
+
+	cloud_out->header.stamp = cloud_input->header.stamp;
+
+	return cloud_out;
+}
+
+bool CloudPreProcessor::doVoxelGridFilteringAndComputeNormals(
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr input,
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr &cloud_normals) {
+
+	// basic statistics about node;
+	LOG(INFO) << "####################################################";
+	LOG(INFO) << "           CLOUD PRE-PROCESSOR        ";
+	LOG(INFO) << "####################################################";
+
+	if (input == nullptr)
+		return false;
+
+	if (cloud_normals == nullptr) {
+		cloud_normals = pcl::PointCloud<pcl::PointXYZINormal>::Ptr(
+				new pcl::PointCloud<pcl::PointXYZINormal>());
+	}
+
+	auto start_chrono = std::chrono::steady_clock::now();
+
+	//------------------------------------------------------------------------
+	// voxel grid filtering;
+	//------------------------------------------------------------------------
+	boost::mutex::scoped_lock lock(mutex_);
+
+	pcl::PCLPointCloud2::Ptr pcl_input(new pcl::PCLPointCloud2());
+	pcl::toPCLPointCloud2(*input, *pcl_input);
+
+	static pcl::VoxelGrid<pcl::PCLPointCloud2> impl_;
+	static bool flag_init = false;
+	if (!flag_init) {
+		impl_.setLeafSize(leaf_voxel_filtering_, leaf_voxel_filtering_,
+				leaf_voxel_filtering_);
+		impl_.setFilterLimits(filter_limit_, filter_limit_);
+		impl_.setFilterLimitsNegative(false);
+
+		flag_init = true;
+	}
+
+	impl_.setInputCloud(pcl_input);
+	pcl::PCLPointCloud2 pcl_output;
+	impl_.filter(pcl_output);
+
+	pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_xyzi(
+			new pcl::PointCloud<pcl::PointXYZI>());
+	pcl::fromPCLPointCloud2(pcl_output, *cloud_xyzi);
+	//----------------------------------------------------------------------------------
+	static pcl::NormalEstimationOMP<pcl::PointXYZI, pcl::Normal> normal_estimator =
+			pcl::NormalEstimationOMP<pcl::PointXYZI, pcl::Normal>(size_cores_);
+	static bool flag_init_ne = false;
+	if (!flag_init_ne) {
+		pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(
+				new pcl::search::KdTree<pcl::PointXYZI>());
+		normal_estimator.setSearchMethod(tree);
+		normal_estimator.setKSearch(size_k_search_);
+		normal_estimator.setRadiusSearch(radius_search_);
+
+		flag_init_ne = true;
+	}
+
+	normal_estimator.setInputCloud(cloud_xyzi);
+	pcl::PointCloud<pcl::Normal>::Ptr normals(
+			new pcl::PointCloud<pcl::Normal>());
+	normal_estimator.compute(*normals);
+
+	// assign normal vector to points;
+	for (size_t i = 0; i < cloud_xyzi->points.size(); i++) {
+		pcl::PointXYZINormal point;
+
+		auto pnt = cloud_xyzi->points[i];
+		point.x = pnt.x;
+		point.y = pnt.y;
+		point.z = pnt.z;
+		point.intensity = pnt.intensity;
+
+		pcl::Normal normal = normals->points[i];
+		point.normal_x = normal.normal_x;
+		point.normal_y = normal.normal_y;
+		point.normal_z = normal.normal_z;
+
+		cloud_normals->points.push_back(point);
+	}
+	cloud_normals->header.stamp = input->header.stamp;
+
+	//-------------------------------------------------------------------------
+	auto end_chrono = std::chrono::steady_clock::now();
+	double dt_chrono = std::chrono::duration<double>(end_chrono - start_chrono).count();
+	LOG(INFO)
+			<< "... time passed for cloud voxel filtering and normal calculation: ["
+			<< dt_chrono * 1e3 << "] ms";
+
+	return true;
+}
+
+// attention of things of interest;
+bool CloudPreProcessor::transformCloudWithNormalCalculation(
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_input,
+		tf2::Transform T,
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr &cloud_normals) {
+
+	auto ts_start = std::chrono::steady_clock::now();
+
+	uint64_t ts = cloud_input->header.stamp;
+	pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_raw_trans =
+			transformCloudCartesian(cloud_input, T);
+
+	cloud_normals = pcl::PointCloud<pcl::PointXYZINormal>::Ptr(
+			new pcl::PointCloud<pcl::PointXYZINormal>());
+
+	bool flag = false;
+
+	if (doVoxelGridFilteringAndComputeNormals(cloud_raw_trans, cloud_normals)) {
+		flag = true;
+	}
+
+	auto ts_end = std::chrono::steady_clock::now();
+	double dt = std::chrono::duration<double>(ts_end - ts_start).count();
+
+	LOG(INFO) << "... dt for cloud pre-processing -> #[" << dt * 1e3 << "]";
+
+	writeDt2LogFile(path_log, "log_dt_ms_cloud_preprocessing.txt", dt * 1e3);
+
+	return false;
+}

src/localization_module_lio_locus/src/lio_locus/complementary_filter.cpp

@@ -0,0 +1,543 @@
+/*
+  @author Roberto G. Valenti <robertogl.valenti@gmail.com>
+
+  @section LICENSE
+  Copyright (c) 2015, City University of New York
+  CCNY Robotics Lab <http://robotics.ccny.cuny.edu>
+  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 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.
+*/
+
+
+#include "lio_locus/complementary_filter.h"
+
+
+namespace imu_tools {
+
+const double ComplementaryFilter::kGravity = 9.81;
+const double ComplementaryFilter::gamma_ = 0.01;
+// Bias estimation steady state thresholds
+const double ComplementaryFilter::kAngularVelocityThreshold = 0.2;
+const double ComplementaryFilter::kAccelerationThreshold = 0.1;
+const double ComplementaryFilter::kDeltaAngularVelocityThreshold = 0.01;
+
+ComplementaryFilter::ComplementaryFilter()
+    : gain_acc_(0.01),
+      gain_mag_(0.01),
+      bias_alpha_(0.01),
+      do_bias_estimation_(true),
+      do_adaptive_gain_(false),
+      initialized_(false),
+      steady_state_(false),
+      q0_(1),
+      q1_(0),
+      q2_(0),
+      q3_(0),
+      wx_prev_(0),
+      wy_prev_(0),
+      wz_prev_(0),
+      wx_bias_(0),
+      wy_bias_(0),
+      wz_bias_(0)
+{
+}
+
+ComplementaryFilter::~ComplementaryFilter() = default;
+
+void ComplementaryFilter::setDoBiasEstimation(bool do_bias_estimation)
+{
+    do_bias_estimation_ = do_bias_estimation;
+}
+
+bool ComplementaryFilter::getDoBiasEstimation() const
+{
+    return do_bias_estimation_;
+}
+
+void ComplementaryFilter::setDoAdaptiveGain(bool do_adaptive_gain)
+{
+    do_adaptive_gain_ = do_adaptive_gain;
+}
+
+bool ComplementaryFilter::getDoAdaptiveGain() const
+{
+    return do_adaptive_gain_;
+}
+
+bool ComplementaryFilter::setGainAcc(double gain)
+{
+    if (gain >= 0 && gain <= 1.0)
+    {
+        gain_acc_ = gain;
+        return true;
+    } else
+        return false;
+}
+bool ComplementaryFilter::setGainMag(double gain)
+{
+    if (gain >= 0 && gain <= 1.0)
+    {
+        gain_mag_ = gain;
+        return true;
+    } else
+        return false;
+}
+
+double ComplementaryFilter::getGainAcc() const
+{
+    return gain_acc_;
+}
+
+double ComplementaryFilter::getGainMag() const
+{
+    return gain_mag_;
+}
+
+bool ComplementaryFilter::getSteadyState() const
+{
+    return steady_state_;
+}
+
+bool ComplementaryFilter::setBiasAlpha(double bias_alpha)
+{
+    if (bias_alpha >= 0 && bias_alpha <= 1.0)
+    {
+        bias_alpha_ = bias_alpha;
+        return true;
+    } else
+        return false;
+}
+
+double ComplementaryFilter::getBiasAlpha() const
+{
+    return bias_alpha_;
+}
+
+void ComplementaryFilter::setOrientation(double q0, double q1, double q2,
+                                         double q3)
+{
+    // Set the state to inverse (state is fixed wrt body).
+    invertQuaternion(q0, q1, q2, q3, q0_, q1_, q2_, q3_);
+}
+
+double ComplementaryFilter::getAngularVelocityBiasX() const
+{
+    return wx_bias_;
+}
+
+double ComplementaryFilter::getAngularVelocityBiasY() const
+{
+    return wy_bias_;
+}
+
+double ComplementaryFilter::getAngularVelocityBiasZ() const
+{
+    return wz_bias_;
+}
+
+void ComplementaryFilter::update(double ax, double ay, double az, double wx,
+                                 double wy, double wz, double dt)
+{
+
+    auto ts_start = std::chrono::steady_clock::now();
+
+    if (!initialized_)
+    {
+        // First time - ignore prediction:
+        getMeasurement(ax, ay, az, q0_, q1_, q2_, q3_);
+        initialized_ = true;
+        return;
+    }
+
+    // Bias estimation.
+    if (do_bias_estimation_) updateBiases(ax, ay, az, wx, wy, wz);
+
+    // Prediction.
+    double q0_pred, q1_pred, q2_pred, q3_pred;
+    getPrediction(wx, wy, wz, dt, q0_pred, q1_pred, q2_pred, q3_pred);
+
+    // Correction (from acc):
+    // q_ = q_pred * [(1-gain) * qI + gain * dq_acc]
+    // where qI = identity quaternion
+    double dq0_acc, dq1_acc, dq2_acc, dq3_acc;
+    getAccCorrection(ax, ay, az, q0_pred, q1_pred, q2_pred, q3_pred, dq0_acc,
+                     dq1_acc, dq2_acc, dq3_acc);
+
+    double gain;
+    if (do_adaptive_gain_)
+    {
+        gain = getAdaptiveGain(gain_acc_, ax, ay, az);
+
+    } else
+    {
+        gain = gain_acc_;
+    }
+
+    scaleQuaternion(gain, dq0_acc, dq1_acc, dq2_acc, dq3_acc);
+
+    quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred, dq0_acc,
+                             dq1_acc, dq2_acc, dq3_acc, q0_, q1_, q2_, q3_);
+
+    normalizeQuaternion(q0_, q1_, q2_, q3_);
+
+	auto ts_end = std::chrono::steady_clock::now();
+	double dt_dura = std::chrono::duration<double>(ts_end - ts_start).count();
+
+	LOG(INFO) << "... inside dt -> [" << dt_dura * 1e3 << "]";
+
+	writeDt2LogFile(path_log, "log_dt_ms_imu_preprocessing.txt", dt_dura * 1e3);
+}
+
+void ComplementaryFilter::update(double ax, double ay, double az, double wx,
+                                 double wy, double wz, double mx, double my,
+                                 double mz, double dt)
+{
+    if (!initialized_)
+    {
+        // First time - ignore prediction:
+        getMeasurement(ax, ay, az, mx, my, mz, q0_, q1_, q2_, q3_);
+        initialized_ = true;
+        return;
+    }
+
+    // Bias estimation.
+    if (do_bias_estimation_) updateBiases(ax, ay, az, wx, wy, wz);
+
+    // Prediction.
+    double q0_pred, q1_pred, q2_pred, q3_pred;
+    getPrediction(wx, wy, wz, dt, q0_pred, q1_pred, q2_pred, q3_pred);
+
+    // Correction (from acc):
+    // q_temp = q_pred * [(1-gain) * qI + gain * dq_acc]
+    // where qI = identity quaternion
+    double dq0_acc, dq1_acc, dq2_acc, dq3_acc;
+    getAccCorrection(ax, ay, az, q0_pred, q1_pred, q2_pred, q3_pred, dq0_acc,
+                     dq1_acc, dq2_acc, dq3_acc);
+    double alpha = gain_acc_;
+    if (do_adaptive_gain_) alpha = getAdaptiveGain(gain_acc_, ax, ay, az);
+    scaleQuaternion(alpha, dq0_acc, dq1_acc, dq2_acc, dq3_acc);
+
+    double q0_temp, q1_temp, q2_temp, q3_temp;
+    quaternionMultiplication(q0_pred, q1_pred, q2_pred, q3_pred, dq0_acc,
+                             dq1_acc, dq2_acc, dq3_acc, q0_temp, q1_temp,
+                             q2_temp, q3_temp);
+
+    // Correction (from mag):
+    // q_ = q_temp * [(1-gain) * qI + gain * dq_mag]
+    // where qI = identity quaternion
+    double dq0_mag, dq1_mag, dq2_mag, dq3_mag;
+    getMagCorrection(mx, my, mz, q0_temp, q1_temp, q2_temp, q3_temp, dq0_mag,
+                     dq1_mag, dq2_mag, dq3_mag);
+
+    scaleQuaternion(gain_mag_, dq0_mag, dq1_mag, dq2_mag, dq3_mag);
+
+    quaternionMultiplication(q0_temp, q1_temp, q2_temp, q3_temp, dq0_mag,
+                             dq1_mag, dq2_mag, dq3_mag, q0_, q1_, q2_, q3_);
+
+    normalizeQuaternion(q0_, q1_, q2_, q3_);
+}
+
+bool ComplementaryFilter::checkState(double ax, double ay, double az, double wx,
+                                     double wy, double wz) const
+{
+    double acc_magnitude = sqrt(ax * ax + ay * ay + az * az);
+    if (fabs(acc_magnitude - kGravity) > kAccelerationThreshold) return false;
+
+    if (fabs(wx - wx_prev_) > kDeltaAngularVelocityThreshold ||
+        fabs(wy - wy_prev_) > kDeltaAngularVelocityThreshold ||
+        fabs(wz - wz_prev_) > kDeltaAngularVelocityThreshold)
+        return false;
+
+    if (fabs(wx - wx_bias_) > kAngularVelocityThreshold ||
+        fabs(wy - wy_bias_) > kAngularVelocityThreshold ||
+        fabs(wz - wz_bias_) > kAngularVelocityThreshold)
+        return false;
+
+    return true;
+}
+
+void ComplementaryFilter::updateBiases(double ax, double ay, double az,
+                                       double wx, double wy, double wz)
+{
+    steady_state_ = checkState(ax, ay, az, wx, wy, wz);
+
+    if (steady_state_)
+    {
+        wx_bias_ += bias_alpha_ * (wx - wx_bias_);
+        wy_bias_ += bias_alpha_ * (wy - wy_bias_);
+        wz_bias_ += bias_alpha_ * (wz - wz_bias_);
+    }
+
+    wx_prev_ = wx;
+    wy_prev_ = wy;
+    wz_prev_ = wz;
+}
+
+void ComplementaryFilter::getPrediction(double wx, double wy, double wz,
+                                        double dt, double& q0_pred,
+                                        double& q1_pred, double& q2_pred,
+                                        double& q3_pred) const
+{
+    double wx_unb = wx - wx_bias_;
+    double wy_unb = wy - wy_bias_;
+    double wz_unb = wz - wz_bias_;
+
+    q0_pred = q0_ + 0.5 * dt * (wx_unb * q1_ + wy_unb * q2_ + wz_unb * q3_);
+    q1_pred = q1_ + 0.5 * dt * (-wx_unb * q0_ - wy_unb * q3_ + wz_unb * q2_);
+    q2_pred = q2_ + 0.5 * dt * (wx_unb * q3_ - wy_unb * q0_ - wz_unb * q1_);
+    q3_pred = q3_ + 0.5 * dt * (-wx_unb * q2_ + wy_unb * q1_ - wz_unb * q0_);
+
+    normalizeQuaternion(q0_pred, q1_pred, q2_pred, q3_pred);
+}
+
+void ComplementaryFilter::getMeasurement(double ax, double ay, double az,
+                                         double mx, double my, double mz,
+                                         double& q0_meas, double& q1_meas,
+                                         double& q2_meas, double& q3_meas)
+{
+    // q_acc is the quaternion obtained from the acceleration vector
+    // representing the orientation of the Global frame wrt the Local frame with
+    // arbitrary yaw (intermediary frame). q3_acc is defined as 0.
+    double q0_acc, q1_acc, q2_acc, q3_acc;
+
+    // Normalize acceleration vector.
+    normalizeVector(ax, ay, az);
+    if (az >= 0)
+    {
+        q0_acc = sqrt((az + 1) * 0.5);
+        q1_acc = -ay / (2.0 * q0_acc);
+        q2_acc = ax / (2.0 * q0_acc);
+        q3_acc = 0;
+    } else
+    {
+        double X = sqrt((1 - az) * 0.5);
+        q0_acc = -ay / (2.0 * X);
+        q1_acc = X;
+        q2_acc = 0;
+        q3_acc = ax / (2.0 * X);
+    }
+
+    // [lx, ly, lz] is the magnetic field reading, rotated into the intermediary
+    // frame by the inverse of q_acc.
+    // l = R(q_acc)^-1 m
+    double lx = (q0_acc * q0_acc + q1_acc * q1_acc - q2_acc * q2_acc) * mx +
+                2.0 * (q1_acc * q2_acc) * my - 2.0 * (q0_acc * q2_acc) * mz;
+    double ly = 2.0 * (q1_acc * q2_acc) * mx +
+                (q0_acc * q0_acc - q1_acc * q1_acc + q2_acc * q2_acc) * my +
+                2.0 * (q0_acc * q1_acc) * mz;
+
+    // q_mag is the quaternion that rotates the Global frame (North West Up)
+    // into the intermediary frame. q1_mag and q2_mag are defined as 0.
+    double gamma = lx * lx + ly * ly;
+    double beta = sqrt(gamma + lx * sqrt(gamma));
+    double q0_mag = beta / (sqrt(2.0 * gamma));
+    double q3_mag = ly / (sqrt(2.0) * beta);
+
+    // The quaternion multiplication between q_acc and q_mag represents the
+    // quaternion, orientation of the Global frame wrt the local frame.
+    // q = q_acc times q_mag
+    quaternionMultiplication(q0_acc, q1_acc, q2_acc, q3_acc, q0_mag, 0, 0,
+                             q3_mag, q0_meas, q1_meas, q2_meas, q3_meas);
+    // q0_meas = q0_acc*q0_mag;
+    // q1_meas = q1_acc*q0_mag + q2_acc*q3_mag;
+    // q2_meas = q2_acc*q0_mag - q1_acc*q3_mag;
+    // q3_meas = q0_acc*q3_mag;
+}
+
+void ComplementaryFilter::getMeasurement(double ax, double ay, double az,
+                                         double& q0_meas, double& q1_meas,
+                                         double& q2_meas, double& q3_meas)
+{
+    // q_acc is the quaternion obtained from the acceleration vector
+    // representing the orientation of the Global frame wrt the Local frame with
+    // arbitrary yaw (intermediary frame). q3_acc is defined as 0.
+
+    // Normalize acceleration vector.
+    normalizeVector(ax, ay, az);
+
+    if (az >= 0)
+    {
+        q0_meas = sqrt((az + 1) * 0.5);
+        q1_meas = -ay / (2.0 * q0_meas);
+        q2_meas = ax / (2.0 * q0_meas);
+        q3_meas = 0;
+    } else
+    {
+        double X = sqrt((1 - az) * 0.5);
+        q0_meas = -ay / (2.0 * X);
+        q1_meas = X;
+        q2_meas = 0;
+        q3_meas = ax / (2.0 * X);
+    }
+}
+
+void ComplementaryFilter::getAccCorrection(double ax, double ay, double az,
+                                           double p0, double p1, double p2,
+                                           double p3, double& dq0, double& dq1,
+                                           double& dq2, double& dq3)
+{
+    // Normalize acceleration vector.
+    normalizeVector(ax, ay, az);
+
+    // Acceleration reading rotated into the world frame by the inverse
+    // predicted quaternion (predicted gravity):
+    double gx, gy, gz;
+    rotateVectorByQuaternion(ax, ay, az, p0, -p1, -p2, -p3, gx, gy, gz);
+
+    // Delta quaternion that rotates the predicted gravity into the real
+    // gravity:
+    dq0 = sqrt((gz + 1) * 0.5);
+    dq1 = -gy / (2.0 * dq0);
+    dq2 = gx / (2.0 * dq0);
+    dq3 = 0.0;
+}
+
+void ComplementaryFilter::getMagCorrection(double mx, double my, double mz,
+                                           double p0, double p1, double p2,
+                                           double p3, double& dq0, double& dq1,
+                                           double& dq2, double& dq3)
+{
+    // Magnetic reading rotated into the world frame by the inverse predicted
+    // quaternion:
+    double lx, ly, lz;
+    rotateVectorByQuaternion(mx, my, mz, p0, -p1, -p2, -p3, lx, ly, lz);
+
+    // Delta quaternion that rotates the l so that it lies in the xz-plane
+    // (points north):
+    double gamma = lx * lx + ly * ly;
+    double beta = sqrt(gamma + lx * sqrt(gamma));
+    dq0 = beta / (sqrt(2.0 * gamma));
+    dq1 = 0.0;
+    dq2 = 0.0;
+    dq3 = ly / (sqrt(2.0) * beta);
+}
+
+void ComplementaryFilter::getOrientation(double& q0, double& q1, double& q2,
+                                         double& q3) const
+{
+    // Return the inverse of the state (state is fixed wrt body).
+    invertQuaternion(q0_, q1_, q2_, q3_, q0, q1, q2, q3);
+}
+
+double ComplementaryFilter::getAdaptiveGain(double alpha, double ax, double ay,
+                                            double az)
+{
+    double a_mag = sqrt(ax * ax + ay * ay + az * az);
+    double error = fabs(a_mag - kGravity) / kGravity;
+    double factor;
+    double error1 = 0.1;
+    double error2 = 0.2;
+    double m = 1.0 / (error1 - error2);
+    double b = 1.0 - m * error1;
+    if (error < error1)
+        factor = 1.0;
+    else if (error < error2)
+        factor = m * error + b;
+    else
+        factor = 0.0;
+    // printf("FACTOR: %f \n", factor);
+    return factor * alpha;
+}
+
+void normalizeVector(double& x, double& y, double& z)
+{
+    double norm = sqrt(x * x + y * y + z * z);
+
+    x /= norm;
+    y /= norm;
+    z /= norm;
+}
+
+void normalizeQuaternion(double& q0, double& q1, double& q2, double& q3)
+{
+    double norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    q0 /= norm;
+    q1 /= norm;
+    q2 /= norm;
+    q3 /= norm;
+}
+
+void invertQuaternion(double q0, double q1, double q2, double q3,
+                      double& q0_inv, double& q1_inv, double& q2_inv,
+                      double& q3_inv)
+{
+    // Assumes quaternion is normalized.
+    q0_inv = q0;
+    q1_inv = -q1;
+    q2_inv = -q2;
+    q3_inv = -q3;
+}
+
+void scaleQuaternion(double gain, double& dq0, double& dq1, double& dq2,
+                     double& dq3)
+{
+    if (dq0 < 0.0)  // 0.9
+    {
+        // Slerp (Spherical linear interpolation):
+        double angle = acos(dq0);
+        double A = sin(angle * (1.0 - gain)) / sin(angle);
+        double B = sin(angle * gain) / sin(angle);
+        dq0 = A + B * dq0;
+        dq1 = B * dq1;
+        dq2 = B * dq2;
+        dq3 = B * dq3;
+    } else
+    {
+        // Lerp (Linear interpolation):
+        dq0 = (1.0 - gain) + gain * dq0;
+        dq1 = gain * dq1;
+        dq2 = gain * dq2;
+        dq3 = gain * dq3;
+    }
+
+    normalizeQuaternion(dq0, dq1, dq2, dq3);
+}
+
+void quaternionMultiplication(double p0, double p1, double p2, double p3,
+                              double q0, double q1, double q2, double q3,
+                              double& r0, double& r1, double& r2, double& r3)
+{
+    // r = p q
+    r0 = p0 * q0 - p1 * q1 - p2 * q2 - p3 * q3;
+    r1 = p0 * q1 + p1 * q0 + p2 * q3 - p3 * q2;
+    r2 = p0 * q2 - p1 * q3 + p2 * q0 + p3 * q1;
+    r3 = p0 * q3 + p1 * q2 - p2 * q1 + p3 * q0;
+}
+
+void rotateVectorByQuaternion(double x, double y, double z, double q0,
+                              double q1, double q2, double q3, double& vx,
+                              double& vy, double& vz)
+{
+    vx = (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * x +
+         2 * (q1 * q2 - q0 * q3) * y + 2 * (q1 * q3 + q0 * q2) * z;
+    vy = 2 * (q1 * q2 + q0 * q3) * x +
+         (q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3) * y +
+         2 * (q2 * q3 - q0 * q1) * z;
+    vz = 2 * (q1 * q3 - q0 * q2) * x + 2 * (q2 * q3 + q0 * q1) * y +
+         (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * z;
+}
+
+}  // namespace imu_tools

src/localization_module_lio_locus/src/lio_locus/lio_locus.cpp

@@ -0,0 +1,590 @@
+/*
+ * lio_locus.cpp
+ *
+ *  Created on: Sep 11, 2025
+ *      Author: solomon
+ */
+
+#include "lio_locus/lio_locus.h"
+
+namespace lio_locus {
+
+LioLocus::LioLocus() {
+	google::SetStderrLogging(google::GLOG_WARNING);
+	FLAGS_colorlogtostderr = true;
+	FLAGS_alsologtostderr = true;
+}
+
+LioLocus::~LioLocus() {
+}
+
+void LioLocus::setPath2Parameters(string path_to_param) {
+	m_path_to_file = path_to_param;
+
+	LOG(INFO) << "path to parameter file -> [" << m_path_to_file << "]";
+}
+
+bool LioLocus::initialize(string path_to_param) {
+
+	mp_cloud_incremental = PointCloudType::Ptr(new PointCloudType());
+
+	setPath2Parameters(path_to_param);
+
+	mp_cloud_odometry = PointCloudOdometryPtr(new PointCloudOdometry());
+	mp_cloud_odometry->Initialize(m_path_to_file);
+	mp_cloud_odometry->EnableImuIntegration();
+
+//	mp_cloud_mapper = (PointCloudMapperPtr) (new PointCloudMapper());
+//	mp_cloud_mapper = (PointCloudMultiThreadedMapperPtr) (new Point CloudMultiThreadedMapper());
+
+	mp_cloud_mapper = PointCloudIkdTreeMapperPtr(new PointCloudIkdTreeMapper());
+	mp_cloud_mapper->Initialize(m_path_to_file);
+
+	mp_cloud_localization = PointCloudLocalizationPtr(
+			new PointCloudLocalization());
+	mp_cloud_localization->Initialize(m_path_to_file);
+
+	return false;
+}
+
+// normal tracking and updating the cloud map in memory;
+void LioLocus::trackCurrentCloudWithOnlineBuiltMap(
+		PointCloudType::Ptr msg_cloud) {
+
+	auto ts_start = std::chrono::steady_clock::now();
+
+	// associate timestamp with pose;
+	std::uint64_t timestamp_now = msg_cloud->header.stamp; // uint64_t;
+
+	auto start_chrono = std::chrono::steady_clock::now();
+	LOG(INFO) << "####################################################";
+	LOG(INFO) << "                      LIO-LOCUS                     ";
+	LOG(INFO) << "####################################################";
+
+	if (!mb_tracker_reset) { // normal case;
+
+		//---------------------------------------------------------------
+		// 0. set delta transform prior;
+		//---------------------------------------------------------------
+		geometry_utils::Transform3 dT_prior_gu =
+				geometry_utils::Transform3::Identity();
+
+		//------------------------------------------------------
+		// 1. get delta transformation from sensor observations;
+		//------------------------------------------------------
+		// use imu orientation prior to update value for lidar odometry;
+		geometry_utils::Quaterniond dq_imu;
+		if (mb_use_imu) {
+			LOG(INFO) << "... a. getting prior from gyro observations ... ";
+
+			auto time = pcl_conversions::fromPCL(msg_cloud->header.stamp);
+
+			getRelativeRotationFromIMU(time.seconds(), dq_imu);
+
+			// TODO: update rotation in prior dT;
+			dT_prior_gu.rotation = dq_imu;
+
+		} // end of scope for imu gyro prior assignment;
+
+		//---------------------------------------------------------------
+		// 2. estimate increments using scan-to-scan lidar odmoetry;
+		//---------------------------------------------------------------
+
+		LOG(INFO) << "... a. get relative displacement from sensors ...";
+		LOG(INFO) << "... translation -> #[" << dT_prior_gu.translation.X()
+				<< "," << dT_prior_gu.translation.Y() << ","
+				<< dT_prior_gu.translation.Z() << "] and angles ->["
+				<< dT_prior_gu.rotation.Roll() << ","
+				<< dT_prior_gu.rotation.Pitch() << ","
+				<< dT_prior_gu.rotation.Yaw() << "]";
+
+		if (mb_use_scan2scan_matching) {
+			LOG(INFO)
+					<< "... b. estimating lidar odometry for consecutive scan matching! ... ";
+			// set imu prior to be later used in lidar odometry scan matching;
+			mp_cloud_odometry->EnableImuIntegration();
+			mp_cloud_odometry->SetImuDelta(toRotationMatrix(dq_imu));
+			mp_cloud_odometry->DisableSensorIntegration();
+
+			mp_cloud_odometry->SetLidar(*msg_cloud);
+
+			if (!mp_cloud_odometry->UpdateEstimate()) {
+				LOG(INFO)
+						<< "... cloud odometry fails in state estimation!";
+				//TODO ... more when scan-to-scan matching fails;
+
+				return;// now directly skip processing;
+			}
+
+			geometry_utils::Transform3 dT_scan2scan =
+					mp_cloud_odometry->GetIncrementalEstimate();
+			dT_prior_gu = dT_scan2scan;
+			LOG(INFO) << "... translation -> #["
+					<< dT_prior_gu.translation.X() << ","
+					<< dT_prior_gu.translation.Y() << ","
+					<< dT_prior_gu.translation.Z() << "] and angles ->["
+					<< dT_prior_gu.rotation.Roll() << ","
+					<< dT_prior_gu.rotation.Pitch() << ","
+					<< dT_prior_gu.rotation.Yaw() << "]";
+		}
+		//---------------------------------------------------------------
+		// 3. estimate pose from lidar localizer: update pose through matching against correspondences;
+
+		// check if milestone or not;
+		static bool flag_ref_pose_recorded = false;
+		static geometry_utils::Transform3 pose_ref;
+
+		// check displacement over thresholds;
+		auto current_pose = mp_cloud_localization->GetIntegratedEstimate();
+		bool flag_record_cloud = false;
+		if (!flag_ref_pose_recorded) {
+			pose_ref = current_pose;
+			flag_ref_pose_recorded = true;
+
+			flag_record_cloud = true;
+		} // record for comparing for first time;
+		else {
+
+			// estimate pose through scan-map matching;
+
+			LOG(INFO) << "... c. registering cloud to map ... ";
+
+			// motion prior from lidar odometry;
+			mp_cloud_localization->MotionUpdate(dT_prior_gu);
+
+			LOG(INFO) << "... translation -> #["
+					<< dT_prior_gu.translation.X() << ","
+					<< dT_prior_gu.translation.Y() << ","
+					<< dT_prior_gu.translation.Z() << "] and  angles ->["
+					<< dT_prior_gu.rotation.Roll() << ","
+					<< dT_prior_gu.rotation.Pitch() << ","
+					<< dT_prior_gu.rotation.Yaw() << "]";
+
+			//----------------------------------------------------------------
+			// refine pose prior within localizer;
+			//----------------------------------------------------------------
+			// data association using euclidean distance metric;
+			PointCloudType::Ptr cloud_query_fixed(new PointCloudType());
+			mp_cloud_localization->TransformPointsToFixedFrame(*msg_cloud,
+					cloud_query_fixed.get());
+			PointCloudType::Ptr cloud_ref_fixed(new PointCloudType());
+			if (mp_cloud_mapper->ApproxNearestNeighbors(*cloud_query_fixed,
+					cloud_ref_fixed.get())) {
+
+				PointCloudType::Ptr cloud_ref_local(new PointCloudType());
+				mp_cloud_localization->TransformPointsToSensorFrame(
+						*cloud_ref_fixed, cloud_ref_local.get());
+
+				// correct relative displacement through scan matching;
+				PointCloudType::Ptr aligned_query(new PointCloudType());
+				mp_cloud_localization->MeasurementUpdate(msg_cloud,
+						cloud_ref_local, aligned_query.get());
+
+				LOG(INFO) << "... measurement update done!";
+
+				geometry_utils::Transform3 current_pose =
+						mp_cloud_localization->GetIntegratedEstimate();
+
+				//TODO
+
+				// set pose of robot;
+
+			} else {
+				LOG(WARNING)
+						<< "... data association fails to find correspondences!";
+
+				//TODO. extends when scan-map matching fails;
+
+				return;
+			}
+
+			geometry_utils::Transform3 delta_pose = geometry_utils::PoseUpdate(
+					geometry_utils::PoseInverse(pose_ref), current_pose);
+
+			// for distance more than threshold;
+			auto translation = delta_pose.translation;
+			if (std::pow(translation.X(), 2) + std::pow(translation.Y(), 2)
+					+ std::pow(translation.Z(), 2)
+					> m_threshold_trans_map * m_threshold_trans_map
+					or std::fabs(delta_pose.rotation.Yaw())
+							> m_threshold_rot_map) {
+
+				flag_ref_pose_recorded = true;
+				pose_ref = current_pose;
+
+				flag_record_cloud = true;
+			}
+
+		}
+
+		//---------------------------------------------------------------
+		// 4. update map if necessary;
+		//---------------------------------------------------------------
+
+		// update cloud map when milestone is found;
+		if (flag_record_cloud) {
+			LOG(INFO) << "... #. inserting cloud to map ... ";
+			transformCloudAndInsertToMap(msg_cloud);
+		}
+	} // end of scope for normal tracking;
+	else {
+
+		LOG(INFO) << "... resetting tracker ... ";
+
+		if (mp_cloud_mapper->getCloudMapInMem() == nullptr) {
+
+			LOG(INFO) << "... cloud map not ready for retrieval!";
+
+			return;
+		}
+
+		// if required to reset, relocalize in the cloud map in memory, and break only when succeeds;
+		if (trackCurrentCloudWithPriorCloudMap(msg_cloud)) {
+
+			// reset components for odometry,
+			mp_cloud_odometry->reset();
+			LOG(INFO)
+					<< "... #. inserting cloud to map during tracking resetting ... ";
+
+			mb_tracker_reset = false;
+		}
+	} // end of scope for resetting case;
+
+	auto end_chrono = std::chrono::steady_clock::now();
+	double dt_chrono =
+			std::chrono::duration<double>(end_chrono - start_chrono).count();
+	LOG(INFO) << "... time used for cloud processing #[" << dt_chrono * 1e3
+			<< "]ms ... ";
+
+	// set time stamp associated pose;
+	geometry_utils::Transform3 pose =
+			mp_cloud_localization->GetIntegratedEstimate();
+	m_pose_latest = TransformStamped(pose, timestamp_now);
+	mp_cloud_incremental->header.stamp = timestamp_now;
+
+	auto ts_end = std::chrono::steady_clock::now();
+	auto dt = std::chrono::duration<double>(ts_end - ts_start).count();
+
+	writeDt2LogFile(path_log, "log_dt_ms_lio_locus_track_online_map.txt", dt * 1e3);
+
+}
+
+void LioLocus::clearCloudMapInMemory() {
+	mp_cloud_mapper->Reset();
+}
+
+bool LioLocus::saveCloudMapInMemory(string path, bool flag_voxel_filtering,
+		double leaf_size) {
+
+	boost::filesystem::path path_cloud(path);
+	if (!boost::filesystem::exists(path)) {
+		boost::filesystem::create_directory(path);
+	}
+
+	string fn = "cloud_map.pcd";
+	// save map from memory to disk;
+	PointCloudType::Ptr ptr_cloud_map(new PointCloudType());
+	if (getCloudMapInMemory(ptr_cloud_map)) {
+		// voxel grid filter  the cloud if necessary;
+		if (flag_voxel_filtering) {
+			ptr_cloud_map = downsampleCloud(ptr_cloud_map, leaf_size);
+		}
+
+		// check: normal vector information ;
+		pcl::io::savePCDFileASCII(path + "/" + fn, *ptr_cloud_map);
+
+		LOG(INFO) << "cloud map saved to [" << path << "] with name [" << fn
+				<< "]";
+
+		return true;
+	}
+	return false;
+}
+
+bool LioLocus::uploadPriorCloudMap(string fn_cloud_map, double leaf_size) {
+
+	LOG(INFO) << "... check file exists -> ["
+			<< boost::filesystem::exists(fn_cloud_map) << "] for ["
+			<< fn_cloud_map << "]";
+	if (!boost::filesystem::exists(fn_cloud_map)) {
+		LOG(ERROR) << "... cloud map not exists! check for cloud map!";
+		return false;
+	}
+
+//--------------------------------------------------------------------
+// load cloud in PCL format;
+	PointCloudType::Ptr cloud_pcl(new PointCloudType());
+	pcl::io::loadPCDFile(fn_cloud_map, *cloud_pcl);
+//--------------------------------------------------------------------
+	LOG(INFO)
+			<< "####################################!!!!!!!!!!!!!!!!!!#####################";
+	LOG(INFO) << "... #cloud map size -> [" << cloud_pcl->size() << "]";
+	LOG(INFO) << "... voxel filering leaf size -> [" << leaf_size << "]";
+	pcl::VoxelGrid<pcl::PointXYZINormal> voxel_grid_filter;
+	voxel_grid_filter.setLeafSize(leaf_size, leaf_size, leaf_size);
+	voxel_grid_filter.setInputCloud(cloud_pcl);
+	voxel_grid_filter.filter(*cloud_pcl);
+
+	mp_cloud_mapper->Reset();
+
+	PointCloudType::Ptr unused(new PointCloudType());
+	mp_cloud_mapper->InsertPoints(cloud_pcl, unused.get());
+	LOG(INFO) << "# inserted cloud map size -> [" << cloud_pcl->size()
+			<< "]";
+	LOG(INFO)
+			<< "####################################!!!!!!!!!!!!!!!!!!#####################";
+
+	return true;
+
+}
+
+// using prior cloud map in memory, do re-localization without updating the memory cloud map;
+bool LioLocus::trackCurrentCloudWithPriorCloudMap(
+		PointCloudType::Ptr msg_cloud) {
+
+	std::uint64_t timestamp_now = msg_cloud->header.stamp; // uint64_t;
+
+	//---------------------------------------------------------------
+	// 0. set delta transform prior;
+	//---------------------------------------------------------------
+	geometry_utils::Transform3 dT_prior_gu =
+			geometry_utils::Transform3::Identity();
+
+	// update with pure lidar odometry;
+
+	LOG(INFO)
+			<< "... b. estimating lidar odometry for consecutive scan matching! ... ";
+	// set imu prior to be later used in lidar odometry scan matching;
+	mp_cloud_odometry->EnableImuIntegration();
+	mp_cloud_odometry->SetImuDelta(
+			toRotationMatrix(dT_prior_gu.rotation.Yaw()));
+	mp_cloud_odometry->DisableSensorIntegration();
+
+	mp_cloud_odometry->SetLidar(*msg_cloud);
+	if (!mp_cloud_odometry->UpdateEstimate()) {
+		LOG(WARNING) << "... cloud odometry fails due to abrupt sudden!";
+		//TODO ... more when scan-to-scan matching fails;
+	}
+
+	geometry_utils::Transform3 dT_scan2scan =
+			mp_cloud_odometry->GetIncrementalEstimate();
+	dT_prior_gu = dT_scan2scan;
+
+	LOG(INFO) << "#########################################";
+	LOG(INFO) << "... increment -> #[" << dT_scan2scan.translation.X()
+			<< "," << dT_scan2scan.translation.Y() << "]";
+	LOG(INFO) << "#########################################";
+
+	// motion prior from lidar odometry;
+	mp_cloud_localization->MotionUpdate(dT_prior_gu);
+
+	// data association using euclidean distance metric;
+	PointCloudType::Ptr cloud_query_fixed(new PointCloudType());
+	mp_cloud_localization->TransformPointsToFixedFrame(*msg_cloud,
+			cloud_query_fixed.get());
+	PointCloudType::Ptr cloud_ref_fixed(new PointCloudType());
+	if (mp_cloud_mapper->ApproxNearestNeighbors(*cloud_query_fixed,
+			cloud_ref_fixed.get())) {
+
+		PointCloudType::Ptr cloud_ref_local(new PointCloudType());
+		mp_cloud_localization->TransformPointsToSensorFrame(*cloud_ref_fixed,
+				cloud_ref_local.get());
+
+		// correct relative displacement through scan matching;
+		PointCloudType::Ptr aligned_query(new PointCloudType());
+		mp_cloud_localization->MeasurementUpdateRelocalization(msg_cloud,
+				cloud_ref_local, aligned_query.get());
+
+		LOG(INFO) << "... measurement update done!";
+
+		// [local cloud] -> [global cloud]
+		mp_cloud_localization->MotionUpdate(
+				geometry_utils::Transform3::Identity());
+
+		// input cloud transformed to global frame;
+
+
+		mp_cloud_localization->TransformPointsToFixedFrame(*msg_cloud,
+				mp_cloud_incremental.get());
+	}
+
+	// set time stamp associated pose;
+	geometry_utils::Transform3 pose =
+			mp_cloud_localization->GetIntegratedEstimate();
+	m_pose_latest = TransformStamped(pose, timestamp_now);
+	mp_cloud_incremental->header.stamp = timestamp_now;
+
+	return false;
+}
+
+void LioLocus::setPosePrior(geometry_utils::Transform3 pose_prior) {
+
+	mp_cloud_localization->SetIntegratedEstimate(pose_prior);
+}
+
+void LioLocus::transformCloudAndInsertToMap(PointCloudType::Ptr msg_cloud) {
+
+	// [local cloud] -> [global cloud]
+	mp_cloud_localization->MotionUpdate(geometry_utils::Transform3::Identity());
+	PointCloudType::Ptr m_cloud_fixed(new PointCloudType());
+	mp_cloud_localization->TransformPointsToFixedFrame(*msg_cloud,
+			m_cloud_fixed.get());
+
+	// insert cloud to local mapper;
+	auto current_pose = mp_cloud_localization->GetIntegratedEstimate();
+	mp_cloud_mapper->UpdateCurrentPose(current_pose);
+
+	mp_cloud_incremental = PointCloudType::Ptr(new PointCloudType());
+	mp_cloud_mapper->InsertPoints(m_cloud_fixed, mp_cloud_incremental.get());
+
+	// notify size of points inserted to cloud map;
+	LOG(INFO) << "#[" << m_cloud_fixed->size()
+			<< "] points inserted for cloud map!";
+
+	return;
+}
+
+PointCloudType::Ptr LioLocus::downsampleCloud(PointCloudType::Ptr input_cloud,
+		double leaf_size) {
+	// do filtering on visualization cloud map;
+	PointCloudType::Ptr cloud_downsampled(new PointCloudType());
+	*cloud_downsampled = *input_cloud;
+	pcl::VoxelGrid<pcl::PointXYZINormal> voxel_grid_filter;
+	voxel_grid_filter.setLeafSize(leaf_size, leaf_size, leaf_size);
+	voxel_grid_filter.setInputCloud(cloud_downsampled);
+	voxel_grid_filter.filter(*cloud_downsampled);
+
+	// RCLCPP_INFO_STREAM(rclcpp::get_logger(params_lio_locus.log_tag),
+	//                    "... during down-sampling cloud -> #[" << input_cloud->size()
+	//                                                           << "] -> #[" << cloud_downsampled->size() << "]");
+
+	return cloud_downsampled;
+}
+
+bool LioLocus::getCloudMapInMemory(PointCloudType::Ptr ptr_cloud) {
+
+	*ptr_cloud = *mp_cloud_mapper->getCloudMapInMem();
+
+	if (ptr_cloud == nullptr)
+		return false;
+
+	// LOG(INFO) << "#[" << ptr_cloud->size() << "] points for cloud map!";
+
+	return true;
+}
+
+void LioLocus::feedIMUData(IMUData imu_data) {
+
+	if (m_imus.size() >= m_size_imus) {
+		m_imus.erase(m_imus.begin());
+	}
+
+	m_imus.insert(std::make_pair(imu_data.timestamp_, imu_data));
+
+}
+
+Eigen::Matrix3d LioLocus::toRotationMatrix(
+		geometry_utils::Quaterniond quat_imu) {
+
+	Eigen::Quaterniond q(quat_imu.W(), quat_imu.X(), quat_imu.Y(),
+			quat_imu.Z());
+	Eigen::Matrix3d rot = q.toRotationMatrix();
+
+	return rot;
+}
+
+// Pose6DOF LioLocus::getCurrentPose6DOF() {
+
+// 	auto T = getCurrentPose();
+// 	auto t_T = T.translation;
+// 	auto q_T = T.rotation;
+
+// 	Pose6DOF pose(Eigen::Vector3d(t_T.X(), t_T.Y(), t_T.Z()),
+// 			Eigen::Quaterniond(q_T.Eigen()));
+
+// 	return pose;
+// }
+
+// insert the input cloud to mapper;
+bool LioLocus::getRelativeRotationFromIMU(double ts,
+		geometry_utils::Quaterniond &quat_imu) {
+
+// get IMU message for the given timestamp;
+
+	LOG(INFO) << "... having #[" << m_imus.size() << "] imus!";
+	IMUData msg_imu_data;
+	if (!getMessageAtTimeStamp(ts, msg_imu_data, m_imus)) {
+		return false;
+	}
+
+	auto q = msg_imu_data.orientation;
+	Eigen::Quaterniond quat_cur(q.w(), q.x(), q.y(), q.z());
+
+	static Eigen::Quaterniond quat_ref = quat_cur;
+
+	Eigen::Quaterniond delta_quat_imu = quat_ref.inverse() * quat_cur;
+	LOG(INFO) << "... # ROTATION from imu observations -> ["
+			<< Eigen::AngleAxisd(delta_quat_imu).angle() / M_PI * 180
+			<< "] degree!";
+
+//  T_cloud2imu * dT_imu * T_imu2cloud = dT_cloud;
+	Eigen::Quaterniond delta_quat_cloud = m_dq_imu2cloud.inverse()
+			* delta_quat_imu * m_dq_imu2cloud;
+
+	LOG(INFO) << "... # ROTATION from cloud observations -> ["
+			<< Eigen::AngleAxisd(delta_quat_cloud).angle() / M_PI * 180
+			<< "] degree!";
+
+	quat_imu = geometry_utils::Quaterniond(delta_quat_cloud.w(),
+			delta_quat_cloud.x(), delta_quat_cloud.y(), delta_quat_cloud.z());
+
+	quat_ref = quat_cur;
+	return true;
+}
+
+bool LioLocus::getMessageAtTimeStamp(double stamp, IMUData &msg,
+		map<double, IMUData> buffer) {
+	if (buffer.size() == 0) {
+		return false;
+	}
+	auto ts_itr = buffer.lower_bound(stamp);
+	double time_diff;
+
+	if (ts_itr == buffer.begin()) {
+		msg = ts_itr->second;
+		time_diff = ts_itr->first - stamp;
+		LOG(INFO) << "ts_itr points to buffer begin";
+	} else if (ts_itr == buffer.end()) {
+		ts_itr--;
+		msg = ts_itr->second;
+		time_diff = stamp - ts_itr->first;
+		LOG(INFO) << "ts_itr points to buffer end";
+	} else {
+		double ts_previous = std::prev(ts_itr, 1)->first;
+		if (ts_itr->first - stamp < stamp - ts_previous) {
+			msg = ts_itr->second;
+			time_diff = ts_itr->first - stamp;
+		} else {
+			msg = std::prev(ts_itr, 1)->second;
+			time_diff = stamp - ts_previous;
+		}
+
+	} // end of search scope;
+
+	LOG(INFO) << "... ts of cloud [" << std::setw(20)
+			<< std::setprecision(20) << stamp << "] v.s. imu [" << std::setw(20)
+			<< std::setprecision(20) << msg.timestamp_ << "] with diff ["
+			<< std::setprecision(20) << "[" << time_diff * 1e3
+			<< "] milli-sec!";
+
+	double threshold = 0.3;
+	if (fabs(time_diff) > threshold) {
+
+		LOG(INFO) << "... #[" << time_diff << "] more than threshold ["
+				<< threshold << "]";
+		return false;
+	}
+	return true;
+}
+
+}

src/localization_module_lio_locus/src/point_cloud_localization/CMakeLists.txt

@@ -0,0 +1,21 @@
+add_library(point_cloud_localization SHARED 
+	PointCloudLocalization.cc utils.cc)
+
+target_link_libraries(point_cloud_localization
+	#  ${PCL_LIBRARIES}
+        pcl_octree
+    pcl_filters
+    pcl_search
+    pcl_common
+    pcl_sample_consensus
+  ${YAML_CPP_LIBRARIES}
+  glog
+)
+
+install(TARGETS point_cloud_localization
+  DESTINATION lib
+)
+
+
+
+

src/localization_module_lio_locus/src/point_cloud_localization/PointCloudLocalization.cc

@@ -0,0 +1,420 @@
+/*
+ Authors:
+ - Matteo Palieri    (matteo.palieri@jpl.nasa.gov)
+ - Benjamin Morrell  (benjamin.morrell@jpl.nasa.gov)
+ - Andrea Tagliabue  (andrea.tagliabue@jpl.nasa.gov)
+ - Andrzej Reinke (andrzej.m.reinke@jpl.nasa.gov)
+
+ - later modified by solomon,
+ */
+
+#include <point_cloud_localization/PointCloudLocalization.h>
+#include <point_cloud_localization/utils.h>
+
+namespace gu = geometry_utils;
+
+using namespace point_cloud_localization;
+
+PointCloudLocalization::PointCloudLocalization()
+{
+
+	google::SetStderrLogging(google::GLOG_INFO);
+	FLAGS_colorlogtostderr = true;
+	FLAGS_alsologtostderr = true;
+}
+
+PointCloudLocalization::~PointCloudLocalization()
+{
+
+}
+
+bool PointCloudLocalization::Initialize(std::string path_to_params)
+{
+
+	if (!loadParametersFromFile(path_to_params))
+		return false;
+
+	applyParameters();
+
+	if (!SetupICP())
+	{
+		LOG(ERROR) << "Failed to SetupICP";
+	}
+
+	return true;
+}
+
+bool PointCloudLocalization::loadParametersFromFile(std::string path_to_file)
+{
+
+	if (!boost::filesystem::exists(path_to_file))
+	{
+		LOG(ERROR) << "file NOT exists for parameter loading -> [" << path_to_file << "]";
+
+		return false;
+	}
+	else
+	{
+
+		LOG(INFO) << "... reading parameter for localization from [" << path_to_file << "]!";
+	}
+
+	YAML::Node yaml_node = YAML::LoadFile(path_to_file);
+	b_is_flat_ground_assumption_ = yaml_node["b_is_flat_ground_assumption"].as<bool>();
+	auto node_loc = yaml_node["localization"];
+
+	// for cloud registration;
+	params_.registration_method =
+		node_loc["registration_method"].as<std::string>();
+	params_.iterations = node_loc["iterations"].as<unsigned int>();
+	params_.num_threads = node_loc["num_threads"].as<int>();
+	params_.k_nearest_neighbours_ = node_loc["normal_search_radius"].as<int>();
+
+	params_.tf_epsilon = node_loc["tf_epsilon"].as<double>();
+	params_.corr_dist = node_loc["corr_dist"].as<double>();
+
+	// for cloud registration result evaluation critera;
+	transform_thresholding_ = node_loc["transform_thresholding"].as<bool>();
+	max_translation_ = node_loc["max_translation"].as<double>();
+	max_rotation_ = node_loc["max_rotation"].as<double>();
+
+	recompute_covariance_local_map_ =
+		node_loc["recompute_covariance_local_map"].as<bool>();
+	recompute_covariance_scan_ =
+		node_loc["recompute_covariance_scan"].as<bool>();
+
+	return true;
+}
+
+void PointCloudLocalization::applyParameters()
+{
+
+	double init_x = 0.0, init_y = 0.0, init_z = 0.0;
+	double init_qx = 0.0, init_qy = 0.0, init_qz = 0.0, init_qw = 1.0;
+	bool b_have_fiducial = true;
+
+	double init_roll = 0.0, init_pitch = 0.0, init_yaw = 0.0;
+	gu::Quat q(gu::Quat(init_qw, init_qx, init_qy, init_qz));
+	gu::Rot3 R;
+	R = gu::QuatToR(q);
+
+	init_roll = R.Roll();
+	init_pitch = R.Pitch();
+	init_yaw = R.Yaw();
+
+	integrated_estimate_.translation = gu::Vec3(init_x, init_y, init_z);
+	integrated_estimate_.rotation = gu::Rot3(init_roll, init_pitch, init_yaw);
+
+	if (b_is_flat_ground_assumption_)
+	{
+		integrated_estimate_.rotation = gu::Rot3(0, 0,
+												 integrated_estimate_.rotation.Yaw());
+	}
+
+	if (!b_have_fiducial)
+	{
+		LOG(WARNING) << 
+			"Can't find fiducials, using origin";
+	}
+	else
+	{
+		LOG(INFO) << "Using:\nTranslation:\n"
+				  << integrated_estimate_.translation << "\nRotation\n"
+				  << integrated_estimate_.rotation.Roll() * 180.0 / M_PI << ", "
+				  << integrated_estimate_.rotation.Pitch() * 180.0 / M_PI << ", " << integrated_estimate_.rotation.Yaw() * 180.0 / M_PI;
+	}
+}
+
+const gu::Transform3 &PointCloudLocalization::GetIncrementalEstimate() const
+{
+	return incremental_estimate_;
+}
+
+const gu::Transform3 &PointCloudLocalization::GetIntegratedEstimate() const
+{
+	return integrated_estimate_;
+}
+
+void PointCloudLocalization::SetIntegratedEstimate(
+	const gu::Transform3 &integrated_estimate)
+{
+	integrated_estimate_ = integrated_estimate;
+}
+
+bool PointCloudLocalization::MotionUpdate(
+	const gu::Transform3 &incremental_odom)
+{
+	// Store the incremental transform from odometry
+	incremental_estimate_ = incremental_odom;
+	return true;
+}
+
+// cloud points transformation vice versa;
+bool PointCloudLocalization::TransformPointsToFixedFrame(
+	const PointCloudF &points, PointCloudF *points_transformed) const
+{
+	if (points_transformed == NULL)
+	{
+
+		return false;
+	}
+
+	// [world] => [robot];
+	const gu::Transform3 estimate = gu::PoseUpdate(integrated_estimate_,
+												   incremental_estimate_);
+
+	Eigen::Matrix4d tf;
+	tf.block(0, 0, 3, 3) = estimate.rotation.Eigen();
+	tf.block(0, 3, 3, 1) = estimate.translation.Eigen();
+
+	pcl::transformPointCloudWithNormals(points, *points_transformed, tf);
+
+	return true;
+}
+
+bool PointCloudLocalization::TransformPointsToSensorFrame(
+	const PointCloudF &points, PointCloudF *points_transformed) const
+{
+	if (points_transformed == NULL)
+	{
+		return false;
+	}
+
+	// [robot] <= [world]
+	const gu::Transform3 estimate = gu::PoseInverse(
+		gu::PoseUpdate(integrated_estimate_, incremental_estimate_));
+
+	Eigen::Matrix4d tf;
+	tf.block(0, 0, 3, 3) = estimate.rotation.Eigen();
+	tf.block(0, 3, 3, 1) = estimate.translation.Eigen();
+
+	pcl::transformPointCloudWithNormals(points, *points_transformed, tf);
+
+	return true;
+}
+
+bool PointCloudLocalization::SetupICP()
+{
+
+	LOG(INFO) << "####################################################";
+	LOG(INFO) << "           PointCloudLocalization - SetupICP        ";
+	LOG(INFO) << "####################################################";
+
+	RegistrationMethod method;
+
+	if (params_.registration_method == "gicp")
+		method = RegistrationMethod::GICP;
+	else if (params_.registration_method == "ndt")
+		method = RegistrationMethod::NDT;
+
+	switch (method)
+	{
+	case RegistrationMethod::GICP:
+	{
+		LOG(INFO) << 
+						   "RegistrationMethod::GICP activated.";
+		pcl::MultithreadedGeneralizedIterativeClosestPoint<PointF, PointF>::Ptr gicp =
+			boost::make_shared<
+				pcl::MultithreadedGeneralizedIterativeClosestPoint<
+					PointF, PointF>>();
+
+		gicp->setTransformationEpsilon(params_.tf_epsilon);
+		gicp->setMaxCorrespondenceDistance(params_.corr_dist);
+		gicp->setMaximumIterations(params_.iterations);
+		gicp->setRANSACIterations(0);
+		gicp->setMaximumOptimizerIterations(50);
+		gicp->setNumThreads(params_.num_threads);
+		gicp->enableTimingOutput(params_.enable_timing_output);
+		gicp->RecomputeTargetCovariance(recompute_covariance_local_map_);
+		gicp->RecomputeSourceCovariance(recompute_covariance_scan_); // local scan we don't need to
+																	 // recompute
+		gicp->setEuclideanFitnessEpsilon(0.01);
+		LOG(INFO) << "GICP activated.";
+
+		LOG(INFO) << "num threads: " << params_.num_threads;
+
+		LOG(INFO) << "MaxCorrespondenceDistance: " << gicp->getMaxCorrespondenceDistance();
+
+		LOG(INFO) << "MaximumIterations: " << gicp->getMaximumIterations();
+		LOG(INFO) << "TransformationEpsilon: " << gicp->getTransformationEpsilon();
+		LOG(INFO) << "EuclideanFitnessEpsilon: " << gicp->getEuclideanFitnessEpsilon();
+		LOG(INFO) << "RANSACIterations: " << gicp->getRANSACIterations();
+
+		LOG(INFO) << "RotationEpsilon " << gicp->getRotationEpsilon();
+		LOG(INFO) << "CorrespondenceRandomness:" << gicp->getCorrespondenceRandomness();
+		LOG(INFO) << "MaximumOptimizerIterations:" << gicp->getMaximumOptimizerIterations();
+		LOG(INFO) << "ConvergeCriteria:" << gicp->getConvergeCriteria();
+		LOG(INFO) << "RANSACOutlierRejectionThreshold: " << gicp->getRANSACOutlierRejectionThreshold();
+		icp_ = gicp;
+
+		break;
+	}
+	case RegistrationMethod::NDT:
+	{
+		LOG(INFO) << "RegistrationMethod::NDT activated.";
+		pclomp::NormalDistributionsTransform<PointF, PointF>::Ptr ndt_omp =
+			boost::make_shared<
+				pclomp::NormalDistributionsTransform<PointF, PointF>>();
+
+		ndt_omp->setTransformationEpsilon(params_.tf_epsilon);
+		ndt_omp->setMaxCorrespondenceDistance(params_.corr_dist);
+		ndt_omp->setMaximumIterations(params_.iterations);
+		ndt_omp->setRANSACIterations(0);
+		ndt_omp->setNumThreads(params_.num_threads);
+		ndt_omp->enableTimingOutput(params_.enable_timing_output);
+		icp_ = ndt_omp;
+		break;
+	}
+	default:
+		throw std::runtime_error(
+			"No such Registration mode or not implemented yet " + params_.registration_method);
+	}
+
+	return true;
+}
+
+// two clouds only deviates for a small range, otherwise, use motion prior;
+bool PointCloudLocalization::MeasurementUpdate(const PointCloudF::Ptr &query,
+											   const PointCloudF::Ptr &reference, PointCloudF *aligned_query)
+{
+
+	// 1. transformation estimation;
+	if (aligned_query == NULL)
+	{
+		LOG(WARNING) << "... aligned query NULL for [MeasurementUpdate]";
+		return false;
+	}
+
+	timestamp_ = query->header.stamp * 1e3;
+	icp_->setInputSource(query);
+	icp_->setInputTarget(reference);
+	PointCloudF icpAlignedPointsLocalization_;
+	icp_->align(icpAlignedPointsLocalization_);
+
+	const Eigen::Matrix4f T = icp_->getFinalTransformation();
+	//-------------------------------------------------------------------------
+	// 2. transformation modification if needed;
+
+	// rotation from eigen isometry to udf;
+	geometry_utils::Rot3 rot_gu = gu::Rot3(T(0, 0), T(0, 1), T(0, 2), T(1, 0),
+										   T(1, 1), T(1, 2), T(2, 0), T(2, 1), T(2, 2));
+
+	gu::Transform3 pose_update;
+	if (b_is_flat_ground_assumption_)
+	{
+
+		// get euler angle from rotation matrix;
+		double roll, pitch, yaw;
+
+		roll = rot_gu.Roll();
+		pitch = rot_gu.Pitch();
+		yaw = rot_gu.Yaw();
+
+		pose_update.translation = gu::Vec3(T(0, 3), T(1, 3), 0);
+		pose_update.rotation = gu::Rot3(cos(yaw), -sin(yaw), 0, sin(yaw),
+										cos(yaw), 0, 0, 0, 1);
+
+		LOG(INFO) << "... using flat ground assumption!";
+	}
+	else
+	{
+		pose_update.translation = gu::Vec3(T(0, 3), T(1, 3), T(2, 3));
+		pose_update.rotation = rot_gu;
+	}
+	//-------------------------------------------------------------------------
+	// 3. transformation evaluation and assignment to incremental update;
+	if (!transform_thresholding_ || (pose_update.translation.Norm() <= max_translation_ && pose_update.rotation.ToEulerZYX().Norm() <= max_rotation_))
+	{
+
+		incremental_estimate_ = gu::PoseUpdate(incremental_estimate_,
+											   pose_update);
+	}
+	else
+	{
+		LOG(WARNING) << 
+				"Discarding incremental transformation with norm (t: %lf, r: %lf)",
+				pose_update.translation.Norm(),
+				pose_update.rotation.ToEulerZYX().Norm();
+	}
+
+	//-------------------------------------------------------------------------
+	// 4. integrated pose update;
+	integrated_estimate_ = gu::PoseUpdate(integrated_estimate_,
+										  incremental_estimate_);
+	//======================================================================================
+	// get data association correspondences for the transformation;
+	score_registration_ = icp_->getFitnessScore(1.0);
+
+	return true;
+}
+
+bool PointCloudLocalization::MeasurementUpdateRelocalization(
+	const PointCloudF::Ptr &query, const PointCloudF::Ptr &reference,
+	PointCloudF *aligned_query)
+{
+
+	if (aligned_query == NULL)
+	{
+		LOG(WARNING) << "... aligned query NULL for [MeasurementUpdateRelocalization]";
+		return false;
+	}
+
+	// Store time stamp
+	timestamp_ = query->header.stamp * 1e3;
+
+	// cloud registration from query to reference;
+	icp_->setInputSource(query);
+	icp_->setInputTarget(reference);
+
+	// query -> transformed, v.s. ref.;
+	PointCloudF icpAlignedPointsLocalization_;
+	icp_->align(icpAlignedPointsLocalization_);
+
+	// Retrieve transformation and estimate and update
+	const Eigen::Matrix4f T = icp_->getFinalTransformation();
+	//---------------------------------------------------------
+	geometry_utils::Rot3 rot_gu = gu::Rot3(T(0, 0), T(0, 1), T(0, 2), T(1, 0),
+										   T(1, 1), T(1, 2), T(2, 0), T(2, 1), T(2, 2));
+
+	gu::Transform3 pose_update;
+	if (b_is_flat_ground_assumption_)
+	{
+		// get euler angle from rotation matrix;
+		double roll, pitch, yaw;
+		//-----------------------------------------------------------------------------------
+		roll = rot_gu.Roll();
+		pitch = rot_gu.Pitch();
+		yaw = rot_gu.Yaw();
+
+		pose_update.translation = gu::Vec3(T(0, 3), T(1, 3), 0);
+		pose_update.rotation = gu::Rot3(cos(yaw), -sin(yaw), 0, sin(yaw),
+										cos(yaw), 0, 0, 0, 1);
+	}
+	else
+	{
+		pose_update.translation = gu::Vec3(T(0, 3), T(1, 3), T(2, 3));
+		pose_update.rotation = rot_gu;
+	}
+	//---------------------------------------------------------
+	// for global positioning, can update pose without incremental motion constraint;
+	incremental_estimate_ = gu::PoseUpdate(incremental_estimate_, pose_update);
+	//---------------------------------------------------------
+	// current time pose prior is concatenated with history record;
+	integrated_estimate_ = gu::PoseUpdate(integrated_estimate_,
+										  incremental_estimate_);
+
+	//=====================================================================================
+	score_registration_ = icp_->getFitnessScore(1.0);
+	LOG(INFO) << "... during re-localization measurement update: score -> #[" << score_registration_ << "]";
+
+	return true;
+} // end of scope for reloc measurement update;
+
+void PointCloudLocalization::SetFlatGroundAssumptionValue(const bool &value)
+{
+	LOG(INFO) << "PointCloudLocalization - SetFlatGroundAssumptionValue - Received: " << value;
+	b_is_flat_ground_assumption_ = value;
+	if (value)
+		integrated_estimate_.rotation = gu::Rot3(0, 0,
+												 integrated_estimate_.rotation.Yaw());
+}

src/localization_module_lio_locus/src/point_cloud_localization/utils.cc

@@ -0,0 +1,163 @@
+#include <point_cloud_localization/utils.h>
+
+void addNormal(const PointCloudF &cloud,
+		pcl::PointCloud<pcl::PointNormal>::Ptr &cloud_with_normals,
+		const int k_nearest_neighbours) {
+	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
+	cloud_with_normals.reset(new pcl::PointCloud<pcl::PointNormal>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(
+			new pcl::PointCloud<pcl::PointXYZ>);
+	// Convert point cloud XYZI to XYZ
+
+	for (size_t i = 0; i < cloud.size(); i++) {
+		pcl::PointXYZ point;
+		point.x = cloud.points[i].x;
+		point.y = cloud.points[i].y;
+		point.z = cloud.points[i].z;
+		cloud_xyz->points.push_back(point);
+	}
+
+	pcl::search::KdTree<pcl::PointXYZ>::Ptr searchTree(
+			new pcl::search::KdTree<pcl::PointXYZ>);
+	searchTree->setInputCloud(cloud_xyz);
+
+	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> normalEstimator(4); // parallel
+	// TODO: if it's commented out => we don't use it => time to remove ?
+	// pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimator;
+	// not
+	// parallel
+
+	normalEstimator.setInputCloud(cloud_xyz);
+	normalEstimator.setSearchMethod(searchTree);
+	normalEstimator.setKSearch(k_nearest_neighbours);
+	normalEstimator.compute(*normals);
+
+	pcl::concatenateFields(*cloud_xyz, *normals, *cloud_with_normals);
+}
+
+void addNormal(const PointCloudF &cloud, PointCloudF::Ptr &cloud_with_normals,
+		const int k_nearest_neighbours) {
+	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz(
+			new pcl::PointCloud<pcl::PointXYZ>);
+	// Convert point cloud XYZI to XYZ
+
+	for (size_t i = 0; i < cloud.size(); i++) {
+		pcl::PointXYZ point;
+		point.x = cloud.points[i].x;
+		point.y = cloud.points[i].y;
+		point.z = cloud.points[i].z;
+		cloud_xyz->points.push_back(point);
+	}
+
+	pcl::search::KdTree<pcl::PointXYZ>::Ptr searchTree(
+			new pcl::search::KdTree<pcl::PointXYZ>);
+	searchTree->setInputCloud(cloud_xyz);
+
+	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> normalEstimator(4); // parallel
+	// TODO: if it's commented out => we don't use it => time to remove ?
+	// pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimator;
+	// not
+	// parallel
+
+	normalEstimator.setInputCloud(cloud_xyz);
+	normalEstimator.setSearchMethod(searchTree);
+	normalEstimator.setKSearch(k_nearest_neighbours);
+	normalEstimator.compute(*normals);
+
+	for (size_t i = 0; i < cloud.size(); i++) {
+		pcl::PointXYZ point;
+		cloud_with_normals->points[i].normal_x = normals->points[i].normal_x;
+		cloud_with_normals->points[i].normal_y = normals->points[i].normal_y;
+		cloud_with_normals->points[i].normal_z = normals->points[i].normal_z;
+	}
+}
+
+// returns a point cloud whose centroid is the origin, and that the mean of
+// the distances to the origin is 1
+
+// get the mean of the cloud;
+
+void normalizePCloud(const pcl::PointCloud<pcl::PointXYZI> &cloud,
+		pcl::PointCloud<pcl::PointXYZI>::Ptr pclptr_normalized) {
+
+	// 1. centroid of the cloud;
+	Eigen::Vector4f centroid_4d;
+	pcl::compute3DCentroid(cloud, centroid_4d);
+	Eigen::Vector3f centroid(centroid_4d.x(), centroid_4d.y(), centroid_4d.z());
+
+	// 2. add up all distance of point to centroid;
+	float dist = 0;
+	for (pcl::PointCloud<pcl::PointXYZI>::const_iterator it =
+			cloud.points.begin(); it != cloud.points.end(); it++) {
+		Eigen::Vector3f a_i(it->x, it->y, it->z);
+		dist = dist + (a_i - centroid).norm();
+	}
+	float factor = cloud.points.size() / dist;
+	Eigen::Matrix4f transform;
+	transform = Eigen::Matrix4f::Identity();
+
+	// centroid *1/(summation of distance/N)
+
+	// factor = 1.0/(sum of distance / N )
+	// factor = 1.0 / mean distance;
+	transform.block(0, 0, 3, 3) = factor * Eigen::Matrix3f::Identity();
+	transform.block(0, 3, 4, 1) = -factor * centroid_4d;
+
+	// [R,t] * p = (R*p + t) * factor;
+	// [I, -centroid] * p = (p - centroid)* factor;
+	// (P- centroid)/mean distance to centroid;
+	pcl::transformPointCloud(cloud, *pclptr_normalized, transform);
+	pcl::compute3DCentroid(*pclptr_normalized, centroid_4d);
+
+	// checkIfPCloudIsNormalized(pclptr_normalized);  //
+}
+
+void normalizePCloud(const PointCloudF &cloud,
+		PointCloudF::Ptr pclptr_normalized) {
+	Eigen::Vector4f centroid_4d;
+	pcl::compute3DCentroid(cloud, centroid_4d);
+	Eigen::Vector3f centroid(centroid_4d.x(), centroid_4d.y(), centroid_4d.z());
+
+	float dist = 0;
+	for (PointCloudF::const_iterator it = cloud.points.begin();
+			it != cloud.points.end(); it++) {
+		Eigen::Vector3f a_i(it->x, it->y, it->z);
+		dist = dist + (a_i - centroid).norm();
+	}
+	float factor = cloud.points.size() / dist;
+	Eigen::Matrix4f transform;
+	transform = Eigen::Matrix4f::Identity();
+	transform.block(0, 0, 3, 3) = factor * Eigen::Matrix3f::Identity();
+	transform.block(0, 3, 4, 1) = -factor * centroid_4d;
+	pcl::transformPointCloud(cloud, *pclptr_normalized, transform);
+	pcl::compute3DCentroid(*pclptr_normalized, centroid_4d);
+
+	// checkIfPCloudIsNormalized(pclptr_normalized);  //
+}
+
+void doEigenDecomp6x6(const Eigen::Matrix<double, 6, 6> &data,
+		Eigen::Matrix<double, 6, 1> &eigenvalues,
+		Eigen::Matrix<double, 6, 6> &eigenvectors) {
+	Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 6, 6>> eigensolver(
+			data);
+	if (eigensolver.info() == Eigen::Success) {
+		eigenvalues = eigensolver.eigenvalues();
+		eigenvectors = eigensolver.eigenvectors(); // the eigenvectors are the columns
+	} else {
+		std::cout << "Eigen failed to do the eigendecomp" << std::endl;
+	}
+}
+
+void doEigenDecomp3x3(const Eigen::Matrix<double, 3, 3> &data,
+		Eigen::Matrix<double, 3, 1> &eigenvalues,
+		Eigen::Matrix<double, 3, 3> &eigenvectors) {
+	Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 3, 3>> eigensolver(
+			data);
+	if (eigensolver.info() == Eigen::Success) {
+		eigenvalues = eigensolver.eigenvalues();
+		eigenvectors = eigensolver.eigenvectors(); // the eigenvectors are the columns
+	} else {
+		std::cout << "Eigen failed to do the eigendecomp" << std::endl;
+	}
+}

src/localization_module_lio_locus/src/point_cloud_mapper/CMakeLists.txt

@@ -0,0 +1,27 @@
+
+add_library(point_cloud_mapper SHARED
+    PointCloudIkdTreeMapper.cc
+    ikd_tree/ikd_Tree.cpp
+)
+
+
+target_include_directories(point_cloud_mapper PUBLIC
+	#	${PCL_INCLUDE_DIRS}
+	/home/demo/deps_install/include/pcl-1.10
+	)
+
+target_link_libraries(point_cloud_mapper
+  ${PCL_LIBRARIRES}
+  ${YAML_CPP_LIBRARIES}
+  glog
+  OpenMP::OpenMP_CXX
+)
+
+
+install(TARGETS point_cloud_mapper
+  DESTINATION lib
+#  DESTINATION lib/${PROJECT_NAME}
+)
+
+
+

src/localization_module_lio_locus/src/point_cloud_mapper/PointCloudIkdTreeMapper.cc

@@ -0,0 +1,148 @@
+#include <omp.h>
+#include <pcl/octree/octree_iterator.h>
+#include <pcl/search/impl/search.hpp>
+#include <point_cloud_mapper/PointCloudIkdTreeMapper.h>
+
+#include <boost/filesystem.hpp>
+#include <yaml-cpp/yaml.h>
+//#include <rclcpp/logging.hpp>
+
+PointCloudIkdTreeMapper::PointCloudIkdTreeMapper() :
+		initialized_(false) {
+
+	map_data_.reset(new PointCloudF);
+
+	google::SetStderrLogging(google::GLOG_INFO);
+	FLAGS_colorlogtostderr = true;
+	FLAGS_alsologtostderr = true;	
+}
+
+PointCloudIkdTreeMapper::~PointCloudIkdTreeMapper() {
+}
+
+bool PointCloudIkdTreeMapper::Initialize(std::string path_to_params) {
+
+	if (!loadParametersFromFile(path_to_params))
+		return false;
+	ikdtree.set_downsample_param(octree_resolution_);
+
+	// mp_cloud_mapper->SetupNumberThreads(6);
+
+
+	initialized_ = true;
+	return true;
+}
+
+bool PointCloudIkdTreeMapper::loadParametersFromFile(std::string path_to_file) {
+
+	if (!boost::filesystem::exists(path_to_file)) {
+		LOG(INFO) << "file NOT exists for parameter loading!\n";
+		return false;
+	} else {
+		LOG(INFO) <<
+				"... reading parameter for mapper from [" << path_to_file << "]!";
+
+	}
+
+	YAML::Node yaml_node = YAML::LoadFile(path_to_file);
+	octree_resolution_ = yaml_node["map"]["octree_resolution"].as<double>();
+	fixed_frame_id_ = yaml_node["map"]["frame_id_fixed"].as<std::string>();
+	num_threads_ = yaml_node["map"]["num_threads"].as<int>();
+
+	SetupNumberThreads(num_threads_);	
+
+	LOG(INFO) << "# octree_resolution -> [" << octree_resolution_ << "]";
+	LOG(INFO) << "# fixed_frame_id -> [" << fixed_frame_id_ << "]"; 
+
+	return true;
+}
+
+void PointCloudIkdTreeMapper::Reset() {
+
+	map_data_.reset(new PointCloudF);
+
+	initialized_ = true;
+}
+
+bool PointCloudIkdTreeMapper::InsertPoints(const PointCloudF::ConstPtr &points,
+		PointCloudF *incremental_points) {
+	if (!initialized_) {
+		return false;
+	}
+
+	if (incremental_points == NULL) {
+		incremental_points = new PointCloudF();
+	}
+	incremental_points->clear();
+
+	if (map_mutex_.try_lock()) {
+		if (ikdtree.size() == 0) {
+
+			ikdtree.Build(*points);
+		} else {
+			PointVector pc_append(*points);
+			ikdtree.Add_Points(pc_append, false);
+		}
+		map_mutex_.unlock();
+	} else {
+		// This won't happen often.
+		LOG(WARNING) <<
+			"... insertion points to cloud map failed for iKD-tree!";
+	}
+
+	*incremental_points = *points;
+
+	// Publish the incremental map update.
+	incremental_points->header = points->header;
+	incremental_points->header.frame_id = fixed_frame_id_;
+
+	return true;
+}
+
+bool PointCloudIkdTreeMapper::ApproxNearestNeighbors(const PointCloudF &points,
+		PointCloudF *neighbors) {
+	if (!initialized_) {
+		
+		LOG(WARNING) <<"... not init yet for point cloud iKD-tree!";
+		return false;
+	}
+
+	if (neighbors == NULL) {
+		neighbors = new PointCloudF();
+	}
+
+	neighbors->points.clear();
+	neighbors->resize(points.points.size());
+	PointVector search_result;
+
+	omp_set_num_threads (number_threads_);
+#pragma omp parallel for schedule(dynamic, 1)
+	for (size_t ii = 0; ii < points.points.size(); ++ii) {
+		PointVector search_result;
+		vector<float> PointDist;
+		ikdtree.Nearest_Search(points[ii], 1, search_result, PointDist);
+
+		neighbors->points[ii] = search_result[0];
+	}
+
+	return neighbors->points.size() > 0;
+}
+
+PointCloudF::Ptr PointCloudIkdTreeMapper::getCloudMapInMem() {
+
+	PointCloudF::Ptr cloud_map(new PointCloudF());
+
+	map_mutex_.lock();
+	PointVector storage;
+	storage.header.frame_id = fixed_frame_id_;
+	ikdtree.flatten(ikdtree.Root_Node, storage, NOT_RECORD);
+	map_mutex_.unlock();
+
+	*cloud_map = storage;
+
+	return cloud_map;
+
+}
+
+void PointCloudIkdTreeMapper::SetBoxFilterSize(const int box_filter_size) {
+}

src/localization_module_lio_locus/src/point_cloud_odometry/CMakeLists.txt

@@ -0,0 +1,26 @@
+
+add_library(point_cloud_odometry SHARED PointCloudOdometry.cc)
+
+#target_link_directories(point_cloud_odometry
+#	/home/demo/deps_install/lib
+#    )
+
+target_link_libraries(point_cloud_odometry
+    glog
+    #    ${PCL_LIBRARIES}
+        pcl_octree
+    pcl_filters
+    pcl_search
+    pcl_common
+    pcl_sample_consensus
+    ${YAML_CPP_LIBRARIES}
+)
+
+
+install(TARGETS point_cloud_odometry
+  DESTINATION lib
+)
+
+
+
+

src/localization_module_lio_locus/src/point_cloud_odometry/PointCloudOdometry.cc

@@ -0,0 +1,307 @@
+/*
+ Authors:
+ - Matteo Palieri    (matteo.palieri@jpl.nasa.gov)
+ - Benjamin Morrell  (benjamin.morrell@jpl.nasa.gov)
+ - Andrzej Reinke    (andrzej.m.reinke@jpl.nasa.gov)
+ */
+
+#include <point_cloud_odometry/PointCloudOdometry.h>
+#include <yaml-cpp/yaml.h>
+
+namespace gu = geometry_utils;
+using pcl::copyPointCloud;
+using namespace point_cloud_odometry;
+
+PointCloudOdometry::PointCloudOdometry() :
+		initialized_(false), b_use_imu_integration_(false) {
+
+	query_.reset(new PointCloudF);
+	reference_.reset(new PointCloudF);
+	query_trans_.reset(new PointCloudF);
+	query_trans_->reserve(50000);
+	query_trans_->clear();
+
+	google::SetStderrLogging(google::GLOG_INFO);
+	FLAGS_colorlogtostderr = true;
+	FLAGS_alsologtostderr = true;	
+}
+
+PointCloudOdometry::~PointCloudOdometry() {
+}
+
+bool PointCloudOdometry::Initialize(std::string path_to_file) {
+
+	if (!loadParametersFromFile(path_to_file))
+		return false;
+
+	if (!SetupICP()) {
+		LOG(WARNING) << "Failed to SetupICP";
+	}
+
+	return true;
+}
+
+void PointCloudOdometry::applyParameters() {
+
+	double init_x = 0.0, init_y = 0.0, init_z = 0.0;
+	double init_qx = 0.0, init_qy = 0.0, init_qz = 0.0, init_qw = 1.0;
+	bool b_have_fiducial = true;
+
+	double init_roll = 0.0, init_pitch = 0.0, init_yaw = 0.0;
+	gu::Quat q(gu::Quat(init_qw, init_qx, init_qy, init_qz));
+	gu::Rot3 R;
+	R = gu::QuatToR(q);
+	init_roll = R.Roll();
+	init_pitch = R.Pitch();
+	init_yaw = R.Yaw();
+
+	integrated_estimate_.translation = gu::Vec3(init_x, init_y, init_z);
+	integrated_estimate_.rotation = gu::Rot3(init_roll, init_pitch, init_yaw);
+
+	if (b_is_flat_ground_assumption_) {
+		integrated_estimate_.rotation = gu::Rot3(0, 0,
+				integrated_estimate_.rotation.Yaw());
+		}
+
+	if (!b_have_fiducial) {
+		LOG(WARNING) << "Can't find fiducials, using origin";
+
+	} else {
+
+
+	}
+
+}
+
+bool PointCloudOdometry::loadParametersFromFile(std::string path_to_file) {
+
+	// if (!boost::filesystem::exists(path_to_file)) {
+	// 	ROS_DEBUG_STREAM(path_to_file);
+	// 	ROS_DEBUG_STREAM("file NOT exists for parameter loading!\n");
+	// 	return false;
+	// } else {
+	// 	ROS_INFO_STREAM(
+	// 			"... reading parameter for lidar odometry from [" << path_to_file << "]!");
+
+	// }
+
+	YAML::Node yaml_node = YAML::LoadFile(path_to_file);
+	b_is_flat_ground_assumption_ = yaml_node["b_is_flat_ground_assumption"].as<
+			bool>();
+
+	auto node_icp = yaml_node["icp"];
+
+	params_.registration_method =
+			node_icp["registration_method"].as<std::string>();
+	params_.num_threads = node_icp["num_threads"].as<int>();
+	
+	params_.icp_corr_dist = node_icp["corr_dist"].as<double>();
+	params_.icp_iterations = node_icp["iterations"].as<unsigned int>();
+	params_.icp_tf_epsilon = node_icp["tf_epsilon"].as<double>();
+
+	params_.enable_timing_output = node_icp["enable_timing_output"].as<bool>();
+	recompute_covariances_ = node_icp["recompute_covariances"].as<bool>();
+
+	transform_thresholding_ = node_icp["transform_thresholding"].as<bool>();
+	max_translation_ = node_icp["max_translation"].as<double>();
+	max_rotation_ = node_icp["max_rotation"].as<double>();
+
+
+	return true;
+}
+
+//------------------------------------------------------------------
+
+bool PointCloudOdometry::SetupICP() {
+
+	LOG(INFO) << "####################################################";
+	LOG(INFO) << "           PointCloudOdometry - SetupICP            ";
+	LOG(INFO) << "####################################################";
+
+	RegistrationMethod method;
+
+	if(params_.registration_method == "gicp")
+		method = RegistrationMethod::GICP;
+	else if (params_.registration_method == "ndt")
+		method = RegistrationMethod::NDT;
+
+	switch (method) {
+	case RegistrationMethod::GICP: {
+		LOG(INFO) << "RegistrationMethod::GICP activated.";
+		pcl::MultithreadedGeneralizedIterativeClosestPoint<PointF, PointF>::Ptr gicp =
+				boost::make_shared<
+						pcl::MultithreadedGeneralizedIterativeClosestPoint<
+								PointF, PointF> >();
+
+		gicp->setMaxCorrespondenceDistance(params_.icp_corr_dist); // for data association;
+
+		gicp->setTransformationEpsilon(params_.icp_tf_epsilon); // for termination criteria in term of transformation deviation;
+		gicp->setEuclideanFitnessEpsilon(0.005); // for termination criteria in term of euclidean distance;
+		gicp->setMaximumIterations(params_.icp_iterations); // for termination criteria in term of iterations number;
+
+		gicp->setRANSACIterations(0);
+		gicp->setNumThreads(params_.num_threads);
+		gicp->enableTimingOutput(params_.enable_timing_output);
+		gicp->RecomputeTargetCovariance(recompute_covariances_);
+		gicp->RecomputeSourceCovariance(recompute_covariances_);
+
+		LOG(INFO) << "num_threads" << params_.num_threads;
+		LOG(INFO) << "getMaxCorrespondenceDistance: " << gicp->getMaxCorrespondenceDistance();
+		LOG(INFO) << "getMaximumIterations: " << gicp->getMaximumIterations();
+		LOG(INFO) << "getTransformationEpsilon: " << gicp->getTransformationEpsilon();
+
+		LOG(INFO) << "getEuclideanFitnessEpsilon: " << gicp->getEuclideanFitnessEpsilon();
+
+		LOG(INFO) << "Ransac: " << gicp->getRANSACIterations();
+
+		LOG(INFO) << "getRANSACIterations: " << gicp->getRANSACIterations();
+		LOG(INFO) << " ROTATION EPSILION: " << gicp->getRotationEpsilon();
+		LOG(INFO) << "getCorrespondenceRandomness:" << gicp->getCorrespondenceRandomness();
+		LOG(INFO) << "getMaximumOptimizerIterations:" << gicp->getMaximumOptimizerIterations();
+		LOG(INFO) << "getConvergeCriteria:" << gicp->getConvergeCriteria();
+		LOG(INFO) << "RANSACOutlie: " << gicp->getRANSACOutlierRejectionThreshold();
+		LOG(INFO) << "CLASS NAME: " << gicp->getClassName();
+
+		icp_ = gicp;
+		break;
+	}
+	case RegistrationMethod::NDT: {
+		LOG(INFO) << "RegistrationMethod::NDT activated.";
+		pclomp::NormalDistributionsTransform<PointF, PointF>::Ptr ndt_omp =
+				boost::make_shared<
+						pclomp::NormalDistributionsTransform<PointF, PointF>>();
+
+		ndt_omp->setTransformationEpsilon(params_.icp_tf_epsilon);
+		ndt_omp->setMaxCorrespondenceDistance(params_.icp_corr_dist);
+		ndt_omp->setMaximumIterations(params_.icp_iterations);
+		ndt_omp->setRANSACIterations(0);
+		ndt_omp->setNumThreads(params_.num_threads);
+		ndt_omp->enableTimingOutput(params_.enable_timing_output);
+		icp_ = ndt_omp;
+		break;
+	}
+
+	default:
+		throw std::runtime_error(
+				"No such Registration mode or not implemented yet "
+						+ params_.registration_method);
+	}
+	return true;
+}
+
+void PointCloudOdometry::EnableImuIntegration() {
+	b_use_imu_integration_ = true;
+}
+
+void PointCloudOdometry::DisableSensorIntegration() {
+	b_use_imu_integration_ = false;
+}
+
+bool PointCloudOdometry::SetLidar(const PointCloudF &points) {
+	points_ = points;
+	return true;
+}
+
+bool PointCloudOdometry::SetImuDelta(const Eigen::Matrix3d &imu_delta) {
+	imu_delta_ = imu_delta;
+	return true;
+}
+
+bool PointCloudOdometry::UpdateEstimate() {
+
+	if (!initialized_) {
+		copyPointCloud(points_, *query_);
+		initialized_ = true;
+		return false;
+	} else {
+		copyPointCloud(*query_, *reference_);
+		copyPointCloud(points_, *query_);
+		return UpdateICP();
+	}
+}
+
+// note:pior used here for matching come from prior transfered;
+bool PointCloudOdometry::UpdateICP() {
+	query_trans_->clear();
+
+	// use IMU delta to update prior value;
+	if (b_use_imu_integration_) {
+		imu_prior_ << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
+		imu_prior_.block(0, 0, 3, 3) = imu_delta_;
+		pcl::transformPointCloud(*query_, *query_trans_, imu_prior_);
+	} else {
+		*query_trans_ = *query_;
+	}
+
+	icp_->setInputSource(query_trans_);
+	icp_->setInputTarget(reference_);
+	icp_->align(icpAlignedPointsOdometry_);
+	Eigen::Matrix4d T;
+	T = icp_->getFinalTransformation().cast<double>();
+
+	if (b_use_imu_integration_) {
+		T = T * imu_prior_;
+	}
+
+	geometry_utils::Rot3 rot_gu = gu::Rot3(T(0, 0), T(0, 1), T(0, 2), T(1, 0),
+			T(1, 1), T(1, 2), T(2, 0), T(2, 1), T(2, 2));
+
+	if (b_is_flat_ground_assumption_) {
+		double roll, pitch, yaw;
+		roll = rot_gu.Roll();
+		pitch = rot_gu.Pitch();
+		yaw = rot_gu.Yaw();
+
+		LOG(INFO) << "... using flat ground assumption in cloud odometry!";
+
+		incremental_estimate_.translation = gu::Vec3(T(0, 3), T(1, 3), 0);
+		incremental_estimate_.rotation = gu::Rot3(cos(yaw), -sin(yaw), 0,
+				sin(yaw), cos(yaw), 0, 0, 0, 1);
+	} else {
+		incremental_estimate_.translation = gu::Vec3(T(0, 3), T(1, 3), T(2, 3));
+		incremental_estimate_.rotation = rot_gu;
+	}
+
+	if (!transform_thresholding_
+			|| (incremental_estimate_.translation.Norm() <= max_translation_
+					&& incremental_estimate_.rotation.ToEulerZYX().Norm()
+							<= max_rotation_)) {
+		integrated_estimate_ = gu::PoseUpdate(integrated_estimate_,
+				incremental_estimate_);
+	} else {
+		LOG(WARNING) <<"%s: Discarding incremental transformation with norm (t: %lf, "
+				"r: %lf)", name_.c_str(),
+				incremental_estimate_.translation.Norm(),
+				incremental_estimate_.rotation.ToEulerZYX().Norm();
+	}
+
+	return true;
+}
+
+void PointCloudOdometry::SetFlatGroundAssumptionValue(const bool &value) {
+	LOG(INFO) << 
+			"PointCloudOdometry - SetFlatGroundAssumptionValue - Received: " << value;
+	b_is_flat_ground_assumption_ = value;
+	if (value)
+		integrated_estimate_.rotation = gu::Rot3(0, 0,
+				integrated_estimate_.rotation.Yaw());
+}
+
+const gu::Transform3& PointCloudOdometry::GetIncrementalEstimate() const {
+	return incremental_estimate_;
+}
+
+const gu::Transform3& PointCloudOdometry::GetIntegratedEstimate() const {
+	return integrated_estimate_;
+}
+
+bool PointCloudOdometry::GetLastPointCloud(PointCloudF::Ptr &out) const {
+	if (!initialized_ || query_ == NULL) {
+		
+		LOG(WARNING) <<"%s: Not initialized.", name_.c_str();
+		return false;
+	}
+	out = query_;
+	return true;
+}
+