开源激光SLAM项目BLAM－－－－－2
万俟穆冉
接上一章节提到的**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_);
// 有了两帧数据 执行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变换 , 位姿矩阵等名词不熟悉的话 需要自行百度去了解一下 再回头来看这段代码。
转载注明出处
类似资料：