| 1624 | } |
| 1625 | |
| 1626 | void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum, bool doFiltering = true) |
| 1627 | { |
| 1628 | // extract near keyframes |
| 1629 | nearKeyframes->clear(); |
| 1630 | int cloudSize = copy_cloudKeyPoses6D->size(); |
| 1631 | for (int i = -searchNum; i <= searchNum; ++i) |
| 1632 | { |
| 1633 | int keyNear = key + i; |
| 1634 | if (keyNear < 0 || keyNear >= cloudSize ) |
| 1635 | continue; |
| 1636 | // it may happen in temporary mapping mode |
| 1637 | float dist = pointDistance(copy_cloudKeyPoses3D->points[key],copy_cloudKeyPoses3D->points[keyNear]); |
| 1638 | if ( dist > surroundingKeyframeSearchRadius) |
| 1639 | continue; |
| 1640 | *nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]); |
| 1641 | *nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]); |
| 1642 | } |
| 1643 | |
| 1644 | if (nearKeyframes->empty()) |
| 1645 | return; |
| 1646 | |
| 1647 | // downsample near keyframes |
| 1648 | if (doFiltering){ |
| 1649 | pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>()); |
| 1650 | downSizeFilterICP.setInputCloud(nearKeyframes); |
| 1651 | downSizeFilterICP.filter(*cloud_temp); |
| 1652 | *nearKeyframes = *cloud_temp; |
| 1653 | } |
| 1654 | } |
| 1655 | // copied to the original, so that reloc part can use this func |
| 1656 | void loopFindNearKeyframesReloc(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum, bool doFiltering = true) |
| 1657 | { |
nothing calls this directly
no test coverage detected