copied to the original, so that reloc part can use this func
| 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 | { |
| 1658 | // extract near keyframes |
| 1659 | nearKeyframes->clear(); |
| 1660 | int cloudSize = cloudKeyPoses6D->size(); |
| 1661 | for (int i = -searchNum; i <= searchNum; ++i) |
| 1662 | { |
| 1663 | int keyNear = key + i; |
| 1664 | if (keyNear < 0 || keyNear >= cloudSize ) |
| 1665 | continue; |
| 1666 | // copied to the original, so that reloc part can use this func too |
| 1667 | *nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], &cloudKeyPoses6D->points[keyNear]); |
| 1668 | *nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear], &cloudKeyPoses6D->points[keyNear]); |
| 1669 | } |
| 1670 | |
| 1671 | if (nearKeyframes->empty()) |
| 1672 | return; |
| 1673 | |
| 1674 | // downsample near keyframes |
| 1675 | if (doFiltering){ |
| 1676 | pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>()); |
| 1677 | downSizeFilterICP.setInputCloud(nearKeyframes); |
| 1678 | downSizeFilterICP.filter(*cloud_temp); |
| 1679 | *nearKeyframes = *cloud_temp; |
| 1680 | } |
| 1681 | } |
| 1682 | |
| 1683 | void visualizeLoopClosure() |
| 1684 | { |