參考Adam博文 基於地面平面擬合的激光雷達地面分割方法和ROS實現
地面平面擬合（Ground Plane Fitting）
我們采用平面模型（Plane Model）來擬合當前的地面，通常來說，由於現實的地面並不是一個“完美的”平面，而且當距離較大時激光雷達會存在一定的測量噪聲，單一的平面模型並不足以描述我們現實的地面。要很好的完成地面分割，就必須要處理存在一定坡度變化的地面的情況（不能將這種坡度的變化視為非地面，不能因為坡度的存在而引入噪聲），一種簡單的處理方法就是沿着x方向（車頭的方向）將空間分割成若干個子平面，然后對每個子平面使用地面平面擬合算法（GPF）從而得到能夠處理陡坡的地面分割方法。
那么如何進行平面擬合呢？其偽代碼如下：
我們來詳細的了解這一流程：對於給定的點雲 P PP ,分割的最終結果為兩個點雲集合 P g P_gP
g
:地面點雲 和 P n g P_{ng}P
ng
：非地面點雲。該算法有四個重要的參數，分別是：
N i t e r N_{iter}N
iter
: 進行奇異值分解（singular value decomposition，SVD）的次數，也即進行優化擬合的次數
N L P R N_{LPR}N
LPR
: 用於選取LPR的最低高度點的數量
T h s e e d Th_{seed}Th
seed
: 用於選取種子點的閾值，當點雲內的點的高度小於LPR的高度加上此閾值時，我們將該點加入種子點集
T h d i s t Th_{dist}Th
dist
: 平面距離閾值，我們會計算點雲中每一個點到我們擬合的平面的正交投影的距離，而這個平面距離閾值，就是用來判定點是否屬於地面
種子點集的選取
我們首先選取一個種子點集(seed point set)，這些種子點來源於點雲中高度（即z值）較小的點，種子點集被用於建立描述地面的初始平面模型，那么如何選取這個種子點集呢？我們引入最低點代表（Lowest Point Representative, LPR）的概念。LPR就是 N L P R N_{LPR}N
LPR
個最低高度點的平均值，LPR保證了平面擬合階段不受測量噪聲的影響。這個LPR被當作是整個點雲 P PP 的最低點，點雲P中高度在閾值 T h s e e d Th_{seed}Th
seed
內的點被當作是種子點，由這些種子點構成一個種子點集合。
種子點集的選取代碼如下：
/*
@brief Extract initial seeds of the given pointcloud sorted segment.
This function filter ground seeds points accoring to heigt.
This function will set the `g_ground_pc` to `g_seed_pc`.
@param p_sorted: sorted pointcloud
@param ::num_lpr_: num of LPR points
@param ::th_seeds_: threshold distance of seeds
@param ::
*/
void PlaneGroundFilter::extract_initial_seeds_(const pcl::PointCloud<VPoint> &p_sorted)
{
// LPR is the mean of low point representative
double sum = 0;
int cnt = 0;
// Calculate the mean height value.
for (int i = 0; i < p_sorted.points.size() && cnt < num_lpr_; i++)
{
sum += p_sorted.points[i].z;
cnt++;
}
double lpr_height = cnt != 0 ? sum / cnt : 0; // in case divide by 0
g_seeds_pc->clear();
// iterate pointcloud, filter those height is less than lpr.height+th_seeds_
for (int i = 0; i < p_sorted.points.size(); i++)
{
if (p_sorted.points[i].z < lpr_height + th_seeds_)
{
g_seeds_pc->points.push_back(p_sorted.points[i]);
}
}
// return seeds points
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
輸入是一個點雲，這個點雲內的點已經沿着z方向（即高度）做了排序，取 num_lpr_ 個最小點，求得高度平均值 lpr_height（即LPR），選取高度小於 lpr_height + th_seeds_的點作為種子點。
平面模型
接下來我們需要確定一個平面，點雲中的點到這個平面的正交投影距離小於閾值 T h d i s t Th_{dist}Th
dist
則認為該點屬於地面，否則屬於非地面，我們采用一個簡單的線性模型用於平面模型估計，如下：
a x + b y + c z + d = 0 ax+by+cz+d = 0
ax+by+cz+d=0
也即：
n T x = − d \bm{n}^T \bm{x} = -d
n
T
x=−d
其中 n = [ a , b , c ] T \bm{n} = [a, b, c]^Tn=[a,b,c]
T
, x = [ x , y , z ] T \bm{x} = [x, y, z]^Tx=[x,y,z]
T
,通過初始點集的協方差矩陣 C ∈ R 3 x 3 C \in R^{3x3}C∈R
3x3
來求解 n \bm{n}n （即a, b, c), 從而確定一個平面, 我們采用種子點集 S ∈ R 3 S \in R^{3}S∈R
3
作為初始點集，其協方差矩陣為：
C = ∑ i = 1 : ∣ S ∣ ( s i − s ^ ) ( s i − s ^ ) T C = \sum_{i=1:|S|}(s_i - \widehat{s})(s_i - \widehat{s})^T
C=
i=1:∣S∣
∑
(s
i
−
s
)(s
i
−
s
)
T
其中，s ^ \widehat{s}
s
是所有點的均值。這個協方差矩陣 C CC 描述了種子點集的散布情況，其三個奇異向量可以通過奇異值分解（Singular Value Decomposition,SVD）求得，這三個奇異向量描述了點集在三個主要方向的散布情況。由於是平面模型，垂直於平面的法向量 n \bm{n}n 表示具有最小方差的方向，可以通過計算具有最小奇異值的奇異向量來求得。
那么在求得了 n \bm{n}n 以后， d dd 可以通過代入種子點集的平均值 s ^ \widehat{s}
s
(它代表屬於地面的點) 直接求得。整個平面模型計算代碼如下：
/*
@brief The function to estimate plane model. The
model parameter `normal_` and `d_`, and `th_dist_d_`
is set here.
The main step is performed SVD(UAV) on covariance matrix.
Taking the sigular vector in U matrix according to the smallest
sigular value in A, as the `normal_`. `d_` is then calculated
according to mean ground points.
@param g_ground_pc:global ground pointcloud ptr.
*/
void PlaneGroundFilter::estimate_plane_(void)
{
// Create covarian matrix in single pass.
// TODO: compare the efficiency.
Eigen::Matrix3f cov;
Eigen::Vector4f pc_mean;
pcl::computeMeanAndCovarianceMatrix(*g_ground_pc, cov, pc_mean);
// Singular Value Decomposition: SVD
JacobiSVD<MatrixXf> svd(cov, Eigen::DecompositionOptions::ComputeFullU);
// use the least singular vector as normal
normal_ = (svd.matrixU().col(2));
// mean ground seeds value
Eigen::Vector3f seeds_mean = pc_mean.head<3>();
// according to normal.T*[x,y,z] = -d
d_ = -(normal_.transpose() * seeds_mean)(0, 0);
// set distance threhold to `th_dist - d`
th_dist_d_ = th_dist_ - d_;
// return the equation parameters
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
優化平面主循環
在確定如何選取種子點集以及如何估計平面模型以后，我們來看一下整個算法的主循環，以下是主循環的代碼：
extract_initial_seeds_(laserCloudIn);
g_ground_pc = g_seeds_pc;
// Ground plane fitter mainloop
for (int i = 0; i < num_iter_; i++)
{
estimate_plane_();
g_ground_pc->clear();
g_not_ground_pc->clear();
//pointcloud to matrix
MatrixXf points(laserCloudIn_org.points.size(), 3);
int j = 0;
for (auto p : laserCloudIn_org.points)
{
points.row(j++) << p.x, p.y, p.z;
}
// ground plane model
VectorXf result = points * normal_;
// threshold filter
for (int r = 0; r < result.rows(); r++)
{
if (result[r] < th_dist_d_)
{
g_all_pc->points[r].label = 1u; // means ground
g_ground_pc->points.push_back(laserCloudIn_org[r]);
}
else
{
g_all_pc->points[r].label = 0u; // means not ground and non clusterred
g_not_ground_pc->points.push_back(laserCloudIn_org[r]);
}
}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
得到這個初始的平面模型以后，我們會計算點雲中每一個點到該平面的正交投影的距離，即 points * normal_，並且將這個距離與設定的閾值 T h d i s t Th_{dist}Th
dist
(即th_dist_d_) 比較，當高度差小於此閾值，我們認為該點屬於地面，當高度差大於此閾值，則為非地面點。經過分類以后的所有地面點被當作下一次迭代的種子點集，迭代優化。
ROS實踐
下面我們編寫一個簡單的ROS節點實現這一地面分割算法。我們仍然采用第24篇博客（激光雷達的地面-非地面分割和pcl_ros實踐）中的bag進行實驗，這個bag來自Velodyne的VLP32C，在此算法的基礎上我們要進一步濾除雷達過近處和過高處的點，因為雷達安裝位置的原因，近處的車身反射會對Detection造成影響，需要濾除; 過高的目標，如大樹、高樓，對於無人車的雷達感知意義也不大，我們對過近過高的點進行切除，代碼如下：
void PlaneGroundFilter::clip_above(const pcl::PointCloud<VPoint>::Ptr in,
const pcl::PointCloud<VPoint>::Ptr out)
{
pcl::ExtractIndices<VPoint> cliper;
cliper.setInputCloud(in);
pcl::PointIndices indices;
#pragma omp for
for (size_t i = 0; i < in->points.size(); i++)
{
if (in->points[i].z > clip_height_)
{
indices.indices.push_back(i);
}
}
cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices));
cliper.setNegative(true); //ture to remove the indices
cliper.filter(*out);
}
void PlaneGroundFilter::remove_close_far_pt(const pcl::PointCloud<VPoint>::Ptr in,
const pcl::PointCloud<VPoint>::Ptr out)
{
pcl::ExtractIndices<VPoint> cliper;
cliper.setInputCloud(in);
pcl::PointIndices indices;
#pragma omp for
for (size_t i = 0; i < in->points.size(); i++)
{
double distance = sqrt(in->points[i].x * in->points[i].x + in->points[i].y * in->points[i].y);
if ((distance < min_distance_) || (distance > max_distance_))
{
indices.indices.push_back(i);
}
}
cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices));
cliper.setNegative(true); //ture to remove the indices
cliper.filter(*out);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
編譯並運行該ROS節點，使用我們在第二十四篇博客中提供的bag進行測試，得到如下結果：
