一.代码意义
低矮机器人在使用点云进行障碍物检测时，会遇到不平整地面，有可能将地面认为成障碍物进行避障。因此进行地面拟合操作去地面障碍物点云。
不上传全部源代码，完整源代码上传到公司内部库，这里只对地面拟合进行笔记整理
二.代码整理
pointCloud_new：最终输出的新鲜障碍物点云
pointCloud_obstacle：认为属于障碍物的点云
pointCloud_ground：认为可能是地面的点云（包括了地面点云及地面上一定高度内的障碍物）
above_plane_cloud：拟合平面以上的点云，将作为障碍物点云发出
below_plane_cloud：拟合平面以下的点云，将作为地面点云，判断后决定是否发出
cameraData1.threshold_angle：水平方向的阈值角度，判断小车通行的角度阈值
cameraData1.left_limit：障碍物识别范围左边界
cameraData1.right_limit：障碍物识别范围右边界
cameraData1.forward_limit：障碍物识别范围远边界
太长了，分两段写然后调用吧。如果不想看第二部分的字可以看第三部分流程介绍，也是字。
第一段：对处理好的点云进行切分分类，分为一定为障碍物的点云pointCloud_obstacle和可能为地面的点云pointCloud_ground
void Obstacle::delProbGround(cameraData &cameraData1)
{
// ROS_INFO("first cameraData1.pointCloud.size: %zu",cameraData1.pointCloud.size());
pcl::PointCloud<pcl::PointXYZ> pointCloud_new;
pcl::PointCloud<pcl::PointXYZ> pointCloud_obstacle;
pcl::PointCloud<pcl::PointXYZ> pointCloud_ground;
pcl::PointCloud<pcl::PointXYZ> above_plane_cloud;
pcl::PointCloud<pcl::PointXYZ> below_plane_cloud;
pcl::PointXYZ pointXyz;
for (int index = 0; index < cameraData1.pointCloud.size(); index++)
{
float threshold_angle = cameraData1.threshold_angle;
float tan_angle = std::tan(threshold_angle * M_PI / 180.0);
float camera_to_car_z = cameraData1.camera_to_car_z;
// ROS_INFO("tan_angle: %f", tan_angle);
pointXyz.x = cameraData1.pointCloud.at(index).x;
pointXyz.y = cameraData1.pointCloud.at(index).y;
pointXyz.z = cameraData1.pointCloud.at(index).z + camera_to_car_z;
// ROS_INFO(" cameraData1.pointCloud %zu", cameraData1.pointCloud.size());
float z = tan_angle * sqrt(pointXyz.x * pointXyz.x + pointXyz.y * pointXyz.y);
if (pointXyz.z > z)
{
/** 圆锥分割面之上的点，还是当做障碍物;圆锥面之下的点，当做可能出现的非平地面而过滤掉 */
pointCloud_obstacle.push_back(pointXyz);
// ROS_INFO(" pointCloud_obstacle %zu", pointCloud_obstacle.size());
}
else
{
pointCloud_ground.push_back(pointXyz);
// ROS_INFO(" pointCloud_ground %zu", pointCloud_ground.size());
}
/* 地面拟合 */
if (ground_fitting_)
{
pcl::PointCloud<pcl::PointXYZ> above_plane_cloud;
pcl::PointCloud<pcl::PointXYZ> below_plane_cloud;
pointCloud_new = fitGround(pointCloud_ground, cameraData1, above_plane_cloud, below_plane_cloud, pointCloud_obstacle);
}
else
{
// 不开启地面拟合，直接发送圆锥切面上方障碍物点云
pointCloud_new = pointCloud_obstacle;
}
}
cameraData1.pointCloud = *(pointCloud_new.makeShared());
cameraData1.pointCloud.is_dense = false;
cameraData1.pointCloud.height = 1;
cameraData1.pointCloud.width = cameraData1.pointCloud.size();
}第二段：地面拟合
先对pointCloud_ground进行栅格化分类，将点云放入对应的栅格中。
对靠近地面的两个栅格中的点云进行取点拟合，得到拟合平面
拟合平面以上的点作为障碍物点云above_plane_cloud，最终会发送这部分点云
拟合平面以下的点为below_plane_cloud，这个需要进行进一步判断
用得到的拟合平面求法线，与设定的阈值进行比较，判断拟合的平面角度是否为可通行，如果不可通行就发送这部分点云，如果判断可以通行则不发送
pcl::PointCloud<pcl::PointXYZ> fitGround(const pcl::PointCloud<pcl::PointXYZ>& pointCloud_ground,
const cameraData& cameraData1,
pcl::PointCloud<pcl::PointXYZ>& above_plane_cloud,
pcl::PointCloud<pcl::PointXYZ>& below_plane_cloud,
pcl::PointCloud<pcl::PointXYZ>& pointCloud_obstacle)
{
pcl::PointCloud<pcl::PointXYZ> pointCloud_new;
/* 栅格大小（m） */
float map_size_x = (cameraData1.left_limit + cameraData1.right_limit);
float map_size_y = cameraData1.forward_limit;
float map_size_z = (0.2 + cameraData1.top_limit);
std::unordered_map<std::string, pcl::PointCloud<pcl::PointXYZ>> grids;
/* 将点云添加到对应的栅格中 */
for (size_t i = 0; i < pointCloud_ground.size(); ++i)
{
/* 分配到不同的栅格 */
pcl::PointXYZ point = pointCloud_ground.points[i];
int grid_index_x = std::floor((point.x - cameraData1.left_limit) / (map_size_x / 2));
int grid_index_y = 0;
int grid_index_z = std::floor((point.z - 0.2) / (map_size_z / 2));
std::string grid_key = std::to_string(grid_index_x) + "_" + std::to_string(grid_index_y) + "_" + std::to_string(grid_index_z);
grids[grid_key].push_back(point);
}
for (const auto &entry : grids)
{
const std::string &grid_key = entry.first;
const pcl::PointCloud<pcl::PointXYZ> &grid_points = entry.second;
// 将栅格索引字符串分割成三个部分
std::vector<std::string> grid_key_parts;
std::stringstream ss(grid_key);
std::string part;
while (std::getline(ss, part, '_'))
{
grid_key_parts.push_back(part);
}
// 确保分割后索引有三个部分
if (grid_key_parts.size() == 3)
{
int grid_index_x = std::stoi(grid_key_parts[0]);
int grid_index_y = std::stoi(grid_key_parts[1]);
int grid_index_z = std::stoi(grid_key_parts[2]);
// 只处理高度较低的两个栅格
if (grid_index_z == 0 || grid_index_z == 1)
// if (grid_index_x == 0 || grid_index_x == 1)
{
// ROS_INFO("Grid %s has %zu points", grid_key.c_str(), grid_points.size());
if (pointCloud_ground.size() >= 10)
{
/**随机采样拟合地面平面*/
std::random_device rd;
std::mt19937 gen(rd());
std::uniform_int_distribution<> distr(0, pointCloud_ground.size() - 1);
pcl::PointCloud<pcl::PointXYZ>::Ptr random_points(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ModelCoefficients plane_coefficients;
pcl::PointIndices plane_inliers;
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(50);
seg.setDistanceThreshold(cameraData1.setDistanceThreshold);
seg.setInputCloud(random_points);
seg.setInputCloud(pointCloud_ground.makeShared());
seg.segment(plane_inliers, plane_coefficients);
double plane_normal_x = plane_coefficients.values[0];
double plane_normal_y = plane_coefficients.values[1];
double plane_normal_z = plane_coefficients.values[2];
double ground_threshold = (cameraData1.ground_threshold);
for (size_t i = 0; i < pointCloud_ground.size(); ++i)
{
pcl::PointXYZ point = pointCloud_ground.points[i];
double x_p = point.x;
double y_p = point.y;
double z_p = point.z;
double distance_to_plane = plane_normal_x * x_p +
plane_normal_y * y_p +
plane_normal_z * z_p +
plane_coefficients.values[3];
if (distance_to_plane > ground_threshold)
{
above_plane_cloud.push_back(point);
}
else
{
below_plane_cloud.push_back(point);
}
}
ROS_INFO("above_plane_cloud Size: %zu", above_plane_cloud.size());
ROS_INFO("below_plane_cloud Size: %zu", below_plane_cloud.size());
ROS_INFO("pointCloud_obstacle Size: %zu", pointCloud_obstacle.size());
/* 拟合地面平面之后，根据平面法线判断留下哪一部分 */
Eigen::Vector3f plane_normal(plane_normal_x, plane_normal_y, plane_normal_z);
Eigen::Vector3f vehicle_axis(0.0, 0.0, 1.0);
float cos_angle = plane_normal.dot(vehicle_axis) / (plane_normal.norm() * vehicle_axis.norm());
float threshold_angle2 = cameraData1.threshold_angle;
float cos_threshold_angle = cos(threshold_angle2);
ROS_INFO("CCCCCCCCCCCCCCCCCCCCos_Angle size : %f", cos_angle);
ROS_INFO("TTTTTTTTTTTTTTTTTTTThreshold_angle size : %f", threshold_angle2);
ROS_INFO("cos_threshold_angle size : %f", cos_threshold_angle);
pointCloud_new = pointCloud_obstacle + above_plane_cloud;
/* 在修改之前的拟合是空间内的拟合，添加栅格化后仅拟合地面及以上0.4米的距离，因此法线的判断方式需要修改 */
/* 可以将法线这里去掉一大部分 */
/* 若地面法线与小车坐标系Z轴几乎平行，意味着面前无障碍物且地面平整。只发送障碍物点云到cameraData1.pointCloud */
if (0.98 <= cos_angle && cos_angle <= 1.0)
{
pointCloud_new = pointCloud_obstacle + above_plane_cloud;
// ROS_ERROR("1.0 1.0 1.0 ---ping xing--- obstacle_cloud + above_plane_cloud");
}
else if (-0.7 < cos_angle && cos_angle <= 0.7)
{
/* 若地面法线与小车坐标系Z轴夹角在-135°~45°之间，面前有认为切割后的地面也为障碍物，发送点云到cameraData1.pointCloud */
pointCloud_new = pointCloud_obstacle + above_plane_cloud + below_plane_cloud;
// ROS_ERROR("0.0 0.0 0.0 ---di mian wei zhang ai wu--- obstacle_cloud + ground_cloud");
}
else
{
/* 其他情况 */
if (cos_angle < cos(threshold_angle2))
{
/* cos值大于cos_angle，代表实际角度要大于预设角度，判断小车不能行驶 */
pointCloud_new = pointCloud_obstacle + above_plane_cloud + below_plane_cloud;
// ROS_ERROR(" zheng chang-----obstacle_cloud + above_plane_cloud");
}
else
{
/* cos值小于等于cos_angle，代表实际角度要小于等于预设角度，判断小车可以行驶 */
pointCloud_new = pointCloud_obstacle + above_plane_cloud;
// ROS_ERROR("di mian ping zheng---ke yi tong guo--- obstacle_cloud");
}
}
}
else
{
/* 地面点云数量不足10个，无法拟合平面并进行夹角计算，安全起见会将其判断为障碍物 */
pointCloud_new = pointCloud_obstacle + pointCloud_ground;
}
}
}
}
ROS_INFO("-------- 11111111111111 pointCloud_new Size --------: %zu", pointCloud_new.size());
return pointCloud_new;
}三.代码流程介绍
流程图×
思维导图√
扫一扫
553
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
