前言

原理文字介绍略微简介，但是在代码注释中非常详细。我相信在代码中学习原理才能理解更加通透。
代码参考自livox sdk:
gitcode

一、算法原理

二、源码解析

函数流:main->BlamSlam::ProcessPointCloudMessage
点云处理流程为:滤波->帧间匹配->帧图匹配->插入地图

滤波类PointCloudFilter

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudFilter::Filter

/*** 滤波* @param points 输入* @param points_filtered 输出* @return */
bool PointCloudFilter::Filter(const PointCloud::ConstPtr& points,PointCloud::Ptr points_filtered) const

根据参数选择各种下采样、噪点滤除操作。详情见代码和注释:

  // Apply a random downsampling filter to the incoming point cloud.if (params_.random_filter) {const int n_points = static_cast<int>((1.0 - params_.decimate_percentage) *points_filtered->size());pcl::RandomSample<pcl::POINT_TYPE> random_filter;random_filter.setSample(n_points);random_filter.setInputCloud(points_filtered);random_filter.filter(*points_filtered);}// Apply a voxel grid filter to the incoming point cloud.if (params_.grid_filter) {pcl::VoxelGrid<pcl::POINT_TYPE> grid;grid.setLeafSize(params_.grid_res, params_.grid_res, params_.grid_res);grid.setInputCloud(points_filtered);grid.filter(*points_filtered);}// Remove statistical outliers in incoming the point cloud.if (params_.outlier_filter) {pcl::StatisticalOutlierRemoval<pcl::POINT_TYPE> sor;sor.setInputCloud(points_filtered);sor.setMeanK(params_.outlier_knn);sor.setStddevMulThresh(params_.outlier_std);sor.filter(*points_filtered);}// Remove points without a threshold number of neighbors within a specified// radius.if (params_.radius_filter) {pcl::RadiusOutlierRemoval<pcl::POINT_TYPE> rad;rad.setInputCloud(points_filtered);rad.setRadiusSearch(params_.radius);rad.setMinNeighborsInRadius(params_.radius_knn);rad.filter(*points_filtered);}

前端里程计类PointCloudOdometry

第一帧不需要icp匹配，直接插入地图

if (!odometry_.UpdateEstimate(*msg_filtered)) {PointCloud::Ptr unused(new PointCloud);mapper_.InsertPoints(msg_filtered, unused.get());//loop_closure_.AddKeyScanPair(0, msg);return;}

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudOdometry::UpdateEstimate->PointCloudOdometry::UpdateICP
如果非第一帧，则开始gicp匹配

bool PointCloudOdometry::UpdateICP() {{TicToc step("odicp");// Compute the incremental transformation.GeneralizedIterativeClosestPoint<POINT_TYPE, POINT_TYPE> icp;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();// Update pose estimates.incremental_estimate_.translation = gu::Vec3(T(0, 3), T(1, 3), T(2, 3));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));}// Only update if the incremental transform is small enough. 目测位姿增量太大就不要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 {std::cout<<"这一帧不靠谱！！！"<<std::endl;}return true;
}

其中query_为当前帧，reference_为前一帧，如果位姿增量在合理范围，就认为匹配有效。

地图类PointCloudMapper

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudMapper::InsertPoints
把点云插入地图

bool PointCloudMapper::InsertPoints(const PointCloud::ConstPtr& points,PointCloud* incremental_points) {if (!initialized_) {return false;}if (incremental_points == NULL) {return false;}incremental_points->clear();if (map_mutex_.try_lock()) {for (size_t ii = 0; ii < points->points.size(); ++ii) {const pcl::POINT_TYPE p = points->points[ii];
//      if (!map_octree_->isVoxelOccupiedAtPoint(p)) {//会报错，百度修改后如下double min_x, min_y, min_z, max_x, max_y, max_z;map_octree_->getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z);if (!isInBox || !map_octree_->isVoxelOccupiedAtPoint(p)) //判断点所处的空间是否存在与八叉树体素中{map_octree_->addPointToCloud(p, map_data_);incremental_points->push_back(p);}}map_mutex_.unlock();} else {}incremental_points->header = points->header;incremental_points->header.frame_id = fixed_frame_id_;map_updated_ = true;return true;
}

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudMapper::ApproxNearestNeighbors
在地图里面寻找当前点云(map坐标系)的邻近点云

bool PointCloudMapper::ApproxNearestNeighbors(const PointCloud& points,PointCloud* neighbors) {if (!initialized_) {}if (neighbors == NULL) {}neighbors->points.clear();for (size_t ii = 0; ii < points.points.size(); ++ii) {float unused = 0.f;int result_index = -1; map_octree_->approxNearestSearch(points.points[ii], result_index, unused);if (result_index >= 0)neighbors->push_back(map_data_->points[result_index]);}   return neighbors->points.size() > 0;
}

帧图匹配类PointCloudLocalization

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudLocalization::MotionUpdate
先保存前端位姿增量

bool PointCloudLocalization::MotionUpdate(const gu::Transform3& incremental_odom) {// Store the incremental transform from odometry.incremental_estimate_ = incremental_odom;return true;
}

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudLocalization::TransformPointsToFixedFrame
将当前帧滤波后点云转换到map坐标系

bool PointCloudLocalization::TransformPointsToFixedFrame(const PointCloud& points, PointCloud* points_transformed) const {if (points_transformed == NULL) {return false;}// Compose the current incremental estimate (from odometry) with the// integrated estimate, and transform the incoming point cloud.const gu::Transform3 estimate =gu::PoseUpdate(integrated_estimate_, incremental_estimate_);const Eigen::Matrix<double, 3, 3> R = estimate.rotation.Eigen();const Eigen::Matrix<double, 3, 1> T = estimate.translation.Eigen();Eigen::Matrix4d tf;tf.block(0, 0, 3, 3) = R;tf.block(0, 3, 3, 1) = T;pcl::transformPointCloud(points, *points_transformed, tf);return true;
}

在地图里面寻找当前点云(map坐标系)的邻近点云
mapper_.ApproxNearestNeighbors(*msg_transformed, msg_neighbors.get());//当前点云(map坐标系)寻找邻近点云

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudLocalization::TransformPointsToSensorFrame
将邻近点点云msg_neighbors反转换到雷达坐标系

bool PointCloudLocalization::TransformPointsToSensorFrame(const PointCloud& points, PointCloud* points_transformed) const {if (points_transformed == NULL) {return false;}// Compose the current incremental estimate (from odometry) with the// integrated estimate, then invert to go from world to sensor frame.const gu::Transform3 estimate = gu::PoseInverse(gu::PoseUpdate(integrated_estimate_, incremental_estimate_));//integrated_estimate from config parametersconst Eigen::Matrix<double, 3, 3> R = estimate.rotation.Eigen();const Eigen::Matrix<double, 3, 1> T = estimate.translation.Eigen();Eigen::Matrix4d tf;tf.block(0, 0, 3, 3) = R;tf.block(0, 3, 3, 1) = T;pcl::transformPointCloud(points, *points_transformed, tf);return true;
}

函数流:BlamSlam::ProcessPointCloudMessage->PointCloudLocalization::MeasurementUpdate
帧图匹配就是在当前帧点云与地图中的临近点之间进行的，更新当前帧到地图位姿integrated_estimate_。其中query为当前帧，reference为地图，如果位姿增量在合理范围，就认为匹配有效。

bool PointCloudLocalization::MeasurementUpdate(const PointCloud::Ptr& query,const PointCloud::Ptr& reference,PointCloud* aligned_query) {if (aligned_query == NULL) {return false;}// ICP-based alignment. Generalized ICP does (roughly) plane-to-plane// matching, and is much more robust than standard ICP.GeneralizedIterativeClosestPoint<pcl::POINT_TYPE, pcl::POINT_TYPE> icp;icp.setTransformationEpsilon(params_.tf_epsilon);icp.setMaxCorrespondenceDistance(params_.corr_dist);icp.setMaximumIterations(params_.iterations);icp.setRANSACIterations(0);icp.setMaximumOptimizerIterations(20); // default 20icp.setInputSource(query);icp.setInputTarget(reference);PointCloud unused;icp.align(unused);// Retrieve transformation and estimate and update.const Eigen::Matrix4f T = icp.getFinalTransformation();pcl::transformPointCloud(*query, *aligned_query, T);gu::Transform3 pose_update;//本帧到地图位姿增量pose_update.translation = gu::Vec3(T(0, 3), T(1, 3), T(2, 3));pose_update.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));// Only update if the transform is small enough.if (!transform_thresholding_ ||(pose_update.translation.Norm() <= max_translation_ &&pose_update.rotation.ToEulerZYX().Norm() <= max_rotation_)) {incremental_estimate_ = gu::PoseUpdate(incremental_estimate_, pose_update);} else { }integrated_estimate_ =gu::PoseUpdate(integrated_estimate_, incremental_estimate_);return true;
}

下一步把增量清零 localization_.MotionUpdate(gu::Transform3::Identity())，再当前帧转到地图
函数流:BlamSlam::ProcessPointCloudMessage->PointCloudLocalization::TransformPointsToFixedFrame

三、实验结果

mkdir build
cd build
cmake …
make
./mapping

四、注意事项

未完待续

多激光雷达外参标定算法与源码解析(一):基于BLAM的建图模块相关推荐

相机和livox激光雷达外参标定：ROS功能包---livox_camera_lidar_calibration 介绍
相机和livox激光雷达外参标定:ROS功能包---livox_camera_lidar_calibration 相机与激光雷达外参标定功能包介绍环境配置及功能包安装功能包节点准备内容 1 l ...
相机和livox激光雷达外参标定：ROS功能包---livox_camera_lidar_calibration 使用方法
相机和livox激光雷达外参标定:ROS功能包---livox_camera_lidar_calibration 使用方法 livox_camera_lidar_calibration 功能包介绍使 ...
安卓开发者必看：Android的数据结构与算法——ArrayList源码解析
作者:JerryloveEmily 原文链接:https://www.jianshu.com/p/159426e2aaf6 文章有点长,比较啰嗦,请耐心看完! 一.概述首先得明白ArrayList在 ...
Android的数据结构与算法----ArrayList源码解析
转载请标明出处: http://blog.csdn.net/abren32/article/details/56669369 本文出自JerryloveEmily的博客文章有点长,比较啰嗦,请耐心看 ...
Java 并发编程CyclicBarrier的应用与源码解析（基于ReentrantLock实现）
什么是CyclicBarrier? CyclicBarrie和上一篇中讲到CountDownLatch很类似,它能阻塞一组线程直到某个事件的发生. 栅栏与闭锁的关键区别在于:所有必须同时到达栅栏位置才 ...
velo2cam_calibration——最新最准确的激光雷达Lidar和相机Camera外参标定算法实现
目录写在前面参考资料算法原理准备工作开始使用检测结果总结写在前面因为实验需求,要实现相机和雷达之间的融合,因此需要完成相机内参标定和雷达与相机外参标定. 相机内参标定使用ros自带的 ...
SLAM论文阅读：M-Loam：具有在线外参校准功能的多LiDAR系统的稳健里程表和建图
基于loam的多激光雷达slam 论文题目: Robust Odometry and Mapping for Multi-LiDAR Systems with Online Extrinsic Cal ...
数据结构算法 - ConcurrentHashMap 源码解析
Linux编程点击右侧关注,免费入门到精通! 作者丨红橙Darren https://www.jianshu.com/p/0b452a6e4f4e 五个线程同时往 HashMap 中 put 数据会 ...
MyBatis源码解析（十二）——binding绑定模块之MapperRegisty
原创作品,可以转载,但是请标注出处地址:http://www.cnblogs.com/V1haoge/p/6758456.html 1.回顾之前解析了解析模块parsing,其实所谓的解析模块就是为 ...

多激光雷达外参标定算法与源码解析(一):基于BLAM的建图模块

前言