【G2O】【G2O实践】【G2O的使用学习记录】

踏破万里无云

已于 2022-08-13 20:30:37 修改

阅读量2.2k

点赞数 3

分类专栏：视觉SLAM14讲文章标签：学习算法 g2o

于 2022-07-17 18:13:27 首次发布

本文链接：https://blog.csdn.net/qq_45954434/article/details/125832010

版权

视觉SLAM14讲专栏收录该内容

33 篇文章 84 订阅

订阅专栏

【G2O】【G2O实践】【G2O的使用学习记录】

0 前言
1 下载安装G2O 2.0.0
2 g2o使用

0 前言

1 下载安装G2O 2.0.0

G2O(General Fraphic Optimization, G2O)
【slam十四讲第二版】【课本例题代码向】【第六讲~非线性优化】【安装对应版本ceres2.0.0和g2o教程】【手写高斯牛顿、ceres2.0.0、g2o拟合曲线及报错解决方案】的3.1 g2o的编译和安装
本文所有的环境是基于g2o2.0.0，与g2o1.1.4的不兼容

2 g2o使用

2.1 头文件的使用

//g2o顶点（Vertex）头文件 视觉slam十四讲p141用顶点表示优化变量，用边表示误差项
#include <g2o/core/base_vertex.h>

//g2o边（edge）头文件
#include <g2o/core/base_binary_edge.h>

#include <g2o/core/base_unary_edge.h>

//求解器头文件
#include <g2o/core/block_solver.h>

#include <g2o/core/optimization_algorithm_levenberg.h>//列文伯格——马尔夸特算法头文件
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>

#include <g2o/solvers/csparse/linear_solver_csparse.h>

#include <g2o/solvers/dense/linear_solver_dense.h>
//需要使用eigne的一些函数
#include <Eigen/Core>

2.2 CMakeLists.txt

cmake文件自取：链接: https://pan.baidu.com/s/1AqRNBa0Cp9cVleVYGeAzwQ 提取码: 94bb

# 添加g2o模块以使用g2o库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake )

# 寻找G2O
find_package( G2O REQUIRED )
Find_Package(CSparse REQUIRED)

include_directories(${G2O_INCLUDE_DIRECTORIES} ${CSPARSE_INCLUDE_DIR})

SET(G2O_LIBS  g2o_core g2o_stuff g2o_types_sba g2o_csparse_extension g2o_types_slam3d cxsparse)

add_executable( xxx src/xxx.cpp )
# 与G2O和OpenCV链接
target_link_libraries(bundle_adjustment_ceres bal_common
        ${G2O_LIBS}
        ${CSPARSE_LIBRARY}
        )

2.3 代码的优化功能实现

2.3.1 定义顶点和边的类型

从g2o派生出了用于曲线拟合的图优化顶点和边：CurveFittingVertex和CurveFittingEdge，这实质上扩展了g2o的使用方式。这两个类分别派生自BaseVertex和BaseUnaryEdge类。

2.3.1.1 曲线模型的顶点和边

参考例子为【slam十四讲第二版】【课本例题代码向】【第六讲~非线性优化】【安装对应版本ceres2.0.0和g2o教程】【手写高斯牛顿、ceres2.0.0、g2o拟合曲线及报错解决方案】的3 使用g2o进行曲线拟合

2.3.1.1.1 曲线模型的顶点

我认为顶点就是被优化的量，也就是求导的分子

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
//解决Eigen库数据结构内存对齐问题
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW//表示在利用Eigen库的数据结构时new的时候 需要对齐，所以加入EIGEN特有的宏定义即可实现
    //下面几个虚函数都是覆盖了基类的对应同名同参数的函数
    virtual void setToOriginImpl() // 重置  这个虚函数override 覆盖了Vertex类的对应函数 函数名字和参数都是一致的，是多态的本质
    {
        _estimate << 0,0,0;//输入优化变量初始值
    }

    virtual void oplusImpl( const double* update ) // 更新 对于拟合曲线这种问题，这里更新优化变量仅仅是简单的加法，
    // 但是到了位姿优化的时候，旋转矩阵更新是左乘一个矩阵 此时这个更新函数就必须要重写了
    { //更新参数估计值
        _estimate += Eigen::Vector3d(update);
    }
    // 存盘和读盘：留空
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
};

2.3.1.1.1.1 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW//表示在利用Eigen库的数据结构时new的时候 需要对齐，所以加入EIGEN特有的宏定义即可实现

2.3.1.1.1.2 输入优化变量初始值|顶点的重置函数`setToOriginmpl`函数

*顶点的重置函数setToOriginmpl函数。一般可以把估计值设置为0

    virtual void setToOriginImpl() // 重置  这个虚函数override 覆盖了Vertex类的对应函数 函数名字和参数都是一致的，是多态的本质
    {
        _estimate << 0,0,0;//输入优化变量初始值
    }

2.3.1.1.1.3 更新所求估计值`oplusImpl()`函数

顶点的更新函数：oplusImpl。

    virtual void oplusImpl( const double* update ) // 更新 对于拟合曲线这种问题，这里更新优化变量仅仅是简单的加法，
    // 但是到了位姿优化的时候，旋转矩阵更新是左乘一个矩阵 此时这个更新函数就必须要重写了
    { //更新参数估计值
        _estimate += Eigen::Vector3d(update);
    }

2.3.1.1.1.4 存盘和读盘：留空

不可省略

    // 存盘和读盘：留空
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}

2.3.1.1.2 曲线模型的边

我认为边就是求导时的分母

// 误差模型 模板参数：观测值维度，类型，连接顶点类型  //这里观测值维度是1维，如果是124页6.12式，则观测值维度是2
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
    // 计算曲线模型误差
    void computeError()
    {
        /*       _vertices是std::vector<Vertex *>类型的变量，我们这里把基类指针_vertices【0】强制转换成const CurveFittingVertex* 自定义子类的常量指针
        这里的转换是上行转换（子类指针转换到基类），对于static_cast 和dynamic_cast两种的结果都是一样的，但是对于这种下行转换则dynamic_cast比static_cast多了类型检查功能
        更安全些，但是dynamic_cast只能用在类类型的指针 引用，static_cast则不限制,即可以用在类型也可以用在其他类型，所以这里应该更改为dynamic_cast
        const CurveFittingVertex* v = dynamic_cast<const CurveFittingVertex*> (_vertices[0]);//但是这里我没有修改，因为我不懂这块不敢乱该，如果你觉得有道理你 就修改试试
        */
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        //获取此时待估计参数的当前更新值 为下面计算误差项做准备
        const Eigen::Vector3d abc = v->estimate();
        //这里的error是1x1的矩阵，因为误差项就是1个 _measurement是测量值yi
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }

/*
    //计算雅可比矩阵，没有用到，如果想尝试的话，直接把注释取消就可以，效果一样
   virtual void linearizeOplus() override{
        const CurveFittingVertex *v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        double y = exp(abc[0] * _x *_x + abc[1] * _x + abc[2]);
        _jacobianOplusXi[0] = -_x * _x * y;
        _jacobianOplusXi[1] = -_x * y;
        _jacobianOplusXi[2] = -y;
 }
 */
    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}
public:
    double _x;  // x 值， y 值为 _measurement
};

2.3.1.1.2.1 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    //表示在利用Eigen库的数据结构时new的时候 需要对齐，
    //所以加入EIGEN特有的宏定义即可实现

2.3.1.1.2.2 构造函数

    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}

2.3.1.1.2.3 曲线模型误差计算函数`computerror()`函数

边的误差计算函数computerror()函数。该函数需要取出边所连接的顶点的当前估计值，根据曲线模型，与它的观测值相比较。这和最小二乘问题中的误差模型是一致的。

    // 计算曲线模型误差
    void computeError()
    {
        /*       _vertices是std::vector<Vertex *>类型的变量，我们这里把基类指针_vertices【0】强制转换成const CurveFittingVertex* 自定义子类的常量指针
        这里的转换是上行转换（子类指针转换到基类），对于static_cast 和dynamic_cast两种的结果都是一样的，但是对于这种下行转换则dynamic_cast比static_cast多了类型检查功能
        更安全些，但是dynamic_cast只能用在类类型的指针 引用，static_cast则不限制,即可以用在类型也可以用在其他类型，所以这里应该更改为dynamic_cast
        const CurveFittingVertex* v = dynamic_cast<const CurveFittingVertex*> (_vertices[0]);//但是这里我没有修改，因为我不懂这块不敢乱该，如果你觉得有道理你 就修改试试，改了也是正常运行的
        */
        const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        //获取此时待估计参数的当前更新值 为下面计算误差项做准备
        const Eigen::Vector3d abc = v->estimate();
        //这里的error是1x1的矩阵，因为误差项就是1个 _measurement是测量值yi
        _error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
    }

2.3.1.1.2.4 边的雅可比矩阵计算函数：`linearizeOplus()`函数

边的雅可比矩阵计算函数：linearizeOplus()函数。这个函数中计算了每条边相对于顶点的雅可比
可以省略

    //计算雅可比矩阵，没有用到,但是可以使用，结果一样
   virtual void linearizeOplus() override{
        const CurveFittingVertex *v = static_cast<const CurveFittingVertex*> (_vertices[0]);
        const Eigen::Vector3d abc = v->estimate();
        double y = exp(abc[0] * _x *_x + abc[1] * _x + abc[2]);
        _jacobianOplusXi[0] = -_x * _x * y;
        _jacobianOplusXi[1] = -_x * y;
        _jacobianOplusXi[2] = -y;
 }

2.3.1.1.2.5 存盘和读盘：留空

存盘和读盘函数：由于我们不想进行读写，所以留空

    virtual bool read( istream& in ) {}
    virtual bool write( ostream& out ) const {}

2.3.1.1.2.6 public参数

public:
    double _x;  // x 值， y 值为 _measurement

2.3.1.2 求解PnP的自定义的点和边

参考【slam十四讲第二版】【课本例题代码向】【第七讲~视觉里程计Ⅰ】【1OpenCV的ORB特征】【2手写ORB特征】【3对极约束求解相机运动】【4三角测量】【5求解PnP】【3D-3D:ICP】的5求解PnP

2.3.1.2.1 PnP的自定义的点

由于这里

//对于用g2o来进行优化的话，首先要定义顶点和边的模板
//顶点，也就是咱们要优化的pose 用李代数表示它 6维
class Vertexpose: public g2o::BaseVertex<6,Sophus::SE3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//必须写，我也不知道为什么
    //重载setToOriginImpl函数 这个应该就是把刚开的待优化的pose放进去
    virtual void setToOriginImpl() override
    {
        _estimate = Sophus::SE3d();
    }
    //重载oplusImpl函数，用来更新pose（待优化的系数）
    virtual void oplusImpl(const double *update) override
    {
        Eigen::Matrix<double,6,1> update_eigen;//更新的量，就是增量呗，dx
        update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
        _estimate=Sophus::SE3d::exp(update_eigen)* _estimate;//更新pose 李代数要转换为李群，这样才可以左乘
    }
    //存盘 读盘 ：留空
    virtual bool read(istream &in) override {}

    virtual bool write(ostream &out) const override {}
};

2.3.1.2.2 PnP的自定义的边

//定义边模板 边也就是误差，二维 并且把顶点也放进去
class EdgeProjection : public g2o::BaseUnaryEdge<2,Eigen::Vector2d,Vertexpose>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//必须写，我也不知道为什么
    //有参构造，初始化 图1中的3d点 以及相机内参K
    EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos),_K(K) {}
    //计算误差
    virtual void computeError() override
    {
        const Vertexpose *v=static_cast<const Vertexpose *>(_vertices[0]);
        Sophus::SE3d T=v->estimate();
        Eigen::Vector3d pos_pixel = _K * (T * _pos3d);//T * _pos3d是图2的相机坐标下的3d点
        pos_pixel /= pos_pixel[2];//得到了像素坐标的齐次形式
        _error = _measurement - pos_pixel.head<2>();
    }
    //计算雅克比矩阵
    virtual void linearizeOplus() override
    {
        const Vertexpose *v = static_cast<Vertexpose *> (_vertices[0]);
        Sophus::SE3d T = v->estimate();
        Eigen::Vector3d pos_cam=T*_pos3d;//图2的相机坐标下的3d点
        double fx = _K(0, 0);
        double fy = _K(1, 1);
        double cx = _K(0, 2);
        double cy = _K(1, 2);
        double X = pos_cam[0];
        double Y = pos_cam[1];
        double Z = pos_cam[2];
        double Z2 = Z * Z;
        //雅克比矩阵见 书 p187 公式7.46
        _jacobianOplusXi
                << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
                0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;


    }
    //存盘 读盘 ：留空
    virtual bool read(istream &in) override {}

    virtual bool write(ostream &out) const override {}
private:
    Eigen::Vector3d _pos3d;
    Eigen::Matrix3d _K;
};

2.3.1.3 收集的顶点和边

2.3.1.3.1 顶点

2.3.1.3.1.1 位姿顶点

摘自：【slam十四讲第二版】【课本例题代码向】【第十三讲~实践:设计SLAM系统】的3.2.13.1 位姿顶点VertexPose
和本文章的2.3.1.2.1 PnP的自定义的点基本一样

/// vertex and edges used in g2o ba
/// 位姿顶点
/// 也就是咱们要优化的pose 用李代数表示它 6维
class VertexPose : public g2o::BaseVertex<6, SE3> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    ///重载setToOriginImpl函数 这个应该就是把刚开的待优化的pose放进去
    virtual void setToOriginImpl() override { _estimate = SE3(); }

    /// left multiplication on SE3
    ///重载oplusImpl函数，用来更新pose（待优化的系数）
    virtual void oplusImpl(const double *update) override {
        Vec6 update_eigen;///更新的量，就是增量呗，dx
        update_eigen << update[0], update[1], update[2], update[3], update[4],
            update[5];
        ///更新pose 李代数要转换为李群，这样才可以左乘
        _estimate = SE3::exp(update_eigen) * _estimate;
    }

    virtual bool read(std::istream &in) override { return true; }

    virtual bool write(std::ostream &out) const override { return true; }
};

2.3.1.3.1.2 路标顶点

摘自：【slam十四讲第二版】【课本例题代码向】【第十三讲~实践:设计SLAM系统】的3.2.13.2 路标顶点VertexXYZ

/// 路标顶点
class VertexXYZ : public g2o::BaseVertex<3, Vec3> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    //重载setToOriginImpl函数 这个应该就是把刚开的待优化的Vec3放进去
    virtual void setToOriginImpl() override { _estimate = Vec3::Zero(); }

    virtual void oplusImpl(const double *update) override {
        _estimate[0] += update[0];
        _estimate[1] += update[1];
        _estimate[2] += update[2];
    }

    virtual bool read(std::istream &in) override { return true; }

    virtual bool write(std::ostream &out) const override { return true; }
};

2.3.1.3.2 边

2.3.1.3.2.1 仅估计位姿的一元边

摘自：【slam十四讲第二版】【课本例题代码向】【第十三讲~实践:设计SLAM系统】的3.2.13.3 仅估计位姿的一元边EdgeProjectionPoseOnly
和本文章的2.3.1.2.2 PnP的自定义的边基本一样

/// 仅估计位姿的一元边
class EdgeProjectionPoseOnly : public g2o::BaseUnaryEdge<2, Vec2, VertexPose> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    ///有参构造，初始化 图1中的3d点 以及相机内参K
    EdgeProjectionPoseOnly(const Vec3 &pos, const Mat33 &K)
        : _pos3d(pos), _K(K) {}
    ///计算误差
    virtual void computeError() override {
        const VertexPose *v = static_cast<VertexPose *>(_vertices[0]);
        SE3 T = v->estimate();//Tcw 形式Pose
        Vec3 pos_pixel = _K * (T * _pos3d);
        pos_pixel /= pos_pixel[2];
        _error = _measurement - pos_pixel.head<2>();
    }
    ///计算雅克比矩阵
    virtual void linearizeOplus() override {
        const VertexPose *v = static_cast<VertexPose *>(_vertices[0]);
        SE3 T = v->estimate();
        Vec3 pos_cam = T * _pos3d;///pos_cam图2的相机坐标下的3d点
        double fx = _K(0, 0);
        double fy = _K(1, 1);
        double X = pos_cam[0];
        double Y = pos_cam[1];
        double Z = pos_cam[2];
        double Zinv = 1.0 / (Z + 1e-18);
        double Zinv2 = Zinv * Zinv;
        _jacobianOplusXi << -fx * Zinv, 0, fx * X * Zinv2, fx * X * Y * Zinv2,
            -fx - fx * X * X * Zinv2, fx * Y * Zinv, 0, -fy * Zinv,
            fy * Y * Zinv2, fy + fy * Y * Y * Zinv2, -fy * X * Y * Zinv2,
            -fy * X * Zinv;
    }

    virtual bool read(std::istream &in) override { return true; }

    virtual bool write(std::ostream &out) const override { return true; }

   private:
    Vec3 _pos3d;
    Mat33 _K;
};

2.3.1.3.2.2 带有地图和位姿的二元边

摘自：【slam十四讲第二版】【课本例题代码向】【第十三讲~实践:设计SLAM系统】的3.2.13.4 带有地图和位姿的二元边EdgeProjection

/// 带有地图和位姿的二元边
class EdgeProjection
    : public g2o::BaseBinaryEdge<2, Vec2, VertexPose, VertexXYZ> {
   public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    /// 构造时传入相机内外参 cam_ext
    EdgeProjection(const Mat33 &K, const SE3 &cam_ext) : _K(K) {
        _cam_ext = cam_ext;
    }

    virtual void computeError() override {
        const VertexPose *v0 = static_cast<VertexPose *>(_vertices[0]);
        const VertexXYZ *v1 = static_cast<VertexXYZ *>(_vertices[1]);
        SE3 T = v0->estimate();
        Vec3 pos_pixel = _K * (_cam_ext * (T * v1->estimate()));
        pos_pixel /= pos_pixel[2];
        _error = _measurement - pos_pixel.head<2>();
    }

    virtual void linearizeOplus() override {
        const VertexPose *v0 = static_cast<VertexPose *>(_vertices[0]);
        const VertexXYZ *v1 = static_cast<VertexXYZ *>(_vertices[1]);
        SE3 T = v0->estimate();
        Vec3 pw = v1->estimate();
        Vec3 pos_cam = _cam_ext * T * pw;
        double fx = _K(0, 0);
        double fy = _K(1, 1);
        double X = pos_cam[0];
        double Y = pos_cam[1];
        double Z = pos_cam[2];
        double Zinv = 1.0 / (Z + 1e-18);
        double Zinv2 = Zinv * Zinv;
        _jacobianOplusXi << -fx * Zinv, 0, fx * X * Zinv2, fx * X * Y * Zinv2,
            -fx - fx * X * X * Zinv2, fx * Y * Zinv, 0, -fy * Zinv,
            fy * Y * Zinv2, fy + fy * Y * Y * Zinv2, -fy * X * Y * Zinv2,
            -fy * X * Zinv;

        _jacobianOplusXj = _jacobianOplusXi.block<2, 3>(0, 0) *
                           _cam_ext.rotationMatrix() * T.rotationMatrix();
    }

    virtual bool read(std::istream &in) override { return true; }

    virtual bool write(std::ostream &out) const override { return true; }

   private:
    Mat33 _K;
    SE3 _cam_ext;
};

2.3.2 构建图

2.3.2.1 补充：求解增量算法的选择（梯度下降方法的选择）

注意这里的选择主要是你构造求解器的时候，选择哪种方法的求解器，同时要注意包含与之配套的头文件

2.3.2.1.1 Levenberg方法

头文件

#include <g2o/core/optimization_algorithm_levenberg.h>

使用方法

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( unique_ptr<Block>(solver_ptr) );

solver_ptr见下文定义

2.3.2.1.2 GaussNewton方法

头文件

#include <g2o/core/optimization_algorithm_gauss_newton.h>

使用方法

    //g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(  unique_ptr<Block>(solver_ptr)  );

2.3.2.1.3 Dogleg方法

头文件

#include <g2o/core/optimization_algorithm_dogleg.h>

使用方法

    //g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg(  unique_ptr<Block>(solver_ptr)  );

2.3.2.2 定义solver求解器

2.3.2.2.1 构建图的第一种方法

//第一种实现方法start
/*
第一种解决方式: 将普通指针强制转换成智能指针 需要注意的是 转化之后 原来的普通指针指向的内容会有变化
 普通指针可以强制转换成智能指针，方式是通过智能指针的一个构造函数来实现的， 比如下面的Block( std::unique_ptr<Block::LinearSolverType>( linearSolver ) );
 这里面就是将linearSolver普通指针作为参数用智能指针构造一个临时的对象，此时原来的普通指针就无效了，一定不要再次用那个指针了，否则会有意想不到的错误，如果还想保留原来的指针
 那么就可以利用第二种方式 定义的时候就直接用智能指针就好，但是就如第二种解决方案那样，也会遇到类型转换的问题。详细见第二种方式说明
 */
    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每个误差项优化变量维度为3，误差值维度为1  后面的那个参数与误差变量无关 仅仅表示路标点的维度 这里因为没有用到路标点 所以为什么值都可以
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 线性方程求解器
    //Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    Block* solver_ptr = new Block( unique_ptr<Block::LinearSolverType>(linearSolver) );      // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( unique_ptr<Block>(solver_ptr) );
    //g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(  unique_ptr<Block>(solver_ptr)  );
    //g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg(  unique_ptr<Block>(solver_ptr)  );
//第一种实现方法end

2.3.2.2.2 构建图的第二种方法

//第二种实现方法start
    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
    typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; //线性求解器类型
    // 梯度下降方法，从GN, LM, DogLeg 中选
    auto solver = new g2o::OptimizationAlgorithmGaussNewton(g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
//第二种实现方法end

2.3.2.2.3 构建图的第三种方法

//第三种实现方法start
    std::unique_ptr<Block::LinearSolverType>linearSolver( new g2o::LinearSolverDense<Block::PoseMatrixType>() );// 线性方程求解器(1)
    std::unique_ptr<Block> solver_ptr ( new  Block( std::move(linearSolver) ) );// 矩阵块求解器 (2)
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( std::move(solver_ptr) );//(3) LM法
//第三种实现方法end

2.3.2.3 定义和配置图模型的参数

    g2o::SparseOptimizer optimizer;     // 图模型
    optimizer.setAlgorithm( solver );   // 设置求解器
    optimizer.setVerbose( true );       // 打开调试输出，也就是输出迭代信息的设置

输出的迭代信息为：

iteration= 0	 chi2= 376795.504941	 time= 3.7278e-05	 cumTime= 3.7278e-05	 edges= 100	 schur= 0	 lambda= 21.496599	 levenbergIter= 1
iteration= 1	 chi2= 35680.687988	 time= 2.4466e-05	 cumTime= 6.1744e-05	 edges= 100	 schur= 0	 lambda= 10.024455	 levenbergIter= 1
iteration= 2	 chi2= 2199.600010	 time= 2.3837e-05	 cumTime= 8.5581e-05	 edges= 100	 schur= 0	 lambda= 3.341485	 levenbergIter= 1
iteration= 3	 chi2= 178.624821	 time= 2.4025e-05	 cumTime= 0.000109606	 edges= 100	 schur= 0	 lambda= 1.113828	 levenbergIter= 1
iteration= 4	 chi2= 103.241078	 time= 2.9405e-05	 cumTime= 0.000139011	 edges= 100	 schur= 0	 lambda= 0.371276	 levenbergIter= 1
iteration= 5	 chi2= 101.938412	 time= 2.3718e-05	 cumTime= 0.000162729	 edges= 100	 schur= 0	 lambda= 0.123759	 levenbergIter= 1
iteration= 6	 chi2= 101.937020	 time= 2.3662e-05	 cumTime= 0.000186391	 edges= 100	 schur= 0	 lambda= 0.082506	 levenbergIter= 1
iteration= 7	 chi2= 101.937020	 time= 2.4079e-05	 cumTime= 0.00021047	 edges= 100	 schur= 0	 lambda= 0.055004	 levenbergIter= 1
iteration= 8	 chi2= 101.937020	 time= 3.6098e-05	 cumTime= 0.000246568	 edges= 100	 schur= 0	 lambda= 1201.577796	 levenbergIter= 6
iteration= 9	 chi2= 101.937020	 time= 2.3588e-05	 cumTime= 0.000270156	 edges= 100	 schur= 0	 lambda= 801.051864	 levenbergIter= 1
iteration= 10	 chi2= 101.937020	 time= 2.3459e-05	 cumTime= 0.000293615	 edges= 100	 schur= 0	 lambda= 534.034576	 levenbergIter= 1
iteration= 11	 chi2= 101.937020	 time= 3.8055e-05	 cumTime= 0.00033167	 edges= 100	 schur= 0	 lambda= 746634452.755710	 levenbergIter= 7
iteration= 12	 chi2= 101.937020	 time= 2.8344e-05	 cumTime= 0.000360014	 edges= 100	 schur= 0	 lambda= 3982050414.697119	 levenbergIter= 3
iteration= 13	 chi2= 101.937020	 time= 3.0654e-05	 cumTime= 0.000390668	 edges= 100	 schur= 0	 lambda= 4077619624649.849609	 levenbergIter= 4

进程已结束,退出代码0

2.3.2.4 往图中加顶点和边

2.3.2.4.1 加2.3.1.1 曲线模型的顶点和边

加一个顶点

    // 往图中增加顶点
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(ae,be,ce) );//迭代5次满足迭代阈值 //增加顶点的初始值，如果是位姿 则初始值是用ICP PNP来提供初始化值
    v->setId(0);//增加顶点标号 多个顶点要依次增加编号
    optimizer.addVertex( v );//将新增的顶点加入到图模型中

加N个边

    // 往图中增加边 N个
    for ( int i=0; i<N; i++ )
    {
        CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
        edge->setId(i);
        edge->setVertex( 0, v );                // 设置连接的顶点
        edge->setMeasurement( y_data[i] );      // 观测数值  经过高斯噪声的
        //这里的信息矩阵可以参考:http://www.cnblogs.com/gaoxiang12/p/5244828.html 里面有说明
        edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵：协方差矩阵之逆 这里为1表示加权为1
        optimizer.addEdge( edge );
    }

2.3.2.4.2 加G2O库中的顶点和边（两个顶点，N个边）和相机的参数

参考【slam十四讲第二版】【课本例题代码向】【第七讲~视觉里程计Ⅰ】【1OpenCV的ORB特征】【2手写ORB特征】【3对极约束求解相机运动】【4三角测量】【5求解PnP】【3D-3D:ICP】的5求解PnP
这是G2O自带的点和边，不需要自定义什么
前面的2.3.2.2 定义solver求解器要改一下 typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3>> Block;// pose 维度为 6, landmark 维度为 3

两种顶点
vertex 有两个顶点，一个是优化位姿，一个是优化特征点的空间位置

1.1 优化位姿 g2o::VertexSE3Expmap*

    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    Eigen::Matrix3d R_mat;
    R_mat <<
          R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
            R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
            R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
    pose->setId(0);//增加顶点标号 多个顶点要依次增加编号
    pose->setEstimate(g2o::SE3Quat(R_mat, Eigen::Vector3d(t.at<double>(0,0), t.at<double>(1,0), t.at<double>(2,0))));//初始值
    optimizer.addVertex(pose);//将新增的顶点加入到图模型中

1.2 优化特征点的空间位置 g2o::VertexPointXYZ*

    int index = 1;
    for (const cv::Point3f p:points_3d) // landmarks
    {
        g2o::VertexPointXYZ* point = new g2o::VertexPointXYZ();
        point->setId(index++);//增加顶点标号 多个顶点要依次增加编号
        point->setEstimate(Eigen::Vector3d(p.x, p.y, p.z));
        point->setMarginalized(true);// g2o 中必须设置 marg 参见第十讲内容，//设置边缘化
        optimizer.addVertex(point);//将新增的顶点加入到图模型中
    }

参数 g2o::CameraParameters*

加入相机参数（当然有另一种方式：查看笔记两种边的不同点）(最后一项为0 默认fx = fy 然后优化位姿与g2o::EdegeSE3ProjectXYZ不同笔记以记载 )

    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
            K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
    );
    camera->setId ( 0 );//增加顶点标号 多个顶点要依次增加编号
    optimizer.addParameter ( camera );

边 g2o::EdgeProjectXYZ2UV*

    // edges
    index = 1;
    for ( const cv::Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId ( index );//增加顶点标号 多个顶点要依次增加编号
        edge->setVertex ( 0, dynamic_cast<g2o::VertexPointXYZ*> ( optimizer.vertex ( index ) ) );  // 设置连接的顶点
        edge->setVertex ( 1, pose );  // 设置连接的顶点
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );  // 观测数值  经过高斯噪声的
        edge->setParameterId ( 0,0 );//最后一位设置使用的相机参数(因为上面仅仅输入了一个相机参数id=0, 对应上面camer->setId(0),第一个参数0不知道是什么，但是必须为0)
        // 信息矩阵：协方差矩阵之逆 这里为1表示加权为1
        edge->setInformation ( Eigen::Matrix2d::Identity() );//这里的信息矩阵可以参考:http://www.cnblogs.com/gaoxiang12/p/5244828.html 里面有说明
        optimizer.addEdge ( edge );
        index++;
    }

2.3.2.4.3 加2.3.1.2pnp优化的自定义的顶点和边

前面的2.3.2.2 定义solver求解器要改一下 typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3>> Block;// pose 维度为 6, landmark 维度为 3

加入顶点

    //加入顶点
    Vertexpose *v=new Vertexpose();
    v->setEstimate(Sophus::SE3d());
    v->setId(0);
    optimizer.addVertex(v);

K相机内参值

    //K
    // K
    Eigen::Matrix3d K_eigen;
    K_eigen <<
            K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
            K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
            K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);

加入边

    //加入边
    int index=1;
    for(size_t i=0;i<points_2d.size();++i)
    {
        //遍历 把3d点和像素点拿出来
        auto p2d = points_2d[i];
        auto p3d = points_3d[i];
        EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);//有参构造
        edge->setId(index);
        edge->setVertex(0,v);
        edge->setMeasurement(p2d);//设置观测值，其实就是图2 里的匹配特征点的像素位置
        edge->setInformation(Eigen::Matrix2d::Identity());//信息矩阵是二维方阵，因为误差是二维
        optimizer.addEdge(edge);//加入边
        index++;//边的编号++
    }

2.3.2.5 执行优化

    optimizer.initializeOptimization();
    optimizer.optimize(100);

2.3.2.6 输出优化值

    // 输出优化值
    Eigen::Vector3d abc_estimate = v->estimate();
    cout<<"estimated model: "<<abc_estimate.transpose()<<endl;

输出：

estimated model: 0.890912   2.1719 0.943629

2.4 额外补充功能

2.4.1 G2O中的SE3表示方式

2.4.1.1 初始化李群SE3

    Eigen::Matrix3d R_mat;
    cv::Mat& t
    
	g2o::SE3Quat(R_mat, Eigen::Vector3d(t.at<double>(0,0), t.at<double>(1,0), t.at<double>(2,0)))

2.4.1.2 G2O的SE3->Eigen中的旋转向量

Eigen::Isometry3d ( pose->estimate() )

2.4.1.3 使用其`map函数`进行位姿变换运算

        g2o::SE3Quat T(pose->estimate());//pose->estimate()的格式g2o::SE3Quat(Eigen::Matrix3d::Identity(),Eigen::Vector3d( 0,0,0 )  ) 
        Eigen::Vector3d xyz_trans = T.map( point->estimate() );//映射到第二帧相机坐标系下的坐标值
        //pose->estimate().map( _point );中用estimate（）估计一个值后，然后用映射函数 就是旋转加平移 将其_point映射到另一个相机坐标系下去

2.5 使用案例一：g2o求解BA

该实例参考* 该历程使用BAL数据集，所以会采用指针的方式读取数据，BAL数据集的说明，可以参考【slam十四讲第二版】【课本例题代码向】【第九讲~后端Ⅰ】的4 实践：g2o求解BA
亮点

对顶点和边进行了结构体的包装
利用H矩阵的稀疏性对其进行Schur边缘化，加速求解
使用稀疏优化，g2o需要手动设置哪些顶点为边缘化顶点，此处为路标点的位姿
使用了Huber的鲁棒核函数

2.5.1 头文件说明

该历程使用BAL数据集，所以会采用指针的方式读取数据，BAL数据集的说明，可以参考【slam十四讲第二版】【课本例题代码向】【第九讲~后端Ⅰ】的2 BAL数据集

2.5.1.1 稀疏边缘化的头文件

#include <g2o/solvers/csparse/linear_solver_csparse.h>

代码实例：

    typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;

2.5.1.2 鲁棒核函数的头文件

鲁棒核函数介绍参考slam14讲（p252），或文章【slam十四讲第二版】【课本例题代码向】【第九讲~后端Ⅰ】的4 实践：g2o求解BA

#include <g2o/core/robust_kernel_impl.h>//鲁棒核函数

代码实例：

edge->setRobustKernel(new g2o::RobustKernelHuber());//核函数 鲁棒核函数

设定Huber核的阈值方式

            auto rk = new g2o::RobustKernelHuber();//核函数 鲁棒核函数
            rk->setDelta(chi2_th);
            edge->setRobustKernel(rk);

参考：【slam十四讲第二版】【课本例题代码向】【第十三讲~实践:设计SLAM系统】的3.2.7.7 Optimize()

2.5.2 代码完整流程

2.5.2.1 总头文件

#include <g2o/core/base_vertex.h>//g2o顶点（Vertex）头文件 视觉slam十四讲p141用顶点表示优化变量，用边表示误差项
#include <g2o/core/base_binary_edge.h>//g2o边（edge）头文件
#include <g2o/core/block_solver.h>//求解器头文件
#include <g2o/core/optimization_algorithm_levenberg.h>//列文伯格——马尔夸特算法头文件
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel_impl.h>//鲁棒核函数
#include <iostream>

#include "common.h"//使用common.h中定义的BALProblem类读入该文件的内容
#include "sophus/se3.hpp"

2.5.2.2 顶点和边的定义

//using namespace Sophus;
using namespace Eigen;
using namespace std;

/// 姿态和内参的结构
struct PoseAndIntrinsics {
    PoseAndIntrinsics() {}

    /// set from given data address
    explicit PoseAndIntrinsics(double *data_addr)
    {
        //每个相机一共有一共有6维的姿态
        rotation = Sophus::SO3d::exp(Vector3d(data_addr[0], data_addr[1], data_addr[2]));//旋转位姿
        translation = Vector3d(data_addr[3], data_addr[4], data_addr[5]);//转换矩阵
        focal = data_addr[6];//1维焦距
        //2维畸变参数
        k1 = data_addr[7];
        k2 = data_addr[8];
    }

    /// 将估计值放入内存
    void set_to(double *data_addr)
    {
        auto r = rotation.log();
        for (int i = 0; i < 3; ++i) data_addr[i] = r[i];//旋转位姿
        for (int i = 0; i < 3; ++i) data_addr[i + 3] = translation[i];//转换矩阵
        data_addr[6] = focal;//1维焦距
        data_addr[7] = k1;//2维畸变参数
        data_addr[8] = k2;//2维畸变参数
    }

    Sophus::SO3d rotation;
    Vector3d translation = Vector3d::Zero();//置0
    double focal = 0;//初始化为0
    double k1 = 0, k2 = 0;//将2维畸变参数初始化为0
};

/// 位姿加相机内参的顶点，9维，前三维为so3，接下去为t, f, k1, k2
class VertexPoseAndIntrinsics : public g2o::BaseVertex<9, PoseAndIntrinsics> {
public://以下定义的成员变量和成员函数都是公有的
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//解决Eigen库数据结构内存对齐问题

    VertexPoseAndIntrinsics() {}

    // 重置
    virtual void setToOriginImpl() override //virtual表示该函数为虚函数，override保留字表示当前函数重写了基类的虚函数
    {
        _estimate = PoseAndIntrinsics();
    }
    // 更新
    virtual void oplusImpl(const double *update) override {
        _estimate.rotation = Sophus::SO3d::exp(Vector3d(update[0], update[1], update[2])) * _estimate.rotation;
        _estimate.translation += Vector3d(update[3], update[4], update[5]);//更新量累加
        _estimate.focal += update[6];//1维焦距更新量累加
        _estimate.k1 += update[7];//2维畸变参数更新量累加
        _estimate.k2 += update[8];//2维畸变参数更新量累加
    }

    /// 根据估计值投影一个点
    Vector2d project(const Vector3d &point)
    {
        Vector3d pc = _estimate.rotation * point + _estimate.translation;
        pc = -pc / pc[2];
        double r2 = pc.squaredNorm();
        double distortion = 1.0 + r2 * (_estimate.k1 + _estimate.k2 * r2);
        return Vector2d(_estimate.focal * distortion * pc[0],
                        _estimate.focal * distortion * pc[1]);
    }

    // 存盘和读盘：留空
    virtual bool read(istream &in) {} //istream类是c++标准输入流的一个基类
    //可参照C++ Primer Plus第六版的6.8节
    virtual bool write(ostream &out) const {} //ostream类是c++标准输出流的一个基类
    //可参照C++ Primer Plus第六版的6.8节
};

class VertexPoint : public g2o::BaseVertex<3, Vector3d> {
public://以下定义的成员变量和成员函数都是公有的
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//解决Eigen库数据结构内存对齐问题

    VertexPoint() {}
    // 重置
    virtual void setToOriginImpl() override //virtual表示该函数为虚函数，override保留字表示当前函数重写了基类的虚函数
    {
        _estimate = Vector3d(0, 0, 0);
    }
    // 更新
    virtual void oplusImpl(const double *update) override
    {
        _estimate += Vector3d(update[0], update[1], update[2]);//更新量累加
    }

    // 存盘和读盘：留空
    virtual bool read(istream &in) {} //istream类是c++标准输入流的一个基类
    //可参照C++ Primer Plus第六版的6.8节
    virtual bool write(ostream &out) const {} //ostream类是c++标准输出流的一个基类
    //可参照C++ Primer Plus第六版的6.8节
};
// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class EdgeProjection :
        public g2o::BaseBinaryEdge<2, Vector2d, VertexPoseAndIntrinsics, VertexPoint> {
public://以下定义的成员变量和成员函数都是公有的
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//解决Eigen库数据结构内存对齐问题

    // 计算误差
    virtual void computeError() override //virtual表示虚函数，保留字override表示当前函数重写了基类的虚函数
    {
        auto v0 = (VertexPoseAndIntrinsics *) _vertices[0];
        auto v1 = (VertexPoint *) _vertices[1];
        auto proj = v0->project(v1->estimate());
        _error = proj - _measurement;
    }

    // use numeric derivatives
    virtual bool read(istream &in) {}//istream类是c++标准输入流的一个基类
    //可参照C++ Primer Plus第六版的6.8节
    virtual bool write(ostream &out) const {}//ostream类是c++标准输出流的一个基类
    //可参照C++ Primer Plus第六版的6.8节

};

2.5.2.3 g2o求解过程

定义g2o求解的时候，定义了采用稀疏性边缘化的求解器： typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;
定义作为顶点时，设置其为边缘化顶点，这是必需的：v->setMarginalized(true);

void SolveBA(BALProblem &bal_problem);//定义SolveBA函数

int main(int argc, char **argv) {

    if (argc != 2) {
        cout << "usage: bundle_adjustment_g2o bal_data.txt" << endl;//输出使用方法
        return 1;
    }

    BALProblem bal_problem(argv[1]);
    bal_problem.Normalize();//归一化 将所有路标点的中心置零，然后做一个合适尺度的缩放
    bal_problem.Perturb(0.1, 0.5, 0.5);//通过Perturb函数给数据加入噪声
    bal_problem.WriteToPLYFile("initial.ply");//存储最初点云
    SolveBA(bal_problem);//BA求解
    bal_problem.WriteToPLYFile("final.ply");//存储最终点云

    return 0;
}

void SolveBA(BALProblem &bal_problem) {
    const int point_block_size = bal_problem.point_block_size();
    const int camera_block_size = bal_problem.camera_block_size();
    double *points = bal_problem.mutable_points();
    double *cameras = bal_problem.mutable_cameras();

    // pose dimension 9, landmark is 3
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<9, 3>> BlockSolverType;// 每个误差项优化变量维度为9，误差值维度为3
    typedef g2o::LinearSolverCSparse<BlockSolverType::PoseMatrixType> LinearSolverType;
    // use LM
    auto solver = new g2o::OptimizationAlgorithmLevenberg(
            g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
    //c++中的make_unique表示智能指针类型
    g2o::SparseOptimizer optimizer;// 图模型
    optimizer.setAlgorithm(solver);// 设置求解器
    optimizer.setVerbose(true);// 打开调试输出

    /// build g2o problem
    const double *observations = bal_problem.observations();
    // vertex
    vector<VertexPoseAndIntrinsics *> vertex_pose_intrinsics;
    vector<VertexPoint *> vertex_points;
    for (int i = 0; i < bal_problem.num_cameras(); ++i) {
        VertexPoseAndIntrinsics *v = new VertexPoseAndIntrinsics();
        double *camera = cameras + camera_block_size * i;
        v->setId(i);
        v->setEstimate(PoseAndIntrinsics(camera));//camera表示优化变量
        optimizer.addVertex(v);// 设置连接的顶点
        vertex_pose_intrinsics.push_back(v);
    }
    for (int i = 0; i < bal_problem.num_points(); ++i) {
        VertexPoint *v = new VertexPoint();
        double *point = points + point_block_size * i;
        v->setId(i + bal_problem.num_cameras());
        v->setEstimate(Vector3d(point[0], point[1], point[2]));
        // g2o在BA中需要手动设置待Marg的顶点
        v->setMarginalized(true);// g2o 中必须设置 marg 参见第十讲内容，//设置边缘化
        optimizer.addVertex(v);//将新增的顶点加入到图模型中
        vertex_points.push_back(v);
    }

    // edge
    for (int i = 0; i < bal_problem.num_observations(); ++i) {
        EdgeProjection *edge = new EdgeProjection;
        edge->setVertex(0, vertex_pose_intrinsics[bal_problem.camera_index()[i]]);
        edge->setVertex(1, vertex_points[bal_problem.point_index()[i]]);
        edge->setMeasurement(Vector2d(observations[2 * i + 0], observations[2 * i + 1]));
        edge->setInformation(Matrix2d::Identity());//信息矩阵
        edge->setRobustKernel(new g2o::RobustKernelHuber());//核函数 鲁棒核函数
        optimizer.addEdge(edge);//添加边
    }

    optimizer.initializeOptimization();
    optimizer.optimize(40);//设置迭代次数为40

    // set to bal problem
    for (int i = 0; i < bal_problem.num_cameras(); ++i) {
        double *camera = cameras + camera_block_size * i;
        //camera_block_size = bal_problem.camera_block_size();
        //*camera = cameras + bal_problem.camera_block_size() * bal_problem.camera_index()[i]
        auto vertex = vertex_pose_intrinsics[i];
        auto estimate = vertex->estimate();
        estimate.set_to(camera);
    }
    for (int i = 0; i < bal_problem.num_points(); ++i) {
        double *point = points + point_block_size * i;
        auto vertex = vertex_points[i];
        for (int k = 0; k < 3; ++k) point[k] = vertex->estimate()[k];
    }
}