两年前，南洋理工的王晗等人在loam的基础上，为减少计算量，提出了一个也是纯激光SLAM的工程，也在IROS上发表一篇文章《F-LOAM : Fast LiDAR Odometry and Mapping》。整体来看和loam的算法没有太大差异，只是工程上进行缩减，在我自己的数据包上实际测试效果也还不错吧。
A 传感器模型与特征提取
机械式三维激光雷达通过旋转一个尺寸为M的垂直排列的激光束阵列来感知周围的环境。它用M个平行读数扫描垂直面。在每个扫描间隔期间，激光阵列在水平面上以恒定速度旋转，同时激光测量按顺时针或逆时针顺序进行。
如上所述，三维机械激光雷达返回的点云在垂直方向上是稀疏的，在水平方向上是稠密的。因此，水平方向的特征更加明显，在水平方向上进行错误特征检测的可能性较小。对于每一个点云，我们聚焦在水平面上，并通过计算局部曲面的平滑度。
C 姿态估计
通过从边缘和平面特征图中收集附近的点来估计全局线和平面。对于每个边缘特征点， 从全局边缘特征图中计算其附近点的协方差矩阵。当点分布在一条直线上时，协方差矩阵包含一个更大的特征值。将与最大特征值相关的特征向量视为直线方向，直线的位置视为附近点的几何中心。类似地，对于每个平面特征点，这里可以得到一个具有位置和曲面范数的全局平面。注意，与全局边不同，全局平面的范数作为与最小特征值相关的特征向量。
f-loam是基于LOAM和ALOAM改进而来，计算时间缩小3倍，精度也有一定提高。本系统只是小提升。 其主要的原理和流程没有变化。首先是点云预处理、然后提取边缘和平面特征，分别进行匹配估计位姿，最后位姿融合。当然这里还提供了mapping和localization的模块，但mapping部分只是将估计的位姿，拼接点云，并没有后端优化等模块。
从以上的代码结构来看，主要有以下功能文件
- lidar.cpp: 系统的参数配置，比如雷达线束数量、扫描周期等
- laserProcessingClass.cpp、laserProcessingNode.cpp：输入原始的多线点云，进行点云预处理节点，与loam一致，包括边缘、平面特征提取
- odomEstimationNode.cpp、odomEstimationClass.cpp：里程计位姿估计节点，根据边缘点、平面点特征，构建点线、点面残差，利用ceres联合滑窗优化，估计位姿。得到机器人在里程计坐标系下的位姿，这里的里程计坐标系就是通常SLAM的map坐标系，它们是重合的。
- laserMappingNode.cpp、laserMappingClass.cpp：点云地图构建节点，根据odom信息拼接经过特征提取的点云，略微不一样的地方在使用栅格点云来表示地图。
- lidarOptimization.cpp：odom评价
由此可以看到，此工程没有后端优化的过程，经过前端匹配scan to local map得到残差进行优化直接得到了位姿估计。
http://xchu.net/2020/08/17/49floam/（1）laserProcessingNode.cpp
接收数据发出，整个工程的实现也不复杂 
laser_processing_process线程中循环执行，取点云数据->提取边缘和平面特征->发布相关点云。首先是取点云数据。
（2）odomEstimationNode.cpp
位姿估计节点主要接收边缘和平面特征，分别构建点到直线，点到平面的残差项，并通过ceres求解旋转四元数q和位移t，最后发出经过优化求解的里程计位姿。
用一个滑窗来维护corner和surface特征的local map，第一帧点云直接加入local map进行初始化。后续点云帧进来就构建约束。
构建点到直线的约束。寻找最近的5个点，对点云协方差矩阵进行主成份分析： 若这5个点分布在直线上，协方差矩阵的特征值包含一个元素显著大于其余两个，与该特征值相关的特征向量表示所处直线的方向; 若这5个点分布在平面上，协方差矩阵的特征值存在一个显著小的元素，与该特征值相关的特征向量表示所处平面的法线方向。参考2016,IROS,fast and robust 3d feature extraction from sparse point clouds.
//添加因子，将当前帧转到里程计坐标系下之后与local map构建残差
void OdomEstimationClass::addEdgeCostFactor(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, const pcl::PointCloud<pcl::PointXYZI>::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function){
int corner_num=0;
for (int i = 0; i < (int)pc_in->points.size(); i++)
{
pcl::PointXYZI point_temp;//输入的坐标为基于雷达的坐标系
//很容易理解，就是将当前点配准转到local map坐标系
pointAssociateToMap(&(pc_in->points[i]), &point_temp);
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
//与当前点最近邻搜索5个附近点
kdtreeEdgeMap->nearestKSearch(point_temp, 5, pointSearchInd, pointSearchSqDis);
if (pointSearchSqDis[4] < 1.0) //若最远的点都是小于1m的，则认为它是邻近点
{
std::vector<Eigen::Vector3d> nearCorners;
Eigen::Vector3d center(0, 0, 0);//初始化中心点为0
for (int j = 0; j < 5; j++)
{
Eigen::Vector3d tmp(map_in->points[pointSearchInd[j]].x,
map_in->points[pointSearchInd[j]].y,
map_in->points[pointSearchInd[j]].z); //返回这五个点的具体坐标
center = center + tmp; //一步步累加为了后面求均值
nearCorners.push_back(tmp);//将附近的点放到邻近角点向量中
}
center = center / 5.0; //累加后取均值作为最终的一个邻近点，与当前点的邻近点
Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero(); //给定协方差，初值2为0
for (int j = 0; j < 5; j++)
{
Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;//各方向给的距离中心的偏差值的平方向量作为方差
covMat = covMat + tmpZeroMean * tmpZeroMean.transpose(); //协方差矩阵相加
}//添加五个点的协方差
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);//矩阵特征值分解和特征向量获取
Eigen::Vector3d unit_direction = saes.eigenvectors().col(2); //得到特征向量Z方向的特征值
Eigen::Vector3d curr_point(pc_in->points[i].x, pc_in->points[i].y, pc_in->points[i].z);
//主成分分析，若有一个特征值明显大于另外一个特征值，说明是一条直线
if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])//通过特征值做对比判断
{
Eigen::Vector3d point_on_line = center; //将当前帧的邻近点作为直线上的一点，那么
Eigen::Vector3d point_a, point_b; //拟合的直线上 有上下两个点A、B
point_a = 0.1 * unit_direction + point_on_line; //OA = OC + 0.1 * AB
point_b = -0.1 * unit_direction + point_on_line;
//构建损失函数进行残差添加
//理想情况下当前点和AB点应该是共线的，因此curr_point 与 AB 线段距离应该为0,通过curr_point 与 AB 线段距离的残差
//注意这三个点都是在上一帧的map坐标系下，即，残差 = 点O到直线AB的距离
ceres::CostFunction *cost_function = new EdgeAnalyticCostFunction(curr_point, point_a, point_b);
problem.AddResidualBlock(cost_function, loss_function, parameters);
corner_num++;
}
}
}
if(corner_num<20){
printf("not enough correct points");
}
}点到平面的约束，通过kdtree获取Local map上最近的5个点，通过QR分解求解最小二乘问题获得平面参数，最后构建点到平面的残差。
然后进行残差计算优化
bool EdgeAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[0] + 4);
Eigen::Vector3d lp;
lp = q_last_curr * curr_point + t_last_curr;
Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b);
Eigen::Vector3d de = last_point_a - last_point_b;
double de_norm = de.norm();
residuals[0] = nu.norm()/de_norm;
if(jacobians != NULL)
{
if(jacobians[0] != NULL)
{
Eigen::Matrix3d skew_lp = skew(lp);
Eigen::Matrix<double, 3, 6> dp_by_se3;
dp_by_se3.block<3,3>(0,0) = -skew_lp;
(dp_by_se3.block<3,3>(0, 3)).setIdentity();
Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > J_se3(jacobians[0]);
J_se3.setZero();
Eigen::Matrix3d skew_de = skew(de);
J_se3.block<1,6>(0,0) = - nu.transpose() / nu.norm() * skew_de * dp_by_se3/de_norm;
}
}
return true;
} （3）laserMappingClass.cpp
在存储点云地图时，将地图按照点云栅格进行存储
void LaserMappingClass::init(double map_resolution){
//init map
//init can have real object, but future added block does not need
for(int i=0;i<LASER_CELL_RANGE_HORIZONTAL*2+1;i++){ //i 0-4 5层
//水平方向一定高度的地图 5X5
std::vector<std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>> map_height_temp;
for(int j=0;j<LASER_CELL_RANGE_HORIZONTAL*2+1;j++){
std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> map_depth_temp;//深度地图 高度方向 5个
for(int k=0;k<LASER_CELL_RANGE_VERTICAL*2+1;k++){
pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_temp(new pcl::PointCloud<pcl::PointXYZI>);
map_depth_temp.push_back(point_cloud_temp);
}
map_height_temp.push_back(map_depth_temp);
}
map.push_back(map_height_temp);//map为三维的栅格点云，每个栅格存储的是点云
}
origin_in_map_x = LASER_CELL_RANGE_HORIZONTAL; //2
origin_in_map_y = LASER_CELL_RANGE_HORIZONTAL; //2
origin_in_map_z = LASER_CELL_RANGE_VERTICAL; //2
map_width = LASER_CELL_RANGE_HORIZONTAL*2+1;
map_height = LASER_CELL_RANGE_HORIZONTAL*2+1;
map_depth = LASER_CELL_RANGE_HORIZONTAL*2+1; //5
//downsampling size
downSizeFilter.setLeafSize(map_resolution, map_resolution, map_resolution);
}将点云按照一个个大栅格划分，一个小栅格的尺寸是50mX50mX50m的立方体栅格，最大为5个栅格，存储5×50=250m范围内的点云地图。存储时只是简单的拼接。
//update points to map
void LaserMappingClass::updateCurrentPointsToMap(const pcl::PointCloud<pcl::PointXYZI>::Ptr& pc_in, const Eigen::Isometry3d& pose_current){
//当前位置，根据雷达LASER_CELL_WIDTH的宽度50得到当前位置的索引，就是在50m范围内划分格子，一个格子50m
int currentPosIdX = int(std::floor(pose_current.translation().x() / LASER_CELL_WIDTH + 0.5)) + origin_in_map_x;
int currentPosIdY = int(std::floor(pose_current.translation().y() / LASER_CELL_HEIGHT + 0.5)) + origin_in_map_y;
int currentPosIdZ = int(std::floor(pose_current.translation().z() / LASER_CELL_DEPTH + 0.5)) + origin_in_map_z;
//check is submap is null
checkPoints(currentPosIdX,currentPosIdY,currentPosIdZ);
pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_pc(new pcl::PointCloud<pcl::PointXYZI>());
pcl::transformPointCloud(*pc_in, *transformed_pc, pose_current.cast<float>());
//save points
for (int i = 0; i < (int)transformed_pc->points.size(); i++)
{
pcl::PointXYZI point_temp = transformed_pc->points[i];
//for visualization only
point_temp.intensity = std::min(1.0 , std::max(pc_in->points[i].z+2.0, 0.0) / 5);
int currentPointIdX = int(std::floor(point_temp.x / LASER_CELL_WIDTH + 0.5)) + origin_in_map_x;
int currentPointIdY = int(std::floor(point_temp.y / LASER_CELL_HEIGHT + 0.5)) + origin_in_map_y;
int currentPointIdZ = int(std::floor(point_temp.z / LASER_CELL_DEPTH + 0.5)) + origin_in_map_z;
map[currentPointIdX][currentPointIdY][currentPointIdZ]->push_back(point_temp);
}
//filtering points
for(int i=currentPosIdX-LASER_CELL_RANGE_HORIZONTAL;i<currentPosIdX+LASER_CELL_RANGE_HORIZONTAL+1;i++){
for(int j=currentPosIdY-LASER_CELL_RANGE_HORIZONTAL;j<currentPosIdY+LASER_CELL_RANGE_HORIZONTAL+1;j++){
for(int k=currentPosIdZ-LASER_CELL_RANGE_VERTICAL;k<currentPosIdZ+LASER_CELL_RANGE_VERTICAL+1;k++){
downSizeFilter.setInputCloud(map[i][j][k]);
downSizeFilter.filter(*(map[i][j][k]));
}
}
}
其他的部分就不赘述了，有机会再继续探讨。
扫一扫
1018
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
