| 483 | } |
| 484 | |
| 485 | void labelComponents(int row, int col) { |
| 486 | // use std::queue std::vector std::deque will slow the program down greatly |
| 487 | float d1, d2, alpha, angle; |
| 488 | int fromIndX, fromIndY, thisIndX, thisIndY; |
| 489 | bool lineCountFlag[N_SCAN] = {false}; |
| 490 | |
| 491 | queueIndX[0] = row; |
| 492 | queueIndY[0] = col; |
| 493 | int queueSize = 1; |
| 494 | int queueStartInd = 0; |
| 495 | int queueEndInd = 1; |
| 496 | |
| 497 | allPushedIndX[0] = row; |
| 498 | allPushedIndY[0] = col; |
| 499 | int allPushedIndSize = 1; |
| 500 | |
| 501 | while (queueSize > 0) { |
| 502 | // Pop point |
| 503 | fromIndX = queueIndX[queueStartInd]; |
| 504 | fromIndY = queueIndY[queueStartInd]; |
| 505 | --queueSize; |
| 506 | ++queueStartInd; |
| 507 | // Mark popped point |
| 508 | labelMat.at<int>(fromIndX, fromIndY) = labelCount; |
| 509 | // Loop through all the neighboring grids of popped grid |
| 510 | for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter) { |
| 511 | // new index |
| 512 | thisIndX = fromIndX + (*iter).first; |
| 513 | thisIndY = fromIndY + (*iter).second; |
| 514 | // index should be within the boundary |
| 515 | if (thisIndX < 0 || thisIndX >= N_SCAN) |
| 516 | continue; |
| 517 | // at range image margin (left or right side) |
| 518 | if (thisIndY < 0) |
| 519 | thisIndY = Horizon_SCAN - 1; |
| 520 | if (thisIndY >= Horizon_SCAN) |
| 521 | thisIndY = 0; |
| 522 | // prevent infinite loop (caused by put already examined point back) |
| 523 | if (labelMat.at<int>(thisIndX, thisIndY) != 0) |
| 524 | continue; |
| 525 | |
| 526 | |
| 527 | //두포인트의 range 값 |
| 528 | d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), |
| 529 | rangeMat.at<float>(thisIndX, thisIndY)); |
| 530 | d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), |
| 531 | rangeMat.at<float>(thisIndX, thisIndY)); |
| 532 | |
| 533 | if ((*iter).first == 0) |
| 534 | alpha = segmentAlphaX; // velodyne64 : segmentAlphaX = 0.2deg |
| 535 | else |
| 536 | alpha = segmentAlphaY; |
| 537 | |
| 538 | // beta값 |
| 539 | angle = atan2(d2 * sin(alpha), (d1 - d2 * cos(alpha))); |
| 540 | |
| 541 | if (angle > segmentTheta) { |
| 542 | |