run on docker ok

src/localization_module_lio_locus/CMakeLists.txt

@@ -1,4 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
+cmake_policy(SET CMP0074 NEW)
 project(lio_locus_humble)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
@@ -26,13 +27,19 @@ find_package(OpenMP)
 find_package(Eigen3)
 
 
+# set(CMAKE_PREFIX_PATH "/home/slam10/hivecore_robot_slam/deps_install" ${CMAKE_PREFIX_PATH})
+# set(CMAKE_INCLUDE_PATH "/home/slam10/hivecore_robot_slam/deps_install/include" ${CMAKE_INCLUDE_PATH})
+# set(CMAKE_LIBRARY_PATH "/home/slam10/hivecore_robot_slam/deps_install/lib" ${CMAKE_LIBRARY_PATH})
 
+# set(PCL_DIR /home/slam10/hivecore_robot_slam/deps_install/share/pcl-1.10)
 
-find_package(PCL 1.12 REQUIRED)
+# set(PCL_ROOT /home/slam10/hivecore_robot_slam/deps_install)
+
+find_package(PCL 1.10 REQUIRED)
 
 message("##################" ${PCL_INCLUDE_DIRS})
 message("##################" ${PCL_LIBRARIES})
-message("---------------------------"${PCL_INCLUDE_DIRS})
+message("---------------------------" ${PCL_INCLUDE_DIRS})
 
 if (OPENMP_FOUND)
     set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
@@ -40,13 +47,6 @@ if (OPENMP_FOUND)
     set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
 endif()
 
-set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--disable-new-dtags")
-set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} -Wl,--disable-new-dtags")
-set(CMAKE_SKIP_BUILD_RPATH  FALSE)
-set(CMAKE_BUILD_WITH_INSTALL_RPATH TRUE)
-set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
-set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
-
 include_directories(
     include/${PROJECT_NAME}
     ${pcl_conversions_INCLUDE_DIRS}
@@ -54,8 +54,12 @@ include_directories(
     ${PCL_INCLUDE_DIRS}
     ${YAML_CPP_INCLUDE_DIR}
     /user/include
+    # /home/slam10/hivecore_robot_slam/deps_install/include/pcl-1.10
     )
 
+# link_directories(
+#         /home/slam10/hivecore_robot_slam/deps_install/lib
+# )
 
 
 

src/localization_module_lio_locus/apps/nodelets/CMakeLists.txt

@@ -24,7 +24,7 @@ ament_target_dependencies(
   "pcl_conversions"
   "std_srvs"
   "tf2"
-  #"tf2_ros"
+  "tf2_ros"
 )
 
 # install exe to folder within lib;

src/localization_module_lio_locus/apps/nodelets/cloud_preprocessing_node.cpp

@@ -85,6 +85,7 @@ void callbackLidarRaw(const sensor_msgs::msg::PointCloud2 &msg)
 	pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_raw(
 		new pcl::PointCloud<pcl::PointXYZINormal>());
 
+	params_cloud_preproc.topic_cloud_raw = "/livox/lidar";
 	if(params_cloud_preproc.topic_cloud_raw == "/rsm1_1")
 	{
 		cloud_raw = CloudPreProcessor::adaptCloudForm(msg);

src/localization_module_lio_locus/apps/nodelets/imu_preprocessing_node.cpp

@@ -49,14 +49,14 @@ double orientation_variance_;
 bool reverse_tf_;
 tf2::Quaternion hamiltonToTFQuaternion(double q0, double q1, double q2,
                                        double q3);
-std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;                                       
+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_ = std::shared_ptr<tf2_ros::TransformBroadcaster>(new tf2_ros::TransformBroadcaster(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");

src/localization_module_lio_locus/apps/nodelets/lio_locus_node.cpp

@@ -30,6 +30,7 @@ LioLocus::Ptr ptr_lio_locus;
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/vector3_stamped.hpp>
 #include <tf2/LinearMath/Quaternion.h>
+#include <tf2_ros/transform_broadcaster.h>
 
 using std::string;
 
@@ -48,6 +49,7 @@ 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_;
+rclcpp::Publisher<geometry_msgs::msg::Vector3Stamped>::SharedPtr pub_reliability;
 void publishRobotPose();
 void publishLocalizedCloud();
 void publishCloudMap(rclcpp::Time ts);
@@ -79,6 +81,8 @@ void serviceInvoking(const std::shared_ptr<std_srvs::srv::Empty::Request> reques
     RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "################## service invoked: !!! ");
 }
 
+std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
 int main(int argc, char **argv)
 {
 
@@ -93,6 +97,9 @@ int main(int argc, char **argv)
 
     node = std::make_shared<rclcpp::Node>("lio_locus_node");
 
+    tf_broadcaster_ =
+      std::make_unique<tf2_ros::TransformBroadcaster>(*node);
+
     // algorithm initialization stuff;
     flag_init_pose_set = false;
 
@@ -126,7 +133,7 @@ int main(int argc, char **argv)
     // 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);
+        ptr_lio_locus->uploadPriorCloudMap(params_lio_locus.fn_cloud_map, params_lio_locus.leaf_size_cloud_map);
 
         // set initial pose;
         geometry_utils::Transform3 pose_prior = geometry_utils::Transform3::Identity();
@@ -163,6 +170,7 @@ int main(int argc, char **argv)
         node->create_publisher<sensor_msgs::msg::PointCloud2>("/cloud_map", 9);
 
     dt_publisher_ = node->create_publisher<geometry_msgs::msg::Vector3Stamped>("/dt_lio_locus", 9);
+    pub_reliability = node->create_publisher<geometry_msgs::msg::Vector3Stamped>("/reliablity_loc", 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);
@@ -304,40 +312,6 @@ void publishCloudMap(rclcpp::Time ts)
     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);
 
@@ -379,8 +353,46 @@ void publishRobotPose()
 
     msg_pose.header.stamp = time_now;
     msg_pose.header.frame_id = params_lio_locus.frame_id_world;
+    msg_pose.child_frame_id = params_lio_locus.frame_id_ego;
     pub_pose->publish(msg_pose);
 
+	// reliablity value;
+	lio_locus::ReliablityStamped stamped_reliablity =
+			ptr_lio_locus->getStampedReliablityValue();
+
+	geometry_msgs::msg::Vector3Stamped msg_reliablity_ts;
+	msg_reliablity_ts.vector.x = stamped_reliablity.reliability;
+	msg_reliablity_ts.header.stamp = time_now;
+
+	pub_reliability->publish(msg_reliablity_ts);
+
+    //--------------------------------------------------------------------------
+    geometry_msgs::msg::TransformStamped t;
+
+    // Read message content and assign it to
+    // corresponding tf variables
+    t.header.stamp = time_now;
+    t.header.frame_id = params_lio_locus.frame_id_world;
+    t.child_frame_id = params_lio_locus.frame_id_ego;
+
+    t.transform.translation.x = p.x;
+    t.transform.translation.y = p.y;
+    t.transform.translation.z = p.z;
+
+    t.transform.rotation.x = q_tf2.x();
+    t.transform.rotation.y = q_tf2.y();
+    t.transform.rotation.z = q_tf2.z();
+    t.transform.rotation.w = q_tf2.w();
+
+    // Send the transformation
+    tf_broadcaster_->sendTransform(t);
+
+    //--------------------------------------------------------------------------
+
+
+
+
+
     bool flag_record_traj = false;
     if (!params_lio_locus.flag_use_prior_map)
     {

src/localization_module_lio_locus/config/parameters_lio_locus.yaml

@@ -12,7 +12,7 @@ ros2_wrapper:
     flag_use_scan2scan_matching: true
 
     flag_use_prior_map: false 
-    path_cloud_map: /home/solomon/cloud_map
+    path_cloud_map: /home/slam10/cloud_map
     fn_cloud_map: cloud_map.pcd
     leaf_size_load_map: 0.35
 
@@ -20,12 +20,12 @@ ros2_wrapper:
     leaf_size_vis_map: 0.2 
 
     flag_save_map_in_memory: true 
-    path_save_map: /home/demo/cloud_map
+    path_save_map: /home/slam10/cloud_map_bkup
     fn_save_cloud_map: cloud_map.pcd
     flag_voxel_filtering: true 
     leaf_size: 0.1
 
-    path_log: no_log
+    path_log: /home/slam10/log_dt
 
     threshold_trans_map: 0.5
     threshold_rot_map: 0.35 
@@ -54,7 +54,7 @@ ros2_wrapper:
 
 
 cloud_preprocessing:
-    leaf_voxel_filtering: 0.5
+    leaf_voxel_filtering: 0.2
     filter_limit: 120
 
     size_cores: 6
@@ -67,6 +67,7 @@ imu_complementary_filtering:
     do_bias_estimation: true
     do_adaptive_gain: true
     bias_alpha: 0.01
+    reverse_tf: true
     frame_fixed_: fixed_imu
     flag_publish_tf: false
     orientation_stddev: 0.0

src/localization_module_lio_locus/config/parameters_lio_locus_reloc.yaml

@@ -12,20 +12,20 @@ ros2_wrapper:
     flag_use_scan2scan_matching: true
 
     flag_use_prior_map: true 
-    path_cloud_map: /home/demo/cloud_map
-    fn_cloud_map: cloud_map.pcd
+    path_cloud_map: /home/slam10/cloud_map_bkup
+    fn_cloud_map: cloud_map_test.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
+    path_save_map: /home/slam10/cloud_map_bkup
     fn_save_cloud_map: cloud_map.pcd
     flag_voxel_filtering: true 
     leaf_size: 0.1
 
-    path_log: no_log
+    path_log: /home/slam10/log_dt
 
     threshold_trans_map: 0.5
     threshold_rot_map: 0.35 
@@ -67,6 +67,7 @@ imu_complementary_filtering:
     do_bias_estimation: true
     do_adaptive_gain: true
     bias_alpha: 0.01
+    reverse_tf: true
     frame_fixed_: fixed_imu
     flag_publish_tf: false
     orientation_stddev: 0.0

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

@@ -3,6 +3,7 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
+
 #include <vector>
 
 typedef pcl::PointXYZINormal PointF;
@@ -12,11 +13,11 @@ typedef pcl::PointCloud<PointF>::Ptr pclPtr;
 
 typedef std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>>
     MatricesVector;
-typedef std::shared_ptr<MatricesVector> MatricesVectorPtr;
+typedef boost::shared_ptr<MatricesVector> MatricesVectorPtr;
 
 typedef std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f>>
     MatricesVectorf;
-typedef std::shared_ptr<MatricesVectorf> MatricesVectorfPtr;
+typedef boost::shared_ptr<MatricesVectorf> MatricesVectorfPtr;
 
 template <typename T>
 using MatricesVectorT =
@@ -24,4 +25,4 @@ using MatricesVectorT =
                 Eigen::aligned_allocator<Eigen::Matrix<T, 3, 3>>>;
 
 template <typename T>
-using MatricesVectorTPtr = std::shared_ptr<MatricesVectorT<T>>;
+using MatricesVectorTPtr = boost::shared_ptr<MatricesVectorT<T>>;

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

@@ -10,8 +10,8 @@ namespace geometry_utils
 template <typename T, size_t N, size_t M>
 struct MatrixNxMBase
 {
-  typedef typename std::shared_ptr<MatrixNxMBase<T, N, M>> Ptr;
-  typedef typename std::shared_ptr<const MatrixNxMBase<T, N, M>> ConstPtr;
+  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;

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

@@ -9,8 +9,8 @@ namespace geometry_utils
 template <typename T>
 struct Transform2Base
 {
-  typedef std::shared_ptr<Transform2Base> Ptr;
-  typedef std::shared_ptr<const Transform2Base> ConstPtr;
+  typedef boost::shared_ptr<Transform2Base> Ptr;
+  typedef boost::shared_ptr<const Transform2Base> ConstPtr;
 
   Vector2Base<T> translation;
   Rotation2Base<T> rotation;

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

@@ -10,8 +10,8 @@ namespace geometry_utils
 template <typename T>
 struct Transform3Base
 {
-  typedef std::shared_ptr<Transform3Base> Ptr;
-  typedef std::shared_ptr<const Transform3Base> ConstPtr;
+  typedef boost::shared_ptr<Transform3Base> Ptr;
+  typedef boost::shared_ptr<const Transform3Base> ConstPtr;
 
   Vector3Base<T> translation;
   Rotation3Base<T> rotation;

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

@@ -13,8 +13,8 @@ namespace geometry_utils
 template <typename T, size_t N>
 struct VectorNBase
 {
-  typedef typename std::shared_ptr<VectorNBase<T, N>> Ptr;
-  typedef typename std::shared_ptr<const VectorNBase<T, N>> ConstPtr;
+  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;
 

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

@@ -65,7 +65,7 @@ class CloudPreProcessor {
 public:
 	CloudPreProcessor();
 	virtual ~CloudPreProcessor();
-	typedef std::shared_ptr<CloudPreProcessor> Ptr;
+	typedef boost::shared_ptr<CloudPreProcessor> Ptr;
 
 	// convert ROS cloud message to PCL cloud format;
 	static pcl::PointCloud<pcl::PointXYZINormal>::Ptr adaptCloudForm(

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

@@ -38,11 +38,11 @@ using point_cloud_odometry::PointCloudOdometry;
 using point_cloud_localization::PointCloudLocalization;
 
 typedef pcl::PointCloud<pcl::PointXYZINormal> PointCloudType;
-typedef std::shared_ptr<PointCloudOdometry> PointCloudOdometryPtr;
-typedef std::shared_ptr<PointCloudLocalization> PointCloudLocalizationPtr;
-typedef std::shared_ptr<PointCloudIkdTreeMapper> PointCloudIkdTreeMapperPtr;
-// typedef std::shared_ptr<PointCloudMultiThreadedMapper> PointCloudMultiThreadedMapperPtr;
-// typedef std::shared_ptr<PointCloudMapper> PointCloudMapperPtr;
+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>
@@ -64,7 +64,7 @@ struct IMUData {
 	}
 
 	Eigen::Quaterniond orientation { Eigen::Quaterniond::Identity() };
-	double timestamp_;
+	double timestamp_; // in seconds;
 };
 
 struct TransformStamped {
@@ -81,11 +81,25 @@ struct TransformStamped {
 	std::uint64_t timestamp { 0 }; // PCL time stamp;
 };
 
+struct ReliablityStamped {
+	ReliablityStamped() {
+
+	}
+
+	ReliablityStamped(double r, std::uint64_t ts) {
+		reliability = r;
+		timestamp = ts;
+	}
+
+	double reliability { 0 };
+	std::uint64_t timestamp { 0 };
+};
+
 class LioLocus {
 public:
 	LioLocus();
 	virtual ~LioLocus();
-	typedef std::shared_ptr<LioLocus> Ptr;
+	typedef boost::shared_ptr<LioLocus> Ptr;
 
 	//------------------------------------------------------------------
 	// initialization;
@@ -179,6 +193,13 @@ public:
 		path_log = path;
 	}
 
+	inline ReliablityStamped getStampedReliablityValue() {
+		return m_reliability_ts;
+	}
+	inline void setLeafSizeCloudMap(double res) {
+		m_res_cloud_map = res;
+	}
+
 	//===============================================================================
 
 private:
@@ -214,6 +235,9 @@ private:
 
 	TransformStamped m_pose_latest;
 
+	double m_res_cloud_map { 0.5 };
+	ReliablityStamped m_reliability_ts;
+
 	// dependency processors;
 	PointCloudOdometryPtr mp_cloud_odometry;
 	PointCloudLocalizationPtr mp_cloud_localization;

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

@@ -51,8 +51,8 @@ struct ParameterLioLocus {
 
 	string path_log { "" };
 
-	double leaf_size_load_map { 0.5 };
-	double leaf_size_vis_map { 0.5 };
+	double leaf_size_cloud_map { 0.2 }; // parameter to compute reliability for localization result;
+	double leaf_size_vis_map { 0.5 }; // parameter to down-sample cloud map for visualization;
 
 	double ext_imu2lidar_dx { 0.0 };
 	double ext_imu2lidar_dy { 0.0 };
@@ -113,6 +113,7 @@ struct ParameterImuPreprocessing {
 	string path_log { "" };
 
 	bool pub_imu_preproc { true };
+	bool reverse_tf { true };
 
 	bool do_bias_estimation { true };
 	bool do_adaptive_gain { true };
@@ -129,6 +130,7 @@ using std::string;
 void writeDt2LogFile(string path, string fn, double dt);
 //-------------------------------------------------------------------------------------------
 
+// assign value of resolution for cloud map;
 bool loadParametersFromFileForLioLocus(std::string path_to_file,
 		ParameterLioLocus &params_lio_locus) {
 	if (!boost::filesystem::exists(path_to_file)) {
@@ -146,12 +148,12 @@ bool loadParametersFromFileForLioLocus(std::string path_to_file,
 	LOG(INFO) << "... reading parameter for lidar odometry from ["
 			<< path_to_file << "]";
 
+	// get parameters for cloud map;
 	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>();
@@ -216,6 +218,17 @@ bool loadParametersFromFileForLioLocus(std::string path_to_file,
 	LOG(INFO) << "... flag_use_prior_map -> ["
 			<< params_lio_locus.flag_use_prior_map << "]";
 
+	if (!params_lio_locus.flag_use_prior_map) {
+		// if not use prior map, then use value from cloud pre-processing;
+		params_lio_locus.leaf_size_cloud_map =
+				yaml_node["cloud_preprocessing"]["leaf_voxel_filtering"].as<
+						double>();
+	} else {
+		// take value from prior map loading;
+		params_lio_locus.leaf_size_cloud_map = yn["leaf_size_load_map"].as<
+				double>();
+	}
+
 	string path_map, nm_map;
 	path_map = yn["path_cloud_map"].as<string>();
 	nm_map = yn["fn_cloud_map"].as<string>();
@@ -227,10 +240,6 @@ bool loadParametersFromFileForLioLocus(std::string path_to_file,
 	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>();
@@ -399,6 +408,10 @@ bool loadParametersFromFileForIMUPreprocessing(std::string path_to_file,
 	LOG(INFO) << "... flag_publish_tf -> ["
 			<< params_imu_preproc.flag_publish_tf << "]";
 
+	params_imu_preproc.reverse_tf = yn["reverse_tf"].as<bool>();
+	LOG(INFO) << "... reverse_tf -> [" << params_imu_preproc.reverse_tf
+			<< "]";
+
 	params_imu_preproc.frame_fixed = yn["frame_fixed_"].as<std::string>();
 
 	params_imu_preproc.orientation_stddev =
@@ -414,9 +427,9 @@ void writeDt2LogFile(string path, string fn, double dt) {
 		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]!";
+	if (path == "no_log") {
+		LOG(INFO)
+				<< "... disabled dt logging ... turn it on if needed by setting value of [path_log]!";
 		return;
 	}
 

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

@@ -95,14 +95,14 @@ public:
   typedef PointIndices::ConstPtr PointIndicesConstPtr;
 
   typedef std::vector<Eigen::Matrix3d, Eigen::aligned_allocator<Eigen::Matrix3d>> MatricesVector;
-  typedef std::shared_ptr<MatricesVector> MatricesVectorPtr;
-  typedef std::shared_ptr<const MatricesVector> MatricesVectorConstPtr;
+  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 std::shared_ptr<MultithreadedGeneralizedIterativeClosestPoint<PointSource, PointTarget>> Ptr;
-  typedef std::shared_ptr<const MultithreadedGeneralizedIterativeClosestPoint<PointSource, PointTarget>> ConstPtr;
+  typedef boost::shared_ptr<MultithreadedGeneralizedIterativeClosestPoint<PointSource, PointTarget>> Ptr;
+  typedef boost::shared_ptr<const MultithreadedGeneralizedIterativeClosestPoint<PointSource, PointTarget>> ConstPtr;
 
   typedef Eigen::Matrix<double, 6, 1> Vector6d;
 

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

@@ -89,10 +89,10 @@ protected:
   typedef typename TargetGrid::LeafConstPtr TargetGridLeafConstPtr;
 
 public:
-  typedef std::shared_ptr<
+  typedef boost::shared_ptr<
       NormalDistributionsTransform<PointSource, PointTarget>>
       Ptr;
-  typedef std::shared_ptr<
+  typedef boost::shared_ptr<
       const NormalDistributionsTransform<PointSource, PointTarget>>
       ConstPtr;
 

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

@@ -81,8 +81,8 @@ protected:
   typedef typename PointCloud::ConstPtr PointCloudConstPtr;
 
 public:
-  typedef std::shared_ptr<pcl::VoxelGrid<PointT>> Ptr;
-  typedef std::shared_ptr<const pcl::VoxelGrid<PointT>> ConstPtr;
+  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

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

@@ -20,7 +20,7 @@ def generate_launch_description():
             Node(
                 package="lio_locus_humble",
                 executable="lio_locus_node",
-                prefix="gdb -ex run --args",
+                # prefix="gdb -ex run --args",
                 name="lio_locus",
                 output="screen",
                 parameters=[

src/localization_module_lio_locus/src/lio_locus/lio_locus.cpp

@@ -185,10 +185,13 @@ void LioLocus::trackCurrentCloudWithOnlineBuiltMap(
 				geometry_utils::Transform3 current_pose =
 						mp_cloud_localization->GetIntegratedEstimate();
 
-				//TODO
-
-				// set pose of robot;
+				// get fitness value and compute confidence/reliablity value;
+				double reliablity = 1
+						- mp_cloud_localization->getFitnessScore()
+								/ m_res_cloud_map;
 
+				m_reliability_ts = ReliablityStamped(reliablity, timestamp_now);
+				
 			} else {
 				LOG(WARNING)
 						<< "... data association fails to find correspondences!";

src/localization_module_lio_locus/src/point_cloud_localization/PointCloudLocalization.cc

@@ -214,7 +214,7 @@ bool PointCloudLocalization::SetupICP()
 		LOG(INFO) << 
 						   "RegistrationMethod::GICP activated.";
 		pcl::MultithreadedGeneralizedIterativeClosestPoint<PointF, PointF>::Ptr gicp =
-			std::make_shared<
+			boost::make_shared<
 				pcl::MultithreadedGeneralizedIterativeClosestPoint<
 					PointF, PointF>>();
 
@@ -253,7 +253,7 @@ bool PointCloudLocalization::SetupICP()
 	{
 		LOG(INFO) << "RegistrationMethod::NDT activated.";
 		pclomp::NormalDistributionsTransform<PointF, PointF>::Ptr ndt_omp =
-			std::make_shared<
+			boost::make_shared<
 				pclomp::NormalDistributionsTransform<PointF, PointF>>();
 
 		ndt_omp->setTransformationEpsilon(params_.tf_epsilon);

src/localization_module_lio_locus/src/point_cloud_localization/utils.cc

@@ -1,6 +1,4 @@
 #include <point_cloud_localization/utils.h>
-#include <Eigen/Core>
-#include <Eigen/Eigenvalues>
 
 void addNormal(const PointCloudF &cloud,
 		pcl::PointCloud<pcl::PointNormal>::Ptr &cloud_with_normals,

src/localization_module_lio_locus/src/point_cloud_mapper/CMakeLists.txt

@@ -6,11 +6,12 @@ add_library(point_cloud_mapper SHARED
 
 
 target_include_directories(point_cloud_mapper PUBLIC
-		${PCL_INCLUDE_DIRS}
+	#	${PCL_INCLUDE_DIRS}
+	/home/demo/deps_install/include/pcl-1.10
 	)
 
 target_link_libraries(point_cloud_mapper
-  ${PCL_LIBRARIES}
+  ${PCL_LIBRARIRES}
   ${YAML_CPP_LIBRARIES}
   glog
   OpenMP::OpenMP_CXX

src/localization_module_lio_locus/src/point_cloud_mapper/PointCloudIkdTreeMapper.cc

@@ -1,5 +1,4 @@
 #include <omp.h>
-#include <pcl/types.h>
 #include <pcl/octree/octree_iterator.h>
 #include <pcl/search/impl/search.hpp>
 #include <point_cloud_mapper/PointCloudIkdTreeMapper.h>

src/localization_module_lio_locus/src/point_cloud_odometry/PointCloudOdometry.cc

@@ -129,7 +129,7 @@ bool PointCloudOdometry::SetupICP() {
 	case RegistrationMethod::GICP: {
 		LOG(INFO) << "RegistrationMethod::GICP activated.";
 		pcl::MultithreadedGeneralizedIterativeClosestPoint<PointF, PointF>::Ptr gicp =
-				std::make_shared<
+				boost::make_shared<
 						pcl::MultithreadedGeneralizedIterativeClosestPoint<
 								PointF, PointF> >();
 
@@ -168,7 +168,7 @@ bool PointCloudOdometry::SetupICP() {
 	case RegistrationMethod::NDT: {
 		LOG(INFO) << "RegistrationMethod::NDT activated.";
 		pclomp::NormalDistributionsTransform<PointF, PointF>::Ptr ndt_omp =
-				std::make_shared<
+				boost::make_shared<
 						pclomp::NormalDistributionsTransform<PointF, PointF>>();
 
 		ndt_omp->setTransformationEpsilon(params_.icp_tf_epsilon);