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