SLAM后端：优化空间点、相机位姿与参数 - 《SLAM 14讲》第十讲

最近研究了一下SLAM后端优化的过程，主要是用BA模型进行优化，顺便也熟悉了如何用g2o实现非线性最小二乘的求解。本文主要介绍一下《SLAM 14讲》第十讲用g2o做BA优化的代码。

1. 开始的开始

物理和数学原理：

我们要优化的量为所有测得的路标在世界坐标系下的坐标，记为 $P$ ；所有相机的位姿，记为 $R,t$ ，其中 $R$ 为旋转向量，$t$ 为平移向量；相机的内参，记为 $f,k_1,k_2$ ，按照书中我们把图像的中心定位像素坐标系的原点，故 $c_x,c_y$ 不用考虑。

我们通过重投影误差最小来优化各个量：
$$\min \sum _i\sum _j||z_{ij}-h({\xi }_i,P_j)|{|}^2$$

其中 $z_{ij}$ 为第 $j$ 个路标在第 $i$ 个相机位姿处的投影坐标（以图像中心为原点）；$h$ 为投影函数；${\xi }_i$ 为第 $i$ 个相机要优化的参数，是一个9维向量，前三位是旋转向量，中间三维是平移向量，后三维分别是 $f,k_1,k_2$；$P_j$ 为第 $j$ 个路标世界坐标系下的坐标。

实现原理： 利用矩阵 $H$ 的稀疏性，用g2o求解。

实现步骤：

1. 设置顶点和边。顶点有两个，相机参数和路标位置；每条边连接相机参数和路标位置。
2. 配置线性求解器。
3. 设置迭代方式。
4. 构造 Bundle Adjustment 问题，即往g2o::SparseOptimizer*中添加顶点和边。
5. 开始求解。

自己简化了一下代码，见这篇文章：《SLAM 14讲》第十讲：后端优化。

---

2. 投影函数

projection.h

// camera : 9维向量
// [0-2] : 旋转向量 
// [3-5] : 平移向量
// [6-8] : 相机参数, [6] 焦距, [7-8] k_1, k_2
// point : 3D 位置
// predictions : 重投影位置，图像中心为原点
template<typename T>
inline bool CamProjectionWithDistortion(const T* camera, const T* point, T* predictions){
    // 这里是把 point 转化到相机坐标系下：P_c = R*P_w + t
    T p[3];
    
	// tools/rotation.h 里的通过旋转向量旋转点的函数
    AngleAxisRotatePoint(camera, point, p);
    p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];

    // 计算畸变中心
    // 这里就是计算 point 在归一化平面的坐标，取负数可能是因为书中生成的图像为倒立的。
    // 感觉我们一般测得的数据这里应该不用取负数。不知道对不对。。
    T xp = -p[0]/p[2];
    T yp = -p[1]/p[2];

    // 取得相机畸变系数 k_1, k_2
    const T& l1 = camera[7];
    const T& l2 = camera[8];

    T r2 = xp*xp + yp*yp;
    T distortion = T(1.0) + r2 * (l1 + l2 * r2);

    const T& focal = camera[6];
    predictions[0] = focal * distortion * xp;
    predictions[1] = focal * distortion * yp;

    return true;
}

---

3. 设置顶点

Eigen::Map 的用法，参见官网。

g2o_bal_class.h

// 这里的参数分别为顶点的维度、顶点的数据类型
class VertexCameraBAL : public g2o::BaseVertex<9,Eigen::VectorXd> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexCameraBAL() {}
	
	// 读取、存储、设置初始值都直接留空就好了，下面也用不到这些
    virtual bool read ( std::istream& is ) {return false;}
    virtual bool write ( std::ostream& os ) const {return false;}
    virtual void setToOriginImpl() {}

	// 更新相机参数
    virtual void oplusImpl ( const double* update ) {
    	// 把 double* 映射到 VectorXd，VertexCameraBAL::Dimension 即为输入的9
    	// 相当于 Eigen::Map<const VectorXd> ( update, VertexCameraBAL::Dimension )
        Eigen::VectorXd::ConstMapType v ( update, VertexCameraBAL::Dimension );
        _estimate += v;
    }
};


class VertexPointBAL : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexPointBAL() {}
	
	// 读取、存储、设置初始值都直接留空
    virtual bool read ( std::istream& ) {return false;}
    virtual bool write ( std::ostream& ) const {return false;}
    virtual void setToOriginImpl() {}
	
	// 更新路标位置
    virtual void oplusImpl ( const double* update ) {
    	// 把 double* 映射到 Vector3d
        Eigen::Vector3d::ConstMapType v ( update );
        _estimate += v;
    }
};

---

4. 设置边

stactic_cast, dynamic_cast, const_cast ：请参见static_cast、dynamic_cast、 const_cast-详解。

Eigen::Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > ：其中 _Options 主要参数有 RowMajor, ColMajor，区别存储时（体现为用data()映射时） RowMajor 映射次序为一行一行的存储，ColMajor 为一列一列的存储。

ceres::internal::AutoDiff ： 在ceres的autodiff.h里有介绍。

g2o_bal_class.h

// 参数为误差的维度、测量值的类型、顶点0、顶点1
class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Eigen::Vector2d,VertexCameraBAL, VertexPointBAL> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeObservationBAL() {}
	
	// 读取、存储直接留空
    virtual bool read ( std::istream& /*is*/ ) {return false;}
    virtual bool write ( std::ostream& /*os*/ ) const { return false;}
	
	// override 为保留字，表示当前函数重写了基类的虚函数
    virtual void computeError() override {
    	// static_cast 就是把 vertex ( i ) 强制转化为 const Vertex*
        const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
        const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );
		
		// 这里用到了下面的 operator()
		// cam->estimate() 返回 Eigen::VectorXd，data() 就是把 VectorXd 映射到 double*
        ( *this ) ( cam->estimate().data(), point->estimate().data(), _error.data() );
    }

	// 计算残差 (residual) 即重投影误差，也为了下面能使用 ceres::AutoDiff 
    template<typename T>
    bool operator() ( const T* camera, const T* point, T* residuals ) const {
        T predictions[2];
        
		// projection.h 里的函数
        CamProjectionWithDistortion ( camera, point, predictions );
	
		// measurement() 返回的是测量值 (_measurement) 类型为 Vector2d
        residuals[0] = predictions[0] - T ( measurement() ( 0 ) );
        residuals[1] = predictions[1] - T ( measurement() ( 1 ) );

        return true;
    }


    virtual void linearizeOplus() override {
    	// 用 ceres 的 autodiff 直接求导，否则系统将直接用 g2o 里面的数值求导求 Jacobians
    	// 或者自己算一下 Jacobians ，但因为涉及12个优化量，所以式子会很复杂
    	// 获取两个顶点的指针 
        const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
        const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );

		// 配置 autodiff，模板参数为：Functor（包含 operator() 的类或函数）、所有用到的数据类型、要求导的优化量的维度
		// 这块找不到资料，所以就照着源代码解释了
        typedef ceres::internal::AutoDiff<EdgeObservationBAL, double, VertexCameraBAL::Dimension, VertexPointBAL::Dimension> BalAutoDiff;
		
		// 误差关于相机参数的导数和关于路标位置的 Jacobians
		// 这里为 RowMajor，为了之后映射的时候顺序对应
        Eigen::Matrix<double, Dimension, VertexCameraBAL::Dimension, Eigen::RowMajor> dError_dCamera;
        Eigen::Matrix<double, Dimension, VertexPointBAL::Dimension, Eigen::RowMajor> dError_dPoint;
		// 两个待求导变量的数据 放到 *parameters[] 里
        double *parameters[] = { const_cast<double*> ( cam->estimate().data() ), const_cast<double*> ( point->estimate().data() ) };
        // 两个 Jacobians的指针 放到 *jacobians[]，感觉顺序和上面都是一一对应的
        double *jacobians[] = { dError_dCamera.data(), dError_dPoint.data() };
        // 用来存误差的数据，Dimension 即为在模板里设置的2
        double value[Dimension];
        // 开始自动求导，参数为：Functor，待求导变量数据，误差维度，误差数据，输出jacobians
        bool diffState = BalAutoDiff::Differentiate ( *this, parameters, Dimension, value, jacobians );

        // 复制 Jacobians 这里的 _jacobianOplusXi 是ColMajor的
        if ( diffState ) {
            _jacobianOplusXi = dError_dCamera;
            _jacobianOplusXj = dError_dPoint;
        }
        else {
        	// 报错
            assert ( 0 && "Error while differentiating" );
            _jacobianOplusXi.setZero();
            _jacobianOplusXj.setZero();
        }
    }
};

---

5. 构造 Bundle Adjustment 问题

鲁棒核函数Huber：
$$H(e)=\{\begin{array}{c}\frac{1}{2}{e}^{2}if|e|<\delta \\ \\ \delta (|e|-\frac{1}{2}\delta )otherwise\end{array}$$
g2o_bal_class.h

// BALPromblem储存所有的数据，比如相机位姿、路标位置...
// BundleParams储存所有配置参数，比如线性求解器类型、鲁棒函数...
void BuildProblem(const BALProblem* bal_problem, g2o::SparseOptimizer* optimizer, const BundleParams& params)
{
	// 从BALProblem中读取一系列的数据
    const int num_points = bal_problem->num_points();
    const int num_cameras = bal_problem->num_cameras();
    const int camera_block_size = bal_problem->camera_block_size();
    const int point_block_size = bal_problem->point_block_size();

    // 从BALProblem中读取相机的初始数据
    const double* raw_cameras = bal_problem->cameras();
    for(int i = 0; i < num_cameras; ++i) {
    
    	// ConstVectorRef是之前typedef的一个Eigen映射，把第i个相机的数据映射到VectorXd
        ConstVectorRef temVecCamera(raw_cameras + camera_block_size * i,camera_block_size);
        
        // 建立第i个相机顶点，添加初始估计值和index
        VertexCameraBAL* pCamera = new VertexCameraBAL();
        pCamera->setEstimate(temVecCamera);  
        pCamera->setId(i);                   
  
        // 记得把顶点加到优化器里面
        optimizer->addVertex(pCamera); 
    }

    // 从BALProblem中读取路标的初始数据
    const double* raw_points = bal_problem->points();
    for(int j = 0; j < num_points; ++j) {
    
    	// 建立第j个路标顶点，添加初始估计值和index
    	// 每个顶点的index都是唯一的，故从相机顶点最后开始添加
        ConstVectorRef temVecPoint(raw_points + point_block_size * j, point_block_size);
        VertexPointBAL* pPoint = new VertexPointBAL();
        pPoint->setEstimate(temVecPoint);
        pPoint->setId(j + num_cameras); 

        // 记得把顶点加到优化器里面
        // 这里要把点的稀疏性设为true
        pPoint->setMarginalized(true);
        optimizer->addVertex(pPoint);
    }

    // 设置边，从BALPromblem中读取观测点数据
    const int  num_observations = bal_problem->num_observations();
    const double* observations = bal_problem->observations();   // 指向第一个观测点的指针

    for(int i = 0; i < num_observations; ++i) {
    	// 建立从第i个观测点对应的相机与路标的边
        EdgeObservationBAL* bal_edge = new EdgeObservationBAL();
		
		// 从BALProblem里获取与第i个观测点对应的相机id和路标id
        const int camera_id = bal_problem->camera_index()[i]; // 获得相机id 
        const int point_id = bal_problem->point_index()[i] + num_cameras; // 获得路标id
        
        // 如果设置鲁棒性为true，则边中加入鲁棒函数Huber
        if(params.robustify) {
            g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
            rk->setDelta(1.0);
            bal_edge->setRobustKernel(rk);
        }
        // 设置第i条边连接的相机顶点和路标顶点，即第i个观测点对应的相机id和路标id
        // 路标id前面已经加过相机数了，故直接作为路标顶点id
        bal_edge->setVertex(0,dynamic_cast<VertexCameraBAL*>(optimizer->vertex(camera_id)));
        bal_edge->setVertex(1,dynamic_cast<VertexPointBAL*>(optimizer->vertex(point_id)));
        // 设置信息矩阵，不太懂这是什么。
        bal_edge->setInformation(Eigen::Matrix2d::Identity());
        // 设置测量值
        bal_edge->setMeasurement(Eigen::Vector2d(observations[2*i+0],observations[2*i + 1]));
		// 把边加入到优化器中
        optimizer->addEdge(bal_edge) ;
    }
}

---

6. 配置线性求解器、设置迭代方式

g2o_bal_class.h

void SetSolverOptionsFromFlags(BALProblem* bal_problem, const BundleParams& params, g2o::SparseOptimizer* optimizer) {   
    BalBlockSolver* solver_ptr;

	// 配置线性求解器
    g2o::LinearSolver<BalBlockSolver::PoseMatrixType>* linearSolver = 0;
    
    // 选择用那种线性方程求解器
    if(params.linear_solver == "dense_schur" ) {
        linearSolver = new g2o::LinearSolverDense<BalBlockSolver::PoseMatrixType>();
    }
    else if(params.linear_solver == "sparse_schur") {
        linearSolver = new g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>();
        dynamic_cast<g2o::LinearSolverCholmod<BalBlockSolver::PoseMatrixType>* >(linearSolver)->setBlockOrdering(true);  // AMD ordering , only needed for sparse cholesky solver
    }
    

    solver_ptr = new BalBlockSolver(linearSolver);

    // 设置迭代方式
    g2o::OptimizationAlgorithmWithHessian* solver;
    if(params.trust_region_strategy == "levenberg_marquardt") {
        solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    }
    else if(params.trust_region_strategy == "dogleg") {
        solver = new g2o::OptimizationAlgorithmDogleg(solver_ptr);
    }
    else  {
        std::cout << "Please check your trust_region_strategy parameter again.."<< std::endl;
        exit(EXIT_FAILURE);
    }

    optimizer->setAlgorithm(solver);
}

---

7. 开始求解

g2o_bal_class.h

void SolveProblem(const char* filename, const BundleParams& params) {
    // 从文件读取BALProblem数据
    BALProblem bal_problem(filename);

    // 输出一些信息
    std::cout << "bal problem file loaded..." << std::endl;
    std::cout << "bal problem have " << bal_problem.num_cameras() << " cameras and "
              << bal_problem.num_points() << " points. " << std::endl;
    std::cout << "Forming " << bal_problem.num_observations() << " observatoins. " << std::endl;
	

    std::cout << "beginning problem..." << std::endl;
    
    // 原来有一个添加噪声的代码，感觉这里其实不用也行
    // 把数据归一化，是为了控制场景的大小，归一化函数在BALProblem.cpp中
    bal_problem.Normalize();
    
    // 存储原始的路标点和相机位姿，函数也在BALProblem.cpp中
    if(!params.initial_ply.empty()){
        bal_problem.WriteToPLYFile(params.initial_ply);
    }


    std::cout << "Normalization complete..." << std::endl;

	// 配置线性求解器、迭代方式，构建问题
    g2o::SparseOptimizer optimizer;
    SetSolverOptionsFromFlags(&bal_problem, params, &optimizer);
    BuildProblem(&bal_problem, &optimizer, params);

    std::cout << "begin optimizaiton .."<< std::endl;
    // 开始优化
    optimizer.initializeOptimization();
    optimizer.setVerbose(true);
    optimizer.optimize(params.num_iterations);  // 设置最大迭代次数

    std::cout << "optimization complete.. "<< std::endl;
    
    // 把优化后的数据写回bal_problem里面
    WriteToBALProblem(&bal_problem, &optimizer);

    // 把结果写进ply文件
    if(!params.final_ply.empty()) {
        bal_problem.WriteToPLYFile(params.final_ply);
    } 
}

---

8. 最后的最后

最后介绍一下一些参数、数据读取的类：

BundleParams.h

struct BundleParams {
public:
    BundleParams(int argc, char** argv);
    virtual ~BundleParams(){};
public:
    string input;
    string trust_region_strategy;
    string linear_solver;

    bool robustify;   // 是否使用鲁棒函数
    
    int num_iterations;
    
    // 制造噪声用的
    int random_seed;
    double rotation_sigma;
    double translation_sigma;
    double point_sigma;

    // 点云文件名
    string initial_ply;
    string final_ply;

    CommandArgs arg;
};

 BundleParams::BundleParams(int argc, char** argv)
 {  
    arg.param("input", input, "", "file which will be processed");
    arg.param("trust_region_strategy", trust_region_strategy, "levenberg_marquardt",                            
              "Options are: levenberg_marquardt, dogleg.");
    arg.param("linear_solver", linear_solver, "dense_schur",                             // iterative schur and cgnr(pcg) leave behind...
              "Options are: sparse_schur, dense_schur");
              
    arg.param("robustify", robustify, false, "Use a robust loss function");
    
    arg.param("num_iterations", num_iterations,20, "Number of iterations.");

	// 制造噪声数据
    arg.param("rotation_sigma", rotation_sigma, 0.0, "Standard deviation of camera rotation "
              "perturbation.");
    arg.param("translation_sigma", translation_sigma,0.0, "translation perturbation.");
    arg.param("point_sigma",point_sigma,0.0,"Standard deviation of the point "
              "perturbation.");
    arg.param("random_seed", random_seed, 38401,"Random seed used to set the state ");
    
    // 点云文件名
    arg.param("initial_ply", initial_ply,"initial.ply","Export the BAL file data as a PLY file.");
    arg.param("final_ply", final_ply, "final.ply", "Export the refined BAL file data as a PLY");

    arg.parseArgs(argc, argv);
 }

BALProblem.h

class BALProblem {
public:
	// 从文件读取数据
    explicit BALProblem(const std::string& filename, bool use_quaternions = false);
    ~BALProblem(){
        delete[] point_index_;
        delete[] camera_index_;
        delete[] observations_;
        delete[] parameters_;
    }
	
	// 把数据写入文件
    void WriteToFile(const std::string& filename)const;
    // 把数据写入点云文件
    void WriteToPLYFile(const std::string& filename)const;
	
	// 归一化函数
    void Normalize();
	
	// 添加噪声函数
    void Perturb(const double rotation_sigma,
                 const double translation_sigma,
                 const double point_sigma);
    
    // 返回相机和路标优化维度
    int camera_block_size()             const{ return use_quaternions_? 10 : 9;  }
    int point_block_size()              const{ return 3;                         }

	// 返回相机个数、路标个数、观测点个数、数据个数
	// 数据结构为：相机个数*相机维度、路标个数*路标维度
	// 相机前三维是旋转向量、中间三维是平移向量、后三维是f，k_1，k_2             
    int num_cameras()                   const{ return num_cameras_;              }
    int num_points()                    const{ return num_points_;               }
    int num_observations()              const{ return num_observations_;         }
    int num_parameters()                const{ return num_parameters_;           }

	// 返回第一个路标id的指针、第一个相机id的指针、第一个观测点数据的指针
    const int* point_index()            const{ return point_index_;              }
    const int* camera_index()           const{ return camera_index_;             }
    const double* observations()        const{ return observations_;             }

	// 返回第一个数据（相机数据+路标数据）的指针
    const double* parameters()          const{ return parameters_;               }

	// 返回相机数据的指针、路标数据的指针
    const double* cameras()             const{ return parameters_;               }
    const double* points()              const{ return parameters_ + camera_block_size() * num_cameras_; }

	// 返回可改变的相机数据、路标数据的指针
	// 这里只是为了和之前不能改变的数据区别开来，更加的清晰
    double* mutable_cameras()                { return parameters_;               }
    double* mutable_points()                 { return parameters_ + camera_block_size() * num_cameras_; }
	
	// 返回第i个相机、路标数据，可变或不可变的指针
    double* mutable_camera_for_observation(int i) {
        return mutable_cameras() + camera_index_[i] * camera_block_size();
    }
    
    double* mutable_point_for_observation(int i) {
        return mutable_points() + point_index_[i] * point_block_size();
    }

    const double* camera_for_observation(int i)const {
        return cameras() + camera_index_[i] * camera_block_size();
    }

    const double* point_for_observation(int i)const {
        return points() + point_index_[i] * point_block_size();
    }


private:
    void CameraToAngelAxisAndCenter(const double* camera,
                                    double* angle_axis,
                                    double* center)const;

    void AngleAxisAndCenterToCamera(const double* angle_axis,
                                    const double* center,
                                    double* camera)const;

    int num_cameras_;
    int num_points_;
    int num_observations_;
    int num_parameters_;
    bool use_quaternions_;

	// 路标id、相机id、观测点数据、所有数据
    int* point_index_;
    int* camera_index_;
    double* observations_;
    double* parameters_; 

};

还有一个command_args.h里面的类就不介绍了，和问题没有太大的关系，这个类的主要作用是解析输入的参数。

翻到一篇很好的文章：《视觉SLAM十四讲》中第十章代码中雅克比的具体形式推导。