| 582 | } |
| 583 | |
| 584 | void createHashTable() { |
| 585 | size_t totalSize = polarCor.size(); |
| 586 | |
| 587 | Eigen::Vector3d cur = Eigen::Vector3d::Zero(); |
| 588 | int polarIndex, pitchIndex, azimuthIndex, voxelIndex; |
| 589 | voxelMap.reserve(totalSize); |
| 590 | |
| 591 | for (size_t item = 0; item < totalSize; ++item) { |
| 592 | cur = polarCor[item]; |
| 593 | polarIndex = getPolarIndex(cur.x()); |
| 594 | pitchIndex = static_cast<int>( |
| 595 | std::round((cur.y() - minPitch) / params_.deltaP)); |
| 596 | azimuthIndex = |
| 597 | static_cast<int>(std::round(cur.z() / params_.deltaA)); |
| 598 | |
| 599 | voxelIndex = (azimuthIndex * (polarNum + 1) + polarIndex) + |
| 600 | pitchIndex * (polarNum + 1) * (width + 1); |
| 601 | |
| 602 | auto iter = voxelMap.find(voxelIndex); |
| 603 | if (iter != voxelMap.end()) { |
| 604 | // iter->second.index.emplace_back(item); |
| 605 | iter->second.emplace_back(item); |
| 606 | } else { |
| 607 | std::vector<int> index{}; |
| 608 | index.emplace_back(item); |
| 609 | voxelMap.insert(std::make_pair(voxelIndex, index)); |
| 610 | } |
| 611 | } |
| 612 | } |
| 613 | |
| 614 | /** |
| 615 | * @brief get the index value in the polar radial direction |