Vins-Mobile AR代碼解析之findGround

Vins-Mobile AR代碼解析之drawGround

傳入的參數有兩個

vector<Vector3f> &point_cloud 
vector<Vector3f> &inlier_points
           

point_cloud是攝像頭采集并處理後的點雲資料，inlier_points則是空的點雲資料

首先對攝像頭朝向進行判斷，如果不是朝下則直接傳回一個原點

if(!look_down)    
    return Vector3f(0,0,0);
           

接着是局部變量的聲明

int height_range[30];
double height_sum[30];
vector<vector<Vector3f>> points_clusters;
points_clusters.resize(30);
           

points_clusters作為點雲的集合，可以看作是平面（由點構成的）的集合，最多不超過30個點雲。

然後進行初始化

for (int i = 0; i < 30; i++){    
    height_range[i] = 0;    
    height_sum[i] = 0;
}
           

range和sum都初始化成0

接着周遊點雲

for (unsigned int i = 0; i < point_cloud.size(); i++){    
        double z = point_cloud[i].z();    
        int index = (z + 2.0) / 0.1;   
        if (0 <= index && index < 30) {        
            height_range[index]++;        
            height_sum[index] += z;
            points_clusters[index].push_back(point_cloud[i]); 
        }
 }
           

對點雲的每一個點，取其z坐标，這裡點雲坐标是世界坐标，通過index将空間劃分成多個平面，即将 [ − 2 , 1 ) [-2,1) [−2,1)映射到 [ 0 , 30 ) [0,30) [0,30)

即考慮-2到1的z值，并将空間劃分成離散的30個平面，将點雲的資料儲存到clusters裡面。height_range[i]是指第i個平面有多少個點，height_sum[i]則是第i個平面所有點的高度和。

int max_num = 0;
int max_index = -1;
for (int i = 1; i < 29; i++){
    if (max_num < height_range[i]){
        max_num = height_range[i];
        max_index = i;
    }
}
           

考慮中間的28個平面，找到有最多點的平面，記其位置為max_index，其點的個數為max_num。

if (max_index == -1)   
    return Vector3f(0,0,0);
else {
    inlier_points = points_clusters[max_index];
    Vector3f tmp_p;
    tmp_p.setZero();
    for(int i = 0; i< inlier_points.size(); i++){
        tmp_p += inlier_points[i];
    }
    return tmp_p/inlier_points.size();
}
           

如果28個平面都沒有點存在，則傳回一個零點。

否則，将最多點的點雲指派給inlier_points, 将平面的中心點傳回