Skip to content

Commit ff77555

Browse files
HuangCongQingHuangCongQing
committed
update：注解
1 parent 6865a58 commit ff77555

2 files changed

Lines changed: 48 additions & 44 deletions

File tree

‎object_tracking/src/cluster/component_clustering.cpp‎

Lines changed: 41 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,3 @@
1-
21
#include <array>
32
#include <pcl/io/pcd_io.h>
43
#include <nav_msgs/OccupancyGrid.h>
@@ -9,43 +8,45 @@ using namespace std;
98
using namespace pcl;
109

1110
//int numGrid = 2;
12-
float roiM = 50;
11+
float roiM = 50; // ???
1312
int kernelSize = 3;
1413

14+
// 下面什么参数？
1515
double g_resolution = 1.0;
16-
int g_cell_width =50;
16+
int g_cell_width =50; // 宽
1717
int g_cell_height=50;
1818
double g_offset_x=0;
1919
double g_offset_y = 25;
2020
double g_offset_z = -2;
2121

2222
double HEIGHT_LIMIT = 0.1; // from sensor
23-
double CAR_LENGTH = 4.5;
23+
double CAR_LENGTH = 4.5; // 车的宽高
2424
double CAR_WIDTH = 2;
2525
//costmap paramter
2626

27-
//
27+
// 初始化网格Grid状态
2828
void mapCartesianGrid(PointCloud<PointXYZ>::Ptr elevatedCloud,
2929
array<array<int, numGrid>, numGrid> & cartesianData){
3030

3131

32-
array<array<int, numGrid>, numGrid> gridNum{}; // ？ gridNums是指？
32+
array<array<int, numGrid>, numGrid> gridNum{}; // gridNums是指： elevatedCloud点的XY平面离散为m×n单元的网格（见paper图）
3333
for(int cellX = 0; cellX < numGrid; cellX++){ // cellX
3434
for(int cellY = 0; cellY < numGrid; cellY++){ // cellY
3535
gridNum[cellX][cellY] = 0; // 全部填充为0
3636
}
3737
}
38-
39-
for(int i = 0; i < elevatedCloud->size(); i++){ // 遍历高点
38+
39+
// ??
40+
for(int i = 0; i < elevatedCloud->size(); i++){ // 遍历高点数
4041
float x = elevatedCloud->points[i].x;
4142
float y = elevatedCloud->points[i].y;
4243
float xC = x+roiM/2; // float roiM = 50;
4344
float yC = y+roiM/2;
4445
// exclude outside roi points 排除外部roi points
4546
if(xC < 0 || xC >= roiM || yC < 0 || yC >=roiM) continue; // continue后，，下面不执行。gridNum[xI][yI] 值不变
46-
int xI = floor(numGrid*xC/roiM); // ?? xI .yI
47-
int yI = floor(numGrid*yC/roiM);
48-
gridNum[xI][yI] = gridNum[xI][yI] + 1;
47+
int xI = floor(numGrid*xC/roiM); // ?? xI .yI const int numGrid = 250; floor(x)返回的是小于或等于x的最大整数
48+
int yI = floor(numGrid*yC/roiM); // 归一化到250x250？？
49+
gridNum[xI][yI] = gridNum[xI][yI] + 1; // 自+1？？=1
4950
// cartesianData[xI][yI] = -1;
5051

5152
// if(xI == 0)
@@ -126,23 +127,25 @@ void mapCartesianGrid(PointCloud<PointXYZ>::Ptr elevatedCloud,
126127
// }
127128
// int a = 0;
128129
}
129-
130-
for(int xI = 0; xI < numGrid; xI++){
130+
// 将x，y位置的单个单元格选作中心单元格，并且clusterID计数器加1。
131+
// 然后所有相邻的相邻像元（即x-1，y + 1，x，y +1，x +1，y +1 x -1，y，x +1，y，x -1，y -1，x，检查y − 1，x + 1，y + 1）的占用状态，并用当前集群ID标记。
132+
// 对m×n网格中的每个x，y重复此过程，直到为所有非空群集分配了ID。
133+
for(int xI = 0; xI < numGrid; xI++){ // const int numGrid = 250;
131134
for(int yI = 0; yI < numGrid; yI++){
132135
if(gridNum[xI][yI] > 1){
133-
cartesianData[xI][yI] = -1;
136+
cartesianData[xI][yI] = -1; // 网格分配有2种初始状态，分别为空（0），已占用（-1）和已分配。随后，将x，y位置的单个单元格选作中心单元格，并且clusterID计数器加1
134137

135138
if(xI == 0)
136139
{
137140
if(yI == 0)
138141
{
139-
cartesianData[xI+1][yI] = -1;
142+
cartesianData[xI+1][yI] = -1; // 角相邻的3个相邻像元
140143
cartesianData[xI][yI+1] = -1;
141144
cartesianData[xI+1][yI+1] = -1;
142145
}
143146
else if(yI < numGrid - 1)
144147
{
145-
cartesianData[xI][yI-1] = -1;
148+
cartesianData[xI][yI-1] = -1; // 边有5个相邻点
146149
cartesianData[xI][yI+1] = -1;
147150
cartesianData[xI+1][yI-1] = -1;
148151
cartesianData[xI+1][yI] = -1;
@@ -165,7 +168,7 @@ void mapCartesianGrid(PointCloud<PointXYZ>::Ptr elevatedCloud,
165168
cartesianData[xI+1][yI] = -1;
166169
cartesianData[xI+1][yI+1] = -1;
167170
}
168-
else if(yI < numGrid - 1)
171+
else if(yI < numGrid - 1) // 一般情况四周有8个相邻点
169172
{
170173
cartesianData[xI-1][yI-1] = -1;
171174
cartesianData[xI-1][yI] = -1;
@@ -214,31 +217,32 @@ void mapCartesianGrid(PointCloud<PointXYZ>::Ptr elevatedCloud,
214217
}
215218
}
216219

217-
// findComponent会引用search函数
218-
void search(array<array<int, numGrid>, numGrid> & cartesianData, int clusterId, int cellX, int cellY){
219-
cartesianData[cellX][cellY] = clusterId;
220-
int mean = kernelSize/2;
221-
for (int kX = 0; kX < kernelSize; kX++){
222-
int kXI = kX-mean;
223-
if((cellX + kXI) < 0 || (cellX + kXI) >= numGrid) continue;
220+
// findComponent会引用search函数
221+
void search(array<array<int, numGrid>, numGrid> & cartesianData, int clusterId, int cellX, int cellY){ // cellX(0-249), cellY(0-249)
222+
cartesianData[cellX][cellY] = clusterId; // 赋值
223+
int mean = kernelSize/2; // kernelSize = 3; mean = 1
224+
for (int kX = 0; kX < kernelSize; kX++){ // kernelSize = 3;
225+
int kXI = kX-mean; // 0， -1 ， 1 // cout << "kXI is "<<kXI<<endl;
226+
if((cellX + kXI) < 0 || (cellX + kXI) >= numGrid) continue; // numGrid = 250;
224227
for( int kY = 0; kY < kernelSize;kY++){
225-
int kYI = kY-mean;
228+
int kYI = kY-mean; // 减去均值？？？？？
226229
if((cellY + kYI) < 0 || (cellY + kYI) >= numGrid) continue;
227230

228231
if(cartesianData[cellX + kXI][cellY + kYI] == -1){
229-
search(cartesianData, clusterId, cellX +kXI, cellY + kYI);
232+
search(cartesianData, clusterId, cellX +kXI, cellY + kYI); // 循环搜索
230233
}
231234

232235
}
233236
}
234237
}
235238

239+
// 对m×n网格中的每个x，y重复此过程，直到为所有非空cluster分配了ID。
236240
void findComponent(array<array<int, numGrid>, numGrid> & cartesianData, int &clusterId){
237-
for(int cellX = 0; cellX < numGrid; cellX++){
241+
for(int cellX = 0; cellX < numGrid; cellX++){ // 循环每个点 numGrid = 250;
238242
for(int cellY = 0; cellY < numGrid; cellY++){
239-
if(cartesianData[cellX][cellY] == -1){
240-
clusterId ++;
241-
search(cartesianData, clusterId, cellX, cellY);
243+
if(cartesianData[cellX][cellY] == -1){ // 随后，将x，y位置的单个单元格选作中心单元格，并且clusterID计数器加1 (网格分配有2种初始状态，分别为空（0），已占用（-1）)
244+
clusterId ++; // 对m×n网格中的每个x，y重复此过程，直到为所有非空cluster分配了ID。
245+
search(cartesianData, clusterId, cellX, cellY); // 对每一个点进行搜索 cellX(0-249), cellY(0-249)
242246
}
243247
}
244248
}
@@ -251,7 +255,7 @@ void componentClustering(PointCloud<pcl::PointXYZ>::Ptr elevatedCloud,
251255
// map 120m radius data(polar grid data) into 100x100 cartesian grid,
252256
// parameter might need to be modified
253257
// in this case 30mx30m with 100x100x grid
254-
mapCartesianGrid(elevatedCloud, cartesianData); // 第一步
258+
mapCartesianGrid(elevatedCloud, cartesianData); // 第一步设置网格Grid状态 网格数组：cartesianData
255259
findComponent(cartesianData, numCluster); // 第二步
256260
}
257261

@@ -306,19 +310,19 @@ void makeClusteredCloud(PointCloud<pcl::PointXYZ>::Ptr& elevatedCloud,
306310
float yC = y+roiM/2;
307311
// exclude outside roi points
308312
if(xC < 0 || xC >= roiM || yC < 0 || yC >=roiM) continue;
309-
int xI = floor(numGrid*xC/roiM);
310-
int yI = floor(numGrid*yC/roiM);
313+
int xI = floor(numGrid*xC/roiM); // (0~249)
314+
int yI = floor(numGrid*yC/roiM); // (0~249)
311315

312-
int clusterNum = cartesianData[xI][yI];
316+
int clusterNum = cartesianData[xI][yI]; // 数值
313317
if(clusterNum != 0){
314318
PointXYZ o;
315-
o.x = grid_size*xI - roiM/2 + grid_size/2;
319+
o.x = grid_size*xI - roiM/2 + grid_size/2; // 网格大小？？const float grid_size = 0.2;
316320
o.y = grid_size*yI - roiM/2 + grid_size/2;
317321
o.z = -1;
318322
// o.r = (500*clusterNum)%255;
319323
// o.g = (100*clusterNum)%255;
320324
// o.b = (150*clusterNum)%255;
321-
clusterCloud->push_back(o);
325+
clusterCloud->push_back(o); //
322326
}
323327
}
324328
}
@@ -406,7 +410,7 @@ void setOccupancyGrid(nav_msgs::OccupancyGrid *og)
406410
og->info.origin.orientation.w = 1.0;
407411
}
408412

409-
// object_tracking/src/cluster/main.cpp会引用此函数
413+
// object_tracking/src/cluster/main.cpp会引用此函数 代价地图
410414
std::vector<int> createCostMap(const pcl::PointCloud<pcl::PointXYZ> &scan)
411415
{
412416
std::vector<int> cost_map(g_cell_width * g_cell_height, 0);

‎object_tracking/src/cluster/main.cpp‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@ ros::Publisher marker_array_pub_;
6060
ros::Publisher box_pub;
6161

6262
// 回调函数
63-
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){
63+
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){ // f非地面数据
6464

6565
PointCloud<pcl::PointXYZ>::Ptr none_ground_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
6666

@@ -69,10 +69,10 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){
6969

7070
//start processing pcl::pointcloud 开始处理 pcl::pointcloud数据
7171

72-
int numCluster = 0; // global variable 聚类数量？
72+
int numCluster = 0; // global variable 聚类ID数量？
7373
array<array<int, numGrid>, numGrid> cartesianData{}; // 笛卡尔数据？
7474
componentClustering(none_ground_cloud, cartesianData, numCluster); // Source: /src/cluster/component_clustering.cpp
75-
cout << "num is "<<numCluster<<endl; //
75+
cout << "初始聚类ID数量numCluster is "<<numCluster<<endl; // 聚类的数量
7676
// for visualization
7777
PointCloud<pcl::PointXYZ>::Ptr clusteredCloud (new pcl::PointCloud<pcl::PointXYZ>);
7878
// PointCloud<pcl::PointXYZRGB>::Ptr clusteredCloud (new pcl::PointCloud<pcl::PointXYZRGB>);
@@ -87,11 +87,11 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){
8787
static int count = 0;
8888
static nav_msgs::OccupancyGrid og; //
8989
if (!count)
90-
setOccupancyGrid(&og); //
90+
setOccupancyGrid(&og); // 设置参数数值
9191

9292
og.header.frame_id = none_ground_cloud->header.frame_id;
9393

94-
// create cost map with pointcloud
94+
// create cost map with pointcloud costmap代码地图 简单来说就是为了在这张地图上进行各种加工，方便我们后面进行路径规划而存在的。
9595
std::vector<int> cost_map = createCostMap(*none_ground_cloud); // Source: /src/cluster/component_clustering.cpp
9696

9797
/*
@@ -117,7 +117,7 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input){
117117

118118
vector<PointCloud<PointXYZ>> bBoxes = boxFitting(none_ground_cloud, cartesianData, numCluster,ma); // bBoxes----一个数组(候选框8个坐标)
119119

120-
object_tracking::trackbox boxArray; // boxArray--候选框8个坐标数组 的 数组
120+
object_tracking::trackbox boxArray; // boxArray--候选框8个坐标数组 的 数组 msg格式：object_tracking/msg/trackbox.msg
121121

122122
boxArray.header = input->header;
123123
boxArray.box_num = bBoxes.size();
@@ -244,7 +244,7 @@ int main (int argc, char** argv){
244244

245245
vis_pub = nh.advertise<visualization_msgs::Marker>( "visualization_marker", 0 ); //发布者 visualization_marker -- 话题topic名
246246
marker_array_pub_ = nh.advertise<visualization_msgs::MarkerArray>("cluster_ma", 10); //发布者 cluster_ma -- 话题topic名
247-
g_costmap_pub = nh.advertise<nav_msgs::OccupancyGrid>("realtime_cost_map", 10); //发布者 realtime_cost_map -- 话题topic名
247+
g_costmap_pub = nh.advertise<nav_msgs::OccupancyGrid>("realtime_cost_map", 10); //���局代价地图？？ 发布者 realtime_cost_map -- 话题topic名
248248

249249
obs_pub = nh.advertise<object_tracking::ObstacleList>("cluster_obs",10); //发布者 cluster_obs -- 话题topic名
250250

0 commit comments

Comments
 (0)