前后段基本实现

include/hdl_backend.hpp

@@ -210,5 +210,8 @@ public:
 // pcl::PointCloud<PointT>::ConstPtr hdl_backend::outlier_removal(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
 
   pcl::Filter<PointT>::Ptr outlier_removal_filter;
+  ros::Publisher odom_pub;
+
+  void publishMatrixAsPose(ros::Publisher& publisher, const Eigen::Matrix4f& matrix);
 
 };

include/hdl_graph_slam/keyframe_updater.hpp

@@ -20,9 +20,10 @@ public:
    * @param pnh
    */
   KeyframeUpdater(ros::NodeHandle& pnh) : is_first(true), prev_keypose(Eigen::Isometry3d::Identity()) {
-    keyframe_delta_trans = pnh.param<double>("keyframe_delta_trans", 0.5);
-    keyframe_delta_angle = pnh.param<double>("keyframe_delta_angle", 0.5);
-
+    // keyframe_delta_trans = pnh.param<double>("keyframe_delta_trans", 0.5); // launch 中是 2
+    // keyframe_delta_angle = pnh.param<double>("keyframe_delta_angle", 0.5); // launch 中是 2
+    keyframe_delta_trans = 2;  // 可调参数
+    keyframe_delta_angle = 2;
     accum_distance = 0.0;
   }
 
@@ -46,12 +47,14 @@ public:
 
     // too close to the previous frame
     // if(dx < 2 && da < 2) {
-    if(dx < 2 && da < 2 ) {  
+    if(dx < 0.2  && da < 0.2) {  
       return false;
     }
 
     accum_distance += dx;
     prev_keypose = pose;
+    ROS_WARN("get new keyframe!!!");
+    // std::cout <<  << std::endl;
     return true;
   }
 

include/hdl_graph_slam/loop_detector.hpp

@@ -9,6 +9,7 @@
 #include <hdl_graph_slam/graph_slam.hpp>
 
 #include <g2o/types/slam3d/vertex_se3.h>
+#include <glog/logging.h>
 
 namespace hdl_graph_slam {
 
@@ -45,12 +46,11 @@ public:
     accum_distance_thresh = 25;
     distance_from_last_edge_thresh = 15;
 
-
-    fitness_score_max_range = pnh.param<double>("fitness_score_max_range", std::numeric_limits<double>::max());
+    fitness_score_max_range = pnh.param<double>("fitness_score_max_range", std::numeric_limits<double>::max()); // 这个参数在launch文件中没给
     // fitness_score_thresh = pnh.param<double>("fitness_score_thresh", 0.5);
     fitness_score_thresh = 2.5;
 
-    registration = select_registration_method(pnh);
+    registration = select_registration_method(pnh); // 选择登记方法
     last_edge_accum_distance = 0.0;
   }
 
@@ -152,10 +152,14 @@ private:
       registration->align(*aligned, guess);
       // std::cout << "." << std::flush;
 
+
       double score = registration->getFitnessScore(fitness_score_max_range);
+      // LOG(INFO) << "score = " << score; // 分数越小越好
+      // best_score = 0.1;
       if(!registration->hasConverged() || score > best_score) {
         continue;
       }
+      
 
       best_score = score;
       best_matched = candidate;

include/laser_odom.hpp

@@ -98,10 +98,15 @@
       ~Laser_Odom();
       void init();
       void cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_scan_msg);
+      void cloud_callback2(const sensor_msgs::PointCloud2::ConstPtr& laser_scan_msg);
+
       Eigen::Matrix4f matching(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud);
       pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud);
       geometry_msgs::TransformStamped matrix2transform(const ros::Time& stamp, const Eigen::Matrix4f& pose, const std::string& frame_id, const std::string& child_frame_id);
       void publish_odometry(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose);
       void publish_keyfrrame(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose);  // 发布里程计数据
+      void opt_odom_callback(const geometry_msgs::PoseStamped::ConstPtr& msg);
+
+    
     };
     // };  // namespace laser_odom_ns

launch/hdl_graph_slam_400.launch

@@ -51,8 +51,8 @@
   <!-- 回放rosbag文件 -->
   <!-- /home/mj/hdl_slam/hdl_400.bag -->
   
-  <node pkg="rosbag" type="play" name="player" output="screen" args="--clock --start 20 /home/mj/hdl_slam/hdl_400.bag"/>
-  <node pkg="hdl_graph_slam" type="laser2cloud" name="laser2cloud" output="log" />
+  <node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/mj/hdl_slam/hdl_400.bag"/>
+  <node pkg="hdl_graph_slam" type="laser2cloud" name="laser2cloud" output="screen" />
 
   <node name="rviz" pkg="rviz" type="rviz" args="-d $(find hdl_graph_slam)/rviz/hdl_graph_slam.rviz"/>  
 

launch/start.launch

@@ -8,9 +8,10 @@
     <!-- 使用仿真时间 -->
     <param name="use_sim_time" value="true" />
     <!-- 回放rosbag文件 -->
-    <!-- <node pkg="rosbag" type="play" name="player" output="log" args="-clock /home/mj/hdl_slam/ros_yfm.bag"/> -->
     <node pkg="rosbag" type="play" name="player" output="log" args="--clock /home/mj/hdl_slam/ros_yfm.bag"/>
-
+    <!-- <node pkg="rosbag" type="play" name="player" output="screen" args="-clock /home/mj/hdl_slam/hdl_400.bag"/> -->
+    
+    <!-- /home/mj/hdl_slam/hdl_400.bag        /home/mj/hdl_slam/ros_yfm.bag    -->
     <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher"  output="screen"/>
 
 </launch>

src/hdl_backend.cpp

@@ -11,6 +11,10 @@
 
 #include <glog/logging.h>
 
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+
+
 hdl_backend* instance = nullptr;  // 初始化静态单例(分配内存, 调用hdl_backend 的默认构造函数)
 // hdl_backend& hdl_opt = hdl_backend::getInstance();  // 因为 静态成员变量 instance 是私有的,需要 静态成员函数 getInstance() 拿到 instance, 并且起个别名(引用)
 
@@ -34,7 +38,7 @@ void hdl_backend::init() {
   max_keyframes_per_update = 10;
   keyframe_updater.reset(new hdl_graph_slam::KeyframeUpdater(nh));
   points_topic = "/velodyne_points";
-  graph_slam.reset(new hdl_graph_slam::GraphSLAM("lm_var"));  // 智能指针，用reset重新初始化，让它重新指向新分配的对象
+  graph_slam.reset(new hdl_graph_slam::GraphSLAM("lm_var_cholmod"));  // 智能指针，用reset重新初始化，让它重新指向新分配的对象
   // graph_slam.reset(new GraphSLAM(private_nh.param<std::string>("g2o_solver_type", "lm_var")));  // 智能指针，用reset重新初始化，让它重新指向新分配的对象
 
   auto voxelgrid = new pcl::VoxelGrid<PointT>();
@@ -48,7 +52,9 @@ void hdl_backend::init() {
   anchor_edge = nullptr;
   odom_sub.reset(new message_filters::Subscriber<nav_msgs::Odometry>(nh, "/laser_odom", 256));  // 创建 message_filters::Subscriber 对象，用于订阅nav_msgs::Odometry类型的消息
   // cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, "/keyframe_points", 32));        // 创建 message_filters::Subscriber 对象，用于订阅sensor_msgs::PointCloud2类型的消息。
-  cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, "/filtered_points", 32));  // 创建 message_filters::Subscriber 对象，用于订阅sensor_msgs::PointCloud2类型的消息。
+  // cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, "/velodyne_points", 32));  // 三维
+  cloud_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, "/filtered_points", 32));  // 二维
+
   // 现在的 /filtered_points 是原始点云数据,没有做坐标转换, 滤波, 将采样等
   // keyframe_points 在前端做了降采样
 
@@ -61,7 +67,8 @@ void hdl_backend::init() {
   map_points_pub2 = nh.advertise<sensor_msgs::PointCloud2>("/hdl_graph_slam/map_points2", 1, true);  // 这个话题会保留以前扫描过的点云！！ 扫过的地方的点云就是
 
   read_until_pub = nh.advertise<std_msgs::Header>("/hdl_graph_slam/read_until", 32);  // 这种类型的消息通常用于指示读取操作应该持续到何时
-
+  odom_pub = nh.advertise<geometry_msgs::PoseStamped>("/opt_odom", 32);
+  
   // save_map_service_server = nh.advertiseService("/hdl_graph_slam/save_map", &hdl_backend::save_map_service, this);  // 保存地图
 
   graph_updated = false;
@@ -107,7 +114,7 @@ void hdl_backend::optimization_timer_callback() {
     }
     if(!keyframe_updated) {
       // 检查地板、GPS、IMU等队列，分别调用相应的 flush 函数（例如 flush_floor_queue、flush_gps_queue、flush_imu_queue），如果这些数据没有更新，则函数直接返回，不进行后续操作。
-      // LOG(INFO) << "关键帧没有更新,退出 optimization_timer_callback 函数";
+      // LOG(INFO) << "关键帧没有更新,usleep(30000);  & continue;";
       // return;
       usleep(30000);
       continue;
@@ -122,10 +129,8 @@ void hdl_backend::optimization_timer_callback() {
       Eigen::Isometry3d relpose(loop->relative_pose.cast<double>());
       Eigen::MatrixXd information_matrix = inf_calclator->calc_information_matrix(loop->key1->cloud, loop->key2->cloud, relpose);
       auto edge = graph_slam->add_se3_edge(loop->key1->node, loop->key2->node, relpose, information_matrix);  // 添加边，位姿之间的关系构成边(edge)
-      graph_slam->add_robust_kernel(edge, "Tukey", 0.05);
+      graph_slam->add_robust_kernel(edge, "Huber", 2);
       // graph_slam->add_robust_kernel(edge, "NONE", 0.01);   // Huber  Cauchy   Tukey  None
-
-      // loop_count++;
     }
     std::copy(new_keyframes.begin(), new_keyframes.end(), std::back_inserter(keyframes));  // 将新关键帧 new_keyframes 合并到已有的关键帧列表 keyframes 中
     new_keyframes.clear();                                                                 // 清空 new_keyframes
@@ -139,7 +144,7 @@ void hdl_backend::optimization_timer_callback() {
       // 如果启用了自适应固定第一帧的功能（参数 "fix_first_node_adaptive"），则将第一个关键帧的锚点位置更新为当前估计的位置，以允许它自由移动但仍然保持在原点附近。
     }
     // optimize the pose graph
-    int num_iterations = 10000000;  // launch文件中都是设置成512
+    int num_iterations = 5000;  // launch文件中都是设置成512
 
     // for(int i = 0; i < keyframes.size(); i++){
     //   // std::cout << "before optimize, odom[" << i << "] = \n" << keyframes[i]->odom.matrix() << std::endl;
@@ -185,12 +190,42 @@ void hdl_backend::optimization_timer_callback() {
     markers_pub.publish(markers);
     // LOG(INFO) << "markers_pub 发布完成";
     // }
+    publishMatrixAsPose(odom_pub, keyframes[keyframes.size() - 1]->node->estimate().matrix().cast<float>());
+    // nav_msgs::Odometry
     usleep(30000);
   }
 
   LOG(INFO) << "end()";
 }
 
+
+void hdl_backend::publishMatrixAsPose(ros::Publisher& publisher, const Eigen::Matrix4f& matrix) {
+    // 提取平移部分
+    Eigen::Vector3f translation = matrix.block<3, 1>(0, 3);
+
+    // 提取旋转部分
+    Eigen::Matrix3f rotation_matrix = matrix.block<3, 3>(0, 0);
+    Eigen::Quaternionf quaternion(rotation_matrix);
+
+    // 创建并填充 PoseStamped 消息
+    geometry_msgs::PoseStamped pose_msg;
+    pose_msg.header.stamp = ros::Time
+    ::now();
+    pose_msg.header.frame_id = "odom";  // 设置坐标系
+
+    pose_msg.pose.position.x = translation.x();
+    pose_msg.pose.position.y = translation.y();
+    pose_msg.pose.position.z = translation.z();
+    pose_msg.pose.orientation.x = quaternion.x();
+    pose_msg.pose.orientation.y = quaternion.y();
+    pose_msg.pose.orientation.z = quaternion.z();
+    pose_msg.pose.orientation.w = quaternion.w();
+
+    // 发布消息
+    publisher.publish(pose_msg);
+}
+
+
 /**
  * @brief generate map point cloud and publish it
  * @param event
@@ -198,7 +233,8 @@ void hdl_backend::optimization_timer_callback() {
 void hdl_backend::map_points_publish_timer_callback(const ros::WallTimerEvent& event) {
   // 源码中，发布是图优化完之后发布才有内容的
   // LOG(INFO) << graph_updated;
-  if(!map_points_pub.getNumSubscribers() || !graph_updated) {
+  if(!graph_updated) {
+  // if(!map_points_pub.getNumSubscribers() || !graph_updated) {
     return;
   }
 
@@ -242,7 +278,6 @@ void hdl_backend::map_points_publish_timer_callback(const ros::WallTimerEvent& e
   // LOG(INFO) << "map_points_publish_timer_callback 函数执行完毕";
 }
 
-
 /**
  * @brief downsample a point cloud
  * @param cloud  input cloud
@@ -271,22 +306,12 @@ pcl::PointCloud<PointT>::ConstPtr hdl_backend::outlier_removal(const pcl::PointC
   return filtered;
 }
 
-
-
-
 /**
  * @brief 接收点云数据并放到 keyframe_queue 中
  * @param odom_msg  前端的激光里程计数据
  * @param cloud_msg 前端滤波后的点云数据
  */
 void hdl_backend::cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) {
-
-  // // 点云下采样 & 滤波
-  // pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>());
-  // downsample_filter->setInputCloud(cloud);
-  // downsample_filter->filter(*filtered);
-
-
   const ros::Time& stamp = cloud_msg->header.stamp;                  // 获取点云数据的时间戳
   Eigen::Isometry3d odom = hdl_graph_slam::odom2isometry(odom_msg);  // 将里程计消息转换为Eigen库中的Isometry3d类型，它表示一个3D刚体变换（包括旋转和平移）
 
@@ -299,7 +324,6 @@ void hdl_backend::cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, con
   auto filtered = downsample(cloud);
   filtered = outlier_removal(filtered);
 
-
   // 更新关键帧判断
   if(!keyframe_updater->update(odom)) {  // 根据当前的里程计信息 odom 判断是否需要生成新的关键帧
     // std::lock_guard<std::mutex> lock(keyframe_queue_mutex);  // 这行代码的作用是确保线程安全，防止多个线程同时访问或修改 keyframe_queue 队列
@@ -362,19 +386,20 @@ bool hdl_backend::flush_keyframe_queue() {
 
     // fix the first node  固定第一个节点,整段代码只运行一次
     if(keyframes.empty() && new_keyframes.size() == 1) {
-      if(nh.param<bool>("fix_first_node", false)) {
-        Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6);
-        std::stringstream sst(nh.param<std::string>("fix_first_node_stddev", "1 1 1 1 1 1"));
-        for(int i = 0; i < 6; i++) {
-          double stddev = 1.0;
-          sst >> stddev;
-          inf(i, i) = 1.0 / stddev;
-        }
-
-        anchor_node = graph_slam->add_se3_node(Eigen::Isometry3d::Identity());  // 添加节点，在graph-based SLAM中，机器人的位姿是一个节点（node）
-        anchor_node->setFixed(true);
-        anchor_edge = graph_slam->add_se3_edge(anchor_node, keyframe->node, Eigen::Isometry3d::Identity(), inf);  // 添加边，位姿之间的关系构成边(edge)
+      // if(nh.param<bool>("fix_first_node", false)) {  // launch 中是 true
+      Eigen::MatrixXd inf = Eigen::MatrixXd::Identity(6, 6);
+      // std::stringstream sst(nh.param<std::string>("fix_first_node_stddev", "1 1 1 1 1 1"));  // 这个是什么意思?
+      std::stringstream sst("10 10 10 1 1 1");  // 这个是什么意思?
+
+      for(int i = 0; i < 6; i++) {
+        double stddev = 1.0;
+        sst >> stddev;
+        inf(i, i) = 1.0 / stddev;
       }
+      anchor_node = graph_slam->add_se3_node(Eigen::Isometry3d::Identity());  // 添加节点，在graph-based SLAM中，机器人的位姿是一个节点（node）
+      anchor_node->setFixed(true);
+      anchor_edge = graph_slam->add_se3_edge(anchor_node, keyframe->node, Eigen::Isometry3d::Identity(), inf);  // 添加边，位姿之间的关系构成边(edge)
+      // }
     }
 
     if(i == 0 && keyframes.empty()) {
@@ -389,6 +414,9 @@ bool hdl_backend::flush_keyframe_queue() {
     auto edge = graph_slam->add_se3_edge(keyframe->node, prev_keyframe->node, relative_pose, information);  // 添加边，位姿之间的关系构成边(edge)
     graph_slam->add_robust_kernel(edge, "NONE", 1.0);
     // graph_slam->add_robust_kernel(edge, "Tukey", 0.1);
+
+    std::cout << "keyframe->node = \n" << keyframe->node->estimate().matrix() << std::endl;
+    std::cout << "prev_keyframe->node = \n" << prev_keyframe->node->estimate().matrix() << std::endl;
   }
 
   std_msgs::Header read_until;

src/hdl_graph_slam/information_matrix_calculator.cpp

@@ -8,7 +8,19 @@
 namespace hdl_graph_slam {
 
 InformationMatrixCalculator::InformationMatrixCalculator(ros::NodeHandle& nh) {
-  use_const_inf_matrix = nh.param<bool>("use_const_inf_matrix", false);
+  // use_const_inf_matrix = nh.param<bool>("use_const_inf_matrix", false);
+  // const_stddev_x = nh.param<double>("const_stddev_x", 0.5);
+  // const_stddev_q = nh.param<double>("const_stddev_q", 0.1);
+
+  // var_gain_a = nh.param<double>("var_gain_a", 20.0);
+  // min_stddev_x = nh.param<double>("min_stddev_x", 0.1);
+  // max_stddev_x = nh.param<double>("max_stddev_x", 5.0);
+  // min_stddev_q = nh.param<double>("min_stddev_q", 0.05);
+  // max_stddev_q = nh.param<double>("max_stddev_q", 0.2);
+  // fitness_score_thresh = nh.param<double>("fitness_score_thresh", 0.5);
+
+
+  use_const_inf_matrix = nh.param<bool>("use_const_inf_matrix", false); // 可调参
   const_stddev_x = nh.param<double>("const_stddev_x", 0.5);
   const_stddev_q = nh.param<double>("const_stddev_q", 0.1);
 

src/hdl_graph_slam/registrations.cpp

@@ -23,16 +23,27 @@ pcl::Registration<pcl::PointXYZI, pcl::PointXYZI>::Ptr select_registration_metho
   using PointT = pcl::PointXYZI;
 
   // select a registration method (ICP, GICP, NDT)
-  std::string registration_method = pnh.param<std::string>("registration_method", "NDT_OMP");
+  std::string registration_method = pnh.param<std::string>("registration_method", "FAST_GICP");
+  // std::cout << "registration_method ===================================================== " << registration_method << std::endl;
   if(registration_method == "FAST_GICP") {    // 源码是用这个方法
+    std::cout << "FAST_GICP----------------------------------------------------------------------" << std::endl;
     std::cout << "registration: FAST_GICP" << std::endl;
     fast_gicp::FastGICP<PointT, PointT>::Ptr gicp(new fast_gicp::FastGICP<PointT, PointT>());
     gicp->setNumThreads(pnh.param<int>("reg_num_threads", 0));
-    gicp->setTransformationEpsilon(pnh.param<double>("reg_transformation_epsilon", 0.01));
-    gicp->setMaximumIterations(pnh.param<int>("reg_maximum_iterations", 64));
-    gicp->setMaxCorrespondenceDistance(pnh.param<double>("reg_max_correspondence_distance", 2.5));
-    gicp->setCorrespondenceRandomness(pnh.param<int>("reg_correspondence_randomness", 20));
+    gicp->setTransformationEpsilon(pnh.param<double>("reg_transformation_epsilon", 1e-10));
+    gicp->setMaximumIterations(pnh.param<int>("reg_maximum_iterations", 6400));
+    gicp->setMaxCorrespondenceDistance(pnh.param<double>("reg_max_correspondence_distance", 0.05));
+    gicp->setCorrespondenceRandomness(pnh.param<int>("reg_correspondence_randomness", 10));
     return gicp;
+
+    // 前端的闭环检测参数
+    // registration->setNumThreads(5);  // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
+    // registration->setTransformationEpsilon(1e-3);
+    // registration->setMaximumIterations(6400);
+    // registration->setMaxCorrespondenceDistance(0.2);
+    // registration->setCorrespondenceRandomness(5);
+
+
   }
 #ifdef USE_VGICP_CUDA
   else if(registration_method == "FAST_VGICP_CUDA") {

src/laser2cloud.cpp

@@ -2,6 +2,7 @@
 #include <sensor_msgs/LaserScan.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <laser_geometry/laser_geometry.h>
+#include <sensor_msgs/point_cloud_conversion.h>
 
 class LaserScanToPointCloud {
 public:
@@ -13,11 +14,43 @@ public:
 
 private:
     void scanCallback(const sensor_msgs::LaserScan::ConstPtr& laser_scan) {
+        sensor_msgs::LaserScan scan = *laser_scan;
+        for(int i = 0; i < scan.ranges.size(); i++){
+            if(scan.intensities[i] < 100)
+            {
+                scan.ranges[i] = 0;
+                scan.intensities[i] = 0;
+            }
+        }
         sensor_msgs::PointCloud2 cloud;
-        laser_proj_.projectLaser(*laser_scan, cloud);  // 转换 LaserScan 到 PointCloud2
+        laser_proj_.projectLaser(scan, cloud);  // 转换 LaserScan 到 PointCloud2
 
         // 发布转换后的点云
         cloud_pub_.publish(cloud);
+        // std::cout << " cloud.header.frame_id = "<< cloud.header.frame_id << std::endl;
+
+        // sensor_msgs::PointCloud cloud;
+        // cloud.header.frame_id = "laser";
+        // for(int i = 0; i < laser_scan->ranges.size(); i++){
+        //     if(laser_scan->intensities[i] > 100){
+        //         geometry_msgs::Point32 p;
+                
+        //         float angle = i * laser_scan->angle_increment + laser_scan->angle_min;
+        //         p.x = laser_scan->ranges[i] * cos(angle);
+        //         p.y = laser_scan->ranges[i] * sin(angle);
+        //         cloud.points.push_back(p);
+                
+        //     }
+        // }
+
+        
+        // sensor_msgs::PointCloud2 cloud2;
+        // cloud2.header.stamp = ros::Time::now();
+        // cloud2.header.frame_id = "laser";
+        // sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2);  // 转换 LaserScan 到 PointCloud2
+
+        // // 发布转换后的点云
+        // cloud_pub_.publish(cloud2);
     }
 
     ros::NodeHandle nh_;

src/laser_odom.cpp

@@ -59,11 +59,21 @@ void Laser_Odom::init() {
   voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
   downsample_filter.reset(voxelgrid);
 
-  registration->setNumThreads(5);  // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
-  registration->setTransformationEpsilon(1e-3);
-  registration->setMaximumIterations(6400);
-  registration->setMaxCorrespondenceDistance(0.2);
-  registration->setCorrespondenceRandomness(5);
+  // 二维数据里程计求解参数
+  // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
+  registration->setNumThreads(5);     // 求解的核心数
+  registration->setTransformationEpsilon(1e-5);   // 配准算法的收敛容差，即当连续迭代的位姿变换小于该值时，算法将认为已经收敛并停止迭代
+  registration->setMaximumIterations(6400);   // 设置配准算法的最大迭代次数
+  registration->setMaxCorrespondenceDistance(0.2);  // 设置配准过程中两点之间的最大对应距离。超出此距离的对应关系将被忽略
+  registration->setCorrespondenceRandomness(5); // 指定计算对应点时使用的随机性，通常用于减少计算量或增强配准的鲁棒性。较小的值会限制对应点的选择范围，较大的值可以增加算法对较少数据的适应性
+
+  // 三维数据里程计求解参数
+  // registration->setNumThreads(5);  // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
+  // registration->setTransformationEpsilon(0.01);
+  // registration->setMaximumIterations(64);
+  // registration->setMaxCorrespondenceDistance(2.5);
+  // registration->setCorrespondenceRandomness(20);
+
   // registration->setRANSACOutlierRejectionThreshold();
   trans_cloud.reset(new pcl::PointCloud<PointT>());
 
@@ -74,15 +84,20 @@ void Laser_Odom::init() {
   keyframe_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/keyframe_points", 32);  // 对齐后的点云
   map_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/map_points", 1, true);
 
-  // ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 1, &Laser_Odom::cloud_callback, this);
-  ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("/filtered_points", 1, &Laser_Odom::cloud_callback, this);
-
+  ros::Subscriber sub = nh.subscribe<sensor_msgs::LaserScan>("/scan", 1, &Laser_Odom::cloud_callback, this);
+  ros::Subscriber sub2 = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 1, &Laser_Odom::cloud_callback2, this);
+  // ros::Subscriber opt_odom_sub = nh.subscribe<nav_msgs::Odometry>("/opt_odom", 1, &Laser_Odom::opt_odom_callback, this);
+  
+  ros::Subscriber opt_odom_sub = nh.subscribe<geometry_msgs::PoseStamped>("opt_odom", 10, &Laser_Odom::opt_odom_callback, this);
+  
   std::thread proc_thread(&Laser_Odom::procThread, this);
   proc_thread.detach();
   
   ros::spin();
 }
 
+
+
 void Laser_Odom::procThread()
 {
   while(ros::ok())
@@ -103,35 +118,6 @@ void Laser_Odom::procThread()
 }
 
 
-
-
-
-
-  void cloud_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {  // cloud_msg：滤波后的点云数据
-    if(!ros::ok()) {
-      return;
-    }
-
-    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());  // 创建一个 pcl::PointCloud<PointT> 类型的指针
-    pcl::fromROSMsg(*cloud_msg, *cloud);                                // 将ROS消息格式的点云（*cloud_msg）转换为PCL（Point Cloud Library）格式的点云（*cloud）
-
-    Eigen::Matrix4f pose = matching(cloud_msg->header.stamp, cloud);              // 点云匹配函数，返回
-    publish_odometry(cloud_msg->header.stamp, cloud_msg->header.frame_id, pose);  // 发布里程计数据
-
-    // In offline estimation, point clouds until the published time will be supplied
-    std_msgs::HeaderPtr read_until(new std_msgs::Header());
-    read_until->frame_id = points_topic;
-    read_until->stamp = cloud_msg->header.stamp + ros::Duration(1, 0);
-    read_until_pub.publish(read_until);
-
-    read_until->frame_id = "/filtered_points";
-    read_until_pub.publish(read_until);
-  }
-
-
-
-
-
 Laser_Odom* Laser_Odom::laser_odom = nullptr;  // 在 .cpp 文件中定义静态成员变量
 // 这种实现方法不算单例实现，如果有多个laser_odom的实例，this指针都会被覆盖，指向最后创建的那个实例
 // 这里就不改了吧,后端不能这么写,因为后端要多线程
@@ -151,24 +137,51 @@ void Laser_Odom::cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_sc
   MatchPCL mpc;
   mpc.cloud = cloud;
   mpc.time = laser_scan_msg->header.stamp;
-
-
   clouds_.push(mpc);
-
   return;
-  // // 点云匹配
-  // Eigen::Matrix4f pose = laser_odom->matching(laser_scan_msg->header.stamp, laser_odom->cloud);  // 点云匹配函数，返回
-
-  // // 发布激光里程计数据
-  // laser_odom->publish_odometry(laser_scan_msg->header.stamp, laser_scan_msg->header.frame_id, pose);
+}
+void Laser_Odom::cloud_callback2(const sensor_msgs::PointCloud2::ConstPtr& laser_scan_msg) {
+  if(!ros::ok()) {
+    return;
+  }
+  // 消息类型转换
+  // LOG(INFO) << "cloud callback 2 ";
+  pcl::PointCloud<PointT> cloud;
+  // laser_odom->projector.projectLaser(*laser_scan_msg, laser_odom->cloud2);
+  pcl::fromROSMsg(*laser_scan_msg, cloud);
 
-  // // 发布对齐后的点云
-  // pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
-  // pcl::transformPointCloud(*laser_odom->cloud, *aligned, laser_odom->odom);
-  // aligned->header.frame_id = "odom";
-  // laser_odom->aligned_points_pub.publish(*aligned);
+  MatchPCL mpc;
+  mpc.cloud = cloud;
+  mpc.time = laser_scan_msg->header.stamp;
+  clouds_.push(mpc);
+  return;
+}
 
-  // // 发布点云地图
+Eigen::Matrix4f matrix;
+void Laser_Odom::opt_odom_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
+    // 创建一个 4x4 的 Eigen::Matrix4f 矩阵
+    matrix = Eigen::Matrix4f::Identity();
+
+    // 提取平移部分
+    matrix(0, 3) = msg->pose.position.x;
+    matrix(1, 3) = msg->pose.position.y;
+    matrix(2, 3) = msg->pose.position.z;
+
+    // 提取旋转部分（四元数转换为旋转矩阵）
+    Eigen::Quaternionf quaternion(
+        msg->pose.orientation.w,
+        msg->pose.orientation.x,
+        msg->pose.orientation.y,
+        msg->pose.orientation.z
+    );
+    Eigen::Matrix3f rotation_matrix = quaternion.toRotationMatrix();
+
+    // 将旋转部分赋值给 4x4 矩阵的左上 3x3 子矩阵
+    matrix.block<3, 3>(0, 0) = rotation_matrix;
+
+    // 输出或使用这个转换后的矩阵
+    ROS_INFO_STREAM("Received 4x4 Transformation Matrix:\n" << matrix);
+    // odom = matrix;
 }
 
 /**
@@ -200,6 +213,10 @@ Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointClo
   pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());  // 创建一个新的点云对象aligned，用于存储对齐后的点云数据
   registration->align(*aligned, prev_trans * msf_delta.matrix());       // 用 registration 来对齐点云, 如果没有imu或者odom信息, msf_delta 是单位矩阵, 即将观测到的点云数据对齐到关键帧?
 
+  // LOG(INFO) << "matrix = \n" << matrix;
+  // LOG(INFO) << "prev_trans * msf_delta.matrix() = \n" << prev_trans * msf_delta.matrix();
+  // LOG(INFO) << "keyframe_pose = \n" << keyframe_pose;
+
   // publish_scan_matching_status(stamp, cloud->header.frame_id, aligned, msf_source, msf_delta);  // 发布扫描匹配的状态信息，包括时间戳、坐标帧ID、对齐后的点云、位姿来源和位姿更新
 
   if(!registration->hasConverged()) {  // 检查扫描匹配是否收敛(是否匹配成功?)。如果没有收敛，输出信息并返回上一次的位姿
@@ -210,6 +227,14 @@ Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointClo
   Eigen::Matrix4f trans = registration->getFinalTransformation();  // 获得当前点云和上一帧点云 关键帧 的仿射变换
   odom = keyframe_pose * trans;                                    // 算出来 odom
 
+  // if(std::abs(matrix(0, 3)  - odom(0, 3)  > 0.5) || std::abs(matrix(1, 3)  - odom(1, 3)  > 0.5)){
+  //   if(matrix(0, 3) != 0 || matrix(1, 3) !=0){
+  //     odom = matrix;
+  //   }
+      
+  // }
+
+
   prev_time = stamp;   // 当前帧的时间戳
   prev_trans = trans;  // 当前帧的仿射变换
 
@@ -227,8 +252,10 @@ Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointClo
                                            // 如果有一个超过阈值,更新关键帧
     keyframe = filtered;
     registration->setInputTarget(keyframe);
-
+    
+    
     keyframe_pose = odom;
+
     keyframe_stamp = stamp;
     prev_time = stamp;
     prev_trans.setIdentity();
@@ -334,54 +361,5 @@ void Laser_Odom::publish_odometry(const ros::Time& stamp, const std::string& bas
   odom.twist.twist.linear.y = 0.0;
   odom.twist.twist.angular.z = 0.0;
 
-  // 上面算出来的odom朝向有问题,需要顺时针转90度修正,代码如下:
-  // 提取原始朝向
-  // tf2::Quaternion original_orientation(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w);
-
-  // // 定义顺时针旋转90度的四元数
-  // tf2::Quaternion rotation_90(0, 0, -0.7071, 0.7071);
-
-  // // 计算旋转后的朝向
-  // tf2::Quaternion new_orientation = rotation_90 * original_orientation;
-  // new_orientation.normalize();  // 确保结果为单位四元数
-
-  // // 将旋转后的朝向赋值回原变量
-  // odom.pose.pose.orientation.x = new_orientation.x();
-  // odom.pose.pose.orientation.y = new_orientation.y();
-  // odom.pose.pose.orientation.z = new_orientation.z();
-  // odom.pose.pose.orientation.w = new_orientation.w();
-
   odom_pub.publish(odom);
 }
-
-// /**
-//  * @brief publish odometry
-//  * @param stamp  timestamp
-//  * @param pose   odometry pose to be published
-//  */
-// void Laser_Odom::publish_keyfrrame(const ros::Time& stamp, const std::string& base_frame_id, const Eigen::Matrix4f& pose) {  // 发布里程计数据
-//   // publish transform stamped for IMU integration
-//   // geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, odom_frame_id, base_frame_id);
-//   geometry_msgs::TransformStamped odom_trans = matrix2transform(stamp, pose, "odom", base_frame_id);
-//   // trans_pub.publish(odom_trans);
-
-//   // // broadcast the transform over tf
-//   // odom_broadcaster.sendTransform(odom_trans);
-
-//   // publish the transform
-//   nav_msgs::Odometry odom;
-//   odom.header.stamp = stamp;
-//   odom.header.frame_id = "odom";
-
-//   odom.pose.pose.position.x = pose(0, 3);
-//   odom.pose.pose.position.y = pose(1, 3);
-//   odom.pose.pose.position.z = pose(2, 3);
-//   odom.pose.pose.orientation = odom_trans.transform.rotation;
-
-//   odom.child_frame_id = base_frame_id;
-//   odom.twist.twist.linear.x = 0.0;
-//   odom.twist.twist.linear.y = 0.0;
-//   odom.twist.twist.angular.z = 0.0;
-
-//   odom_pub.publish(odom);
-// }