int SlamSystem::findConstraintsForNewKeyFrames(Frame* newKeyFrame, bool forceParent, bool useFABMAP, float closeCandidatesTH) { if(!newKeyFrame->hasTrackingParent()) { newConstraintMutex.lock(); keyFrameGraph->addKeyFrame(newKeyFrame); newConstraintAdded = true; newConstraintCreatedSignal.notify_all(); newConstraintMutex.unlock(); return 0; } if(!forceParent && (newKeyFrame->lastConstraintTrackedCamToWorld * newKeyFrame->getScaledCamToWorld().inverse()).log().norm() < 0.01) return 0; newKeyFrame->lastConstraintTrackedCamToWorld = newKeyFrame->getScaledCamToWorld(); // =============== get all potential candidates and their initial relative pose. ================= std::vector<KFConstraintStruct*, Eigen::aligned_allocator<KFConstraintStruct*> > constraints; Frame* fabMapResult = 0; std::unordered_set<Frame*, std::hash<Frame*>, std::equal_to<Frame*>, Eigen::aligned_allocator< Frame* > > candidates = trackableKeyFrameSearch->findCandidates(newKeyFrame, fabMapResult, useFABMAP, closeCandidatesTH); std::map< Frame*, Sim3, std::less<Frame*>, Eigen::aligned_allocator<std::pair<Frame*, Sim3> > > candidateToFrame_initialEstimateMap; // erase the ones that are already neighbours. for(std::unordered_set<Frame*>::iterator c = candidates.begin(); c != candidates.end();) { if(newKeyFrame->neighbors.find(*c) != newKeyFrame->neighbors.end()) { if(enablePrintDebugInfo && printConstraintSearchInfo) printf("SKIPPING %d on %d cause it already exists as constraint.\n", (*c)->id(), newKeyFrame->id()); c = candidates.erase(c); } else ++c; } poseConsistencyMutex.lock_shared(); for (Frame* candidate : candidates) { Sim3 candidateToFrame_initialEstimate = newKeyFrame->getScaledCamToWorld().inverse() * candidate->getScaledCamToWorld(); candidateToFrame_initialEstimateMap[candidate] = candidateToFrame_initialEstimate; } std::unordered_map<Frame*, int> distancesToNewKeyFrame; if(newKeyFrame->hasTrackingParent()) keyFrameGraph->calculateGraphDistancesToFrame(newKeyFrame->getTrackingParent(), &distancesToNewKeyFrame); poseConsistencyMutex.unlock_shared(); // =============== distinguish between close and "far" candidates in Graph ================= // Do a first check on trackability of close candidates. std::unordered_set<Frame*, std::hash<Frame*>, std::equal_to<Frame*>, Eigen::aligned_allocator< Frame* > > closeCandidates; std::vector<Frame*, Eigen::aligned_allocator<Frame*> > farCandidates; Frame* parent = newKeyFrame->hasTrackingParent() ? newKeyFrame->getTrackingParent() : 0; int closeFailed = 0; int closeInconsistent = 0; SO3 disturbance = SO3::exp(Sophus::Vector3d(0.05,0,0)); for (Frame* candidate : candidates) { if (candidate->id() == newKeyFrame->id()) continue; if(!candidate->pose->isInGraph) continue; if(newKeyFrame->hasTrackingParent() && candidate == newKeyFrame->getTrackingParent()) continue; if(candidate->idxInKeyframes < INITIALIZATION_PHASE_COUNT) continue; SE3 c2f_init = se3FromSim3(candidateToFrame_initialEstimateMap[candidate].inverse()).inverse(); c2f_init.so3() = c2f_init.so3() * disturbance; SE3 c2f = constraintSE3Tracker->trackFrameOnPermaref(candidate, newKeyFrame, c2f_init); if(!constraintSE3Tracker->trackingWasGood) {closeFailed++; continue;} SE3 f2c_init = se3FromSim3(candidateToFrame_initialEstimateMap[candidate]).inverse(); f2c_init.so3() = disturbance * f2c_init.so3(); SE3 f2c = constraintSE3Tracker->trackFrameOnPermaref(newKeyFrame, candidate, f2c_init); if(!constraintSE3Tracker->trackingWasGood) {closeFailed++; continue;} if((f2c.so3() * c2f.so3()).log().norm() >= 0.09) {closeInconsistent++; continue;} closeCandidates.insert(candidate); } int farFailed = 0; int farInconsistent = 0; for (Frame* candidate : candidates) { if (candidate->id() == newKeyFrame->id()) continue; if(!candidate->pose->isInGraph) continue; if(newKeyFrame->hasTrackingParent() && candidate == newKeyFrame->getTrackingParent()) continue; if(candidate->idxInKeyframes < INITIALIZATION_PHASE_COUNT) continue; if(candidate == fabMapResult) { farCandidates.push_back(candidate); continue; } if(distancesToNewKeyFrame.at(candidate) < 4) continue; farCandidates.push_back(candidate); } int closeAll = closeCandidates.size(); int farAll = farCandidates.size(); // erase the ones that we tried already before (close) for(std::unordered_set<Frame*>::iterator c = closeCandidates.begin(); c != closeCandidates.end();) { if(newKeyFrame->trackingFailed.find(*c) == newKeyFrame->trackingFailed.end()) { ++c; continue; } auto range = newKeyFrame->trackingFailed.equal_range(*c); bool skip = false; Sim3 f2c = candidateToFrame_initialEstimateMap[*c].inverse(); for (auto it = range.first; it != range.second; ++it) { if((f2c * it->second).log().norm() < 0.1) { skip=true; break; } } if(skip) { if(enablePrintDebugInfo && printConstraintSearchInfo) printf("SKIPPING %d on %d (NEAR), cause we already have tried it.\n", (*c)->id(), newKeyFrame->id()); c = closeCandidates.erase(c); } else ++c; } // erase the ones that are already neighbours (far) for(unsigned int i=0;i<farCandidates.size();i++) { if(newKeyFrame->trackingFailed.find(farCandidates[i]) == newKeyFrame->trackingFailed.end()) continue; auto range = newKeyFrame->trackingFailed.equal_range(farCandidates[i]); bool skip = false; for (auto it = range.first; it != range.second; ++it) { if((it->second).log().norm() < 0.2) { skip=true; break; } } if(skip) { if(enablePrintDebugInfo && printConstraintSearchInfo) printf("SKIPPING %d on %d (FAR), cause we already have tried it.\n", farCandidates[i]->id(), newKeyFrame->id()); farCandidates[i] = farCandidates.back(); farCandidates.pop_back(); i--; } } if (enablePrintDebugInfo && printConstraintSearchInfo) printf("Final Loop-Closure Candidates: %d / %d close (%d failed, %d inconsistent) + %d / %d far (%d failed, %d inconsistent) = %d\n", (int)closeCandidates.size(),closeAll, closeFailed, closeInconsistent, (int)farCandidates.size(), farAll, farFailed, farInconsistent, (int)closeCandidates.size() + (int)farCandidates.size()); // =============== limit number of close candidates =============== // while too many, remove the one with the highest connectivity. while((int)closeCandidates.size() > maxLoopClosureCandidates) { Frame* worst = 0; int worstNeighbours = 0; for(Frame* f : closeCandidates) { int neightboursInCandidates = 0; for(Frame* n : f->neighbors) if(closeCandidates.find(n) != closeCandidates.end()) neightboursInCandidates++; if(neightboursInCandidates > worstNeighbours || worst == 0) { worst = f; worstNeighbours = neightboursInCandidates; } } closeCandidates.erase(worst); } // =============== limit number of far candidates =============== // delete randomly int maxNumFarCandidates = (maxLoopClosureCandidates +1) / 2; if(maxNumFarCandidates < 5) maxNumFarCandidates = 5; while((int)farCandidates.size() > maxNumFarCandidates) { int toDelete = rand() % farCandidates.size(); if(farCandidates[toDelete] != fabMapResult) { farCandidates[toDelete] = farCandidates.back(); farCandidates.pop_back(); } } // =============== TRACK! =============== // make tracking reference for newKeyFrame. newKFTrackingReference->importFrame(newKeyFrame); for (Frame* candidate : closeCandidates) { KFConstraintStruct* e1=0; KFConstraintStruct* e2=0; testConstraint( candidate, e1, e2, candidateToFrame_initialEstimateMap[candidate], loopclosureStrictness); if(enablePrintDebugInfo && printConstraintSearchInfo) printf(" CLOSE (%d)\n", distancesToNewKeyFrame.at(candidate)); if(e1 != 0) { constraints.push_back(e1); constraints.push_back(e2); // delete from far candidates if it's in there. for(unsigned int k=0;k<farCandidates.size();k++) { if(farCandidates[k] == candidate) { if(enablePrintDebugInfo && printConstraintSearchInfo) printf(" DELETED %d from far, as close was successful!\n", candidate->id()); farCandidates[k] = farCandidates.back(); farCandidates.pop_back(); } } } } for (Frame* candidate : farCandidates) { KFConstraintStruct* e1=0; KFConstraintStruct* e2=0; testConstraint( candidate, e1, e2, Sim3(), loopclosureStrictness); if(enablePrintDebugInfo && printConstraintSearchInfo) printf(" FAR (%d)\n", distancesToNewKeyFrame.at(candidate)); if(e1 != 0) { constraints.push_back(e1); constraints.push_back(e2); } } if(parent != 0 && forceParent) { KFConstraintStruct* e1=0; KFConstraintStruct* e2=0; testConstraint( parent, e1, e2, candidateToFrame_initialEstimateMap[parent], 100); if(enablePrintDebugInfo && printConstraintSearchInfo) printf(" PARENT (0)\n"); if(e1 != 0) { constraints.push_back(e1); constraints.push_back(e2); } else { float downweightFac = 5; const float kernelDelta = 5 * sqrt(6000*loopclosureStrictness) / downweightFac; printf("warning: reciprocal tracking on new frame failed badly, added odometry edge (Hacky).\n"); poseConsistencyMutex.lock_shared(); constraints.push_back(new KFConstraintStruct()); constraints.back()->firstFrame = newKeyFrame; constraints.back()->secondFrame = newKeyFrame->getTrackingParent(); constraints.back()->secondToFirst = constraints.back()->firstFrame->getScaledCamToWorld().inverse() * constraints.back()->secondFrame->getScaledCamToWorld(); constraints.back()->information << 0.8098,-0.1507,-0.0557, 0.1211, 0.7657, 0.0120, 0, -0.1507, 2.1724,-0.1103,-1.9279,-0.1182, 0.1943, 0, -0.0557,-0.1103, 0.2643,-0.0021,-0.0657,-0.0028, 0.0304, 0.1211,-1.9279,-0.0021, 2.3110, 0.1039,-0.0934, 0.0005, 0.7657,-0.1182,-0.0657, 0.1039, 1.0545, 0.0743,-0.0028, 0.0120, 0.1943,-0.0028,-0.0934, 0.0743, 0.4511, 0, 0,0, 0.0304, 0.0005,-0.0028, 0, 0.0228; constraints.back()->information *= (1e9/(downweightFac*downweightFac)); constraints.back()->robustKernel = new g2o::RobustKernelHuber(); constraints.back()->robustKernel->setDelta(kernelDelta); constraints.back()->meanResidual = 10; constraints.back()->meanResidualD = 10; constraints.back()->meanResidualP = 10; constraints.back()->usage = 0; poseConsistencyMutex.unlock_shared(); } } newConstraintMutex.lock(); keyFrameGraph->addKeyFrame(newKeyFrame); for(unsigned int i=0;i<constraints.size();i++) keyFrameGraph->insertConstraint(constraints[i]); newConstraintAdded = true; newConstraintCreatedSignal.notify_all(); newConstraintMutex.unlock(); newKFTrackingReference->invalidate(); candidateTrackingReference->invalidate(); return constraints.size(); }