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