Настройка пакета с библиотекой g2o. Как проверить? - PullRequest
Новая
       76

Настройка пакета с библиотекой g2o. Как проверить?

0 голосов
/ 30 октября 2018

У меня есть простая функция настройки связки, использующая библиотеки g2o для оптимизации поз камеры и точек трехмерной карты. Я передаю два KeyFrames, каждый из которых содержит: 

std::vector<cv::Point2d> vObs_pos; //the 2d coordinates that align to the 3d map points

int id; // the frame id
cv::Mat Rt; // the pose as a 4x4 matrix

// Calibration parameters
float fx, fy, cx, cy;

А также vector из Mappoint, который содержит: 

cv::Point3f pos; // the 3d position
long long id; // the map point id
std::map<KeyFrame*, size_t> mObs; //a list of Keyframe/2d point id pairs

Моя функция: 

void BundleAdjustment::Solve(const std::vector<KeyFrame *> &vpKFs, const std::vector<MapPoint *> &allMapPoints)
{

    g2o::SparseOptimizer optimizer;
    optimizer.setVerbose(true);
    std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;

    linearSolver = g2o::make_unique<g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>>();
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
        g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver)));


    optimizer.setAlgorithm(solver); 

    int num_cameras_ = vpKFs.size();
    int num_points_ = allMapPoints.size();
    const float thHuber2D = sqrt(5.99);

    //camera poses
    for (size_t i = 0; i < vpKFs.size(); i++)
    {
        KeyFrame *pKF = vpKFs[i];
        g2o::VertexSE3Expmap *vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKF->Rt));

        vSE3->setId(pKF->id);
        if (pKF->id == 0)
        {
            vSE3->setFixed(true);
        }
        optimizer.addVertex(vSE3);
    }

    // set point vertex
    for (size_t i = 0; i < allMapPoints.size(); i++)
    {
        MapPoint* pMP = allMapPoints[i];

        g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
        g2o::Vector3 pp(pMP->pos.x, pMP->pos.y, pMP->pos.z);
        vPoint->setEstimate(pp);
        vPoint->setId(i + num_cameras_);    
        optimizer.addVertex(vPoint);


        //
        //SET EDGES
        const map<KeyFrame*, size_t> observations = pMP->mObs;
        for (std::map<KeyFrame *, size_t>::const_iterator mit = observations.begin(); mit != observations.end(); mit++)
        {

            KeyFrame *pKF = mit->first;
            const cv::Point2f &kpUn = pKF->vObs_pos[mit->second];

            Eigen::Matrix<double, 2, 1> obs;
            obs << kpUn.x, kpUn.y;

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

            const int camera_id = pKF->id;
            int point_id = i + num_cameras_;

            e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(point_id)));
            e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex *>(optimizer.vertex(camera_id)));
            e->setMeasurement(obs);
            e->setInformation(Eigen::Matrix2d::Identity());     

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

            optimizer.addEdge(e);
        }

    }


    // start optimization
    std::cout << "Optimization starts..." << std::endl;
    optimizer.initializeOptimization();
    optimizer.setVerbose(true);
    optimizer.optimize(20);
    std::cout << "Optimization completed!" << std::endl;
}

Я запускаю функцию, и она, кажется, работает, печатая: 

Optimization starts...
iteration= 0     chi2= 751902.547901     time= 0.118062  cumTime= 0.118062       edges= 454      schur= 0        lambda= 40.936130       levenbergIter= 6
iteration= 1     chi2= 748570.165390     time= 0.0783672         cumTime= 0.196429       edges= 454      schur= 0
 lambda= 873.304112      levenbergIter= 4
iteration= 2     chi2= 739010.123008     time= 0.0199937         cumTime= 0.216423       edges= 454      schur= 0
 lambda= 291.101371      levenbergIter= 1
iteration= 3     chi2= 725682.849983     time= 0.0391275         cumTime= 0.25555        edges= 454      schur= 0
 lambda= 194.067581      levenbergIter= 2
iteration= 4     chi2= 720377.943229     time= 0.0586551         cumTime= 0.314205       edges= 454      schur= 0
 lambda= 517.513548      levenbergIter= 3
iteration= 5     chi2= 714174.854659     time= 0.039606  cumTime= 0.353811       edges= 454      schur= 0        lambda= 690.018064      levenbergIter= 2
iteration= 6     chi2= 708794.450845     time= 0.0410522         cumTime= 0.394864       edges= 454      schur= 0
 lambda= 491.458828      levenbergIter= 2
iteration= 7     chi2= 703574.671138     time= 0.0409294         cumTime= 0.435793       edges= 454      schur= 0
 lambda= 655.278438      levenbergIter= 2
iteration= 8     chi2= 698092.802055     time= 0.0395358         cumTime= 0.475329       edges= 454      schur= 0
 lambda= 517.076870      levenbergIter= 2
iteration= 9     chi2= 696224.050949     time= 0.0589929         cumTime= 0.534322       edges= 454      schur= 0
 lambda= 1378.871654     levenbergIter= 3
iteration= 10    chi2= 693660.045281     time= 0.0394453         cumTime= 0.573767       edges= 454      schur= 0
 lambda= 919.247769      levenbergIter= 2
iteration= 11    chi2= 690353.709795     time= 0.0395756         cumTime= 0.613343       edges= 454      schur= 0
 lambda= 1225.663693     levenbergIter= 2
iteration= 12    chi2= 685633.855487     time= 0.0201159         cumTime= 0.633458       edges= 454      schur= 0
 lambda= 817.109128      levenbergIter= 1
iteration= 13    chi2= 681798.350668     time= 0.039852  cumTime= 0.67331        edges= 454      schur= 0        lambda= 685.975324      levenbergIter= 2
iteration= 14    chi2= 678163.586494     time= 0.0398645         cumTime= 0.713175       edges= 454      schur= 0
 lambda= 914.633765      levenbergIter= 2
iteration= 15    chi2= 675402.095013     time= 0.0202596         cumTime= 0.733435       edges= 454      schur= 0
 lambda= 609.755843      levenbergIter= 1
iteration= 16    chi2= 671423.770362     time= 0.0399762         cumTime= 0.773411       edges= 454      schur= 0
 lambda= 813.007791      levenbergIter= 2
iteration= 17    chi2= 668154.458614     time= 0.0398402         cumTime= 0.813251       edges= 454      schur= 0
 lambda= 1084.010388     levenbergIter= 2
iteration= 18    chi2= 665945.177033     time= 0.0201547         cumTime= 0.833406       edges= 454      schur= 0
 lambda= 722.673592      levenbergIter= 1
iteration= 19    chi2= 662338.490227     time= 0.039615  cumTime= 0.873021       edges= 454      schur= 0        lambda= 963.564789      levenbergIter= 2
Optimization completed!

Но как я могу проверить эти результаты? Есть ли способ в g2o напечатать ошибку перепроецирования до и после запуска оптимизации? Или информация, которая мне нужна в приведенном выше результате, где-то?

...