g2o helper

#pragma once
#include <g2o/core/sparse_optimizer.h>
#include <g2o/types/slam3d/types_slam3d.h>
#include <Eigen/eigen>
#include "KeyFrame.h";
#include "MapPoint.h"
#include "Measurement.h"

class PoseRefine {

public:
    PoseRefine();
    ~PoseRefine();
    Eigen::Isometry3d Adjust(int maxIterations,bool v = true);
    void addCamParam(double focal, double cx, double cy);
    void addPose(Eigen::Isometry3d pose);

    void addEdge(const Eigen::Vector2d imgpt,const Eigen::Vector3d objpt);
    void addAllEdges(const cv::Mat inlier,const  std::vector< Measurement*>& meas);
protected:
    g2o::SparseOptimizer optimizer_;

    const double gainTerminateThreshold_;
    const double robust_cost_function_delta_;
    int edge_idx ;
};

class BundleAdjust {
protected:
    g2o::SparseOptimizer optimizer_;

    const double gainTerminateThreshold_;
    const double robust_cost_function_delta_;
public:
    BundleAdjust();
    ~BundleAdjust();
    int Adjust(int maxIter,std::vector<KeyFrame*>& vKeys,std::vector< MapPoint*>& mpts, KeyFrame* baseKf,bool verbos);
    
};

 

 

#include "G2oHelper.h"
#include <g2o/config.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/types/icp/types_icp.h>
//#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/solvers/pcg/linear_solver_pcg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/core/sparse_optimizer_terminate_action.h>

PoseRefine::PoseRefine()
    :gainTerminateThreshold_ (1e-8),robust_cost_function_delta_ ( std::sqrt(5.991)),edge_idx(1)
{
    std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver(new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>());
    std::unique_ptr<g2o::BlockSolver_6_3> solver_ptr(new g2o::BlockSolver_6_3(std::move(linearSolver)));
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));
    optimizer_.setAlgorithm(solver);
    g2o::SparseOptimizerTerminateAction* terminateAction = new g2o::SparseOptimizerTerminateAction();
    terminateAction->setGainThreshold(gainTerminateThreshold_);
    optimizer_.addPostIterationAction(terminateAction);
}
PoseRefine::~PoseRefine()
{
}

Eigen::Isometry3d PoseRefine::Adjust(int maxIterations,bool verbos)
{
        optimizer_.setVerbose(verbos);
        optimizer_.initializeOptimization();
        optimizer_.optimize(maxIterations);
        g2o::VertexSE3Expmap* vp = dynamic_cast<g2o::VertexSE3Expmap*> (optimizer_.vertex(0));
        return (vp->estimate());
}

void PoseRefine::addCamParam(double f, double cx, double cy)
{
    g2o::CameraParameters* camera = new g2o::CameraParameters(f, Eigen::Vector2d(cx, cy),0);
    camera->setId(0);
    optimizer_.addParameter(camera);
}

void PoseRefine::addAllEdges(const cv::Mat inlier, const  std::vector< Measurement*>& meas)
{
    for (int i = 0; i < inlier.rows;++i) {
        int j = inlier.at<int>(i);
        auto ms = meas[j];
        addEdge(Eigen::Vector2d(ms->pt2_left.x, ms->pt2_left.y), ms->mappoint->position);
    }
}

void PoseRefine::addEdge(const Eigen::Vector2d imgpt, const Eigen::Vector3d objpt)
{
    g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
    point->setId(edge_idx*2);
    point->setEstimate(objpt);
    point->setMarginalized(true); 
    point->setFixed(true);
    optimizer_.addVertex(point);

    g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
    edge->setId(edge_idx*2+1);
    edge->setVertex(0, point);
    edge->setVertex(1, dynamic_cast<g2o::VertexSE3Expmap*> (optimizer_.vertex(0)));
    edge->setMeasurement(imgpt);
    edge->setParameterId(0, 0);
    edge->setInformation(Eigen::Matrix2d::Identity());
    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
    edge->setRobustKernel(rk);
    optimizer_.addEdge(edge);

    edge_idx++;
}

void PoseRefine::addPose(Eigen::Isometry3d pose)
{
    g2o::SE3Quat se3(pose.rotation(), pose.translation());
    g2o::VertexSE3Expmap * v_se3 = new g2o::VertexSE3Expmap();
    v_se3->setId(0);
    v_se3->setFixed(false);
    v_se3->setEstimate(se3);
    optimizer_.addVertex(v_se3);
}

//----------------------------------------------

BundleAdjust::BundleAdjust()
    :gainTerminateThreshold_(1e-8), robust_cost_function_delta_(std::sqrt(5.991))
{
    optimizer_.setVerbose(false);
    std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver(new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>());
    std::unique_ptr<g2o::BlockSolver_6_3> solver_ptr(new g2o::BlockSolver_6_3(std::move(linearSolver)));
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));
    optimizer_.setAlgorithm(solver);
     Set Convergence Criterion
    g2o::SparseOptimizerTerminateAction* terminateAction = new g2o::SparseOptimizerTerminateAction();
    terminateAction->setGainThreshold(gainTerminateThreshold_);
    optimizer_.addPostIterationAction(terminateAction);
}
BundleAdjust::~BundleAdjust()
{

}
int BundleAdjust::Adjust(int maxIter, std::vector<KeyFrame*>& vKeys, std::vector< MapPoint*>& mpts, KeyFrame* baseKf,bool verbos)
{
    {
        StereoCamera *stc = vKeys[0]->m_stc;
        //std::cout << "stc->fx:" << stc->fx << "\n";
        g2o::CameraParameters* camera = new g2o::CameraParameters(stc->fx, Eigen::Vector2d(stc->cx, stc->cy), 0);
        camera->setId(0);
        if (!optimizer_.addParameter(camera)) {
            assert(false);
        }
    }
    
    std::set<KeyFrame*> kfmSet;
    for (auto kk : vKeys) {
        kfmSet.insert(kk);
    }

    for (int i = 0; i < vKeys.size(); ++i) {
        KeyFrame *kf = vKeys[i];
        Eigen::Isometry3d pose(kf->frmPose.inverse());
        //std::cout << "pose:" << pose.matrix() << "\n";
        g2o::SE3Quat se3(pose.rotation(), pose.translation());
        //std::cout << "se3:" << se3.to_homogeneous_matrix() << "\n";
        g2o::VertexSE3Expmap * v_se3 = new g2o::VertexSE3Expmap();
        v_se3->setId(kf->KEYFRM_ID*2);
        if (kf->isFixed()) {
            v_se3->setFixed(true);
        }else{
            bool is_covi = baseKf->covisible.count(kf) == 0;
            v_se3->setFixed(is_covi);
            if (baseKf==kf) {
                v_se3->setFixed(false);
            }
        }
    
        v_se3->setEstimate(se3);
        optimizer_.addVertex(v_se3);
    }

    for (int i = 0; i < mpts.size(); ++i) {
        MapPoint *mpt = mpts[i];
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId(mpt->MPT_ID*2+1);
        point->setEstimate(mpt->position);
        point->setMarginalized(true);
        optimizer_.addVertex(point);
    }

    double sum_chi2 = 0;
    int idx = 0;
    for (auto mp : mpts) {
        for (auto ms : mp->get_measurements()) {

            if (kfmSet.count(ms->keyframe)==0 ) {
                continue;
            }
            g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
            edge->setId(idx++);
            edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer_.vertex(ms->mappoint->MPT_ID * 2+1)));
            edge->setVertex(1, dynamic_cast<g2o::VertexSE3Expmap*> (optimizer_.vertex(ms->keyframe->KEYFRM_ID*2)));
            edge->setMeasurement(Eigen::Vector2d(ms->pt2_left.x, ms->pt2_left.y));
            edge->setParameterId(0, 0);
            edge->setInformation(Eigen::Matrix2d::Identity());
            g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
            edge->setRobustKernel(rk);

            optimizer_.addEdge(edge);
            edge->computeError();
            sum_chi2 += edge->chi2();
            //std::cout << "chi2:" << edge->chi2() << "\n";
            //std::cout << "focal:" << edge->_cam->focal_length << "\n";
        }
    }
    std::cout << "sum_chi2:" << sum_chi2 << "\n";
    optimizer_.setVerbose(verbos);
    optimizer_.initializeOptimization();
    optimizer_.optimize(maxIter);
    //g2o::VertexSE3Expmap* vp = dynamic_cast<g2o::VertexSE3Expmap*> (optimizer_.vertex(0));
    //update

    for (auto& kf : vKeys) {
        std::cout << "ori:" << kf->frmPose << "\n";
        g2o::VertexSE3Expmap* vp = dynamic_cast<g2o::VertexSE3Expmap*> (optimizer_.vertex(kf->KEYFRM_ID*2));
        kf->frmPose = vp->estimate().to_homogeneous_matrix().inverse();
        std::cout << "opt:" << kf->frmPose << "\n";
    }
    for (auto &mp : mpts) {
        g2o::VertexSBAPointXYZ* vp = dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer_.vertex(mp->MPT_ID * 2 + 1));
        mp->position = vp->estimate();
    }

}