Initial Chi2 is zero, therefore no optimisation

[Patrixe]

Hello,

I want to implement a 2D-version of point to point ICP, where the first (reference) camera is fixed at (0, 0) and the second cameras rotation and translation should be adapted to fit the measurements. I therefore created edge classes based on the given ICP example:

void edge2d::computeError() {
        const VertexSE2 *vp0 = static_cast<const VertexSE2 *>(_vertices[0]);
        const VertexSE2 *vp1 = static_cast<const VertexSE2 *>(_vertices[1]);
        Vector2 p1;

        p1 = vp1->estimate() * measurement().pos1;
        p1 = vp0->estimate().inverse() * p1;

        _error = measurement().pos0 - p1;
    }

    void edge2d::linearizeOplus() {
        VertexSE2 *cameraPose0 = static_cast<VertexSE2 *>(_vertices[0]);
        VertexSE2 *cameraPose1 = static_cast<VertexSE2 *>(_vertices[1]);

        Rotation2D R1 = cameraPose1->estimate().rotation();
        Vector2 p1 = measurement().pos1;
        Vector2 dt = cameraPose1->estimate().translation();// - cameraPose0->estimate().translation();
        number_t si = std::sin(R1.angle()), ci = std::cos(R1.angle());
        _jacobianOplusXj <<
                            -si * dt.x() - ci * dt.y(), 1, 0,
                            ci * dt.x() - si * dt.y(), 0, 1;
    }

and adapted the example itself:

    double euc_noise = 0.01;       // noise in position, m

    SparseOptimizer optimizer;
    optimizer.setVerbose(false);

    // variable-size block solver
    OptimizationAlgorithmLevenberg* solver = new OptimizationAlgorithmLevenberg(
            make_unique<BlockSolverX>(make_unique<LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>>()));

    optimizer.setAlgorithm(solver);

    vector<Vector2d> true_points;
    for (size_t i=0;i<1000; ++i)
    {
        true_points.push_back(Vector2d((Sampler::uniformRand(0., 1.) - 0.5) * 3,
                                       Sampler::uniformRand(0., 1.)-0.5));
    }

    // set up two poses
    int vertex_id = 0;
    for (size_t i=0; i<2; ++i)
    {
        // set up rotation and translation for this node
        Vector2d t(3,2 * i);

        Isometry2d cam; // camera pose
        cam.setIdentity();
        cam.translation() = t;

        // set up node for mono camera
        VertexSE2 *vc = new VertexSE2();
        vc->setEstimate(cam);

        vc->setId(vertex_id);      // vertex id

        // set first cam pose fixed
        if (i==0)
            vc->setFixed(true);

        // add to optimizer
        optimizer.addVertex(vc);

        vertex_id++;
    }

    for (size_t i=0; i<true_points.size(); ++i)
    {
        // get two poses
        VertexSE2* vp0 =
                dynamic_cast<VertexSE2*>(optimizer.vertices().find(0)->second);
        VertexSE2* vp1 =
                dynamic_cast<VertexSE2*>(optimizer.vertices().find(1)->second);

        // calculate the relative 2D position of the point
        Vector2d pt0,pt1;
        // points coordinates are calculated with respect to the two different camera poses
        pt0 = vp0->estimate().inverse() * true_points[i];
        pt1 = vp1->estimate().inverse() * true_points[i];

        // add in noise
        pt0 += Vector2d(Sampler::gaussRand(0., euc_noise),
                        Sampler::gaussRand(0., euc_noise));

        pt1 += Vector2d(Sampler::gaussRand(0., euc_noise),
                        Sampler::gaussRand(0., euc_noise));
       edge2d * e           // new edge with correct cohort for caching
                = new edge2d();

        e->setVertex(0, vp0);      // first viewpoint
        e->setVertex(1, vp1);      // second viewpoint

        EdgeGICP2d meas;
        meas.pos0 = pt0;
        meas.pos1 = pt1;
        e->setMeasurement(meas);

        optimizer.addEdge(e);
    }

    VertexSE2* vc =
            dynamic_cast<VertexSE2*>(optimizer.vertices().find(1)->second);
    SE2 cam = vc->estimate();
    cam.setTranslation(Vector2d(50,1));
    cam.setRotation(Rotation2Dd(10));
    vc->setEstimate(cam);

    optimizer.initializeOptimization();
    optimizer.computeActiveErrors();

    optimizer.setVerbose(true);

    optimizer.optimize(10);

    cout << endl << "Second vertex should be near 3,2" << endl;
    cout <<  dynamic_cast<VertexSE2*>(optimizer.vertices().find(0)->second)
            ->estimate().translation().transpose() << endl;
    cout <<  dynamic_cast<VertexSE2*>(optimizer.vertices().find(1)->second)
            ->estimate().translation().transpose() << endl;
}

Unfortunetaly, the first iteration always reports a chi2 value of 0, and (I guess) therefore immediately stops. Any ideas why this is happening?

Thanks in advance.

[sjulier]

What value of _error is computed? If it's 0 for all the edges, the chi2 value will be 0 as well.

[Patrixe]

The error is a non zero vector for all edges, depends mainly on how the translation of the second camera. Looks good to me.

[RainerKuemmerle]

Can you check that your information matrix is non-zero for the edges?

On Sat, Aug 28, 2021 at 5:20 PM Patrick Liedtke @.***> wrote:

The error is a non zero vector for all edges, depends mainly on how the translation of the second camera. Looks good to me.

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/RainerKuemmerle/g2o/issues/537#issuecomment-907640954, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAKBDWCYCR2FJ7RIHXW7AVTT7D5DLANCNFSM5C5SCHRQ . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub.

[Patrixe]

Can you check that your information matrix is non-zero for the edges?

You are right, they are all zero. I am not sure when and how I need to update them, but I guess this is beyond this issue tracker, so I will figure this out myself. Thanks for your help!