| 1586 | } |
| 1587 | |
| 1588 | bool detectLoopClosureDistance(int *latestID, int *closestID) |
| 1589 | { |
| 1590 | int loopKeyCur = copy_cloudKeyPoses3D->size() - 1; |
| 1591 | int loopKeyPre = -1; |
| 1592 | |
| 1593 | // check loop constraint added before |
| 1594 | auto it = loopIndexContainer.find(loopKeyCur); |
| 1595 | if (it != loopIndexContainer.end()) |
| 1596 | return false; |
| 1597 | |
| 1598 | // find the closest history key frame |
| 1599 | std::vector<int> pointSearchIndLoop; |
| 1600 | std::vector<float> pointSearchSqDisLoop; |
| 1601 | // reset before usage! |
| 1602 | kdtreeHistoryKeyPoses.reset(new pcl::KdTreeFLANN<PointType>()); |
| 1603 | kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D); |
| 1604 | kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0); |
| 1605 | // cout<<copy_cloudKeyPoses6D->points[id].time-cloudKeyPoses6D->points[id].time<<" "<<copy_cloudKeyPoses6D->points[id].time-cloudInfoTime <<endl; |
| 1606 | for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i) |
| 1607 | { |
| 1608 | int id = pointSearchIndLoop[i]; |
| 1609 | |
| 1610 | if (abs(copy_cloudKeyPoses6D->points[id].time - cloudInfoTime) > historyKeyframeSearchTimeDiff) |
| 1611 | { |
| 1612 | loopKeyPre = id; |
| 1613 | break; |
| 1614 | } |
| 1615 | } |
| 1616 | |
| 1617 | if (loopKeyPre == -1 || loopKeyCur == loopKeyPre) |
| 1618 | return false; |
| 1619 | |
| 1620 | *latestID = loopKeyCur; |
| 1621 | *closestID = loopKeyPre; |
| 1622 | |
| 1623 | return true; |
| 1624 | } |
| 1625 | |
| 1626 | void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum, bool doFiltering = true) |
| 1627 | { |
nothing calls this directly
no test coverage detected