| 422 | } |
| 423 | |
| 424 | void cloudSegmentation() { |
| 425 | // segmentation process |
| 426 | for (size_t i = 0; i < N_SCAN; ++i) |
| 427 | for (size_t j = 0; j < Horizon_SCAN; ++j) |
| 428 | if (labelMat.at<int>(i, j) == 0) |
| 429 | labelComponents(i, j); |
| 430 | |
| 431 | // label: 999999 -> invalid |
| 432 | // label: over 0 -> valid |
| 433 | |
| 434 | int sizeOfSegCloud = 0; |
| 435 | // extract segmented cloud for lidar odometry |
| 436 | for (size_t i = 0; i < N_SCAN; ++i) { |
| 437 | |
| 438 | segMsg.startRingIndex[i] = sizeOfSegCloud - 1 + 5; |
| 439 | |
| 440 | for (size_t j = 0; j < Horizon_SCAN; ++j) { |
| 441 | if (labelMat.at<int>(i, j) > 0 || groundMat.at<int8_t>(i, j) == 1) { |
| 442 | // outliers that will not be used for optimization (always continue) |
| 443 | if (labelMat.at<int>(i, j) == 999999) { |
| 444 | if (j % 1 == 0) { // i > groundScanInd && j % 5 == 0 |
| 445 | outlierCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]); |
| 446 | continue; |
| 447 | } else { |
| 448 | continue; |
| 449 | } |
| 450 | } |
| 451 | // majority of ground points are skipped |
| 452 | if (groundMat.at<int8_t>(i, j) == 1) { |
| 453 | if (j % 5 != 0 && j > 5 && j < Horizon_SCAN - 5) |
| 454 | continue; |
| 455 | } |
| 456 | // mark ground points so they will not be considered as edge features later |
| 457 | segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i, j) == 1); |
| 458 | // mark the points' column index for marking occlusion later |
| 459 | segMsg.segmentedCloudColInd[sizeOfSegCloud] = j; |
| 460 | // save range info |
| 461 | segMsg.segmentedCloudRange[sizeOfSegCloud] = rangeMat.at<float>(i, j); |
| 462 | // save seg cloud |
| 463 | segmentedCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]); |
| 464 | // size of seg cloud |
| 465 | ++sizeOfSegCloud; |
| 466 | } |
| 467 | } |
| 468 | segMsg.endRingIndex[i] = sizeOfSegCloud - 1 - 5; |
| 469 | } |
| 470 | |
| 471 | // extract segmented cloud for visualization |
| 472 | // 해당토픽을 subscribe하지 않으면, 처리하지 않고 publish안함(아래코드) |
| 473 | // if (pubSegmentedCloudPure.getNumSubscribers() != 0){ |
| 474 | for (size_t i = 0; i < N_SCAN; ++i) { |
| 475 | for (size_t j = 0; j < Horizon_SCAN; ++j) { |
| 476 | if (labelMat.at<int>(i, j) > 0 && labelMat.at<int>(i, j) != 999999) { // |
| 477 | segmentedCloudPure->push_back(fullCloud->points[j + i * Horizon_SCAN]); |
| 478 | segmentedCloudPure->points.back().intensity = labelMat.at<int>(i, j); |
| 479 | } |
| 480 | } |
| 481 | } |