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();
}
