开源激光SLAM项目BLAM－－－－

接上一章节提到的**ProcessPointCloudMessage(m->msg)**函数，它传入一个const PointCloud::ConstPtr& 类型，即点云常指针的引用，程序源代码如下

void BlamSlam::ProcessPointCloudMessage(const PointCloud::ConstPtr& msg) {static int T=0;ROS_INFO("The times is:%d",T++);  // 进行一些基本的过滤  提高后面运算速度  降低数据存储量PointCloud::Ptr msg_filtered(new PointCloud);filter_.Filter(msg, msg_filtered);// 激光里程计 得到粗略的 变换矩阵 ， 第一次执行时候进入if语句初始化地图if (!odometry_.UpdateEstimate(*msg_filtered)) {// First update ever.PointCloud::Ptr unused(new PointCloud);mapper_.InsertPoints(msg_filtered, unused.get());loop_closure_.AddKeyScanPair(0, msg);return;}PointCloud::Ptr msg_transformed(new PointCloud);PointCloud::Ptr msg_neighbors(new PointCloud);PointCloud::Ptr msg_base(new PointCloud);PointCloud::Ptr msg_fixed(new PointCloud);// 将当前帧数据通过前面的变换矩阵 转换到 地图坐标系下localization_.MotionUpdate(odometry_.GetIncrementalEstimate());localization_.TransformPointsToFixedFrame(*msg_filtered,msg_transformed.get());// 在地图坐标系下得到当前帧数据对应的最近邻点mapper_.ApproxNearestNeighbors(*msg_transformed, msg_neighbors.get());// 最近邻点转换回传感器的坐标系 与当前帧再进行一次匹配 得到精确的位姿变换矩阵localization_.TransformPointsToSensorFrame(*msg_neighbors, msg_neighbors.get());localization_.MeasurementUpdate(msg_filtered, msg_neighbors, msg_base.get());// 回环检测bool new_keyframe;if (HandleLoopClosures(msg, &new_keyframe)) {T=0;// 找到了一个回环，对地图进行调整PointCloud::Ptr regenerated_map(new PointCloud);loop_closure_.GetMaximumLikelihoodPoints(regenerated_map.get());mapper_.Reset();PointCloud::Ptr unused(new PointCloud);mapper_.InsertPoints(regenerated_map, unused.get());// 对储存的机器人位姿也重新进行调整localization_.SetIntegratedEstimate(loop_closure_.GetLastPose());} else {if (new_keyframe) { // 不是回环检测，但是是一个**关键帧**添加点云数据到地图中localization_.MotionUpdate(gu::Transform3::Identity());//当前帧数据使用上述的精确位姿变换矩阵 转换到地图坐标系下 localization_.TransformPointsToFixedFrame(*msg, msg_fixed.get()); PointCloud::Ptr unused(new PointCloud);loop_closure_.GetMapPoints(msg_fixed.get());ROS_INFO("The size of get::%d",msg_fixed->points.size());//加入到地图中mapper_.InsertPoints(msg_fixed, unused.get());}}// 发布位姿节点，里程计数据等loop_closure_.PublishPoseGraph();// 发布当前帧点云数据if (base_frame_pcld_pub_.getNumSubscribers() != 0) {PointCloud base_frame_pcld = *msg;base_frame_pcld.header.frame_id = base_frame_id_;base_frame_pcld_pub_.publish(base_frame_pcld);}
}

程序有一点长，不过相对于其他开源激光SLAM项目还是挺好理解的。下面就一段段的分析

  static int T=0;ROS_INFO("The times is:%d",T++);  // 进行一些基本的过滤  提高后面运算速度  降低数据存储量PointCloud::Ptr msg_filtered(new PointCloud);filter_.Filter(msg, msg_filtered);// 激光里程计 得到粗略的 变换矩阵 ， 第一次执行时候进入if语句初始化地图if (!odometry_.UpdateEstimate(*msg_filtered)) {// First update ever.PointCloud::Ptr unused(new PointCloud);mapper_.InsertPoints(msg_filtered, unused.get());loop_closure_.AddKeyScanPair(0, msg);return;}

filter_.Filter(msg, msg_filtered); 这里就是调用PCL库进行一些基本的数据过滤，其配置在config.yaml文件进行设置，比较见到就不进去看了，不了解的可以去PCL官网教程里去看一下(http://pointclouds.org/documentation/tutorials/)

*odometry_.UpdateEstimate(msg_filtered) odometry_是 odometry类实例化的一个对象，odometry类定义在point_cloud_odometry这个package下。进入这个函数

bool PointCloudOdometry::UpdateEstimate(const PointCloud& points) {// 和pcl到ros的时间戳转换有关 转换成合理的形式stamp_.fromNSec(points.header.stamp*1e3);// 如果这是第一个消息  储存下来等待下一个消息 if (!initialized_) {copyPointCloud(points, *query_);initialized_ = true;return false;}// Move current query points (acquired last iteration) to reference points.copyPointCloud(*query_, *reference_);// Set the incoming point cloud as the query point cloud.copyPointCloud(points, *query_);// 有了两帧数据 执行icpreturn UpdateICP();
}

可以看到这里是保存当前帧和上一帧的数据,通过两帧数据做匹配。下面就进入匹配的函数

bool PointCloudOdometry::UpdateICP() {// 计算两帧之间的转换---incremental transformGeneralizedIterativeClosestPoint<PointXYZ, PointXYZ> icp;  //这里使用的GICPicp.setTransformationEpsilon(params_.icp_tf_epsilon);icp.setMaxCorrespondenceDistance(params_.icp_corr_dist);icp.setMaximumIterations(params_.icp_iterations);icp.setRANSACIterations(0);icp.setInputSource(query_);icp.setInputTarget(reference_);PointCloud unused_result;icp.align(unused_result);  //执行转换 但是不需要用到对齐后的点云const Eigen::Matrix4f T = icp.getFinalTransformation();  //得到粗略的  位姿变换//将Eigen库的Matrix4f 转换到 blam自定义的 位姿变换矩阵 transform3incremental_estimate_.translation = gu::Vec3(T(0, 3), T(1, 3), T(2, 3));  //4*4的位姿变换矩阵 设置平移量incremental_estimate_.rotation = 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 (!transform_thresholding_ ||  (incremental_estimate_.translation.Norm() <= max_translation_ &&incremental_estimate_.rotation.ToEulerZYX().Norm() <= max_rotation_)) {integrated_estimate_ =gu::PoseUpdate(integrated_estimate_, incremental_estimate_);  //通过两帧之间的变换矩阵进行累计  得到累计的位姿变换 即当前位置} else {ROS_WARN("%s: Discarding incremental transformation with norm (t: %lf, r: %lf)",name_.c_str(), incremental_estimate_.translation.Norm(),incremental_estimate_.rotation.ToEulerZYX().Norm());}// Convert pose estimates to ROS format and publish.PublishPose(incremental_estimate_, incremental_estimate_pub_);   //发布两帧之间的位姿变换PublishPose(integrated_estimate_, integrated_estimate_pub_);     //发布累计的位姿变换// Publish point clouds for visualization.PublishPoints(query_, query_pub_);PublishPoints(reference_, reference_pub_);// Convert transform between fixed frame and odometry frame.geometry_msgs::TransformStamped tf;tf.transform = gr::ToRosTransform(integrated_estimate_);         //发布tf变换tf.header.stamp = stamp_;tf.header.frame_id = fixed_frame_id_;tf.child_frame_id = odometry_frame_id_;tfbr_.sendTransform(tf);return true;
}

这段代码的主要功能就是计算两帧之间的位姿变换，注意这里只是一个粗略的计算，后面还有精确计算。另外就是得到累计的位姿估计。如果对上述代码注释中的GICP , ICP , TF变换 , 位姿矩阵等名词不熟悉的话需要自行百度去了解一下再回头来看这段代码。

                                                                            转载注明出处

开源激光SLAM项目BLAM－－－－－2相关推荐

开源激光SLAM项目BLAM－－－－－１
最近在学习SLAM和ROS,首先接触到的是github上的开源项目BLAM,是berkely的一位小哥所写,油管上有相关的视频.这篇教程面向于SLAM和ROS的初学者,如果有问题还希望各位大神进行指正 ...
开源3D激光SLAM项目BLAM
最近在学习SLAM和ROS,首先接触到的是github上的开源项目BLAM,是berkely的一位小哥所写,油管上有相关的视频.这篇教程面向于SLAM和ROS的初学者,如果有问题还希望各位大神进行指正 ...
使用开源激光SLAM方案LIO-SAM运行KITTI数据集，如有用，请评论雷锋
第一次写博客~ LIOSAM作为优秀的激光slam方案,当然想尝试着运行更多数据啦,然而没有发现类似的方法分享到底如何实现,在B站看到有伙伴发布了测试视频,但是仍然没有写出方法.所以我跑通了之后记录一 ...
基于LOAM框架的激光SLAM开源程序集合
一.前言 LOAM, 即Lidar Odometry and Mapping,是 Ji Zhang 博士于2014年提出的使用激光雷达完成定位与三维建图的算法.其算法流程如下: LOAM算法中主要包含 ...
【激光SLAM】 01 cartographer环境建立以及建图测试（详细级）
[激光SLAM]cartographer环境建立以及建图测试(详细级) cartographer Launch the 2D backpack demo. Download the 3D backpa ...
激光SLAM理论与实践(一)--激光SLAM简要介绍
疫情原因一直在家,所以把之前学习的某蓝学院的激光SLAM的教程做一个学习笔记记录一下.话不多说,直接开始! 第一章主要是是激光SLAM的基本脉络的简介,综述类的介绍,主要分为以下四部分,讲了激光SLA ...
ROS2_Foxy学习10——多机激光SLAM准备篇
ROS2_Foxy学习10--多机激光SLAM准备篇 1 安装Ubuntu20.04 mate 2 安装ROS noetic 3 安装cartographer 4 详细配置cartographer 5 ...
激光SLAM技术总结（1）激光SLAM对比视觉V-SLAM
目录 1. SLAM 2. 激光SLAM 3. 视觉SLAM地图构建 4. 激光SLAM VS V-SLAM 5. 小结 1. SLAM SLAM(同步定位与地图构建),是指运动物体根据传感器的信息 ...
3D激光开源项目——BLAM安装使用过程的一些问题
最近由于在做3D激光slam的相关工作,在网上看见 BLAM 的效果不错,就下载了相关的代码跑了一下,在编译安装过程中遇到了一些问题,在这里记录一下,供有需要的同学看一下. 具体的安装过程这里就不在赘 ...

开源激光SLAM项目BLAM－－－－－2

开源激光SLAM项目BLAM－－－－－2相关推荐

最新文章

热门文章