void SlamGraph<Sim3, LinearCamera, Sim3XYZ, 2>
::addConstraintToG2o(const Sim3 & T_2_from_1,
const Matrix6d & Lambda_2_from_1,
int pose_id_1,
int pose_id_2,
g2o::SparseOptimizer * optimizer)
{
G2oEdgeSim3 * e = new G2oEdgeSim3();
g2o::HyperGraph::Vertex * pose1_vertex
= GET_MAP_ELEM(pose_id_1, optimizer->vertices());
e->vertices()[0]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>(pose1_vertex);
g2o::HyperGraph::Vertex * pose2_vertex
= GET_MAP_ELEM(pose_id_2, optimizer->vertices());
e->vertices()[1]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>(pose2_vertex);
e->measurement() = T_2_from_1;
e->inverseMeasurement() = T_2_from_1.inverse();
e->information() = Lambda_2_from_1;
optimizer->addEdge(e);
}
Matrix<double,7,1> Sim3
::log(const Sim3& sim3)
{
Vector7d res;
double scale = sim3.scale();
Vector3d t = sim3.translation_;
double theta;
Vector4d omega_sigma = ScSO3::logAndTheta(sim3.scso3_, &theta);
Vector3d omega = omega_sigma.head<3>();
double sigma = omega_sigma[3];
Matrix3d Omega = SO3::hat(omega);
Matrix3d W = calcW(theta, sigma, scale, Omega);
//Vector3d upsilon = W.jacobiSvd(ComputeFullU | ComputeFullV).solve(t);
Vector3d upsilon = W.partialPivLu().solve(t);
res.segment(0,3) = upsilon;
res.segment(3,3) = omega;
res[6] = sigma;
return res;
}
bool SlamSystem::optimizationIteration(int itsPerTry, float minChange)
{
struct timeval tv_start, tv_end;
gettimeofday(&tv_start, NULL);
g2oGraphAccessMutex.lock();
// lock new elements buffer & take them over.
newConstraintMutex.lock();
keyFrameGraph->addElementsFromBuffer();
newConstraintMutex.unlock();
// Do the optimization. This can take quite some time!
int its = keyFrameGraph->optimize(itsPerTry);
// save the optimization result.
poseConsistencyMutex.lock_shared();
keyFrameGraph->keyframesAllMutex.lock_shared();
float maxChange = 0;
float sumChange = 0;
float sum = 0;
for(size_t i=0;i<keyFrameGraph->keyframesAll.size(); i++)
{
// set edge error sum to zero
keyFrameGraph->keyframesAll[i]->edgeErrorSum = 0;
keyFrameGraph->keyframesAll[i]->edgesNum = 0;
if(!keyFrameGraph->keyframesAll[i]->pose->isInGraph) continue;
// get change from last optimization
Sim3 a = keyFrameGraph->keyframesAll[i]->pose->graphVertex->estimate();
Sim3 b = keyFrameGraph->keyframesAll[i]->getScaledCamToWorld();
Sophus::Vector7f diff = (a*b.inverse()).log().cast<float>();
for(int j=0;j<7;j++)
{
float d = fabsf((float)(diff[j]));
if(d > maxChange) maxChange = d;
sumChange += d;
}
sum +=7;
// set change
keyFrameGraph->keyframesAll[i]->pose->setPoseGraphOptResult(
keyFrameGraph->keyframesAll[i]->pose->graphVertex->estimate());
// add error
for(auto edge : keyFrameGraph->keyframesAll[i]->pose->graphVertex->edges())
{
keyFrameGraph->keyframesAll[i]->edgeErrorSum += ((EdgeSim3*)(edge))->chi2();
keyFrameGraph->keyframesAll[i]->edgesNum++;
}
}
haveUnmergedOptimizationOffset = true;
keyFrameGraph->keyframesAllMutex.unlock_shared();
poseConsistencyMutex.unlock_shared();
g2oGraphAccessMutex.unlock();
if(enablePrintDebugInfo && printOptimizationInfo)
printf("did %d optimization iterations. Max Pose Parameter Change: %f; avgChange: %f. %s\n", its, maxChange, sumChange / sum,
maxChange > minChange && its == itsPerTry ? "continue optimizing":"Waiting for addition to graph.");
gettimeofday(&tv_end, NULL);
msOptimizationIteration = 0.9*msOptimizationIteration + 0.1*((tv_end.tv_sec-tv_start.tv_sec)*1000.0f + (tv_end.tv_usec-tv_start.tv_usec)/1000.0f);
nOptimizationIteration++;
return maxChange > minChange && its == itsPerTry;
}
void SlamSystem::testConstraint(
Frame* candidate,
KFConstraintStruct* &e1_out, KFConstraintStruct* &e2_out,
Sim3 candidateToFrame_initialEstimate,
float strictness)
{
candidateTrackingReference->importFrame(candidate);
Sim3 FtoC = candidateToFrame_initialEstimate.inverse(), CtoF = candidateToFrame_initialEstimate;
Matrix7x7 FtoCInfo, CtoFInfo;
float err_level3 = tryTrackSim3(
newKFTrackingReference, candidateTrackingReference, // A = frame; b = candidate
SIM3TRACKING_MAX_LEVEL-1, 3,
USESSE,
FtoC, CtoF);
if(err_level3 > 3000*strictness)
{
if(enablePrintDebugInfo && printConstraintSearchInfo)
printf("FAILE %d -> %d (lvl %d): errs (%.1f / - / -).",
newKFTrackingReference->frameID, candidateTrackingReference->frameID,
3,
sqrtf(err_level3));
e1_out = e2_out = 0;
newKFTrackingReference->keyframe->trackingFailed.insert(std::pair<Frame*,Sim3>(candidate, candidateToFrame_initialEstimate));
return;
}
float err_level2 = tryTrackSim3(
newKFTrackingReference, candidateTrackingReference, // A = frame; b = candidate
2, 2,
USESSE,
FtoC, CtoF);
if(err_level2 > 4000*strictness)
{
if(enablePrintDebugInfo && printConstraintSearchInfo)
printf("FAILE %d -> %d (lvl %d): errs (%.1f / %.1f / -).",
newKFTrackingReference->frameID, candidateTrackingReference->frameID,
2,
sqrtf(err_level3), sqrtf(err_level2));
e1_out = e2_out = 0;
newKFTrackingReference->keyframe->trackingFailed.insert(std::pair<Frame*,Sim3>(candidate, candidateToFrame_initialEstimate));
return;
}
e1_out = new KFConstraintStruct();
e2_out = new KFConstraintStruct();
float err_level1 = tryTrackSim3(
newKFTrackingReference, candidateTrackingReference, // A = frame; b = candidate
1, 1,
USESSE,
FtoC, CtoF, e1_out, e2_out);
if(err_level1 > 6000*strictness)
{
if(enablePrintDebugInfo && printConstraintSearchInfo)
printf("FAILE %d -> %d (lvl %d): errs (%.1f / %.1f / %.1f).",
newKFTrackingReference->frameID, candidateTrackingReference->frameID,
1,
sqrtf(err_level3), sqrtf(err_level2), sqrtf(err_level1));
delete e1_out;
delete e2_out;
e1_out = e2_out = 0;
newKFTrackingReference->keyframe->trackingFailed.insert(std::pair<Frame*,Sim3>(candidate, candidateToFrame_initialEstimate));
return;
}
if(enablePrintDebugInfo && printConstraintSearchInfo)
printf("ADDED %d -> %d: errs (%.1f / %.1f / %.1f).",
newKFTrackingReference->frameID, candidateTrackingReference->frameID,
sqrtf(err_level3), sqrtf(err_level2), sqrtf(err_level1));
const float kernelDelta = 5 * sqrt(6000*loopclosureStrictness);
e1_out->robustKernel = new g2o::RobustKernelHuber();
e1_out->robustKernel->setDelta(kernelDelta);
e2_out->robustKernel = new g2o::RobustKernelHuber();
e2_out->robustKernel->setDelta(kernelDelta);
}
float SlamSystem::tryTrackSim3(
TrackingReference* A, TrackingReference* B,
int lvlStart, int lvlEnd,
bool useSSE,
Sim3 &AtoB, Sim3 &BtoA,
KFConstraintStruct* e1, KFConstraintStruct* e2 )
{
BtoA = constraintTracker->trackFrameSim3(
A,
B->keyframe,
BtoA,
lvlStart,lvlEnd);
Matrix7x7 BtoAInfo = constraintTracker->lastSim3Hessian;
float BtoA_meanResidual = constraintTracker->lastResidual;
float BtoA_meanDResidual = constraintTracker->lastDepthResidual;
float BtoA_meanPResidual = constraintTracker->lastPhotometricResidual;
float BtoA_usage = constraintTracker->pointUsage;
if (constraintTracker->diverged ||
BtoA.scale() > 1 / Sophus::SophusConstants<sophusType>::epsilon() ||
BtoA.scale() < Sophus::SophusConstants<sophusType>::epsilon() ||
BtoAInfo(0,0) == 0 ||
BtoAInfo(6,6) == 0)
{
return 1e20;
}
AtoB = constraintTracker->trackFrameSim3(
B,
A->keyframe,
AtoB,
lvlStart,lvlEnd);
Matrix7x7 AtoBInfo = constraintTracker->lastSim3Hessian;
float AtoB_meanResidual = constraintTracker->lastResidual;
float AtoB_meanDResidual = constraintTracker->lastDepthResidual;
float AtoB_meanPResidual = constraintTracker->lastPhotometricResidual;
float AtoB_usage = constraintTracker->pointUsage;
if (constraintTracker->diverged ||
AtoB.scale() > 1 / Sophus::SophusConstants<sophusType>::epsilon() ||
AtoB.scale() < Sophus::SophusConstants<sophusType>::epsilon() ||
AtoBInfo(0,0) == 0 ||
AtoBInfo(6,6) == 0)
{
return 1e20;
}
// Propagate uncertainty (with d(a * b) / d(b) = Adj_a) and calculate Mahalanobis norm
Matrix7x7 datimesb_db = AtoB.cast<float>().Adj();
Matrix7x7 diffHesse = (AtoBInfo.inverse() + datimesb_db * BtoAInfo.inverse() * datimesb_db.transpose()).inverse();
Vector7 diff = (AtoB * BtoA).log().cast<float>();
float reciprocalConsistency = (diffHesse * diff).dot(diff);
if(e1 != 0 && e2 != 0)
{
e1->firstFrame = A->keyframe;
e1->secondFrame = B->keyframe;
e1->secondToFirst = BtoA;
e1->information = BtoAInfo.cast<double>();
e1->meanResidual = BtoA_meanResidual;
e1->meanResidualD = BtoA_meanDResidual;
e1->meanResidualP = BtoA_meanPResidual;
e1->usage = BtoA_usage;
e2->firstFrame = B->keyframe;
e2->secondFrame = A->keyframe;
e2->secondToFirst = AtoB;
e2->information = AtoBInfo.cast<double>();
e2->meanResidual = AtoB_meanResidual;
e2->meanResidualD = AtoB_meanDResidual;
e2->meanResidualP = AtoB_meanPResidual;
e2->usage = AtoB_usage;
e1->reciprocalConsistency = e2->reciprocalConsistency = reciprocalConsistency;
}
return reciprocalConsistency;
}
Sim3 Sim3
::operator*(const Sim3& sim3) const
{
return Sim3(scso3_*sim3.scso3(),
(scso3_*sim3.translation()) + translation_);
}
SE3 Sim3
::to_SE3(const Sim3 & sim3)
{
return sim3.to_SE3();
}
void ConstraintSearchThread::testConstraint(
const KeyFrame::SharedPtr &keyframe,
const KeyFrame::SharedPtr &candidate,
KFConstraintStruct* e1_out, KFConstraintStruct* e2_out,
Sim3 candidateToFrame_initialEstimate,
float strictness)
{
// candidateTrackingReference->importFrame(candidate);
Sim3 FtoC = candidateToFrame_initialEstimate.inverse(), CtoF = candidateToFrame_initialEstimate;
Matrix7x7 FtoCInfo, CtoFInfo;
float err_level3 = tryTrackSim3(keyframe, candidate, // A = frame; b = candidate
SIM3TRACKING_MAX_LEVEL-1, 3,
USESSE,
FtoC, CtoF);
if(err_level3 > 3000*strictness)
{
// LOGF_IF(DEBUG,Conf().print.constraintSearchInfo,
// "FAILED %d -> %d (lvl %d): errs (%.1f / - / -).",
// newKFTrackingReference->frameID, candidateTrackingReference->frameID,
// 3,
// sqrtf(err_level3));
e1_out = e2_out = 0;
keyframe->trackingFailed.insert(std::pair< KeyFrame::SharedPtr,Sim3>(candidate, candidateToFrame_initialEstimate));
return;
}
float err_level2 = tryTrackSim3(
keyframe, candidate,
2, 2,
USESSE,
FtoC, CtoF);
if(err_level2 > 4000*strictness)
{
// LOGF_IF(DEBUG,Conf().print.constraintSearchInfo,
// "FAILED %d -> %d (lvl %d): errs (%.1f / %.1f / -).",
// newKFTrackingReference->frameID, candidateTrackingReference->frameID,
// 2,
// sqrtf(err_level3), sqrtf(err_level2));
e1_out = e2_out = 0;
keyframe->trackingFailed.insert(std::pair<KeyFrame::SharedPtr,Sim3>(candidate, candidateToFrame_initialEstimate));
return;
}
e1_out = new KFConstraintStruct();
e2_out = new KFConstraintStruct();
float err_level1 = tryTrackSim3(
keyframe, candidate,
1, 1,
USESSE,
FtoC, CtoF, e1_out, e2_out);
if(err_level1 > 6000*strictness)
{
// LOGF_IF(DEBUG,Conf().print.constraintSearchInfo,
// "FAILED %d -> %d (lvl %d): errs (%.1f / %.1f / %.1f).",
// newKFTrackingReference->frameID, candidateTrackingReference->frameID,
// 1,
// sqrtf(err_level3), sqrtf(err_level2), sqrtf(err_level1));
delete e1_out;
delete e2_out;
e1_out = e2_out = 0;
keyframe->trackingFailed.insert(std::pair<KeyFrame::SharedPtr,Sim3>(candidate, candidateToFrame_initialEstimate));
return;
}
// LOGF_IF(DEBUG,Conf().print.constraintSearchInfo,
// "ADDED %d -> %d: errs (%.1f / %.1f / %.1f).",
// newKFTrackingReference->frameID, candidateTrackingReference->frameID,
// sqrtf(err_level3), sqrtf(err_level2), sqrtf(err_level1));
const float kernelDelta = 5 * sqrt(6000*loopclosureStrictness);
e1_out->robustKernel = new g2o::RobustKernelHuber();
e1_out->robustKernel->setDelta(kernelDelta);
e2_out->robustKernel = new g2o::RobustKernelHuber();
e2_out->robustKernel->setDelta(kernelDelta);
}
