Optimizer does not converge on Raspberry Pi 4

[chenNinini]

Hello,
I'm encountering some issues when using an optimizer on my platform,
which is Raspberry Pi 4 with a 64-bit operating system.
When optimizing the pose, I'm getting inconsistent optimization results with the same code.
Sometimes optimization is successful, and the optimization process looks like this:

iteration= 0 chi2= 127571.717953 time= 0.000213127 cumTime= 0.000213127 lambda= 3830.396636 levenbergIter= 1 iteration= 1 chi2= 38872.465515 time= 0.000117646 cumTime= 0.000330773 lambda= 1276.798879 levenbergIter= 1 iteration= 2 chi2= 33038.090407 time= 7.424e-05 cumTime= 0.000405013 lambda= 425.599626 levenbergIter= 1 iteration= 3 chi2= 28809.425425 time= 8.8647e-05 cumTime= 0.00049366 lambda= 141.866542 levenbergIter= 1 iteration= 4 chi2= 27810.356458 time= 0.000103629 cumTime= 0.000597289 lambda= 94.577695 levenbergIter= 1 iteration= 5 chi2= 20433.198636 time= 8.7499e-05 cumTime= 0.000684788 lambda= 31.525898 levenbergIter= 1 iteration= 6 chi2= 12730.752309 time= 8.8888e-05 cumTime= 0.000773676 lambda= 10.508633 levenbergIter= 1 iteration= 7 chi2= 6870.522653 time= 8.4795e-05 cumTime= 0.000858471 lambda= 3.502878 levenbergIter= 1 iteration= 8 chi2= 1728.857694 time= 8.687e-05 cumTime= 0.000945341 lambda= 1.167626 levenbergIter= 1 iteration= 9 chi2= 329.559309 time= 7.4592e-05 cumTime= 0.00101993 lambda= 0.389209 levenbergIter= 1 iteration= 10 chi2= 286.684396 time= 6.8722e-05 cumTime= 0.00108865 lambda= 0.129736 levenbergIter= 1 iteration= 11 chi2= 286.419669 time= 0.000104314 cumTime= 0.00119297 lambda= 0.043245 levenbergIter= 1 iteration= 12 chi2= 286.418368 time= 0.000132258 cumTime= 0.00132523 lambda= 0.028830 levenbergIter= 1 iteration= 13 chi2= 286.418328 time= 0.000122757 cumTime= 0.00144798 lambda= 0.019220 levenbergIter= 1 iteration= 14 chi2= 286.418327 time= 6.7518e-05 cumTime= 0.0015155 lambda= 0.012813 levenbergIter= 1 iteration= 15 chi2= 286.418327 time= 7.0555e-05 cumTime= 0.00158606 lambda= 0.008542 levenbergIter= 1 iteration= 16 chi2= 286.418327 time= 6.7499e-05 cumTime= 0.00165356 lambda= 0.005695 levenbergIter= 1 iteration= 17 chi2= 286.418327 time= 9.6665e-05 cumTime= 0.00175022 lambda= 0.003797 levenbergIter= 1 iteration= 18 chi2= 286.418327 time= 5.9592e-05 cumTime= 0.00180981 lambda= 0.002531 levenbergIter= 1 iteration= 19 chi2= 286.418327 time= 0.000241294 cumTime= 0.00205111 lambda= 1.727865 levenbergIter= 5 iteration= 0 chi2= 27004.963719 time= 0.000107203 cumTime= 0.000107203 lambda= 26.283675 levenbergIter= 1 iteration= 1 chi2= 8929.944303 time= 6.2018e-05 cumTime= 0.000169221 lambda= 8.761225 levenbergIter= 1 iteration= 2 chi2= 2337.588517 time= 5.6277e-05 cumTime= 0.000225498 lambda= 2.920408 levenbergIter= 1 iteration= 3 chi2= 113.050021 time= 5.2685e-05 cumTime= 0.000278183 lambda= 0.973469 levenbergIter= 1 iteration= 4 chi2= 53.057078 time= 5.1074e-05 cumTime= 0.000329257 lambda= 0.324490 levenbergIter= 1 iteration= 5 chi2= 52.103262 time= 0.00160104 cumTime= 0.00193029 lambda= 0.108163 levenbergIter= 1 iteration= 6 chi2= 52.103258 time= 7.7276e-05 cumTime= 0.00200757 lambda= 0.072109 levenbergIter= 1 iteration= 7 chi2= 52.103258 time= 4.6333e-05 cumTime= 0.0020539 lambda= 0.048073 levenbergIter= 1 iteration= 8 chi2= 52.103258 time= 5.3185e-05 cumTime= 0.00210709 lambda= 0.032048 levenbergIter= 1 iteration= 9 chi2= 52.103258 time= 0.000118814 cumTime= 0.0022259 lambda= 1154664443893608.750000 levenbergIter= 10 iteration= 0 chi2= 26739.717812 time= 7.0518e-05 cumTime= 7.0518e-05 lambda= 26.296382 levenbergIter= 1 iteration= 1 chi2= 8809.381939 time= 5.3759e-05 cumTime= 0.000124277 lambda= 8.765461 levenbergIter= 1 iteration= 2 chi2= 2265.317650 time= 6.4814e-05 cumTime= 0.000189091 lambda= 2.921820 levenbergIter= 1 iteration= 3 chi2= 110.340883 time= 5.1203e-05 cumTime= 0.000240294 lambda= 0.973940 levenbergIter= 1 iteration= 4 chi2= 58.849148 time= 4.8426e-05 cumTime= 0.00028872 lambda= 0.324647 levenbergIter= 1 iteration= 5 chi2= 57.803873 time= 4.661e-05 cumTime= 0.00033533 lambda= 0.108216 levenbergIter= 1 iteration= 6 chi2= 57.803866 time= 5.15e-05 cumTime= 0.00038683 lambda= 0.072144 levenbergIter= 1 iteration= 7 chi2= 57.803866 time= 6.511e-05 cumTime= 0.00045194 lambda= 0.048096 levenbergIter= 1 iteration= 8 chi2= 57.803866 time= 0.00012385 cumTime= 0.00057579 lambda= 1128147161436.999023 levenbergIter= 10 iteration= 0 chi2= 1562003.763644 time= 5.5832e-05 cumTime= 5.5832e-05 lambda= 70728.075281 levenbergIter= 1 iteration= 1 chi2= 424485.158865 time= 4.9648e-05 cumTime= 0.00010548 lambda= 47152.050187 levenbergIter= 1 iteration= 2 chi2= 416184.675232 time= 4.9814e-05 cumTime= 0.000155294 lambda= 31434.700125 levenbergIter= 1 iteration= 3 chi2= 71182.012047 time= 5.3185e-05 cumTime= 0.000208479 lambda= 16349.529676 levenbergIter= 1 iteration= 4 chi2= 5084.083499 time= 4.7463e-05 cumTime= 0.000255942 lambda= 5449.843225 levenbergIter= 1 iteration= 5 chi2= 60.249360 time= 4.8462e-05 cumTime= 0.000304404 lambda= 1816.614408 levenbergIter= 1 iteration= 6 chi2= 57.803879 time= 4.624e-05 cumTime= 0.000350644 lambda= 605.538136 levenbergIter= 1 iteration= 7 chi2= 57.803866 time= 5.0962e-05 cumTime= 0.000401606 lambda= 403.692091 levenbergIter= 1 iteration= 8 chi2= 57.803866 time= 4.611e-05 cumTime= 0.000447716 lambda= 269.128061 levenbergIter= 1 iteration= 9 chi2= 57.803866 time= 9.311e-05 cumTime= 0.000540826 lambda= 12329559661943.916016 levenbergIter= 9 iteration= 10 chi2= 57.803866 time= 0.000101684 cumTime= 0.00064251 lambda= 98636477295551.328125 levenbergIter= 2

resulting in good pose estimation.

However, at times, optimization fails, and the optimization process looks like this:
iteration= 0 chi2= 14508874.180055 time= 0.000191016 cumTime= 0.000191016 lambda= 2313709717.602640levenbergIter= 1 iteration= 1 chi2= 7890404.887239 time= 0.00011037 cumTime= 0.000301386 lambda= 771236572.534213 levenbergIter= 1 iteration= 2 chi2= 4579428.167143 time= 7.4018e-05 cumTime= 0.000375404 lambda= 257078857.511404 levenbergIter= 1 iteration= 3 chi2= 2912194.932188 time= 6.0314e-05 cumTime= 0.000435718 lambda= 85692952.503801 levenbergIter= 1 iteration= 4 chi2= 2057343.918543 time= 0.000113887 cumTime= 0.000549605 lambda= 28564317.501267 levenbergIter= 1 iteration= 5 chi2= 1624066.350868 time= 0.000125406 cumTime= 0.000675011 lambda= 9521439.167089 levenbergIter= 1 iteration= 6 chi2= 1441013.313477 time= 8.0277e-05 cumTime= 0.000755288 lambda= 6325158.440855 levenbergIter= 1 iteration= 7 chi2= 1361795.984550 time= 5.2721e-05 cumTime= 0.000808009 lambda= 2108386.146952 levenbergIter= 1 iteration= 8 chi2= 1209154.867002 time= 6.3148e-05 cumTime= 0.000871157 lambda= 702795.382317 levenbergIter= 1 iteration= 9 chi2= 995436.419269 time= 5.6018e-05 cumTime= 0.000927175 lambda= 468530.254878 levenbergIter= 1 iteration= 10 chi2= 740701.321810 time= 5.1592e-05 cumTime= 0.000978767 lambda= 184605.998295 levenbergIter= 1 iteration= 11 chi2= 514602.890176 time= 6.4833e-05 cumTime= 0.0010436 lambda= 80244.261872 levenbergIter= 1 iteration= 12 chi2= 342445.937814 time= 0.000188979 cumTime= 0.00123258 lambda= 26748.087291 levenbergIter= 1 iteration= 13 chi2= 291513.223480 time= 0.000108258 cumTime= 0.00134084 lambda= 8916.029097 levenbergIter= 1 iteration= 14 chi2= 237602.029070 time= 6.4647e-05 cumTime= 0.00140548 lambda= 2972.009699 levenbergIter= 1 iteration= 15 chi2= 220343.232036 time= 9.0777e-05 cumTime= 0.00149626 lambda= 990.669900 levenbergIter= 1 iteration= 16 chi2= 211808.397330 time= 0.000103758 cumTime= 0.00160002 lambda= 330.223300 levenbergIter= 1 iteration= 17 chi2= 205391.672322 time= 6.0351e-05 cumTime= 0.00166037 lambda= 110.074433 levenbergIter= 1 iteration= 18 chi2= 198422.338974 time= 6.4629e-05 cumTime= 0.001725 lambda= 36.691478 levenbergIter= 1 iteration= 19 chi2= 188793.570582 time= 5.7389e-05 cumTime= 0.00178239 lambda= 12.230493 levenbergIter= 1
I apologize for the formatting.

I have already checked the 2D points, 3D points, and poses used for optimization, and there don't seem to be any issues. However, the problem of convergence failure during optimization frequently occurs, as described earlier.
Thank you very much for your guidance.

[RainerKuemmerle]

Do you have some minimal working example that provokes the issue?
The same code works fine on another platform, e.g., amd64 or x86?

[chenNinini]

Thank you very much for taking the time to reply to me.
I have attached the code for my g2o part.

`
int Optimizer::PoseOptimization(Frame* pFrame,int FileCount)
{

  g2o::SparseOptimizer optimizer;
  g2o::BlockSolver_6_3::LinearSolverType* linearSolver;
  linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();

g2o::BlockSolver_6_3* solver_ptr = new g2o::BlockSolver_6_3(unique_ptr<g2o::BlockSolver_6_3::LinearSolverType>(linearSolver));

 g2o::OptimizationAlgorithmLevenberg* solver = new 
 g2o::OptimizationAlgorithmLevenberg(unique_ptr<g2o::BlockSolver_6_3>(solver_ptr));
optimizer.setAlgorithm(solver);

int nInitialCorrespondences = 0;

g2o::VertexSE3Expmap* vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Convert::Converter_toSE3Quat(pFrame->mTcw));
vSE3->setId(0);
vSE3->setFixed(false);
optimizer.addVertex(vSE3);

const int N = pFrame->MatchCount;
vector<g2o::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
vector<size_t> vnIndexEdgeMono;
vpEdgesMono.reserve(N);
vnIndexEdgeMono.reserve(N);

const float deltaMono = sqrt(5.991);


for(int i =0;i<N;i++)
{
    nInitialCorrespondences++;
    Eigen::Matrix<double, 2, 1> obs;
    const cv::KeyPoint& kpUn = pFrame->MatchKeys[i];
    obs << kpUn.pt.x, kpUn.pt.y;
    g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();
    e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
    e->setMeasurement(obs);
    
    const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
    e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);


    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
    e->setRobustKernel(rk);
    rk->setDelta(deltaMono);

    e->fx = pFrame->fx;
    e->fy = pFrame->fy;
    e->cx = pFrame->cx;
    e->cy = pFrame->cy;


    cv::Mat Xw = pFrame->mWorldPose[i];
    e->Xw[0] = Xw.at<float>(0,0);
    e->Xw[1] = Xw.at<float>(1,0);
    e->Xw[2] = Xw.at<float>(2,0);


    optimizer.addEdge(e);
    vpEdgesMono.push_back(e);
    vnIndexEdgeMono.push_back(i);

}
if (nInitialCorrespondences < 3)
    return 0;
const float chi2Mono[4] = { 5.991,5.991 ,5.991 ,5.991 };
const int its[4] = { 20,20,15,15 };

int nBad = 0;


if(nInitialCorrespondences < 3)
    return 0;


for(size_t it = 0;it<4;it++)
{
    vSE3->setEstimate(Convert::Converter_toSE3Quat(pFrame->mTcw));
    optimizer.setVerbose(false);
    optimizer.initializeOptimization(0);
    optimizer.optimize(its[it]);
    nBad = 0;
    for (size_t i = 0, iend = vpEdgesMono.size(); i < iend; i++)
    {
        g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];
        const size_t idx = vnIndexEdgeMono[i];

        if (pFrame->mvbOutlier[idx])
            e->computeError();

        const float chi2 = e->chi2();
        //std::cout <<chi2 << endl;

        if (chi2 > chi2Mono[it])
        {
            pFrame->mvbOutlier[idx] = true;
            e->setLevel(1);
            nBad++;
        }
        else
        {
            pFrame->mvbOutlier[idx] = false;
            e->setLevel(0);
        }
        if (it == 2)
            e->setRobustKernel(0);
    }

    if (optimizer.edges().size() < 10)
         break;
}

g2o::VertexSE3Expmap* vSE3_recov = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0));
g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();

cv::Mat pose = Convert::Converter_toCvMat(SE3quat_recov);
pFrame->SetPose(pose);

optimizer.clear();
return nInitialCorrespondences-nBad;
}`

And this code works well on Windows.
I use Qt5 as my IDE on Linux, and the GCC version is 10.2.1.

[RainerKuemmerle]

Any chance to post a minimal example that is self contained and can be run to reproduce the issue?
Otherwise you might want to compile with debug symbols and check if valgrind can spot any memory issues.