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, 将平面的中心点返回