前言
原理文字介绍略微简介,但是在代码注释中非常详细。我相信在代码中学习原理才能理解更加通透。
代码参考自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 parameters
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;
}
函数流: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 20
icp.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
四、注意事项
未完待续