激光slam写好了

CMakeLists.txt

@@ -113,6 +113,15 @@ target_link_libraries(laser2cloud
   ${PCL_LIBRARIES}
 )
 
+# nodelets
+add_executable(tf src/tf.cpp)
+# add_library(laser_odom src/laser_odom.cpp)
+target_link_libraries(tf
+  ${catkin_LIBRARIES}
+  ${PCL_LIBRARIES}
+)
+
+
 add_executable(laser_odom src/laser_odom.cpp)
 # add_library(laser_odom src/laser_odom.cpp)
 target_link_libraries(laser_odom

apps/hdl_graph_slam_nodelet.cpp

@@ -208,7 +208,8 @@ private:
     }
 
     // 更新关键帧判断
-    if(!keyframe_updater->update(odom)) {                      // 根据当前的里程计信息 odom 判断是否需要生成新的关键帧
+    if(!keyframe_updater->update(odom, 0)) {                      // 根据当前的里程计信息 odom 判断是否需要生成新的关键帧
+    // 这里我改了源码 update函数多加了个double参数
       std::lock_guard<std::mutex> lock(keyframe_queue_mutex);  // 这行代码的作用是确保线程安全，防止多个线程同时访问或修改 keyframe_queue 队列
       // std::lock_guard<std::mutex> 是 C++ 标准库中的一个类，用来简化互斥锁（std::mutex）的使用。它的作用是在其作用域内自动对给定的互斥量加锁，并在作用域结束时自动解锁
       // keyframe_queue_mutex 是一个互斥量（std::mutex），用于保护关键帧队列 keyframe_queue。当多个线程试图同时访问或修改 keyframe_queue 时，互斥量确保只有一个线程能够访问该资源，避免数据竞争和并发问题

include/hdl_backend.hpp

@@ -122,7 +122,7 @@ private:
 
   ros::ServiceServer save_map_service_server;  // 一个 ROS 服务服务器，用于保存地图
 
-  bool save_map_service(hdl_graph_slam::SaveMapRequest& req, hdl_graph_slam::SaveMapResponse& res);
+  // bool save_map_service(hdl_graph_slam::SaveMapRequest& req, hdl_graph_slam::SaveMapResponse& res);
 
 
 
@@ -204,7 +204,7 @@ public:
 
   pcl::PointCloud<PointT>::ConstPtr downsample(const pcl::PointCloud<PointT>::ConstPtr& cloud);
   pcl::Filter<PointT>::Ptr downsample_filter;
-  double downsample_resolution = 0.1;
+  double downsample_resolution = 0.02;
 
   pcl::PointCloud<PointT>::ConstPtr outlier_removal(const pcl::PointCloud<PointT>::ConstPtr& cloud);
 // pcl::PointCloud<PointT>::ConstPtr hdl_backend::outlier_removal(const pcl::PointCloud<PointT>::ConstPtr& cloud) const {
@@ -213,5 +213,6 @@ public:
   ros::Publisher odom_pub;
 
   void publishMatrixAsPose(ros::Publisher& publisher, const Eigen::Matrix4f& matrix);
+  bool save_map_service(hdl_graph_slam::SaveMapRequest& req, hdl_graph_slam::SaveMapResponse& res);
 
 };

include/hdl_graph_slam/keyframe_updater.hpp

@@ -32,7 +32,7 @@ public:
    * @param pose  pose of the frame
    * @return  if true, the frame should be registered
    */
-  bool update(const Eigen::Isometry3d& pose) {
+  bool update(const Eigen::Isometry3d& pose, double delta_time) {
     // first frame is always registered to the graph
     if(is_first) {
       is_first = false;
@@ -48,12 +48,13 @@ public:
     // too close to the previous frame
     // if(dx < 2 && da < 2) {
     if(dx < 0.2  && da < 0.2) {  
+    // if(dx < 0.1  && da < 0.1 && delta_time < 1) {  
       return false;
     }
 
     accum_distance += dx;
     prev_keypose = pose;
-    ROS_WARN("get new keyframe!!!");
+    // ROS_WARN("get new keyframe!!!");
     // std::cout <<  << std::endl;
     return true;
   }

include/hdl_graph_slam/loop_detector.hpp

@@ -46,11 +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()); // 这个参数在launch文件中没给
+    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;
   }
 
@@ -62,10 +62,10 @@ public:
    */
   std::vector<Loop::Ptr> detect(const std::vector<KeyFrame::Ptr>& keyframes, const std::deque<KeyFrame::Ptr>& new_keyframes, hdl_graph_slam::GraphSLAM& graph_slam) {
     std::vector<Loop::Ptr> detected_loops;
-    for(const auto& new_keyframe : new_keyframes) { // 便利所有new_keyframes
-      auto candidates = find_candidates(keyframes, new_keyframe); // 候选闭环检测, 基于某些策略（如距离、时间等）从已有关键帧中寻找潜在的闭环候选。
-      // std::cout << "candidates.size() = " << candidates.size() << std::endl;  
-      auto loop = matching(candidates, new_keyframe, graph_slam); // 对候选关键帧和新关键帧new_keyframe进行进一步匹配，通过计算相似度、位姿差异等来确定是否有闭环关系。
+    for(const auto& new_keyframe : new_keyframes) {                // 便利所有new_keyframes
+      auto candidates = find_candidates(keyframes, new_keyframe);  // 候选闭环检测, 基于某些策略（如距离、时间等）从已有关键帧中寻找潜在的闭环候选。
+      // std::cout << "candidates.size() = " << candidates.size() << std::endl;
+      auto loop = matching(candidates, new_keyframe, graph_slam);  // 对候选关键帧和新关键帧new_keyframe进行进一步匹配，通过计算相似度、位姿差异等来确定是否有闭环关系。
       if(loop) {
         detected_loops.push_back(loop);
       }
@@ -152,16 +152,15 @@ 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;
+      // LOG(INFO) << "score = " << score<< "   best_score = " << best_score;  // 分数越小越好
       best_matched = candidate;
       relative_pose = registration->getFinalTransformation();
     }

include/laser_odom.hpp

@@ -82,7 +82,7 @@
       static Laser_Odom* laser_odom;
       Eigen::Matrix4f odom;
 
-      double downsample_resolution = 0.1;  // 下采样分辨率
+      double downsample_resolution = 0.02;  // 下采样分辨率
 
       ros::NodeHandle private_nh;
 

launch/start.launch

@@ -2,16 +2,24 @@
     <node pkg="hdl_graph_slam" type="laser_odom" name="laser_odom" output="screen" />
     <node pkg="hdl_graph_slam" type="hdl_backend" name="hdl_backend" output="screen" />
     <node pkg="hdl_graph_slam" type="laser2cloud" name="laser2cloud" output="screen" />
+    <!-- <node pkg="hdl_graph_slam" type="tf" name="tf" output="screen" /> -->
 
     <node name="rviz" pkg="rviz" type="rviz" args="-d $(find hdl_graph_slam)/rviz/hdl_graph_slam.rviz"/>  
 
-    <!-- 使用仿真时间 -->
-    <param name="use_sim_time" value="true" />
+    <!-- 使用仿真时间 这个要用,不然会报错:message removed because it is too old  然后忽略数据-->
+    <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="screen" args="-clock /home/mj/hdl_slam/hdl_400.bag"/> -->
     
-    <!-- /home/mj/hdl_slam/hdl_400.bag        /home/mj/hdl_slam/ros_yfm.bag    -->
+    <!-- /home/mj/hdl_slam/hdl_400.bag        /home/mj/hdl_slam/ros_yfm.bag  /home/mj/hdl_slam/IntelResearchLab.bag  -->
     <node pkg="hdl_graph_slam" type="map2odom_publisher.py" name="map2odom_publisher"  output="screen"/>
 
+  <!-- <node pkg="slam_karto" type="slam_karto" name="slam_karto" output="screen">
+    <remap from="scan" to="scan"/>
+    <param name="odom_frame" value="odom"/>
+    <param name="map_update_interval" value="1"/>
+    <param name="resolution" value="0.025"/>
+  </node> -->
+
 </launch>

src/hdl_backend.cpp

@@ -11,10 +11,11 @@
 
 #include <glog/logging.h>
 
+// #include <vector.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, 并且起个别名(引用)
 
@@ -33,7 +34,7 @@ int main(int argc, char* argv[]) {
 void hdl_backend::init() {
   map_frame_id = "map";
   odom_frame_id = "odom";
-  map_cloud_resolution = 0.05;   // 地图点云分辨率
+  map_cloud_resolution = 0.01;   // 地图点云分辨率
   trans_odom2map.setIdentity();  // 设置为单位矩阵
   max_keyframes_per_update = 10;
   keyframe_updater.reset(new hdl_graph_slam::KeyframeUpdater(nh));
@@ -68,8 +69,8 @@ void hdl_backend::init() {
 
   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);  // 保存地图
+
+  save_map_service_server = nh.advertiseService("/hdl_graph_slam/save_map", &hdl_backend::save_map_service, this);  // 保存地图
 
   graph_updated = false;
 
@@ -79,8 +80,6 @@ void hdl_backend::init() {
   // optimization_timer = nh.createWallTimer(ros::WallDuration(graph_update_interval), &hdl_backend::optimization_timer_callback, this);  // 创建一个定时器，定时器会在每个 graph_update_interval 秒后触发，然后调用 optimization_timer_callback 函数
   map_publish_timer = nh.createWallTimer(ros::WallDuration(map_cloud_update_interval), &hdl_backend::map_points_publish_timer_callback, this);  // 创建一个定时器，定时器会在每个 map_cloud_update_interval 秒后触发，然后调用 map_points_publish_timer_callback 函数
 
-  map_cloud_resolution = 0.05;
-
   pcl::RadiusOutlierRemoval<PointT>::Ptr rad(new pcl::RadiusOutlierRemoval<PointT>());
   rad->setRadiusSearch(0.8);
   rad->setMinNeighborsInRadius(1);
@@ -92,6 +91,9 @@ void hdl_backend::init() {
   ros::spin();
 }
 
+
+
+
 /**
  * @brief 这个函数将队列中的所有数据添加到姿势图中，然后优化姿势图
  * @param event   // 是ROS提供的定时器时间类，包含信息有：计时器出发的时间戳、上一次出发的时间戳、计时器的周期信息等
@@ -198,34 +200,31 @@ void hdl_backend::optimization_timer_callback() {
   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);
+  // 提取平移部分
+  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
@@ -234,7 +233,7 @@ void hdl_backend::map_points_publish_timer_callback(const ros::WallTimerEvent& e
   // 源码中，发布是图优化完之后发布才有内容的
   // LOG(INFO) << graph_updated;
   if(!graph_updated) {
-  // if(!map_points_pub.getNumSubscribers() || !graph_updated) {
+    // if(!map_points_pub.getNumSubscribers() || !graph_updated) {
     return;
   }
 
@@ -261,7 +260,7 @@ void hdl_backend::map_points_publish_timer_callback(const ros::WallTimerEvent& e
   std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> snapshot;
   snapshot = keyframes_snapshot;
 
-  LOG(INFO) << "keyframes_snapshot.size() = " << keyframes_snapshot.size();
+  // LOG(INFO) << "keyframes_snapshot.size() = " << keyframes_snapshot.size();
   auto cloud = map_cloud_generator->generate(snapshot, map_cloud_resolution);
   if(!cloud) {
     return;
@@ -273,7 +272,7 @@ void hdl_backend::map_points_publish_timer_callback(const ros::WallTimerEvent& e
   pcl::toROSMsg(*cloud, *cloud_msg);
 
   map_points_pub.publish(*cloud_msg);
-  LOG(INFO) << "点云数据发布完成";
+  // LOG(INFO) << "点云数据发布完成";
 
   // LOG(INFO) << "map_points_publish_timer_callback 函数执行完毕";
 }
@@ -325,7 +324,9 @@ void hdl_backend::cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, con
   filtered = outlier_removal(filtered);
 
   // 更新关键帧判断
-  if(!keyframe_updater->update(odom)) {  // 根据当前的里程计信息 odom 判断是否需要生成新的关键帧
+  double delta_time = 1000;
+  if(keyframe_queue.size() != 0) delta_time = (stamp - keyframe_queue.back()->stamp).toSec();
+  if(!keyframe_updater->update(odom, delta_time)) {  // 根据当前的里程计信息 odom 判断是否需要生成新的关键帧
     // std::lock_guard<std::mutex> lock(keyframe_queue_mutex);  // 这行代码的作用是确保线程安全，防止多个线程同时访问或修改 keyframe_queue 队列
 
     if(keyframe_queue.empty()) {  // 如果关键帧队列是空的，发布一个 ROS 消息通知系统继续读取点云数据，直到指定时间点（stamp + ros::Duration(10, 0)）。这段代码确保在没有关键帧生成时，系统能够继续读取点云数据
@@ -344,6 +345,8 @@ void hdl_backend::cloud_callback(const nav_msgs::OdometryConstPtr& odom_msg, con
   // std::cout << "============ accum_d ============" << accum_d << std::endl;
   hdl_graph_slam::KeyFrame::Ptr keyframe(new hdl_graph_slam::KeyFrame(stamp, odom, accum_d, filtered));  // 创建一个新的关键帧 keyframe，其中包含当前的时间戳 stamp，里程计信息 odom，累计距离 accum_d，以及处理后的点云数据 cloud（这里的处理就是做了个消息类型的转换）
 
+  // LOG(INFO) << "get new keyframe!!!";
+
   // std::lock_guard<std::mutex> lock(keyframe_queue_mutex);  // 使用 std::lock_guard 加锁，确保对关键帧队列 keyframe_queue 的操作是线程安全的
   keyframe_queue.push_back(keyframe);  // 将新生成的关键帧 keyframe 推入 keyframe_queue 队列，供后续的处理使用
   // LOG(INFO) << "keyframe_queue.push_back(keyframe)";
@@ -415,8 +418,8 @@ bool hdl_backend::flush_keyframe_queue() {
     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::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;
@@ -430,6 +433,47 @@ bool hdl_backend::flush_keyframe_queue() {
   return true;
 }
 
+  /**
+   * @brief save map data as pcd
+   * @param req
+   * @param res
+   * @return
+   */
+  bool hdl_backend::save_map_service(hdl_graph_slam::SaveMapRequest& req, hdl_graph_slam::SaveMapResponse& res) {
+    std::cout << "get in save_map_service !!!" << std::endl;
+    std::vector<hdl_graph_slam::KeyFrameSnapshot::Ptr> snapshot1;
+
+    // keyframes_snapshot_mutex.lock();
+    snapshot1 = keyframes_snapshot;
+    // keyframes_snapshot_mutex.unlock();
+
+    auto cloud = map_cloud_generator->generate(snapshot1, req.resolution);
+    if(!cloud) {
+      res.success = false;
+      return true;
+    }
+
+    if(zero_utm && req.utm) {
+      for(auto& pt : cloud->points) {
+        pt.getVector3fMap() += (*zero_utm).cast<float>();
+      }
+    }
+
+    cloud->header.frame_id = "map";
+    cloud->header.stamp = snapshot1.back()->cloud->header.stamp;
+
+    if(zero_utm) {
+      std::ofstream ofs(req.destination + ".utm");
+      ofs << boost::format("%.6f %.6f %.6f") % zero_utm->x() % zero_utm->y() % zero_utm->z() << std::endl;
+    }
+
+    int ret = pcl::io::savePCDFileBinary(req.destination, *cloud);
+    res.success = ret == 0;
+
+    return true;
+  }
+
+
 /**
  * @brief create visualization marker
  * @param stamp

src/hdl_graph_slam/registrations.cpp

@@ -25,25 +25,24 @@ pcl::Registration<pcl::PointXYZI, pcl::PointXYZI>::Ptr select_registration_metho
   // select a registration method (ICP, GICP, NDT)
   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") {    // 源码是用这个方法
+  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->setNumThreads(pnh.param<int>("reg_num_threads", 0));
+    // 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", 2));
+    // gicp->setCorrespondenceRandomness(pnh.param<int>("reg_correspondence_randomness", 10));
+
+    // 自己数据的参数
+    gicp->setNumThreads(pnh.param<int>("reg_num_threads", 3));
     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->setMaxCorrespondenceDistance(pnh.param<double>("reg_max_correspondence_distance", 0.02));
     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);
-
 
+    return gicp;
   }
 #ifdef USE_VGICP_CUDA
   else if(registration_method == "FAST_VGICP_CUDA") {
@@ -102,7 +101,7 @@ pcl::Registration<pcl::PointXYZI, pcl::PointXYZI>::Ptr select_registration_metho
     }
 
     double ndt_resolution = pnh.param<double>("reg_resolution", 0.5);
-    if(registration_method.find("OMP") == std::string::npos) {    // **
+    if(registration_method.find("OMP") == std::string::npos) {  // **
       std::cout << "registration: NDT " << ndt_resolution << std::endl;
       pcl::NormalDistributionsTransform<PointT, PointT>::Ptr ndt(new pcl::NormalDistributionsTransform<PointT, PointT>());
       ndt->setTransformationEpsilon(pnh.param<double>("reg_transformation_epsilon", 0.01));

src/laser2cloud.cpp

@@ -14,16 +14,16 @@ 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::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(scan, cloud);  // 转换 LaserScan 到 PointCloud2
+        laser_proj_.projectLaser(*laser_scan, cloud);  // 转换 LaserScan 到 PointCloud2
 
         // 发布转换后的点云
         cloud_pub_.publish(cloud);

src/laser_odom.cpp

@@ -61,11 +61,20 @@ void Laser_Odom::init() {
 
   // 二维数据里程计求解参数
   // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
-  registration->setNumThreads(5);     // 求解的核心数
-  registration->setTransformationEpsilon(1e-5);   // 配准算法的收敛容差，即当连续迭代的位姿变换小于该值时，算法将认为已经收敛并停止迭代
-  registration->setMaximumIterations(6400);   // 设置配准算法的最大迭代次数
+  // registration->setNumThreads(5);     // 求解的核心数
+  // registration->setTransformationEpsilon(1e-5);   // 配准算法的收敛容差，即当连续迭代的位姿变换小于该值时，算法将认为已经收敛并停止迭代
+  // registration->setMaximumIterations(6400);   // 设置配准算法的最大迭代次数
+  // registration->setMaxCorrespondenceDistance(2);  // 设置配准过程中两点之间的最大对应距离。超出此距离的对应关系将被忽略
+  // registration->setCorrespondenceRandomness(5); // 指定计算对应点时使用的随机性，通常用于减少计算量或增强配准的鲁棒性。较小的值会限制对应点的选择范围，较大的值可以增加算法对较少数据的适应性
+
+  // 自己数据的参数
+  registration->setNumThreads(8);                   // 求解的核心数
+  registration->setTransformationEpsilon(1e-5);     // 配准算法的收敛容差，即当连续迭代的位姿变换小于该值时，算法将认为已经收敛并停止迭代
+  registration->setMaximumIterations(6400);         // 设置配准算法的最大迭代次数
   registration->setMaxCorrespondenceDistance(0.2);  // 设置配准过程中两点之间的最大对应距离。超出此距离的对应关系将被忽略
-  registration->setCorrespondenceRandomness(5); // 指定计算对应点时使用的随机性，通常用于减少计算量或增强配准的鲁棒性。较小的值会限制对应点的选择范围，较大的值可以增加算法对较少数据的适应性
+  registration->setCorrespondenceRandomness(5);  // 指定计算对应点时使用的随机性，通常用于减少计算量或增强配准的鲁棒性。较小的值会限制对应点的选择范围，较大的值可以增加算法对较少数据的适应性
+
+  // 应该是这里的参数没调好?
 
   // 三维数据里程计求解参数
   // registration->setNumThreads(5);  // 把 registration = hdl_graph_slam::select_registration_method(nh);  核心部分弄出来
@@ -78,6 +87,7 @@ void Laser_Odom::init() {
   trans_cloud.reset(new pcl::PointCloud<PointT>());
 
   odom_pub = nh.advertise<nav_msgs::Odometry>("/laser_odom", 32);  // 发布机器人的里程计信息
+
   // keyframe_pub = nh.advertise<pcl::PointCloud<PointT>>("/keyframe", 32);
   // read_until_pub = nh.advertise<std_msgs::Header>("/keyframe", 32);
   aligned_points_pub = nh.advertise<sensor_msgs::PointCloud2>("/aligned_points", 32);    // 对齐后的点云
@@ -87,23 +97,18 @@ void Laser_Odom::init() {
   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())
-  {
-    if(clouds_.empty())
-    {
+void Laser_Odom::procThread() {
+  while(ros::ok()) {
+    if(clouds_.empty()) {
       usleep(10);
       continue;
     }
@@ -117,7 +122,6 @@ void Laser_Odom::procThread()
   }
 }
 
-
 Laser_Odom* Laser_Odom::laser_odom = nullptr;  // 在 .cpp 文件中定义静态成员变量
 // 这种实现方法不算单例实现，如果有多个laser_odom的实例，this指针都会被覆盖，指向最后创建的那个实例
 // 这里就不改了吧,后端不能这么写,因为后端要多线程
@@ -130,8 +134,18 @@ void Laser_Odom::cloud_callback(const sensor_msgs::LaserScan::ConstPtr& laser_sc
   }
   // 消息类型转换
 
+  sensor_msgs::LaserScan scan = *laser_scan_msg;
+  for(int i = 0; i < scan.ranges.size(); i++) {
+    // if(scan.intensities[i] < 100)
+    // {
+    //     scan.ranges[i] = 0;
+    //     scan.intensities[i] = 0;
+    // }
+    scan.intensities.push_back(200);
+  }
+
   pcl::PointCloud<PointT> cloud;
-  laser_odom->projector.projectLaser(*laser_scan_msg, laser_odom->cloud2);
+  laser_odom->projector.projectLaser(scan, laser_odom->cloud2);
   pcl::fromROSMsg(laser_odom->cloud2, cloud);
 
   MatchPCL mpc;
@@ -159,29 +173,29 @@ void Laser_Odom::cloud_callback2(const sensor_msgs::PointCloud2::ConstPtr& laser
 
 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;
+  // 创建一个 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;
 }
 
 /**
@@ -231,9 +245,8 @@ Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointClo
   //   if(matrix(0, 3) != 0 || matrix(1, 3) !=0){
   //     odom = matrix;
   //   }
-      
-  // }
 
+  // }
 
   prev_time = stamp;   // 当前帧的时间戳
   prev_trans = trans;  // 当前帧的仿射变换
@@ -247,13 +260,12 @@ Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointClo
 
   // LOG(INFO)<<"delta_angle:"<<delta_angle<<" delta_trans:"<<delta_trans;
 
-  if(delta_trans > 1 || delta_angle > 1 || delta_time > 0.5) {   
-    
-                                           // 如果有一个超过阈值,更新关键帧
+  // if(delta_trans > 1 || delta_angle > 1 || delta_time > 0.5) {
+  if(delta_trans > 1 || delta_angle > 0.2) {
+    // 如果有一个超过阈值,更新关键帧
     keyframe = filtered;
     registration->setInputTarget(keyframe);
-    
-    
+
     keyframe_pose = odom;
 
     keyframe_stamp = stamp;
@@ -270,15 +282,15 @@ Eigen::Matrix4f Laser_Odom::matching(const ros::Time& stamp, const pcl::PointClo
   }
   // else
   // {
-    pcl::PointCloud<PointT>::Ptr aligned_trans(new pcl::PointCloud<PointT>());
-    pcl::transformPointCloud(*filtered, *aligned_trans, odom);
-    *trans_cloud = *aligned_trans;
-
-    // 发布
-    sensor_msgs::PointCloud2Ptr cloud2_msg(new sensor_msgs::PointCloud2());
-    pcl::toROSMsg(*trans_cloud, *cloud2_msg);
-    cloud2_msg->header.frame_id = "map";
-    keyframe_points_pub.publish(*cloud2_msg);  // 话题名:
+  pcl::PointCloud<PointT>::Ptr aligned_trans(new pcl::PointCloud<PointT>());
+  pcl::transformPointCloud(*filtered, *aligned_trans, odom);
+  *trans_cloud = *aligned_trans;
+
+  // 发布
+  sensor_msgs::PointCloud2Ptr cloud2_msg(new sensor_msgs::PointCloud2());
+  pcl::toROSMsg(*trans_cloud, *cloud2_msg);
+  cloud2_msg->header.frame_id = "map";
+  keyframe_points_pub.publish(*cloud2_msg);  // 话题名:
   // }
   //
   // Eigen::Matrix4f odom;

src/tf.cpp

@@ -0,0 +1,23 @@
+#include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
+
+int main(int argc, char* argv[]) {
+  ros::init(argc, argv, "tf_pub");
+  ros::NodeHandle nh;
+    tf::TransformBroadcaster br;
+    ros::Rate rate(20.0);
+
+
+  while(ros::ok()) {
+    tf::Transform transform;
+    transform.setOrigin(tf::Vector3(0, 0, 0));  // 设置坐标变换的平移部分
+    tf::Quaternion q;
+    q.setRPY(0, 0, 0);  // 设置旋转部分（即绕 x、y、z 轴的旋转角度）
+    transform.setRotation(q);
+    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "laser"));   // 父坐标系   子坐标系
+    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "base_link"));
+    rate.sleep();
+    ros::spinOnce();
+  }
+  return 0;
+}