Skip to content

Commit 56283b2

Browse files
HuangCongQingHuangCongQing
committed
update: 注解MAR&L拟合
1 parent 5f209d3 commit 56283b2

1 file changed

Lines changed: 86 additions & 83 deletions

File tree

‎object_tracking/src/cluster/box_fitting.cpp‎

Lines changed: 86 additions & 83 deletions
Original file line numberDiff line numberDiff line change
@@ -15,15 +15,15 @@ using namespace pcl;
1515
using namespace cv;
1616

1717
// float picScale = 30;
18-
float picScale = 900/roiM;
18+
float picScale = 900/roiM; // ???
1919
int ramPoints = 80;
2020
int lSlopeDist = 1.0;
2121
////////////////////////int lSlopeDist = 3.0;
2222
////////////////////////int lnumPoints = 300;
2323
int lnumPoints = 5;
2424

2525
//////////////////////////////float sensorHeight = 1.73;
26-
float sensorHeight = 2;
26+
float sensorHeight = 2; // 激光雷达距离地面高度
2727
// float tHeightMin = 1.2;
2828
float tHeightMin = 0.8;
2929
float tHeightMax = 2.6;
@@ -58,7 +58,7 @@ void getClusteredPoints(PointCloud<PointXYZ>::Ptr elevatedCloud,
5858
int yI = floor(numGrid * yC / roiM);
5959

6060
int clusterNum = cartesianData[xI][yI]; //1 ~ numCluster 初始聚类ID数量numCluster
61-
// cout << "clusterNum is " << clusterNum << endl; // 连续几个相同的值（聚类在一块的？）
61+
// cout << "clusterNum is " << clusterNum << endl; // 连续几个相同的值（聚类在一块的）
6262
int vectorInd = clusterNum - 1; //0 ~ (numCluster -1 )
6363
if (clusterNum != 0) {
6464
PointXYZ o;
@@ -71,6 +71,7 @@ void getClusteredPoints(PointCloud<PointXYZ>::Ptr elevatedCloud,
7171
}
7272
}
7373

74+
//
7475
void getPointsInPcFrame(Point2f rectPoints[], vector<Point2f>& pcPoints, int offsetX, int offsetY){
7576
// loop 4 rect points
7677
for (int pointI = 0; pointI < 4; pointI++){
@@ -94,10 +95,10 @@ void getPointsInPcFrame(Point2f rectPoints[], vector<Point2f>& pcPoints, int off
9495
}
9596

9697
bool ruleBasedFilter(vector<Point2f> pcPoints, float maxZ, int numPoints){
97-
bool isPromising = false;
98+
bool isPromising = false; // 是否舍弃此聚类
9899
//minnimam points thresh
99-
if(numPoints < 30) return isPromising; // 小于30个点，返回false
100-
// length is longest side of the rectangle while width is the shorter side.
100+
if(numPoints < 30) return isPromising; // 小于30个点，返回false 舍弃此聚类 cout << "numPoints is "<< numPoints <<endl; // 差不多(3~, 800~)
101+
// length is longest side of the rectangle while width is the shorter side. 长度是矩形的最长边，而宽度是较短的边。
101102
float width, length, height, area, ratio, mass;
102103

103104
float x1 = pcPoints[0].x;
@@ -109,36 +110,37 @@ bool ruleBasedFilter(vector<Point2f> pcPoints, float maxZ, int numPoints){
109110

110111
float dist1 = sqrt((x1-x2)*(x1-x2)+ (y1-y2)*(y1-y2));
111112
float dist2 = sqrt((x3-x2)*(x3-x2)+ (y3-y2)*(y3-y2));
112-
if(dist1 > dist2){
113+
if(dist1 > dist2){ // 判断长边和宽边
113114
length = dist1;
114115
width = dist2;
115116
}
116117
else{
117118
length = dist2;
118119
width = dist1;
119120
}
120-
// assuming ground = sensor height
121-
height = maxZ + sensorHeight;
121+
// assuming ground = sensor height 假设地面=传感器高度
122+
// cout << "maxZ is "<< maxZ <<endl; // 差不多这个范围(0.553392,0.999263)
123+
height = maxZ + sensorHeight; // float sensorHeight = 2;
122124
// assuming right angle
123-
area = dist1*dist2;
124-
mass = area*height;
125-
ratio = length/width;
125+
area = dist1*dist2; //平面框面积
126+
mass = area*height; //框 体积
127+
ratio = length/width; // 长宽比
126128

127-
// cout<< "height is " <<height<<endl;
128-
// cout<< "width is " <<width<<endl;
129-
// cout<< "length is " <<length<<endl;
130-
// cout<< "area is " <<area<<endl;
131-
// cout<< "mass is " <<mass<<endl;
132-
// cout<< "ratio is " <<ratio<<endl;
129+
// cout<< "height is " <<height<<endl; // 2.3821
130+
// cout<< "width is " <<width<<endl; // 2.17553
131+
// cout<< "length is " <<length<<endl; //4.92502
132+
// cout<< "area is " <<area<<endl; //10.7145
133+
// cout<< "mass is " <<mass<<endl; //25.523
134+
// cout<< "ratio is " <<ratio<<endl; // 2.26383
133135

134136
//start rule based filtering // 开始 Rule-based Filter
135-
if(height > tHeightMin && height < tHeightMax){
136-
if(width > tWidthMin && width < tWidthMax){
137-
if(length > tLenMin && length < tLenMax){
138-
if(area < tAreaMax){
139-
if(numPoints > mass*tPtPerM3){
140-
if(length > minLenRatio){
141-
if(ratio > tRatioMin && ratio < tRatioMax){
137+
if(height > tHeightMin && height < tHeightMax){ //(0.8 2.6) tHeightMin = 0.8; tHeightMax = 2.6;
138+
if(width > tWidthMin && width < tWidthMax){ // (0.2 3.5)
139+
if(length > tLenMin && length < tLenMax){ // (0.2, 14)
140+
if(area < tAreaMax){ // tAreaMax = 20.0;
141+
if(numPoints > mass*tPtPerM3){ // tPtPerM3 = 8;
142+
if(length > minLenRatio){ // minLenRatio = 3.0;
143+
if(ratio > tRatioMin && ratio < tRatioMax){ //长宽比 (1.0, 8.0)
142144
isPromising = true;
143145
return isPromising;
144146
}
@@ -211,160 +213,161 @@ void getBoundingBox(vector<PointCloud<PointXYZ>> clusteredPoints,
211213
vector<PointCloud<PointXYZ>>& bbPoints,visualization_msgs::MarkerArray& ma){
212214
// cout << "the number of cluster is "<< clusteredPoints.size() <<endl; // 初始聚类ID数量numCluster is
213215

214-
for (int iCluster = 0; iCluster < clusteredPoints.size(); iCluster++){ // 循环的次数就是 初始聚类ID数量numCluster is 33
216+
for (int iCluster = 0; iCluster < clusteredPoints.size(); iCluster++){ // 循环的次数就是 初始聚类ID数量numCluster
215217
Mat m (picScale*roiM, picScale*roiM, CV_8UC1, Scalar(0));
216-
float initPX = clusteredPoints[iCluster][0].x + roiM/2;
217-
float initPY = clusteredPoints[iCluster][0].y + roiM/2;
218-
int initX = floor(initPX*picScale);
219-
int initY = floor(initPY*picScale);
218+
float initPX = clusteredPoints[iCluster][0].x + roiM/2; // (0 ~ 50) 第0个点的x，y坐标
219+
float initPY = clusteredPoints[iCluster][0].y + roiM/2; // (0 ~ 50)
220+
int initX = floor(initPX*picScale); // picScale = 900/roiM, 放大倍数900
221+
int initY = floor(initPY*picScale); //
220222
int initPicX = initX;
221-
int initPicY = picScale*roiM - initY;
222-
int offsetInitX = roiM*picScale/2 - initPicX;
223+
int initPicY = picScale*roiM - initY; // Y值取余 ： Y = 900-Y
224+
int offsetInitX = roiM*picScale/2 - initPicX; //
223225
int offsetInitY = roiM*picScale/2 - initPicY;
224226

225-
int numPoints = clusteredPoints[iCluster].size(); // 比如聚类33，每一类里面有多少点 numPoints
226-
vector<Point> pointVec(numPoints);
227+
int numPoints = clusteredPoints[iCluster].size(); // 每一聚类里面有多少点 numPoints
228+
vector<Point> pointVec(numPoints); // 定义
227229
vector<Point2f> pcPoints(4); // ???
228230
float minMx, minMy, maxMx, maxMy;
229231
float minM = 999; float maxM = -999; float maxZ = -99;
230232
float maxdistance=-999;
231-
// for center of gravity
233+
// for center of gravity重力
232234
float sumX = 0; float sumY = 0;
233235

234236
// cout << "the number of cluster i is "<< numPoints <<endl;
235237

236238
// 循环每一聚类里面的点
237239
for (int iPoint = 0; iPoint < clusteredPoints[iCluster].size(); iPoint++){
238-
float pX = clusteredPoints[iCluster][iPoint].x;
240+
float pX = clusteredPoints[iCluster][iPoint].x; // 聚类中每个点的x，y，z
239241
float pY = clusteredPoints[iCluster][iPoint].y;
240242
float pZ = clusteredPoints[iCluster][iPoint].z;
241243
// cast (-15 < x,y < 15) into (0 < x,y < 30)
242244
float roiX = pX + roiM/2;
243245
float roiY = pY + roiM/2;
244-
// cast 30mx30m into 900x900 scale
246+
// cast 30mx30m into 900x900 scale 投射放大
245247
int x = floor(roiX*picScale);
246248
int y = floor(roiY*picScale);
247249
// cast into image coordinate
248250
int picX = x;
249-
int picY = picScale*roiM - y;
250-
// offset so that the object would be locate at the center
251+
int picY = picScale*roiM - y; // Y轴换个反方向
252+
// offset so that the object would be locate at the center offset偏移量以便将object定位在中心
251253
int offsetX = picX + offsetInitX;
252254
int offsetY = picY + offsetInitY;
253255

254256
/* std::cout << "----------------------" << std::endl;
255-
std::cout << offsetX << std::endl;
256-
std::cout << offsetY << std::endl;
257-
std::cout << "---------------------" << std::endl;
257+
std::cout << offsetX << std::endl; // 举例 444
258+
std::cout << offsetY << std::endl; // 举例 452
259+
std::cout << "---------------------" << std::endl;
258260
*/
261+
259262
// m.at<uchar>(offsetY, offsetX) = 255;
260-
pointVec[iPoint] = Point(offsetX, offsetY);
263+
pointVec[iPoint] = Point(offsetX, offsetY); // 偏移量
261264
// calculate min and max slope 斜率
262-
float m = pY/pX;
265+
float m = pY/pX; // 斜率
263266
////////////std::cout << "m:" << m << std::endl;
264267

265-
if(m < minM) {
268+
if(m < minM) { // float minM = 999; float maxM = -999 根据斜率判断
266269
minM = m;
267270
minMx = pX;
268271
minMy = pY;
269272
/////////std::cout << "mmmmmm" << m << std::endl;
270273

271274
}
272-
if(m > maxM) {
275+
if(m > maxM) { // float minM = 999; float maxM = -999
273276
maxM = m;
274277
maxMx = pX;
275278
maxMy = pY;
276279
/////////std::cout << "MMMMM" << std::endl;
277280
}
278281

279-
float xydis = sqrt(pX*pX+pY*pY);
282+
float xydis = sqrt(pX*pX+pY*pY); //
280283

281284
if(xydis > maxdistance)maxdistance = xydis; // 最大距离
282285

283286

284287
//get maxZ
285-
if(pZ > maxZ) maxZ = pZ; // 最大距离
288+
if(pZ > maxZ) maxZ = pZ; // 最大高度
286289

287-
sumX += offsetX;
290+
sumX += offsetX; // ???
288291
sumY += offsetY;
289292

290-
}
293+
} // 小循环结束
291294
// L shape fitting parameters === 将最小面积矩形（MAR）[128]应用于每个聚类对象，从而生成一个2D框，
292-
// 某些稀疏点（带红色圆圈）被认为是异常值，并且MAR过程导致不正确的框，使用L形拟合可解决此问题
293-
float xDist = maxMx - minMx;
295+
// 某些稀疏点（带红色圆圈）被认为是异常值，并且MAR过程导致不正确的框，使用L形拟合可解决此问题 可参考:https://www.yuque.com/huangzhongqing/hre6tf/pcohs1#FONbX
296+
float xDist = maxMx - minMx; // 选择两个最远的离群点x1和x2
294297
float yDist = maxMy - minMy;
295-
float slopeDist = sqrt(xDist*xDist + yDist*yDist); // 选择两个最远的离群点x1和x2，它们位于面向LIDAR传感器的对象的相对侧。然后在两点之间绘制一条线Ldis
298+
float slopeDist = sqrt(xDist*xDist + yDist*yDist); // 选择两个最远的离群点x1和x2，它们位于面向LIDAR传感器的对象的相对侧。然后在两点之间绘制一条线Ld
296299
/////////////////////std::cout << "boxFitting slopeDist" << slopeDist << std::endl;
297-
float slope = (maxMy - minMy)/(maxMx - minMx); //斜率
300+
float slope = (maxMy - minMy)/(maxMx - minMx); //最大斜率
298301

299302
// random variable
300-
mt19937_64 mt(0);
303+
mt19937_64 mt(0); // ???
301304
uniform_int_distribution<> randPoints(0, numPoints-1);
302305

303306
// start l shape fitting for car like object 开始为汽车之类的物体 l shape fitting
304307
// lSlopeDist = 10000, lnumPoints = 30000;
305-
if(slopeDist > lSlopeDist && numPoints > lnumPoints && (maxMy > 8 || maxMy < -5)){
308+
if(slopeDist > lSlopeDist && numPoints > lnumPoints && (maxMy > 8 || maxMy < -5)){ // int lSlopeDist = 1.0; int lnumPoints = 5;判断框的大小以及框内的点数
306309
// if(1){
307310
float maxDist = 0;
308311
float maxDx, maxDy;
309312

310313
// 80 random points, get max distance
311-
for(int i = 0; i < ramPoints; i++){ // int ramPoints = 80;
312-
int pInd = randPoints(mt);
313-
assert(pInd >= 0 && pInd < clusteredPoints[iCluster].size());
314+
for(int i = 0; i < ramPoints; i++){ // ramPoints = 80;
315+
int pInd = randPoints(mt); // 随机点
316+
assert(pInd >= 0 && pInd < clusteredPoints[iCluster].size()); // 如果其值为假（即为0），那么它先向stderr打印一条出错信息，
314317
float xI = clusteredPoints[iCluster][pInd].x;
315318
float yI = clusteredPoints[iCluster][pInd].y;
316319

317-
// from equation of distance between line and point
318-
float dist = abs(slope*xI-1*yI+maxMy-slope*maxMx)/sqrt(slope*slope + 1);
319-
if(dist > maxDist) { // 赋值最大值 maxDist
320-
maxDist = dist;
320+
// from equation of distance between line and point /点到直线的距离方程
321+
float dist = abs(slope*xI-1*yI+maxMy-slope*maxMx)/sqrt(slope*slope + 1); //点到直线的距离
322+
if(dist > maxDist) { // 寻找最远的距离 maxDist
323+
maxDist = dist; // 赋值给最远的距离 maxDist
321324
maxDx = xI;
322325
maxDy = yI;
323326
}
324327
}
325328

326-
// for center of gravity
329+
// for center of gravity 重心
327330
// maxDx = sumX/clusteredPoints[iCluster].size();
328331
// maxDy = sumY/clusteredPoints[iCluster].size();
329332

330-
// vector adding
333+
// vector adding 向量加法 闭合连接X1、X2和X3得到L形折线
331334
float maxMvecX = maxMx - maxDx;
332335
float maxMvecY = maxMy - maxDy;
333336
float minMvecX = minMx - maxDx;
334337
float minMvecY = minMy - maxDy;
335-
float lastX = maxDx + maxMvecX + minMvecX;
338+
float lastX = maxDx + maxMvecX + minMvecX; // X4
336339
float lastY = maxDy + maxMvecY + minMvecY;
337340

338341
pcPoints[0] = Point2f(minMx, minMy); // ??pcPoints
339342
pcPoints[1] = Point2f(maxDx, maxDy);
340343
pcPoints[2] = Point2f(maxMx, maxMy);
341-
pcPoints[3] = Point2f(lastX, lastY);
344+
pcPoints[3] = Point2f(lastX, lastY); // 得到第四个点
342345

343-
////////////std::cout << " pcPoints[0] " << pcPoints[0].x << " " << pcPoints[0].y << std::endl;
344-
///////////std::cout << " pcPoints[1] " << pcPoints[1].x << " " << pcPoints[1].y << std::endl;
345-
///////////std::cout << " pcPoints[2] " << pcPoints[2].x << " " << pcPoints[2].y << std::endl;
346-
//////////std::cout << " pcPoints[3] " << pcPoints[3].x << " " << pcPoints[3].y << std::endl;
346+
// std::cout << " pcPoints[0] " << pcPoints[0].x << " " << pcPoints[0].y << std::endl;
347+
// std::cout << " pcPoints[1] " << pcPoints[1].x << " " << pcPoints[1].y << std::endl;
348+
// std::cout << " pcPoints[2] " << pcPoints[2].x << " " << pcPoints[2].y << std::endl;
349+
// std::cout << " pcPoints[3] " << pcPoints[3].x << " " << pcPoints[3].y << std::endl;
347350

348351
// std::cout << "boxFitting maxZ " << maxZ << std::endl; // boxFitting maxZ 0.525725
349352
// 最后一步:消除大多数无关对象，例如墙壁，灌木丛，建筑物和树木。实现尺寸阈值化以实现此目的。
350-
bool isPromising = ruleBasedFilter(pcPoints, maxZ, numPoints); // 比如聚类33，每一类里面有多少点 numPoints
353+
bool isPromising = ruleBasedFilter(pcPoints, maxZ, numPoints); // 比如聚类33，每一类里面有多少点 numPoints(四个点,z最大值, 每个聚类中的点数)
351354
// std::cout << "boxFitting isPromising:" << isPromising << std::endl; // 1, 0
352-
if(!isPromising) continue;
355+
if(!isPromising) continue; // 没有,就舍弃此聚类,到下一个循环
353356
}
354357
else{
355358
//MAR fitting
356-
RotatedRect rectInfo = minAreaRect(pointVec);
359+
RotatedRect rectInfo = minAreaRect(pointVec); //自带函数
357360
Point2f rectPoints[4]; rectInfo.points( rectPoints );
358-
// covert points back to lidar coordinate
361+
// covert points back to lidar coordinate 隐蔽点返回激光雷达坐标
359362
getPointsInPcFrame(rectPoints, pcPoints, offsetInitX, offsetInitY);
360363
// rule based filter
361364
bool isPromising = ruleBasedFilter(pcPoints, maxZ, numPoints);
362-
if(!isPromising) continue;
363-
// for visualization
364-
// for( int j = 0; j < 4; j++ )
365-
// line( m, rectPoints[j], rectPoints[(j+1)%4ne], Scalar(255,255,0), 1, 8 );
366-
// imshow("Display Image", m);
367-
// waitKey(0);
365+
if(!isPromising) continue; // 没有,就舍弃此聚类,到下一个循环
366+
// // for visualization
367+
// for( int j = 0; j < 4; j++ )
368+
// line( m, rectPoints[j], rectPoints[(j+1)%4ne], Scalar(255,255,0), 1, 8 );
369+
// imshow("Display Image", m);
370+
// waitKey(0);
368371
/////////std::cout << " else pcPoints[0] " << pcPoints[0].x << " " << pcPoints[0].y << std::endl;
369372
////////std::cout << " else pcPoints[1] " << pcPoints[1].x << " " << pcPoints[1].y << std::endl;
370373
///////std::cout << " else pcPoints[2] " << pcPoints[2].x << " " << pcPoints[2].y << std::endl;
@@ -374,12 +377,12 @@ void getBoundingBox(vector<PointCloud<PointXYZ>> clusteredPoints,
374377

375378
// make pcl cloud for 3d bounding box 3D边界框
376379
PointCloud<PointXYZ> oneBbox;
377-
for(int pclH = 0; pclH < 2; pclH++){
380+
for(int pclH = 0; pclH < 2; pclH++){ // 上下个4个坐标点
378381
for(int pclP = 0; pclP < 4; pclP++){
379382
PointXYZ o;
380383
o.x = pcPoints[pclP].x;
381384
o.y = pcPoints[pclP].y;
382-
if(pclH == 0) o.z = -sensorHeight;
385+
if(pclH == 0) o.z = -sensorHeight; // -2
383386
else o.z = maxZ;
384387
oneBbox.push_back(o); // 一个边界框
385388
}

0 commit comments

Comments
 (0)