Skip to content

Commit 946f8e1

Browse files
jizhang-cmualexlin2
jizhang-cmu
authored and
alexlin2
committed
update terrain analysis
1 parent ef0fac3 commit 946f8e1

2 files changed

Lines changed: 73 additions & 75 deletions

File tree

‎src/base_autonomy/terrain_analysis/launch/terrain_analysis.launch‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,7 @@
1212
<param name="maxGroundLift" value="0.15" />
1313
<param name="clearDyObs" value="true" />
1414
<param name="minDyObsDis" value="0.3" />
15-
<param name="minDyObsAngle" value="0.0" />
16-
<param name="minDyObsRelZ" value="-0.5" />
15+
<param name="minDyObsHeight" value="0.1" />
1716
<param name="absDyObsRelZThre" value="0.2" />
1817
<param name="minDyObsVFOV" value="-30.0" />
1918
<param name="maxDyObsVFOV" value="35.0" />

‎src/base_autonomy/terrain_analysis/src/terrainAnalysis.cpp‎

Lines changed: 72 additions & 73 deletions
Original file line numberDiff line numberDiff line change
@@ -45,8 +45,7 @@ bool limitGroundLift = false;
4545
double maxGroundLift = 0.15;
4646
bool clearDyObs = false;
4747
double minDyObsDis = 0.3;
48-
double minDyObsAngle = 0;
49-
double minDyObsRelZ = -0.5;
48+
double minDyObsHeight = 0.2;
5049
double absDyObsRelZThre = 0.2;
5150
double minDyObsVFOV = -16.0;
5251
double maxDyObsVFOV = 16.0;
@@ -212,8 +211,7 @@ int main(int argc, char **argv) {
212211
nh->declare_parameter<double>("maxGroundLift", maxGroundLift);
213212
nh->declare_parameter<bool>("clearDyObs", clearDyObs);
214213
nh->declare_parameter<double>("minDyObsDis", minDyObsDis);
215-
nh->declare_parameter<double>("minDyObsAngle", minDyObsAngle);
216-
nh->declare_parameter<double>("minDyObsRelZ", minDyObsRelZ);
214+
nh->declare_parameter<double>("minDyObsHeight", minDyObsHeight);
217215
nh->declare_parameter<double>("absDyObsRelZThre", absDyObsRelZThre);
218216
nh->declare_parameter<double>("minDyObsVFOV", minDyObsVFOV);
219217
nh->declare_parameter<double>("maxDyObsVFOV", maxDyObsVFOV);
@@ -239,8 +237,7 @@ int main(int argc, char **argv) {
239237
nh->get_parameter("maxGroundLift", maxGroundLift);
240238
nh->get_parameter("clearDyObs", clearDyObs);
241239
nh->get_parameter("minDyObsDis", minDyObsDis);
242-
nh->get_parameter("minDyObsAngle", minDyObsAngle);
243-
nh->get_parameter("minDyObsRelZ", minDyObsRelZ);
240+
nh->get_parameter("minDyObsHeight", minDyObsHeight);
244241
nh->get_parameter("absDyObsRelZThre", absDyObsRelZThre);
245242
nh->get_parameter("minDyObsVFOV", minDyObsVFOV);
246243
nh->get_parameter("maxDyObsVFOV", maxDyObsVFOV);
@@ -354,11 +351,9 @@ int main(int argc, char **argv) {
354351
for (int i = 0; i < laserCloudCropSize; i++) {
355352
point = laserCloudCrop->points[i];
356353

357-
int indX = int((point.x - vehicleX + terrainVoxelSize / 2) /
358-
terrainVoxelSize) +
354+
int indX = int((point.x - vehicleX + terrainVoxelSize / 2) / terrainVoxelSize) +
359355
terrainVoxelHalfWidth;
360-
int indY = int((point.y - vehicleY + terrainVoxelSize / 2) /
361-
terrainVoxelSize) +
356+
int indY = int((point.y - vehicleY + terrainVoxelSize / 2) / terrainVoxelSize) +
362357
terrainVoxelHalfWidth;
363358

364359
if (point.x - vehicleX + terrainVoxelSize / 2 < 0)
@@ -450,8 +445,65 @@ int main(int argc, char **argv) {
450445
}
451446
}
452447
}
448+
}
449+
450+
if (useSorting) {
451+
for (int i = 0; i < planarVoxelNum; i++) {
452+
int planarPointElevSize = planarPointElev[i].size();
453+
if (planarPointElevSize > 0) {
454+
sort(planarPointElev[i].begin(), planarPointElev[i].end());
455+
456+
int quantileID = int(quantileZ * planarPointElevSize);
457+
if (quantileID < 0)
458+
quantileID = 0;
459+
else if (quantileID >= planarPointElevSize)
460+
quantileID = planarPointElevSize - 1;
461+
462+
if (planarPointElev[i][quantileID] >
463+
planarPointElev[i][0] + maxGroundLift &&
464+
limitGroundLift) {
465+
planarVoxelElev[i] = planarPointElev[i][0] + maxGroundLift;
466+
} else {
467+
planarVoxelElev[i] = planarPointElev[i][quantileID];
468+
}
469+
}
470+
}
471+
} else {
472+
for (int i = 0; i < planarVoxelNum; i++) {
473+
int planarPointElevSize = planarPointElev[i].size();
474+
if (planarPointElevSize > 0) {
475+
float minZ = 1000.0;
476+
int minID = -1;
477+
for (int j = 0; j < planarPointElevSize; j++) {
478+
if (planarPointElev[i][j] < minZ) {
479+
minZ = planarPointElev[i][j];
480+
minID = j;
481+
}
482+
}
483+
484+
if (minID != -1) {
485+
planarVoxelElev[i] = planarPointElev[i][minID];
486+
}
487+
}
488+
}
489+
}
490+
491+
if (clearDyObs) {
492+
for (int i = 0; i < terrainCloudSize; i++) {
493+
point = terrainCloud->points[i];
494+
495+
int indX =
496+
int((point.x - vehicleX + planarVoxelSize / 2) / planarVoxelSize) +
497+
planarVoxelHalfWidth;
498+
int indY =
499+
int((point.y - vehicleY + planarVoxelSize / 2) / planarVoxelSize) +
500+
planarVoxelHalfWidth;
501+
502+
if (point.x - vehicleX + planarVoxelSize / 2 < 0)
503+
indX--;
504+
if (point.y - vehicleY + planarVoxelSize / 2 < 0)
505+
indY--;
453506

454-
if (clearDyObs) {
455507
if (indX >= 0 && indX < planarVoxelWidth && indY >= 0 &&
456508
indY < planarVoxelWidth) {
457509
float pointX1 = point.x - vehicleX;
@@ -460,8 +512,8 @@ int main(int argc, char **argv) {
460512

461513
float dis1 = sqrt(pointX1 * pointX1 + pointY1 * pointY1);
462514
if (dis1 > minDyObsDis) {
463-
float angle1 = atan2(pointZ1 - minDyObsRelZ, dis1) * 180.0 / PI;
464-
if (angle1 > minDyObsAngle) {
515+
float h1 = point.z - planarVoxelElev[planarVoxelWidth * indX + indY];
516+
if (h1 > minDyObsHeight) {
465517
float pointX2 =
466518
pointX1 * cosVehicleYaw + pointY1 * sinVehicleYaw;
467519
float pointY2 =
@@ -492,17 +544,13 @@ int main(int argc, char **argv) {
492544
}
493545
}
494546
}
495-
}
496547

497-
if (clearDyObs) {
498548
for (int i = 0; i < laserCloudCropSize; i++) {
499549
point = laserCloudCrop->points[i];
500550

501-
int indX = int((point.x - vehicleX + planarVoxelSize / 2) /
502-
planarVoxelSize) +
551+
int indX = int((point.x - vehicleX + planarVoxelSize / 2) / planarVoxelSize) +
503552
planarVoxelHalfWidth;
504-
int indY = int((point.y - vehicleY + planarVoxelSize / 2) /
505-
planarVoxelSize) +
553+
int indY = int((point.y - vehicleY + planarVoxelSize / 2) / planarVoxelSize) +
506554
planarVoxelHalfWidth;
507555

508556
if (point.x - vehicleX + planarVoxelSize / 2 < 0)
@@ -512,70 +560,22 @@ int main(int argc, char **argv) {
512560

513561
if (indX >= 0 && indX < planarVoxelWidth && indY >= 0 &&
514562
indY < planarVoxelWidth) {
515-
float pointX1 = point.x - vehicleX;
516-
float pointY1 = point.y - vehicleY;
517-
float pointZ1 = point.z - vehicleZ;
518-
519-
float dis1 = sqrt(pointX1 * pointX1 + pointY1 * pointY1);
520-
float angle1 = atan2(pointZ1 - minDyObsRelZ, dis1) * 180.0 / PI;
521-
if (angle1 > minDyObsAngle) {
563+
float h1 = point.z - planarVoxelElev[planarVoxelWidth * indX + indY];
564+
if (h1 > minDyObsHeight) {
522565
planarVoxelDyObs[planarVoxelWidth * indX + indY] = 0;
523566
}
524567
}
525568
}
526569
}
527570

528-
if (useSorting) {
529-
for (int i = 0; i < planarVoxelNum; i++) {
530-
int planarPointElevSize = planarPointElev[i].size();
531-
if (planarPointElevSize > 0) {
532-
sort(planarPointElev[i].begin(), planarPointElev[i].end());
533-
534-
int quantileID = int(quantileZ * planarPointElevSize);
535-
if (quantileID < 0)
536-
quantileID = 0;
537-
else if (quantileID >= planarPointElevSize)
538-
quantileID = planarPointElevSize - 1;
539-
540-
if (planarPointElev[i][quantileID] >
541-
planarPointElev[i][0] + maxGroundLift &&
542-
limitGroundLift) {
543-
planarVoxelElev[i] = planarPointElev[i][0] + maxGroundLift;
544-
} else {
545-
planarVoxelElev[i] = planarPointElev[i][quantileID];
546-
}
547-
}
548-
}
549-
} else {
550-
for (int i = 0; i < planarVoxelNum; i++) {
551-
int planarPointElevSize = planarPointElev[i].size();
552-
if (planarPointElevSize > 0) {
553-
float minZ = 1000.0;
554-
int minID = -1;
555-
for (int j = 0; j < planarPointElevSize; j++) {
556-
if (planarPointElev[i][j] < minZ) {
557-
minZ = planarPointElev[i][j];
558-
minID = j;
559-
}
560-
}
561-
562-
if (minID != -1) {
563-
planarVoxelElev[i] = planarPointElev[i][minID];
564-
}
565-
}
566-
}
567-
}
568-
569571
terrainCloudElev->clear();
570572
int terrainCloudElevSize = 0;
571573
for (int i = 0; i < terrainCloudSize; i++) {
572574
point = terrainCloud->points[i];
573575
if (point.z - vehicleZ > minRelZ && point.z - vehicleZ < maxRelZ) {
574-
int indX = int((point.x - vehicleX + planarVoxelSize / 2) /
575-
planarVoxelSize) +
576+
int indX = int((point.x - vehicleX + planarVoxelSize / 2) / planarVoxelSize) +
576577
planarVoxelHalfWidth;
577-
int indY = int((point.y - vehicleY + planarVoxelSize / 2) /
578-
planarVoxelSize) +
578+
int indY = int((point.y - vehicleY + planarVoxelSize / 2) / planarVoxelSize) +
579579
planarVoxelHalfWidth;
580580

581581
if (point.x - vehicleX + planarVoxelSize / 2 < 0)
@@ -585,8 +585,7 @@ int main(int argc, char **argv) {
585585

586586
if (indX >= 0 && indX < planarVoxelWidth && indY >= 0 &&
587587
indY < planarVoxelWidth) {
588-
if (planarVoxelDyObs[planarVoxelWidth * indX + indY] <
589-
minDyObsPointNum ||
588+
if (planarVoxelDyObs[planarVoxelWidth * indX + indY] < minDyObsPointNum ||
590589
!clearDyObs) {
591590
float disZ =
592591
point.z - planarVoxelElev[planarVoxelWidth * indX + indY];

0 commit comments

Comments
 (0)