FAEL/fael_planner/robot_move/traversability_analysis/src /terrain_map_empty.cpp中computerTerrainCloudReletiveElevationAndNoDataAreaObstacled函数的一点小疑问

[LiYang-Robot]

void TerrainMapEmpty::computerTerrainCloudReletiveElevationAndNoDataAreaObstacled() {
    if (no_data_init_ == 1) {
        for (int i = 0; i < grid_width_num_; ++i) {
            for (int j = 0; j < grid_width_num_; ++j) {
                double grid_center_x = grid_size_ * i + min_x_ + grid_size_ / 2;
                double grid_center_y = grid_size_ * j + min_y_ + grid_size_ / 2;
                double dis = sqrt(std::pow((grid_center_x - init_pose_.position.x), 2) +
                                  std::pow((grid_center_y - init_pose_.position.y), 2));
                if (dis < 2 * vehicle_height_ / (tan(15 * M_PI / 180))) {
                    if (grids_cloud_[i][j].size() < min_grid_point_num_) {  
                        terrain_grids_[i][j].status = Status::Free;
                    } else {
                        bool occupied = false;
                        for (auto point: grids_cloud_[i][j].points) {
                            float dis_z = fabs(point.z - grids_min_elevation_[i][j]);
                            if (dis_z > lower_z_) { 
                                occupied = true;
                                break;
                            }
                        }
                        if (occupied) { 
                            terrain_grids_[i][j].status = Status::Occupied;
                        } else {
                            terrain_grids_[i][j].status = Status::Free;
                        }
                    }
                } else if (grids_cloud_[i][j].size() < min_grid_point_num_) {
                    terrain_grids_[i][j].status = Status::Occupied;
                } else {
                    bool occupied = false;
                    for (auto point: grids_cloud_[i][j].points) {
                        float dis_z = fabs(point.z - grids_min_elevation_[i][j]);
                        if (dis_z > lower_z_) { 
                            occupied = true;
                            break;
                        }
                    }
                    if (occupied) { 
                        terrain_grids_[i][j].status = Status::Empty;
                    } else {
                        terrain_grids_[i][j].status = Status::Free;
                    }
                }
            }
        }
    }
    if (no_data_init_ == 2) { 
        for (int i = 0; i < grid_width_num_; ++i) {
            for (int j = 0; j < grid_width_num_; ++j) {
                if (grids_cloud_[i][j].size() < min_grid_point_num_) {                       
                    terrain_grids_[i][j].status = Status::Empty;
                } else {
                    bool occupied = false;
                    for (auto point: grids_cloud_[i][j].points) {
                        float dis_z = fabs(point.z - grids_min_elevation_[i][j]);
                        if (dis_z > lower_z_) { 
                            occupied = true;
                            break;
                        }
                    }
                    if (occupied) { 
                        terrain_grids_[i][j].status = Status::Occupied;
                    } else {
                        terrain_grids_[i][j].status = Status::Free;
                    }
                }
            }
        }
    }

}

函数的27-28行 else if (gridscloud[i][j].size() < min_grid_pointnum) { terraingrids[i][j].status = Status::Occupied; 作者您好 这一段中，min_grid_point_num_个人理解应该是一个网格点数量的阈值，如果超过这个阈值才应该设置状态为占用吧，这里是不是应该设置为Status::Free呢？

[LiYang-Robot]

还有想问您一下 enum Status { Free = 0, Occupied = 1, Empty = 2, Unknown = 3 }; 个人理解是，这个状态其中free是几乎没有点的网格，occupied是有一些点且点之间高度差大于阈值的网格，unknown是完全没有信息的网格，请问这样理解是否对呢。 那这个empty是什么样的一种状态呢，您有时间能回答我一下么，十分感谢

[hjlstudy]

您好，很感动你看的这么细，这里的地形分析是参考了cmu它们开源代码里分析地面点判断障碍物的方法，通过一个grid里的点云高度差大于阈值来判断地形的可通过性，可以识别上下坡地形，我们在这里增加了一种地面点云有空洞的情况，可能是暂时没有扫到的地方（无法判断是否可通过），也可能是本来就是有洞或无法扫到点的地方，比如下行的楼梯，无栏杆的桥面边缘外，没井盖的下水道等危险区域。 如果一个grid里的点数太少说明这里可能为空洞，27-28行那里应该是个empty，39行那里应该为occupied。

此外，这个地形分析只是简单根据点云高度差来分析可通行区，但实际中，往往由于slam的精度和车身抖动很容易导致点云地图误差较大，无法正常识别可通行区，因此在实际中最好采用别的分析方法。

[LiYang-Robot]

谢谢您的解答。 我不知道这么理解是否对. 在距离机器人也就是传感器足够近的情况下，这个距离与机器人的高度等有关，点云点是比较密集的，也就是说这种情况下的感知是十分确定的，当网格中点数量比较多超过阈值min_grid_point_num_的时候就认为是占据状态，也就是status中的occupied； 在距离机器人也就是传感器比较远的情况下，也就是说这种情况下的感知是十分不确定的，可能是由于物理结构或是传感器距离的限制没有扫到或者是地形问题，当网格中点数量没有超过min_grid_pointnum，也就不认为是free，针对这种情况设置了一个新的状态，也就是empty，表示空的意思； 并且这里代码28以及39的状态也需要按照您说的那样分别改称empty和occupied；