【视觉SLAM十四讲源码解读】BA问题用g2o求解重新投影误差优化相机位姿和路标点

前言

视觉SLAM十四讲系列之第十讲g2o_bundle.cpp源码解读

编译运行

mkdir build
cd build
cmake ..
make

cd build

./g2o_customBundle -input /home/q/projects/slambook/ch10/g2o_custombundle/data/problem-16-22106-pre.txt

./g2o_customBundle -help

-final_ply <string>               Export the refined BAL file data as a PLY (default: final.ply)
-initial_ply <string>             Export the BAL file data as a PLY file. (default: initial.ply)
-input <string>                   file which will be processed
-linear_solver <string>           Options are: sparse_schur, dense_schur (default: dense_schur)
-num_iterations <int>             Number of iterations. (default: 20)
-point_sigma <double>             Standard deviation of the point perturbation. (default: 0)
-random_seed <int>                Random seed used to set the state  (default: 38401)
-robustify                        Use a robust loss function
-rotation_sigma <double>          Standard deviation of camera rotation perturbation. (default: 0)
-translation_sigma <double>       translation perturbation. (default: 0)
-trust_region_strategy <string>   Options are: levenberg_marquardt, dogleg. (default: levenberg_marquardt)

初始值

meshlab initial.ply 

优化值
meshlab final.ply

算法解析

• 命令行参数和文件参数处理
  
• 生成随机数，位姿的角轴和四元数表示的互相转换
  
• 相机９各维度参数的含义和计算
  
• 调用参数解析
  
• 将数据导出到PLY文件通过Meshlab软件可查看点云
  
• 添加顶点、边、观测方程
  

BuildProblem():
主要作用是往g2o中增加顶点和边，并将初始值添加进去。

SetSolverOptionsFromFlags():
主要作用就是构造优化器

g2o常用路数
1、typedef块求解器的维度。
2、用线性求解器构造块求解器。
3、用块求解器构造下降策略。
4、将下降策略设定为optimizer的setAlgorithm()。

源码解读

g2o_bundle.cpp

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "g2o/stuff/sampler.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/batch_stats.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/optimization_algorithm_dogleg.h"
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/types/sba/types_six_dof_expmap.h"
#include "g2o/solvers/structure_only/structure_only_solver.h"
#include "common/BundleParams.h"
#include "common/BALProblem.h"
#include "g2o_bal_class.h"

using namespace Eigen;
using namespace std;

typedef Eigen::Map<Eigen::VectorXd> VectorRef;
typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;
typedef g2o::BlockSolver<g2o::BlockSolverTraits<9,3> > BalBlockSolver;// 块求解器:pose的维度为9维，landmark维度为3维


void BuildProblem(const BALProblem* bal_problem, g2o::SparseOptimizer* optimizer, const BundleParams& params)
{
    // 读取数据
    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();

    // Set camera vertex with initial value in the dataset.
    const double* raw_cameras = bal_problem->cameras();
    for(int i = 0; i < num_cameras; ++i)
    {
        //将9维相机位姿从数组中取出来构建成矩阵，用于下面的顶点的初始值赋值
        ConstVectorRef temVecCamera(raw_cameras + camera_block_size * i,camera_block_size);
        //相机顶点类指针
        VertexCameraBAL* pCamera = new VertexCameraBAL();
        pCamera->setEstimate(temVecCamera);   // initial value for the camera i..
        pCamera->setId(i);                    // set id for each camera vertex 
  
        // remeber to add vertex into optimizer..
        optimizer->addVertex(pCamera);
        
    }

    // Set point vertex with initial value in the dataset. 
    const double* raw_points = bal_problem->points();
    // const int point_block_size = bal_problem->point_block_size();
    for(int j = 0; j < num_points; ++j)
    {
        // 将数组中的路标数据读出构建成矩阵
        ConstVectorRef temVecPoint(raw_points + point_block_size * j, point_block_size);
        // 路标顶点指针
        VertexPointBAL* pPoint = new VertexPointBAL();
        pPoint->setEstimate(temVecPoint);   // initial value for the point i..
        pPoint->setId(j + num_cameras);     // each vertex should have an unique id, no matter it is a camera vertex, or a point vertex 

        // remeber to add vertex into optimizer..
        pPoint->setMarginalized(true);
        optimizer->addVertex(pPoint);
    }

    // Set edges for graph..
    const int  num_observations = bal_problem->num_observations();
    const double* observations = bal_problem->observations();   // pointer for the first observation..

    // 用观测个数控制循环，来添加所有的边
    for(int i = 0; i < num_observations; ++i)
    {
        // 内存指针
        EdgeObservationBAL* bal_edge = new EdgeObservationBAL();

        // 
        const int camera_id = bal_problem->camera_index()[i]; // get id for the camera; 
        const int point_id = bal_problem->point_index()[i] + num_cameras; // get id for the point 
        
        if(params.robustify)
        {
            g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
            rk->setDelta(1.0);
            bal_edge->setRobustKernel(rk);
        }
        // set the vertex by the ids for an edge observation
        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) ;
    }

}


void WriteToBALProblem(BALProblem* bal_problem, g2o::SparseOptimizer* optimizer){
    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();
    double* raw_cameras = bal_problem->mutable_cameras();
    for(int i = 0; i < num_cameras; ++i){
        VertexCameraBAL* pCamera = dynamic_cast<VertexCameraBAL*>(optimizer->vertex(i));
        Eigen::VectorXd NewCameraVec = pCamera->estimate();
        memcpy(raw_cameras + i * camera_block_size, NewCameraVec.data(), sizeof(double) * camera_block_size);
    }
    double* raw_points = bal_problem->mutable_points();
    for(int j = 0; j < num_points; ++j){
        VertexPointBAL* pPoint = dynamic_cast<VertexPointBAL*>(optimizer->vertex(j + num_cameras));
        Eigen::Vector3d NewPointVec = pPoint->estimate();
        memcpy(raw_points + j * point_block_size, NewPointVec.data(), sizeof(double) * point_block_size);
    }
}


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);
    //SetLinearSolver(solver_ptr, params);

    //SetMinimizerOptions(solver_ptr, params, optimizer);
    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);
}


void SolveProblem(const char* filename, const BundleParams& params)
{
    BALProblem bal_problem(filename);
    // show some information here ...
    std::cout << "\nbal 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;
    // store the initial 3D cloud points and camera pose..
    if(!params.initial_ply.empty()){bal_problem.WriteToPLYFile(params.initial_ply);}
    std::cout << "beginning problem..." << std::endl;
    // add some noise for the intial value
    srand(params.random_seed);
    bal_problem.Normalize();
    bal_problem.Perturb(params.rotation_sigma,params.translation_sigma,params.point_sigma);
    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;
    // perform the optimizaiton 
    optimizer.initializeOptimization();
    optimizer.setVerbose(true);
    optimizer.optimize(params.num_iterations);

    std::cout << "optimization complete... "<< std::endl;
    // write the optimized data into BALProblem class
    WriteToBALProblem(&bal_problem, &optimizer);

    // write the result into a .ply file.
    if(!params.final_ply.empty()){bal_problem.WriteToPLYFile(params.final_ply);}}


int main(int argc, char** argv){
    BundleParams params(argc,argv);  // set the parameters here.
    if(params.input.empty()){
        std::cout << "Usage: bundle_adjuster -input ";
        return 1;
    }
    SolveProblem(params.input.c_str(), params);
    return 0;
}

第二版 g2o_bal_class.h

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include "g2o/stuff/sampler.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/solver.h"
#include "g2o/core/robust_kernel_impl.h"
#include "g2o/core/batch_stats.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/optimization_algorithm_dogleg.h"
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/solvers/dense/linear_solver_dense.h"
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
#include "g2o/types/sba/types_six_dof_expmap.h"
#include "g2o/solvers/structure_only/structure_only_solver.h"
#include "common/BundleParams.h"
#include "common/BALProblem.h"
#include "g2o_bal_class.h"

using namespace Eigen;
using namespace std;

typedef Eigen::Map<Eigen::VectorXd> VectorRef;
typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;
typedef g2o::BlockSolver<g2o::BlockSolverTraits<9,3> > BalBlockSolver;// 块求解器:pose的维度为9维，landmark维度为3维


void BuildProblem(const BALProblem* bal_problem, g2o::SparseOptimizer* optimizer, const BundleParams& params)
{
    // 读取数据
    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();

    // Set camera vertex with initial value in the dataset.
    const double* raw_cameras = bal_problem->cameras();
    for(int i = 0; i < num_cameras; ++i)
    {
        //将9维相机位姿从数组中取出来构建成矩阵，用于下面的顶点的初始值赋值
        ConstVectorRef temVecCamera(raw_cameras + camera_block_size * i,camera_block_size);
        //相机顶点类指针
        VertexCameraBAL* pCamera = new VertexCameraBAL();
        pCamera->setEstimate(temVecCamera);   // initial value for the camera i..
        pCamera->setId(i);                    // set id for each camera vertex 
  
        // remeber to add vertex into optimizer..
        optimizer->addVertex(pCamera);
        
    }

    // Set point vertex with initial value in the dataset. 
    const double* raw_points = bal_problem->points();
    // const int point_block_size = bal_problem->point_block_size();
    for(int j = 0; j < num_points; ++j)
    {
        // 将数组中的路标数据读出构建成矩阵
        ConstVectorRef temVecPoint(raw_points + point_block_size * j, point_block_size);
        // 路标顶点指针
        VertexPointBAL* pPoint = new VertexPointBAL();
        pPoint->setEstimate(temVecPoint);   // initial value for the point i..
        pPoint->setId(j + num_cameras);     // each vertex should have an unique id, no matter it is a camera vertex, or a point vertex 

        // remeber to add vertex into optimizer..
        pPoint->setMarginalized(true);
        optimizer->addVertex(pPoint);
    }

    // Set edges for graph..
    const int  num_observations = bal_problem->num_observations();
    const double* observations = bal_problem->observations();   // pointer for the first observation..

    // 用观测个数控制循环，来添加所有的边
    for(int i = 0; i < num_observations; ++i)
    {
        // 内存指针
        EdgeObservationBAL* bal_edge = new EdgeObservationBAL();

        // 
        const int camera_id = bal_problem->camera_index()[i]; // get id for the camera; 
        const int point_id = bal_problem->point_index()[i] + num_cameras; // get id for the point 
        
        if(params.robustify)
        {
            g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
            rk->setDelta(1.0);
            bal_edge->setRobustKernel(rk);
        }
        // set the vertex by the ids for an edge observation
        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) ;
    }

}


void WriteToBALProblem(BALProblem* bal_problem, g2o::SparseOptimizer* optimizer){
    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();
    double* raw_cameras = bal_problem->mutable_cameras();
    for(int i = 0; i < num_cameras; ++i){
        VertexCameraBAL* pCamera = dynamic_cast<VertexCameraBAL*>(optimizer->vertex(i));
        Eigen::VectorXd NewCameraVec = pCamera->estimate();
        memcpy(raw_cameras + i * camera_block_size, NewCameraVec.data(), sizeof(double) * camera_block_size);
    }
    double* raw_points = bal_problem->mutable_points();
    for(int j = 0; j < num_points; ++j){
        VertexPointBAL* pPoint = dynamic_cast<VertexPointBAL*>(optimizer->vertex(j + num_cameras));
        Eigen::Vector3d NewPointVec = pPoint->estimate();
        memcpy(raw_points + j * point_block_size, NewPointVec.data(), sizeof(double) * point_block_size);
    }
}


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);
    //SetLinearSolver(solver_ptr, params);

    //SetMinimizerOptions(solver_ptr, params, optimizer);
    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);
}


void SolveProblem(const char* filename, const BundleParams& params)
{
    BALProblem bal_problem(filename);
    // show some information here ...
    std::cout << "\nbal 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;
    // store the initial 3D cloud points and camera pose..
    if(!params.initial_ply.empty()){bal_problem.WriteToPLYFile(params.initial_ply);}
    std::cout << "beginning problem..." << std::endl;
    // add some noise for the intial value
    srand(params.random_seed);
    bal_problem.Normalize();
    bal_problem.Perturb(params.rotation_sigma,params.translation_sigma,params.point_sigma);
    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;
    // perform the optimizaiton 
    optimizer.initializeOptimization();
    optimizer.setVerbose(true);
    optimizer.optimize(params.num_iterations);

    std::cout << "optimization complete... "<< std::endl;
    // write the optimized data into BALProblem class
    WriteToBALProblem(&bal_problem, &optimizer);

    // write the result into a .ply file.
    if(!params.final_ply.empty()){bal_problem.WriteToPLYFile(params.final_ply);}}


int main(int argc, char** argv){
    BundleParams params(argc,argv);  // set the parameters here.
    if(params.input.empty()){
        std::cout << "Usage: bundle_adjuster -input ";
        return 1;
    }
    SolveProblem(params.input.c_str(), params);
    return 0;
}

第一版 g2o_bal_class.h

#include 
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_binary_edge.h"

#include "ceres/autodiff.h"

#include "tools/rotation.h"
#include "common/projection.h"


// 定义g2o优化变量顶点相机位姿 <相机位姿维度,相机位姿类型>
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 ){
        Eigen::VectorXd::ConstMapType v ( update, VertexCameraBAL::Dimension );
        // 顶点相机位姿估计值 += 增量
        _estimate += v;
    }

};

// 定义g2o优化变量顶点路标点 <路标点维度, 路标点类型>
class VertexPointBAL : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    VertexPointBAL() {}
    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 ){
        Eigen::Vector3d::ConstMapType v ( update );
        // 顶点路标点估计值 += 增量
        _estimate += v;
    }
};

// 定义g2o误差边重投影误差 <误差维度2维,误差类型,顶点相机位姿,顶点路标点>
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;
    }
    // 误差边重投影误差计算函数
    virtual void computeError() override{
        // 取出第0个顶点相机位姿
        const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
        // 取出第1个顶点路标点
        const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );
        // 重载的()运算符输入相机位姿估计值和路标点估计值输出误差
        ( *this ) ( cam->estimate().data(), point->estimate().data(), _error.data() );

    }

    // 模板类实现重投影误差
    template<typename T>
    bool operator() ( const T* camera, const T* point, T* residuals ) const{
        // 根据相机内外参和路标点重投影得到像素坐标的估计值
        T predictions[2];
        CamProjectionWithDistortion ( camera, point, predictions );
        // 误差 = 估计值 - 观测值(真实值)
        residuals[0] = predictions[0] - T ( measurement() ( 0 ) );
        residuals[1] = predictions[1] - T ( measurement() ( 1 ) );
        return true;
    }


    // 重写线性增量方程雅克比矩阵
    virtual void linearizeOplus() override
    {
        // 使用ceres的自动求导，不调用g2o的数值求导
        // 取出第0个顶点相机位姿
        const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
        // 取出第1个顶点路标点
        const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );

        typedef ceres::internal::AutoDiff<EdgeObservationBAL, double, VertexCameraBAL::Dimension, VertexPointBAL::Dimension> BalAutoDiff;
        // 误差对相机位姿的导数 2*9
        Eigen::Matrix<double, Dimension, VertexCameraBAL::Dimension, Eigen::RowMajor> dError_dCamera;
        // 误差对路标点的导数 2*3
        Eigen::Matrix<double, Dimension, VertexPointBAL::Dimension, Eigen::RowMajor> dError_dPoint;
        // 相机估位姿估计值数组指针和路标点估计值数组指针
        double *parameters[] = { const_cast<double*> ( cam->estimate().data() ), const_cast<double*> ( point->estimate().data() ) };
        // 雅克比矩阵为两块导数拼合起来的，一块是误差对相机估位姿的导数，一块是误差路标点的导数
        double *jacobians[] = { dError_dCamera.data(), dError_dPoint.data() };
        double value[Dimension];
        bool diffState = BalAutoDiff::Differentiate ( *this, parameters, Dimension, value, jacobians );
        if ( diffState ){
            _jacobianOplusXi = dError_dCamera;
            _jacobianOplusXj = dError_dPoint;
        }
        else{
            assert ( 0 && "Error while differentiating" );
            _jacobianOplusXi.setZero();
            _jacobianOplusXj.setZero();
        }
    }
};