vins-mono中的初始化详解(2)

目录

如何用Ceres库进行非线性最小二乘求解

1.前言

2.初始化中优化问题的构建

3.使用ceres进行求解的流程

4.注意事项


如何用Ceres库进行非线性最小二乘求解

1.前言

        本篇博文主要介绍在vins-mono中如何用ceres对其中的优化问题进行求解,包括vins初始化中最小二乘问题的构建、ceres求解问题的步骤以及使用过程中的一些解释说明。本篇内容是对上一篇内容的一部分补充,对vins初始化还不清楚的可以参考一下我的上一篇博文:vins-mono中的初始化详解(1)-CSDN博客文章浏览阅读97次。本文详细介绍vins-mono在初始化阶段完成的任务,以及如何使用ceres进行优化问题的求解。https://blog.csdn.net/weixin_44587828/article/details/134106398?spm=1001.2014.3001.5501

        接下来就闲言少叙,直奔主题。

2.初始化中优化问题的构建

        在vins中用到ceres求解的部分是GlobalSFM::construct函数里面,该函数是在获得枢纽帧和滑窗内的最后一帧位姿后进行处理,具体功能是通过三角化地图点+pnp得到all_image_frame里面的所有图像帧的位姿以及所有可以被两帧以上图像观测到的地图点的3d坐标。获得这些信息后,为了让这些状态量更加准确,通过ceres solver进行优化。

        构建问题的基本思想是重投影误差,就是三角化出的3d点经过外参矩阵变换投影到2d像素平面上,这样得到的点的坐标理论上和通过光流追踪得到的同一特征点的像素坐标不应该相差过大,据此就可以构建一个包含外参和3d点的最小二乘方程,求解该最小二乘方程就可以得到置信度更高的位姿和3d点坐标。

        具体构建的实现代码在下文中有所体现。

3.使用ceres进行求解的流程

        在使用ceres进行求解优化问题时,首先得创建一个问题对象,ceres::Problem ,创建好对象后,要进行以下操作:

  • 问题对象中添加参数块(不是必须的)   

        对于一般最小二乘优化问题,如果优化的参数符合正常的“+”,比如地图点的3d坐标,那么该部分无需添加参数块,在添加残差块时候参数块也会被添加进去。但是对于处于流形空间的参数,比如位姿并不符合普通的加法,严格意义讲,对于位姿中的旋转姿态不符合常规的加法。这种需要进行广义加法才能更新状态量的情况需要添加参数块。

        在ceres2.20以前,通常是自定义一个子类,以public方式继承ceres::LocalParameterization基类,在函数体内部重写Plus函数,该函数要用virtual关键字,从而实现多态。在Plus函数体内部,要对自定义的加法进行实现。除了重写Plus函数外,还有Compute Jacobian , MultiplyByJacobian, GlobalSize和LocalSize五个函数,但是并不一定需要都重新定义这些函数。除了自定义子类实现自定义加法外,对于一般四元数,也可以直接使用ceres提供的new ceres::QuaternionParameterization()然后用ceres::LocalParameterization*的对象去接收该指针即可。然后在添加参数块时把该对象作为参数传入。

        那么,有一个问题,同样是计算位姿相关的优化问题,什么时候我去自己定义一个子类然后重写加法,什么时候我直接用ceres自己的四元数计算的对象呢?其实在vins中我们也可以得到该问题的答案,那就是如果旋转作为一个独立的参数块准备进行优化那就没有必要自己再去重新定义了,如果待优化的参数块和旋转融合到了一起,有的部分可以直接加,有的部分又得使用广义的加,此时重写这个Plus就变得十分实用了。在vins中初始化部分旋转作为一个独立的参数块进行优化,他就用的ceres自己的计算四元数的对象,而在后端优化中,位姿绑定在一起作为一个参数块,这里面的位移可以直接加而旋转不能直接加,作者就重写了自定义的LocalParameterization。

        在ceres2.2.0以后,LocalParameterization接口被取缔,使用Manifold替代,具体使用方法可以参考ceres solver官网里面的说明:

Modeling Non-linear Least Squares — Ceres Solvericon-default.png?t=N7T8http://ceres-solver.org/nnls_modeling.html#_CPPv4N5ceres8ManifoldE

  • 问题对象中添加残差块

        该部分至关重要,是使用ceres求解优化问题的关键部分。主要内容分为两部分,创建CostFunction和鲁棒核函数

        在介绍如何创建CostFunction之前介绍ceres进行优化过程中几种求导方式。我们知道,对于优化问题的数值求解,基本都是找增量然后迭代的过程,而增量的选取几乎都离不开一阶导数,也就是Jaconbian矩阵,在ceres中,求导大体分为3类:自动求导、数值求导和解析求导。其中,前两种求导方式是ceres自己完成的,解析求导需要用户自己定义雅克比矩阵。

        先来介绍这3中求导的特点和在构建CostFunction时候的特点。

        自动求导采用的是链式求导,也是ceres官方比较推崇的一种求导方式,对于中小型最优化问题处理起来行云流水。在用自动求导时,构建的残差函数(CostFunction)需要自己创建一个计算残差的类(或结构体),该类需要以模板形式重载小括号,返回值为bool类型。类内实现就是残差的计算,如下代码所示:


struct ReprojectionError3D
{
	ReprojectionError3D(double observed_u, double observed_v)
		:observed_u(observed_u), observed_v(observed_v)
		{}
	//必须以模板方式重载括号 括号内参数分别是待优化变量和残差,残差一定在最后
	//函数主要是如何计算残差
	template <typename T>
	bool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const
	{
		T p[3];
		ceres::QuaternionRotatePoint(camera_R, point, p);//3d点坐标在相机坐标系下的坐标p
		p[0] += camera_T[0]; p[1] += camera_T[1]; p[2] += camera_T[2];//相当于Rp+t
		//放到归一化平面
		T xp = p[0] / p[2];
    	T yp = p[1] / p[2];
		//计算残差
    	residuals[0] = xp - T(observed_u);
    	residuals[1] = yp - T(observed_v);
    	return true;
	}

	static ceres::CostFunction* Create(const double observed_x,
	                                   const double observed_y) 
	{
	  return (new ceres::AutoDiffCostFunction<//表示自动求导
	          ReprojectionError3D, 2, 4, 3, 3>(//分别是残差维度 待优化变量维度 顺序和重载小括号参数一致
	          	new ReprojectionError3D(observed_x,observed_y)));
	}

	double observed_u;
	double observed_v;
};

        该类中对于小括号的重载形参列表为待优化参数块和残差块。对于初始化问题,待优化参数块有相机的相对于枢纽帧的位姿和3d地图点,位姿分为旋转和平移,维数分别是4,3地图点的维数是3,残差维度是2。注意这里的数字,在Create函数中有用。关于残差的计算比较简单,代码中也有相关注释,在此不再赘述。

        可以看到,该类中除了计算残差,还有一个Create函数,该函数主要返回一个ceres::AutoDiffCostFunction的对象,从名字可以看出这是一个自动求导类型。模板参数为CostFunction的类名,残差维度,参数块维度。注意这里的顺序要和前面重载小括号里面的参数顺序一致,只不过残差维数放前面。如此,关于自动求导的CostFunction就构建完成了。

        解析求导,顾名思义就是自己定义出损失函数关于待优化参数的雅克比矩阵。对于有实时性要求的大规模优化问题解析求导通常可以选择此种方式。总体上看计算效率要比自动求导快很多,不过具体能相差多少笔者也没有进行实验,这里主要介绍解析求导如何构建CostFunction。

        与自动求导对比,构建CostFunction需要重新定义一个类,该类需要继承ceres::SizedCostFunction或者ceres::CostFunction,二者的区别是你是否能够确定残差的维度和需要优化的参数的维度。继承前者时候,需要跟随模板参数,分别对应残差维度和参数块维度。下面是vins后端优化构建imu相关CostFunction。

class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>//解析求导 残差维度是15维 两个图像帧位姿都是7维 速度和残差都是9维
{
  public:
    IMUFactor() = delete;
    IMUFactor(IntegrationBase* _pre_integration):pre_integration(_pre_integration)
    {
    }
    //在evaluate中写出残差计算方式和雅克比矩阵
    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
    {

        Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
        Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

        Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
        Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);
        Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);

        Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);
        Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);

        Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);
        Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);
        Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);

#if 0
        if ((Bai - pre_integration->linearized_ba).norm() > 0.10 ||
            (Bgi - pre_integration->linearized_bg).norm() > 0.01)
        {
            pre_integration->repropagate(Bai, Bgi);
        }
#endif

        Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);  //map相当于引用 将double类型的数组映射成为Eigen类型的矩阵
        residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                            Pj, Qj, Vj, Baj, Bgj);
        //ceres没有设置信息矩阵接口,处理办法是将信息矩阵(协方差矩阵)直接乘到残差上面 这里采用了LLT分解 相当于开根号 
        //取了协方差矩阵的逆表示协方差越大认为越不可信
        Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
        //sqrt_info.setIdentity();
        residual = sqrt_info * residual;

        if (jacobians)
        {
            double sum_dt = pre_integration->sum_dt;
            Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA);
            Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG);

            Eigen::Matrix3d dq_dbg = pre_integration->jacobian.template block<3, 3>(O_R, O_BG);

            Eigen::Matrix3d dv_dba = pre_integration->jacobian.template block<3, 3>(O_V, O_BA);
            Eigen::Matrix3d dv_dbg = pre_integration->jacobian.template block<3, 3>(O_V, O_BG);

            if (pre_integration->jacobian.maxCoeff() > 1e8 || pre_integration->jacobian.minCoeff() < -1e8)
            {
                ROS_WARN("numerical unstable in preintegration");
                //std::cout << pre_integration->jacobian << std::endl;
///                ROS_BREAK();
            }

            if (jacobians[0])//预积分残差
            {
                Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
                jacobian_pose_i.setZero();
                //预积分位置残差对于位置求导
                jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
                jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));

#if 0
            jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();
#else
                //预积分位置残差对于旋转求导
                //先根据零偏修正旋转量
                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
#endif

                jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));
                //因为前面残差也乘以信息矩阵
                jacobian_pose_i = sqrt_info * jacobian_pose_i;

                if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8)
                {
                    ROS_WARN("numerical unstable in preintegration");
                    //std::cout << sqrt_info << std::endl;
                    //ROS_BREAK();
                }
            }
            if (jacobians[1])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_i(jacobians[1]);
                jacobian_speedbias_i.setZero();
                jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt;
                jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;
                jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;

#if 0
            jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg;
#else
                //Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                //jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * corrected_delta_q).bottomRightCorner<3, 3>() * dq_dbg;
                jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * pre_integration->delta_q).bottomRightCorner<3, 3>() * dq_dbg;
#endif

                jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();
                jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;
                jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;

                jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();

                jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();

                jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;

                //ROS_ASSERT(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8);
                //ROS_ASSERT(fabs(jacobian_speedbias_i.minCoeff()) < 1e8);
            }
            if (jacobians[2])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[2]);
                jacobian_pose_j.setZero();

                jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();

#if 0
            jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();
#else
                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>();
#endif

                jacobian_pose_j = sqrt_info * jacobian_pose_j;

                //ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8);
                //ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8);
            }
            if (jacobians[3])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_j(jacobians[3]);
                jacobian_speedbias_j.setZero();

                jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();

                jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();

                jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();

                jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;

                //ROS_ASSERT(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8);
                //ROS_ASSERT(fabs(jacobian_speedbias_j.minCoeff()) < 1e8);
            }
        }

        return true;
    }
    IntegrationBase* pre_integration;

};

        可以看到,自动求导没有重载小括号,而是重写了Evaluate函数,并用virtual关键字声明,目的是能够实现多态。在Evaluate函数中除了计算残差外还加入了计算雅克比的内容。而且并没有使用模板类型重写Evaluate函数,在形参列表中,多了一个雅克比项,参数块和雅克比都是二维数组,残差为一维数组,这些是和自动求导中不一样的地方。

        可以看到,上述代码中并没有create函数,以及,上述代码中将信息矩阵融入到残差计算里面,这是因为ceres并没有专门的信息矩阵接口供用户调用。

        数值求导很不常用,其具体用法和自动求导大致相同,但是重载小括号时候用的不是模板,而是特定的数据类型。官方文档关于这个求导方式应用场景的解释是,当计算残差时候,有些库函数不能使用模板数据类型,此时,该类型求导就有了用武之地。但是,官方对于该种求导方式极不推荐,计算慢、易出错、不容易收敛。既然家长都认为这孩子不提气,具体使用就不再详细介绍了。

        除了这常见的3中求导方式外,ceres中还有DynamicAutoDiffCostFunction、DynamicNumericDiffCostFunction、CostFunctionToFunctor等。每种都有对应的应用场景,只不过在slam中不太常用。有的是应用在没办法在编译阶段确定维度的,有的又是其他的场景,详细介绍还得看官网教程。

      鲁棒核函数:

       所谓鲁棒核函数是为了限制外点对于优化结果的负面影响。最小二乘问题本来就是一种折中策略,让最后求解的参数离所有的状态综合距离越小越好。那么如果有一些坏点,比如说传感器数据因为存在干扰产生突变,又或者图像特征点匹配出错了,这种问题会导致他们产生一个非常大的误差,如果不进行处理,那么所有的状态都要迁就这些错误的点,从而导致得到的优化数据并不是最优。鲁棒核函数就是降低这些敌对分子的影响,让这些外点产生较大误差时候采用别的数学计算,而其他的优秀群众得到的误差进行平方。

        常用的鲁棒核函数具体表现如下图:

        介绍完如何构建CostFunction和鲁棒核函数后,就可以向问题对象中添加残差块了。

        对于imu残差块添加如下:

 IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
        problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);

        可以看到,添加残差块函数第一个参数是计算残差和雅克比的CostFunction,第二个参数是鲁棒核函数,剩余参数是待优化参数。

        对于图像重投影残差:

 ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
                problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);

        可以看到,相比较imu多了鲁棒核函数。

        自动求导添加残差块也一样操作。在处理优化问题时,可以设置某些变量不去优化,ceres也有对应的接口函数:

problem.SetParameterBlockConstant(c_translation[i]); 
  • 配置求解器求解

        在做好以上准备工作就可以配置求解器进行求解该优化问题。

        求解器对象是ceres::Solver::Options,通过它可以配置很多优化求解的配置,比如说优化求解方式、最大迭代时间等。

        主要介绍一下求解器的类型:

        求解器类型包括DENSE_QR、DENSE_NORMAL_CHOLESKY 、SPARSE_NORMAL_CHOLESKY、DENSE_SCHUR 、 SPARSE_SCHUR五种。对于小规模的(残差维数小于1000)的优化问题,如果雅可比矩阵是稠密的就选择DENSE_QR,对于中大型的优化问题,根据雅克比稠密或者稀疏可以分别使用DENSE_NORMAL_CHOLESKY 和SPARSE_NORMAL_CHOLESKY,而对于slam问题可以使用DENSE_SCHUR 、 SPARSE_SCHUR,后者效率更高,具体原因可以参考slam14讲边缘化部分。

        在完成优化主体任务后,可以将优化日志打印输出。比较简单,在此不展开说明。优化器配置和报告输出代码参考如下:

ceres::Solver::Options options;
	options.linear_solver_type = ceres::DENSE_SCHUR;//优化求解的方式
	//options.minimizer_progress_to_stdout = true;
	options.max_solver_time_in_seconds = 0.2;//最大迭代时间
	ceres::Solver::Summary summary;
	ceres::Solve(options, &problem, &summary);
	//判断优化结束条件是收敛
	if (summary.termination_type == ceres::CONVERGENCE || summary.final_cost < 5e-03)
	{
		//cout << "vision only BA converge" << endl;
	}
	else
	{
		//cout << "vision only BA not converge " << endl;
		return false;
	}

4.注意事项

  • ceres优化的参数块数据类型是double,有时候优化前后要进行数据格式转换;
  • 在添加残差块和定义CostFunction时候参数顺序要一致;    

        至此,本文全部内容已经结束。

5.参考文献

  • T. Qin, P. Li and S. Shen, "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator," in IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004-1020, Aug. 2018, doi: 10.1109/TRO.2018.2853729.
  • Non-linear Least Squares — Ceres Solver
  • 高翔, 张涛, 刘毅. 视觉 SLAM 十四讲———从理论到实践[M]. 北京: 电子工业出版社, 2017
  • 5
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 5
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 5
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值