| 2113 | |
| 2114 | |
| 2115 | void LoopClosing::SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap, vector<MapPoint*> &vpMapPoints) |
| 2116 | { |
| 2117 | ORBmatcher matcher(0.8); |
| 2118 | |
| 2119 | int total_replaces = 0; |
| 2120 | |
| 2121 | //cout << "[FUSE]: Initially there are " << vpMapPoints.size() << " MPs" << endl; |
| 2122 | //cout << "FUSE: Intially there are " << CorrectedPosesMap.size() << " KFs" << endl; |
| 2123 | for(KeyFrameAndPose::const_iterator mit=CorrectedPosesMap.begin(), mend=CorrectedPosesMap.end(); mit!=mend;mit++) |
| 2124 | { |
| 2125 | int num_replaces = 0; |
| 2126 | KeyFrame* pKFi = mit->first; |
| 2127 | Map* pMap = pKFi->GetMap(); |
| 2128 | |
| 2129 | g2o::Sim3 g2oScw = mit->second; |
| 2130 | Sophus::Sim3f Scw = Converter::toSophus(g2oScw); |
| 2131 | |
| 2132 | vector<MapPoint*> vpReplacePoints(vpMapPoints.size(),static_cast<MapPoint*>(NULL)); |
| 2133 | int numFused = matcher.Fuse(pKFi,Scw,vpMapPoints,4,vpReplacePoints); |
| 2134 | |
| 2135 | // Get Map Mutex |
| 2136 | unique_lock<mutex> lock(pMap->mMutexMapUpdate); |
| 2137 | const int nLP = vpMapPoints.size(); |
| 2138 | for(int i=0; i<nLP;i++) |
| 2139 | { |
| 2140 | MapPoint* pRep = vpReplacePoints[i]; |
| 2141 | if(pRep) |
| 2142 | { |
| 2143 | |
| 2144 | |
| 2145 | num_replaces += 1; |
| 2146 | pRep->Replace(vpMapPoints[i]); |
| 2147 | |
| 2148 | } |
| 2149 | } |
| 2150 | |
| 2151 | total_replaces += num_replaces; |
| 2152 | } |
| 2153 | //cout << "[FUSE]: " << total_replaces << " MPs had been fused" << endl; |
| 2154 | } |
| 2155 | |
| 2156 | |
| 2157 | void LoopClosing::SearchAndFuse(const vector<KeyFrame*> &vConectedKFs, vector<MapPoint*> &vpMapPoints) |