0. 简介
现在越来越多的激光雷达方法如雨后春笋一般冒了出来，最近以FAST-LIO为代表的系列渐渐地被大众接受。而FAST-LIO2的工作越来越被大众所熟知与研究。最近作者也在研究与学习FAST-LIO2的相关知识，这里将会以长文的形式来介绍FAST-LIO2.0的学习心得。本文将通过代码的形式与论文结合，直观的来分析论文的核心算法部分。也同时作为自己的学习笔记来留给后来人学习。
1. 激光雷达
对于FAST-LIO2代码来说，主要的数据是通过preprocess与IMU_Processing拿到相应的传感器数据，并在主程序当中调用ESF与IKD-Tree完成优化。首先我们来看一下激光雷达部分。
1.1 preprocess.h
首先在最上面先定义了一系列enum的信息，通过enum选择不同的激光雷达类型，特征点特征等，并通过orgtype存放一些激光雷达点的一些其他属性。
// 枚举类型：表示支持的雷达类型
enum LID_TYPE
{
AVIA = 1,
VELO16,
OUST64
}; //{1, 2, 3}
// 枚举类型：表示特征点的类型
enum Feature
{
Nor, // 正常点
Poss_Plane, // 可能的平面点
Real_Plane, // 确定的平面点
Edge_Jump, // 有跨越的边
Edge_Plane, // 边上的平面点
Wire, // 线段 这个也许当了无效点？也就是空间中的小线段？
ZeroPoint // 无效点 程序中未使用
};
// 枚举类型：位置标识
enum Surround
{
Prev, // 前一个
Next // 后一个
};
// 枚举类型：表示有跨越边的类型
enum E_jump
{
Nr_nor, // 正常
Nr_zero, // 0
Nr_180, // 180
Nr_inf, // 无穷大 跳变较远？
Nr_blind // 在盲区？
};
// orgtype类：用于存储激光雷达点的一些其他属性
struct orgtype
{
double range; // 点云在xy平面离雷达中心的距离
double dista; // 当前点与后一个点之间的距离
//假设雷达原点为O 前一个点为M 当前点为A 后一个点为N
double angle[2]; // 这个是角OAM和角OAN的cos值
double intersect; // 这个是角MAN的cos值
E_jump edj[2]; // 前后两点的类型
Feature ftype; // 点类型
// 构造函数
orgtype()
{
range = 0;
edj[Prev] = Nr_nor;
edj[Next] = Nr_nor;
ftype = Nor; //默认为正常点
intersect = 2;
}
};
其中LID_TYPE这些与下面的数据结构有关系。通过enum选择不同的激光雷达数据，来设置不同的数据结构。
// velodyne数据结构
namespace velodyne_ros
{
struct EIGEN_ALIGN16 Point
{
PCL_ADD_POINT4D; // 4D点坐标类型
float intensity; // 强度
float time; // 时间
uint16_t ring; // 点所属的圈数
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 进行内存对齐
};
} // namespace velodyne_ros
// 注册velodyne_ros的Point类型
POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, time, time)(uint16_t, ring, ring))
// ouster数据结构
namespace ouster_ros
{
struct EIGEN_ALIGN16 Point
{
PCL_ADD_POINT4D; // 4D点坐标类型
float intensity; // 强度
uint32_t t; // 时间
uint16_t reflectivity; // 反射率
uint8_t ring; // 点所属的圈数
uint16_t ambient; // 没用到
uint32_t range; // 距离
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 进行内存对齐
};
} // namespace ouster_ros
// clang-format off
// 注册ouster的Point类型
POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
// use std::uint32_t to avoid conflicting with pcl::uint32_t
(std::uint32_t, t, t)
(std::uint16_t, reflectivity, reflectivity)
(std::uint8_t, ring, ring)
(std::uint16_t, ambient, ambient)
(std::uint32_t, range, range)
)
然后下面的这些就是Preprocess的主体函数了，主要作用是用于对激光雷达点云数据进行预处理。
// Preproscess类：用于对激光雷达点云数据进行预处理
class Preprocess
{
public:
// EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Preprocess(); // 构造函数
~Preprocess(); // 析构函数
void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); // 对Livox自定义Msg格式的激光雷达数据进行处理
void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); // 对ros的Msg格式的激光雷达数据进行处理
void set(bool feat_en, int lid_type, double bld, int pfilt_num);
// sensor_msgs::PointCloud2::ConstPtr pointcloud;
PointCloudXYZI pl_full, pl_corn, pl_surf; // 全部点、边缘点、平面点
PointCloudXYZI pl_buff[128]; //maximum 128 line lidar
vector<orgtype> typess[128]; //maximum 128 line lidar
int lidar_type, point_filter_num, N_SCANS, SCAN_RATE; // 雷达类型、采样间隔、扫描线数、扫描频率
double blind; // 最小距离阈值(盲区)
bool feature_enabled, given_offset_time; // 是否提取特征、是否进行时间偏移
ros::Publisher pub_full, pub_surf, pub_corn; // 发布全部点、发布平面点、发布边缘点
private:
void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); // 用于对Livox激光雷达数据进行处理
void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); // 用于对ouster激光雷达数据进行处理
void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); // 用于对velodyne激光雷达数据进行处理
void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
void pub_func(PointCloudXYZI &pl, const ros::Time &ct);
int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
bool small_plane(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); // 没有用到
bool edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir);
int group_size;
double disA, disB, inf_bound;
double limit_maxmid, limit_midmin, limit_maxmin;
double p2l_ratio;
double jump_up_limit, jump_down_limit;
double cos160;
double edgea, edgeb;
double smallp_intersect, smallp_ratio;
double vx, vy, vz;
};
1.2 preprocess.cpp
上面是头文件，详细的代码在preprocess.cpp中，这里我们来详细介绍每一个函数，首先是构造函数和析构函数，里面主要存放一些雷达的参数
Preprocess::Preprocess()
: feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1)
{
inf_bound = 10; // 有效点集合,大于10m则是盲区
N_SCANS = 6; //多线激光雷达的线数
group_size = 8; // 8个点为一组
disA = 0.01; // 点集合的距离阈值,判断是否为平面
disB = 0.1; // 点集合的距离阈值,判断是否为平面
p2l_ratio = 225; // 点到线的距离阈值，需要大于这个值才能判断组成面
limit_maxmid = 6.25; // 中点到左侧的距离变化率范围
limit_midmin = 6.25; // 中点到右侧的距离变化率范围
limit_maxmin = 3.24; // 左侧到右侧的距离变化率范围
jump_up_limit = 170.0;
jump_down_limit = 8.0;
cos160 = 160.0;
edgea = 2; //点与点距离超过两倍则认为遮挡
edgeb = 0.1; //点与点距离超过0.1m则认为遮挡
smallp_intersect = 172.5;
smallp_ratio = 1.2; //三个点如果角度大于172.5度，且比例小于1.2倍，则认为是平面
given_offset_time = false; //是否提供时间偏移
jump_up_limit = cos(jump_up_limit / 180 * M_PI); //角度大于170度的点跳过，认为在
jump_down_limit = cos(jump_down_limit / 180 * M_PI); //角度小于8度的点跳过
cos160 = cos(cos160 / 180 * M_PI); //夹角限制
smallp_intersect = cos(smallp_intersect / 180 * M_PI); //三个点如果角度大于172.5度，且比例小于1.2倍，则认为是平面
}
下面一个函数是set函数，这里可以在laserMapping.cpp被使用，可是该函数并未被使用。
void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)
{
feature_enabled = feat_en; //是否提取特征点
lidar_type = lid_type; //雷达的种类
blind = bld; //最小距离阈值，即过滤掉0～blind范围内的点云
point_filter_num = pfilt_num; //采样间隔，即每隔point_filter_num个点取1个点
}
下面的Preprocess预处理函数主要包含了不同激光雷达的处理，这里在laserMapping.cpp被调用，从而拿到处理后的点云，并初步完成了筛选。下面两个大同小异就不仔细说了
/**
* @brief Livox激光雷达点云预处理函数
*
* @param msg livox激光雷达点云数据，格式为livox_ros_driver::CustomMsg
* @param pcl_out 输出处理后的点云数据，格式为pcl::PointCloud<pcl::PointXYZINormal>
*/
void Preprocess::process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
{
avia_handler(msg);
*pcl_out = pl_surf;
}
void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
{
switch (lidar_type)
{
case OUST64:
oust64_handler(msg);
break;
case VELO16:
velodyne_handler(msg);
break;
default:
printf("Error LiDAR Type");
break;
}
*pcl_out = pl_surf;
}
下面就是handle函数来预处理发出的点云数据。这里我们展示了Livox激光雷达点云数据进行预处理。这里的操作是拿到livox原始数据，然后通过线束和反射率重新拼接了
void Preprocess::avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg)
{
// 清除之前的点云缓存
pl_surf.clear(); // 清除之前的平面点云缓存
pl_corn.clear(); // 清除之前的角点云缓存
pl_full.clear(); // 清除之前的全点云缓存
double t1 = omp_get_wtime(); // 后面没用到
int plsize = msg->point_num; // 一帧中的点云总个数
// cout<<"plsie: "<<plsize<<endl;
pl_corn.reserve(plsize); // 分配空间
pl_surf.reserve(plsize); // 分配空间
pl_full.resize(plsize); // 分配空间
for (int i = 0; i < N_SCANS; i++)
{
pl_buff[i].clear();
pl_buff[i].reserve(plsize); // 每一个scan保存的点云数量
}
uint valid_num = 0; // 有效的点云数
// 特征提取（FastLIO2默认不进行特征提取）
if (feature_enabled)
{
// 分别对每个点云进行处理
for (uint i = 1; i < plsize; i++)
{
// 只取线数在0~N_SCANS内并且回波次序为0或者1的点云
if ((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
{
pl_full[i].x = msg->points[i].x; // 点云x轴坐标
pl_full[i].y = msg->points[i].y; // 点云y轴坐标
pl_full[i].z = msg->points[i].z; // 点云z轴坐标
pl_full[i].intensity = msg->points[i].reflectivity; // 点云强度
pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // 使用曲率作为每个激光点的时间
bool is_new = false;
// 只有当当前点和上一点的间距足够大（>1e-7），才将当前点认为是有用的点，分别加入到对应line的pl_buff队列中
if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7))
{
pl_buff[msg->points[i].line].push_back(pl_full[i]); // 将当前点加入到对应line的pl_buff队列中
}
}
}
static int count = 0;
static double time = 0.0;
count++;
double t0 = omp_get_wtime();
// 对每个line中的激光雷达分别进行处理
for (int j = 0; j < N_SCANS; j++)
{
// 如果该line中的点云过小，则继续处理下一条line
if (pl_buff[j].size() <= 5)
continue;
pcl::PointCloud<PointType> &pl = pl_buff[j];
plsize = pl.size();
vector<orgtype> &types = typess[j];
types.clear();
types.resize(plsize);
plsize--;
for (uint i = 0; i < plsize; i++)
{
types[i].range = pl[i].x * pl[i].x + pl[i].y * pl[i].y; // 计算每个点到机器人本身的距离
vx = pl[i].x - pl[i + 1].x;
vy = pl[i].y - pl[i + 1].y;
vz = pl[i].z - pl[i + 1].z;
types[i].dista = vx * vx + vy * vy + vz * vz; // 计算两个间隔点的距离
}
//因为i最后一个点没有i+1了所以就单独求了一个range，没有distance
types[plsize].range = pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y;
give_feature(pl, types); //给特征
// pl_surf += pl;
}
time += omp_get_wtime() - t0;
printf("Feature extraction time: %lf \n", time / count);
}
else
{
// 分别对每个点云进行处理
for (uint i = 1; i < plsize; i++)
{
// 只取线数在0~N_SCANS内并且回波次序为0或者1的点云
if ((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
{
valid_num++; // 有效的点云数
// 等间隔降采样
if (valid_num % point_filter_num == 0)
{
pl_full[i].x = msg->points[i].x; // 点云x轴坐标
pl_full[i].y = msg->points[i].y; // 点云y轴坐标
pl_full[i].z = msg->points[i].z; // 点云z轴坐标
pl_full[i].intensity = msg->points[i].reflectivity; // 点云强度
pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points
// 只有当当前点和上一点的间距足够大（>1e-7），并且在最小距离阈值之外，才将当前点认为是有用的点，加入到pl_surf队列中
if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7) || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7) || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7) && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y > blind))
{
pl_surf.push_back(pl_full[i]);
}
}
}
}
}
}
在程序中我们可以看到give_feature(pl, types);这个函数主要是特征提取，下面我们对该函数进行学习和了解，对于每条line的点云提取特征
void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types)
{
int plsize = pl.size(); //单条线的点数
int plsize2;
if (plsize == 0)
{
printf("something wrong\n");
return;
}
uint head = 0;
//不能在盲区 从这条线非盲区的点开始
while (types[head].range < blind)
{
head++;
}
// Surf
// group_size默认等于8
plsize2 = (plsize > group_size) ? (plsize - group_size) : 0; //判断当前点后面是否还有8个点 够的话就逐渐减少
Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); //当前平面的法向量
Eigen::Vector3d last_direct(Eigen::Vector3d::Zero()); //上一个平面的法向量
uint i_nex = 0, i2; // i2为当前点的下一个点
uint last_i = 0; // last_i为上一个点的保存的索引
uint last_i_nex = 0; // last_i_nex为上一个点的下一个点的索引
int last_state = 0; //为1代表上个状态为平面 否则为0
//判断面点
int plane_type;
// 拿到8个点用于判断平面
for (uint i = head; i < plsize2; i++)
{
if (types[i].range < blind) // 在盲区范围内的点不做处理
continue;
{
continue;
}
i2 = i; //更新i2
plane_type = plane_judge(pl, types, i, i_nex, curr_direct); //求得平面，并返回类型0 1 2
if (plane_type == 1) //返回1一般默认是平面
{
//设置确定的平面点和可能的平面点
for (uint j = i; j <= i_nex; j++)
{
if (j != i && j != i_nex)
{
//把起始点和终止点之间的所有点设置为确定的平面点
types[j].ftype = Real_Plane;
}
else
{
//把起始点和终止点设置为可能的平面点
types[j].ftype = Poss_Plane;
}
}
// if(last_state==1 && fabs(last_direct.sum())>0.5)
//最开始last_state=0直接跳过
//之后last_state=1
//如果之前状态是平面则判断当前点是处于两平面边缘的点还是较为平坦的平面的点
if (last_state == 1 && last_direct.norm() > 0.1)
{
double mod = last_direct.transpose() * curr_direct;
if (mod > -0.707 && mod < 0.707)
{
//修改ftype，两个面法向量夹角在45度和135度之间 认为是两平面边缘上的点
types[i].ftype = Edge_Plane;
}
else
{
//否则认为是真正的平面点
types[i].ftype = Real_Plane;
}
}
i = i_nex - 1;
last_state = 1;
}
else // if(plane_type == 2)
{
// plane_type=0或2的时候
i = i_nex;
last_state = 0; //设置为不是平面点
}
last_i = i2; //更新last_i
last_i_nex = i_nex; //更新last_i_nex
last_direct = curr_direct; //更新last_direct
}
//判断边缘点
plsize2 = plsize > 3 ? plsize - 3 : 0; //如果剩下的点数小于3则不判断边缘点，否则计算哪些点是边缘点
for (uint i = head + 3; i < plsize2; i++)
{
//点不能在盲区 或者 点必须属于正常点和可能的平面点
if (types[i].range < blind || types[i].ftype >= Real_Plane)
{
continue;
}
//该点与前后点的距离不能挨的太近
if (types[i - 1].dista < 1e-16 || types[i].dista < 1e-16)
{
continue;
}
Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); //当前点组成的向量
Eigen::Vector3d vecs[2];
for (int j = 0; j < 2; j++)
{
int m = -1;
if (j == 1)
{
m = 1;
}
//若当前的前/后一个点在盲区内（4m)
if (types[i + m].range < blind)
{
if (types[i].range > inf_bound) //若其大于10m
{
types[i].edj[j] = Nr_inf; //赋予该点Nr_inf(跳变较远)
}
else
{
types[i].edj[j] = Nr_blind; //赋予该点Nr_blind(在盲区)
}
continue;
}
vecs[j] = Eigen::Vector3d(pl[i + m].x, pl[i + m].y, pl[i + m].z);
vecs[j] = vecs[j] - vec_a; //前/后点指向当前点的向量
//若雷达坐标系原点为O 当前点为A 前/后一点为M和N
//则下面OA点乘MA/（|OA|*|MA|）
//得到的是cos角OAM的大小
types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); //角MAN的cos值
//前一个点是正常点 && 下一个点在激光线上 && 当前点与后一个点的距离大于0.0225m && 当前点与后一个点的距离大于当前点与前一个点距离的四倍
//这种边缘点像是7字形这种的边缘？
if (types[i].angle[j] < jump_up_limit) // cos(170)
{
types[i].edj[j] = Nr_180; // M在OA延长线上
}
else if (types[i].angle[j] > jump_down_limit) // cos(8)
{
types[i].edj[j] = Nr_zero; // M在OA上
}
}
types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm();
if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_zero && types[i].dista > 0.0225 && types[i].dista > 4 * types[i - 1].dista)
{
if (types[i].intersect > cos160) //角MAN要小于160度 不然就平行于激光了
{
if (edge_jump_judge(pl, types, i, Prev))
{
types[i].ftype = Edge_Jump;
}
}
}
//与上面类似
//前一个点在激光束上 && 后一个点正常 && 前一个点与当前点的距离大于0.0225m && 前一个点与当前点的距离大于当前点与后一个点距离的四倍
else if (types[i].edj[Prev] == Nr_zero && types[i].edj[Next] == Nr_nor && types[i - 1].dista > 0.0225 && types[i - 1].dista > 4 * types[i].dista)
{
if (types[i].intersect > cos160)
{
if (edge_jump_judge(pl, types, i, Next))
{
types[i].ftype = Edge_Jump;
}
}
}
//前面的是正常点 && (当前点到中心距离>10m并且后点在盲区)
else if (types[i].edj[Prev] == Nr_nor && types[i].edj[Next] == Nr_inf)
{
if (edge_jump_judge(pl, types, i, Prev))
{
types[i].ftype = Edge_Jump;
}
}
//(当前点到中心距离>10m并且前点在盲区) && 后面的是正常点
else if (types[i].edj[Prev] == Nr_inf && types[i].edj[Next] == Nr_nor)
{
if (edge_jump_judge(pl, types, i, Next))
{
types[i].ftype = Edge_Jump;
}
}
//前后点都不是正常点
else if (types[i].edj[Prev] > Nr_nor && types[i].edj[Next] > Nr_nor)
{
if (types[i].ftype == Nor)
{
types[i].ftype = Wire; //程序中应该没使用 当成空间中的小线段或者无用点了
}
}
}
plsize2 = plsize - 1;
double ratio;
//继续找平面点
for (uint i = head + 1; i < plsize2; i++)
{
//前面、当前、之后三个点都需要不在盲区内
if (types[i].range < blind || types[i - 1].range < blind || types[i + 1].range < blind)
{
continue;
}
//前面和当前 当前和之后的点与点之间距离都不能太近
if (types[i - 1].dista < 1e-8 || types[i].dista < 1e-8)
{
continue;
}
//还剩下来一些正常点继续找平面点
if (types[i].ftype == Nor)
{
//求点与点间距的比例 大间距/小间距
if (types[i - 1].dista > types[i].dista)
{
ratio = types[i - 1].dista / types[i].dista;
}
else
{
ratio = types[i].dista / types[i - 1].dista;
}
//如果夹角大于172.5度 && 间距比例<1.2
if (types[i].intersect < smallp_intersect && ratio < smallp_ratio)
{
//前后三个点认为是平面点
if (types[i - 1].ftype == Nor)
{
types[i - 1].ftype = Real_Plane;
}
if (types[i + 1].ftype == Nor)
{
types[i + 1].ftype = Real_Plane;
}
types[i].ftype = Real_Plane;
}
}
}
//存储平面点
int last_surface = -1;
for (uint j = head; j < plsize; j++)
{
//可能的平面点和确定的平面点
if (types[j].ftype == Poss_Plane || types[j].ftype == Real_Plane)
{
if (last_surface == -1)
{
last_surface = j;
}
//通常连着好几个都是面点
//必须在采样间隔上的平面点才使用（这里的是无差别滤波 从每次新找到面点开始每几个点才取一个）
if (j == uint(last_surface + point_filter_num - 1))
{
PointType ap;
ap.x = pl[j].x;
ap.y = pl[j].y;
ap.z = pl[j].z;
ap.intensity = pl[j].intensity;
ap.curvature = pl[j].curvature;
pl_surf.push_back(ap);
last_surface = -1;
}
}
else
{
//跳变较大的边缘边的点 位于平面边缘的点
if (types[j].ftype == Edge_Jump || types[j].ftype == Edge_Plane)
{
pl_corn.push_back(pl[j]);
}
//假如上次找到的面点被无差别滤波掉了，而此时已经到了边缘
if (last_surface != -1)
{
PointType ap;
//取上次面点到此次边缘线之间的所有点的重心当作一个面点存储进去
for (uint k = last_surface; k < j; k++)
{
ap.x += pl[k].x;
ap.y += pl[k].y;
ap.z += pl[k].z;
ap.intensity += pl[k].intensity;
ap.curvature += pl[k].curvature;
}
ap.x /= (j - last_surface);
ap.y /= (j - last_surface);
ap.z /= (j - last_surface);
ap.intensity /= (j - last_surface);
ap.curvature /= (j - last_surface);
pl_surf.push_back(ap);
}
last_surface = -1;
}
}
}
在give_feature函数中存在了提取面特征与提取角特征的函数，下面我们来详细分析，面特征主要是主要是依据点与点之间的距离来拟合成一个向量，并通过乘积来计算出面特征的提取。这里的面特征提取和边缘特征提取和LOAM的类似，这里就用LOAM的图片来表示了。
//平面判断
int Preprocess::plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct)
{
double group_dis = disA * types[i_cur].range + disB; // 0.01*sqrt(x^2+y^2)+0.1 基本上可以近似看成是0.1 100m的时候才到0.2
group_dis = group_dis * group_dis;
// i_nex = i_cur;
double two_dis;
vector<double> disarr; //前后点距离数组
disarr.reserve(20);
//距离小 点与点之间较近 先取够8个点
for (i_nex = i_cur; i_nex < i_cur + group_size; i_nex++)
{
if (types[i_nex].range < blind)
{
curr_direct.setZero(); //距离雷达原点太小了将法向量设置为零向量
return 2;
}
disarr.push_back(types[i_nex].dista); //存储当前点与后一个点的距离
}
//看看后续的点有没有满足条件的
for (;;)
{
if ((i_cur >= pl.size()) || (i_nex >= pl.size())) //索引超出所有点的个数直接BREAK
break;
if (types[i_nex].range < blind)
{
curr_direct.setZero(); //距离雷达原点太小了将法向量设置为零向量
return 2;
}
//最后的i_nex点到i_cur点的距离
vx = pl[i_nex].x - pl[i_cur].x;
vy = pl[i_nex].y - pl[i_cur].y;
vz = pl[i_nex].z - pl[i_cur].z;
two_dis = vx * vx + vy * vy + vz * vz;
if (two_dis >= group_dis) //距离i_cur点太远了就直接break
{
break;
}
disarr.push_back(types[i_nex].dista); //存储当前点与后一个点的距离
i_nex++; // i_nex点加一
}
double leng_wid = 0;
double v1[3], v2[3];
for (uint j = i_cur + 1; j < i_nex; j++)
{
if ((j >= pl.size()) || (i_cur >= pl.size()))
break;
//假设i_cur点为A j点为B i_nex点为C
//向量AB
v1[0] = pl[j].x - pl[i_cur].x;
v1[1] = pl[j].y - pl[i_cur].y;
v1[2] = pl[j].z - pl[i_cur].z;
//向量AB叉乘向量AC
v2[0] = v1[1] * vz - vy * v1[2];
v2[1] = v1[2] * vx - v1[0] * vz;
v2[2] = v1[0] * vy - vx * v1[1];
//物理意义是组成的ABC组成的平行四边形面积的平方(为|AC|*h，其中为B到线AC的距离)
double lw = v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2];
if (lw > leng_wid)
{
leng_wid = lw; //寻找最大面积的平方(也就是寻找距离AC最远的B)
}
}
//|AC|*|AC|/(|AC|*|AC|*h*h)<225
//也就是h>1/15 B点到AC的距离要大于0.06667m
//太近了不好拟合一个平面
if ((two_dis * two_dis / leng_wid) < p2l_ratio)
{
curr_direct.setZero(); //太近了法向量直接设置为0
return 0;
}
//把两点之间的距离 按从大到小排个顺序
uint disarrsize = disarr.size();
for (uint j = 0; j < disarrsize - 1; j++)
{
for (uint k = j + 1; k < disarrsize; k++)
{
if (disarr[j] < disarr[k])
{
leng_wid = disarr[j];
disarr[j] = disarr[k];
disarr[k] = leng_wid;
}
}
}
//这里可能最近的点还是太近了
if (disarr[disarr.size() - 2] < 1e-16)
{
curr_direct.setZero();
return 0;
}
//目前还不太懂为什么给AVIA单独弄了一种，其实我觉得全都可以采用上面这种判定方式（虽然FAST-LIO默认不开特征提取）
if (lidar_type == AVIA)
{
//点与点之间距离变化太大的时候 可能与激光束是平行的 就也舍弃了
double dismax_mid = disarr[0] / disarr[disarrsize / 2];
double dismid_min = disarr[disarrsize / 2] / disarr[disarrsize - 2];
if (dismax_mid >= limit_maxmid || dismid_min >= limit_midmin)
{
curr_direct.setZero();
return 0;
}
}
else
{
double dismax_min = disarr[0] / disarr[disarrsize - 2];
if (dismax_min >= limit_maxmin)
{
curr_direct.setZero();
return 0;
}
}
curr_direct << vx, vy, vz;
curr_direct.normalize(); //法向量归一化
return 1;
}
边界特征主要是为了防止下面的两种情况所加入的判断。在LOAM中也有体现，这里也不展开讲了。
//边缘判断
bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, Surround nor_dir)
{
if (nor_dir == 0)
{
if (types[i - 1].range < blind || types[i - 2].range < blind) //前两个点不能在盲区
{
return false;
}
}
else if (nor_dir == 1)
{
if (types[i + 1].range < blind || types[i + 2].range < blind) //后两个点不能在盲区
{
return false;
}
}
//下面分别对i-2 i-1和i i+1两种情况时点与点间距进行了判断
double d1 = types[i + nor_dir - 1].dista;
double d2 = types[i + 3 * nor_dir - 2].dista;
double d;
//将大小间距进行调换 大在前 小在后
if (d1 < d2)
{
d = d1;
d1 = d2;
d2 = d;
}
d1 = sqrt(d1);
d2 = sqrt(d2);
if (d1 > edgea * d2 || (d1 - d2) > edgeb)
{
//假如间距太大 可能是被遮挡，就不把它当作边缘点
return false;
}
return true;
}
3. 参考链接
https://blog.csdn.net/MWY123_/article/details/124110021
https://zhuanlan.zhihu.com/p/471876531
评论（7）
您还未登录，请登录后发表或查看评论