ikdtree地图更新部分

[chenxiaocongAI]

为啥增加了下面部分策略，是出于什么考虑？
PointToAdd.reserve(feats_down_size);
PointNoNeedDownsample.reserve(feats_down_size);
for (int i = 0; i < feats_down_size; i++) {
/* transform to world frame /
pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));
/ decide if need add to map */
if (!Nearest_Points[i].empty() && flg_EKF_inited) {
const PointVector &points_near = Nearest_Points[i];
bool need_add = true;
BoxPointType Box_of_Point;
PointType downsample_result, mid_point;
mid_point.x = floor(feats_down_world->points[i].x / filter_size_map_min) * filter_size_map_min +
0.5 * filter_size_map_min;
mid_point.y = floor(feats_down_world->points[i].y / filter_size_map_min) * filter_size_map_min +
0.5 * filter_size_map_min;
mid_point.z = floor(feats_down_world->points[i].z / filter_size_map_min) * filter_size_map_min +
0.5 * filter_size_map_min;
float dist = calc_dist(feats_down_world->points[i], mid_point);
if (fabs(points_near[0].x - mid_point.x) > 0.5 * filter_size_map_min &&
fabs(points_near[0].y - mid_point.y) > 0.5 * filter_size_map_min &&
fabs(points_near[0].z - mid_point.z) > 0.5 * filter_size_map_min) {
PointNoNeedDownsample.push_back(feats_down_world->points[i]);
continue;
}
for (int readd_i = 0; readd_i < NUM_MATCH_POINTS; readd_i++) {
if (points_near.size() < NUM_MATCH_POINTS) break;
if (calc_dist(points_near[readd_i], mid_point) < dist) {
need_add = false;
break;
}
}
if (need_add) PointToAdd.push_back(feats_down_world->points[i]);
} else {
PointToAdd.push_back(feats_down_world->points[i]);
}
}

[chenxiaocongAI]

@zfc-zfc