上一章中的* * processpointcloudmessage (m-msg ) *函数传递对constpointcloud 33603360 const ptr类型或点云常规指针的引用。 程序的源代码如下所示
voidblamslam :3360 processpointcloudmessage (constpointcloud :3360 constptrmsg ) { static int T=0； ROS_info(thetimesis:%d )，t )； //进行一些基本过滤，提高后面的运算速度，降低数据存储量的点云：3360 ptr msg _ filtered (new point cloud )； filter_.filter(msg，msg_filtered )； //激光里程表得到大致的变换矩阵，第一次运行时进入if语句初始化地图if (！ odometry _.update estimate (msg _ filtered )/firstupdateever.point cloud :3360 ptr unused ) newpointcloud )； mapper _.insert points (msg _ filtered，unused.get ) )； LOOP_closure_.addkeyscanpair(0，msg )； 返回； } point cloud : ptr msg _ transformed (new point cloud； point cloud : ptr msg _ neighbors (new point cloud； 点云： ptr msg _ base (new point cloud； 点云： ptr msg _ fixed (new point cloud； //将当前帧数据转换为先前变换矩阵中的地图坐标系中的localization _.motion update (odometry _.getincrementalestimate ) )； localization _.transformpointstofixedframe (* msg _ filtered，msg_transformed.get ) )； //对应于地图坐标系中当前帧数据的最近邻点mapper _.approxnearestneighbors (* msg _ transformed，msg_neighbors.get ) )； //再次匹配将最近点返回到传感器的坐标系和当前帧，以获取准确的姿态变换矩阵localization _.transformpointstosensorframe (* msg _ neighbors， msg_neighbors.get ) ) localization _.measurement update (msg _ filtered，msg_neighbors，msg_base.get ) ) //环回检测bool new_keyframe； handleloopclosures (msg，new_keyframe ) ) t=0； //发现环回，调整地图的point cloud :3360 ptr regenerated _ map (new point cloud )； loop _ closure _.getmaximumlikelihoodpoints (regenerated _ map.get )； mapper_.Reset (； 点云： ptr unused (new point cloud )； mapper _.insert points (regenerated _ map，unused.get )； //保存的机器人的姿势也重新设定为localization _.setintegratedestimate (loop _ closure _.getlastpose ) )； }else{if(new_keyframe ) /不是环回检测，但将**关键帧**点云数据添加到地图的localization_.motionupdate ) gu 33603360 trext 通过使用上面的精确姿势变换矩阵，在地图坐标系中使用localization _.transformpointstofixedframe (* msg，msg_fixed.get ) )； 点云： ptr unused (new point cloud )； loop _ closure _.getmappoints (msg _ fixed.get )； ROS _ info (thesizeofget :3360 % d )，msg_fixed-points.size )； //映射中包含mapper_.insertpoints(msg_fixed，unused.get ) )； //公开姿势节点、里程表数据等loop_closure_.PublishPos
eGraph(); // 发布当前帧点云数据 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_); // 有了两帧数据 执行icp return UpdateICP();}可以看到这里是保存当前帧和上一帧的数据,通过两帧数据做匹配。下面就进入匹配的函数
bool PointCloudOdometry::UpdateICP() { // 计算两帧之间的转换---incremental transform GeneralizedIterativeClosestPoint<PointXYZ, PointXYZ> icp; //这里使用的GICP icp.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自定义的 位姿变换矩阵 transform3 incremental_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变换 , 位姿矩阵等名词不熟悉的话 需要自行百度去了解一下 再回头来看这段代码。
转载注明出处