利用李代数做位姿图优化的简单实现

#include<bits/stdc++.h>
#include <ceres/autodiff_cost_function.h>
#include <ceres/cost_function.h>
#include <ceres/local_parameterization.h>
#include <ceres/loss_function.h>
#include <ceres/problem.h>
#include <ceres/solver.h>
#include <ceres/types.h>
#include <fstream>
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
#include <string>
#include <unistd.h> 
#include <ceres/ceres.h>
#include <map>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>


using namespace std;

struct Pose{
    Eigen::Vector3d p;
    Eigen::Quaterniond q;
    Sophus::SE3d se3;
    Pose(){}
    void ToSE3(){
        se3 = Sophus::SE3d(q,p);
    }
    void FromSE3() {
        p = se3.translation();
        q = se3.unit_quaternion();
    }
};

inline std::istream& operator>>(std::istream& input, Pose& pose) {
    input >> pose.p.x() >> pose.p.y() >> pose.p.z() >> pose.q.x() >>pose.q.y() >> pose.q.z() >> pose.q.w();
    pose.q.normalize();
    pose.ToSE3();
    return input;
}

struct Edge{
    int id1,id2;
    Pose pose;
    Eigen::Matrix<double,6,6> info;
    Edge(){}
};

inline std::istream& operator>>(std::istream& input, Edge& edge) {
    input >> edge.id1 >> edge.id2 >> edge.pose;
    for (int i = 0; i < 6 && input.good(); ++i) {
        for (int j = i; j < 6 && input.good(); ++j) {
            input >> edge.info(i, j);
            if (i != j) {
                edge.info(j, i) = edge.info(i, j);
            }
        }
    }
    return input;
}

void readG2oFile(string path, map<int, Pose*>& vertexs, vector<Edge>&edges){
    ifstream infile(path.c_str());
    string type;
    while(infile.good()){
        infile >> type;
        if(type=="VERTEX_SE3:QUAT"){
            Pose* pose = new Pose();// 如果直接 Pose pose,得到的是同一个对象
            int id;
            infile>>id>> *pose;
            if(id>100) continue; // g2o文件太大了
            vertexs.emplace(id,pose);
        }else if(type=="EDGE_SE3:QUAT"){
            Edge edge;
            infile>>edge;
            if(vertexs.count(edge.id1)&&vertexs.count(edge.id2))
                edges.push_back(edge);
        }
        infile>>ws;
    }
}

struct residualFunc{
    residualFunc( Pose pose12){
        this->pose = pose12.se3;
    }
    Sophus::SE3d pose;

    template<typename T>
    bool operator()(const T*const P1,const T*const P2,T*e) const{
        Eigen::Map<Sophus::SE3<T> const> const p1(P1);
        Eigen::Map<Sophus::SE3<T> const> const p2(P2);
        Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(e);
        residuals = (pose.template cast<T>().inverse() * p1.inverse() * p2).log();
        return true;
    }
};
//自定义数据更新
class LocalParamSE3Analytic : public ceres::LocalParameterization {
public: 
    virtual ~LocalParamSE3Analytic() {}

    // SE3 plus operation for Ceres
    //
    //  T * exp(x)
    //
    virtual bool Plus(double const* T_raw, double const* delta_raw,
                        double* T_plus_delta_raw) const {
        Eigen::Map<Sophus::SE3d const> const T(T_raw);
        Eigen::Map<Sophus::Vector6d const> const delta(delta_raw);
        Eigen::Map<Sophus::SE3d> T_plus_delta(T_plus_delta_raw);
        T_plus_delta = Sophus::SE3d::exp(delta) * T;
        return true;
    }

    // Jacobian of SE3 plus operation for Ceres
    //
    // dx T * exp(x)  with  x=0
    //
    virtual bool ComputeJacobian(double const* T_raw,
                                double* jacobian_raw) const {
        Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor> > jacobian(jacobian_raw);
        jacobian.setZero();
        jacobian.block<6,6>(0, 0).setIdentity();
        return true;
    }

    virtual int GlobalSize() const { return Sophus::SE3d::num_parameters; }
    virtual int LocalSize() const { return Sophus::SE3d::DoF; }
};

void buildProblem(ceres::Problem& problem, map<int, Pose*>&vertexs, vector<Edge>&edges){
    ceres::LossFunction *loss_function = nullptr;
   // ceres::LocalParameterization *localparam = new LocalParamSE3Analytic();
    for(auto& edge:edges){
        //这里的位姿应该是用6维的
        //problem.AddParameterBlock(vertexs[edge.id1]->se3.data(), 7, localparam);
        //problem.AddParameterBlock(vertexs[edge.id2]->se3.data(), 7, localparam);
        //自动求导
        ceres::CostFunction *cost_function = new ceres::AutoDiffCostFunction<residualFunc,6, 7,7>
            (new residualFunc(edge.pose));
        problem.AddResidualBlock(cost_function, new ceres::HuberLoss(0.01),vertexs[edge.id1]->se3.data(),vertexs[edge.id2]->se3.data()); //优化参数为李代数形式
    }
    return;
}

void solveProblem(ceres::Problem& problem){
    ceres::Solver::Options options;
    options.minimizer_progress_to_stdout = false;
    options.linear_solver_type = ceres::DENSE_QR;
    options.max_num_iterations = 200;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    return;
}

bool savePose(const string &filename,  map<int, Pose*>&poses) {
    std::fstream outfile;
    outfile.open(filename.c_str(), std::istream::out);
    if (!outfile) {
        LOG(ERROR) << "Error opening the file: " << filename;
        return false;
    }
    for(auto& pose:poses){
        pose.second->FromSE3();
        outfile << pose.first << " " << pose.second->p.transpose() << " "
        << pose.second->q.x() << " " << pose.second->q.y() << " "
        << pose.second->q.z() << " " << pose.second->q.w() << '\n';
    }
    return true;
}

int main(int argc, char**argv){
    bool DEBUG = true;
    map<int, Pose*> vertexs; 
    vector<Edge> edges; 
    string path = getcwd(NULL,0);  
    string file = path +"/../sphere.g2o";
    if(DEBUG) cout<<file<<endl;
    readG2oFile(file,vertexs,edges); //这里就是要把g2o文件读下来
    if(DEBUG) cout<<vertexs.size()<<" "<<edges.size()<<endl;
    // 接下来就是把所有的vertex和edge加入优化器
    savePose(path +"/../original.txt",vertexs);
    ceres::Problem problem;
    buildProblem(problem,vertexs,edges);//构建问题
    solveProblem(problem);//求解
    savePose(path +"/../optimized.txt",vertexs);
    cout<<"bingo!!"<<endl;
    return 0;
}