ceres小结 -- vins为例

状态不好估计

已于 2023-02-27 20:13:48 修改

阅读量739

点赞数 1

文章标签：学习

于 2023-02-14 15:36:46 首次发布

本文链接：https://blog.csdn.net/qq_45520011/article/details/129027874

版权

从 typora 复制的,排版有问题,见谅

在estimator.cpp文件里这个函数 void Estimator::optimization()

1 声明problem

ceres::Problem problem

2 引入核函数loss_function

ceres::LossFunction *loss_function;// 核函数

loss_function = new ceres::CauchyLoss(1.0);//柯西

3 添加参数块 problem.AddParameterBlock()

voidAddParameterBlock(double*values, int size); // values表示优化变量的指针，size表示优化变量的维度

// 显式 自己定义加法
voidAddParameterBlock( double*values, 
                    int size,
               LocalParameterization*local_parameterization); // slam 四元数旋转过参数化的时候用这个

位姿需要自己定义加法 ceres::LocalParameterization 是ceres自带的类, 自定义加法要自己写一个 PoseLocalParameterization 类继承它

ceres::LocalParameterization*local_parameterization=

                                   new PoseLocalParameterization();//自己写PoseLocalParameterization

vins 是在 pose_local_parameterization.h 这个文件里定义了这个类

classPoseLocalParameterization : publicceres::LocalParameterization

{

virtualboolPlus(constdouble*x, constdouble*delta, double*x_plus_delta) const;// 自定义加法

virtualboolComputeJacobian(constdouble*x, double*jacobian) const;// 计算雅克比

virtualintGlobalSize() const { return 7; };//变量的维数

virtualintLocalSize() const { return 6; };//优化的维数

};

在 pose_local_parameterization.cpp 实现了 plus() 和 ComputeJacobian()

bool PoseLocalParameterization::Plus(const double*x, const double*delta, double*x_plus_delta) const

{
 
Eigen::Map<const Eigen::Vector3d>_p(x); // 把double 数组 映射(Map)成vector 能更方便的使用eigen x是指针 3d 自动取三维

Eigen::Map<const Eigen::Quaterniond>_q(x+3);

Eigen::Map<const Eigen::Vector3d> dp(delta);

Eigen::Quaterniond dq=Utility::deltaQ(Eigen::Map<constEigen::Vector3d>(delta+3)); // 旋转向量映射成四元数

Eigen::Map<Eigen::Vector3d>p(x_plus_delta);

Eigen::Map<Eigen::Quaterniond>q(x_plus_delta+3);

p=_p+dp;

q= (_q*dq).normalized();// 四元数乘法 (重载了*) , 归一化

return true;

}

bool PoseLocalParameterization::ComputeJacobian(constdouble*x, double*jacobian) const

{

Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>>j(jacobian);

j.topRows<6>().setIdentity();

j.bottomRows<1>().setZero();

return true;

}

4 添加残差

有两种形式

// 需要提供三种参数 —— cost_function对象、鲁棒核函数对象、 该残差的关联参数 。

problem.AddResidualBlock( cost_function对象、
                            鲁棒核函数对象、
                            该残差的关联参数。)



//1
template<typename... Ts>
ResidualBlockIdAddResidualBlock(CostFunction*cost_function,
                            LossFunction*loss_function,
                            double*x0,// 依次传入所有参数的指针double*<double*>
                            Ts*... xs)



//2
ResidualBlockIdAddResidualBlock(
                                CostFunction*cost_function,
                                LossFunction*loss_function,
                                conststd::vector<double*>&parameter_blocks); //**一次性传入所有参数的指针容器vector<double*>

vins 中 imu 为例 , 采用的 // 2

// AddResidualBlock 参数 : cost function, loss function(核函数)NULL 表示不使用, 相关的参数块
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);

!!! CostFunction* cost_function

这个需要定义残差的计算方式, 如果是解析求导,还要给出雅克比的计算方式

这里的使用了仿函数的技巧，即在CostFunction结构体内，对（）进行重载，这样的话，该结构体的一个实例就能具有类似一个函数的性质，在代码编写过程中就能当做一个函数一样来使用。

需要自己定义一个结构体或者类, 如果知道残差维数, 就继承SizedCostFunction, 例如imu 视觉重投影, 如果不知到维数, 例如路标, 就继承CostFunction

如果使用解析求导, 必须重载Evaluate() 函数, 所以类里面定义了这个函数,分块计算了雅克比矩阵

/ 在vins里后端解析求导, cost_function定义成类, 命名都是 xxx_factor

例如 imu : 在imu_factor .h 文件里定义了这个类

class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> 
//残差维度15 (位姿)7  (速度 ba bg) 9  两帧 7 9 7 9

应用:

IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);// cost function
 // AddResidualBlock 参数 :           cost function,   loss function(核函数),    相关的参数块
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);

/ 初始化自动求导 , cost_function 定义为结构体

如果想使用ceres 的自动求导, 必须定义一个类或者结构体, 需要重载()同时还要使用模板

例如重投影 , 定义ReprojectionError3D结构体

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 	{ ...略... }
	
	// Create 函数
	static ceres::CostFunction* Create(const double observed_x, const double observed_y) 	{ ...略... }

	// 成员变量
	double observed_u;
	double observed_v;
};

应用:

ceres::CostFunction* cost_function = ReprojectionError3D::Create(
												sfm_f[i].observation[j].second.x(), // 特征点在这个图像帧的归一化坐标
												sfm_f[i].observation[j].second.y());
			// 添加残差块
    		problem.AddResidualBlock(cost_function, NULL, // 没有使用核函数
									c_rotation[l], c_translation[l],  sfm_f[i].position);	 // 约束了这一帧的位姿和3d地图点

5 配置求解

ceres::Solver::Optionsoptions;
options.linear_solver_type=ceres::DENSE_SCHUR;//稠密
//options.num_threads = 2;
options.trust_region_strategy_type=ceres::DOGLEG; // dog-leg
options.max_num_iterations=NUM_ITERATIONS;//8
//options.use_explicit_schur_complement = true;
//options.minimizer_progress_to_stdout = true;
//options.use_nonmonotonic_steps = true;

ceres::Solver::Summarysummary;//优化信息
ceres::Solve(options, &problem, &summary); // 求解, 求解的结果仍然放在double数组里面
//cout << summary.BriefReport() << endl;

vins示例(部分)

voidEstimator::optimization()

{

//------------------------------ 1 -------------------------------声明和 引入鲁棒核函数

ceres::Problemproblem;
ceres::LossFunction*loss_function;// 核函数
//loss_function = new ceres::HuberLoss(1.0);
loss_function=newceres::CauchyLoss(1.0);//柯西

//------------------------------2 -------------------------------- 添加ceres参数块 AddParameterBlock

//各种待优化量X——2.1 位姿优化量, 速度bias
//因为ceres用的是double数组，所以在下面用 vector2double 里初始化的,做类型装换
//Ps、Rs转变成 para_Pose, Vs、Bas、Bgs转变成 para_SpeedBias
for(int i=0; i<WINDOW_SIZE+1; i++)//还包括最新的第11帧
{
// !位姿需要自己定义加法 local_parameterization
ceres::LocalParameterization*local_parameterization=newPoseLocalParameterization();
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization); // 参数: 指针,大小, 自定义的加法
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);//优化变量V Ba Bg SIZE_SPEEDBIAS=9
}

//添加各种待优化量X——2.2 相机外参
//ESTIMATE_EXTRINSIC!=0 则camera到IMU的外参 也添加到估计
for (inti=0; i<NUM_OF_CAM; i++)// 单目只有1个
{
// 也是位姿, 也需要单独定义,不过是和pose 一样的
ceres::LocalParameterization*local_parameterization=newPoseLocalParameterization();
problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);//2.3 优化变量 外参


// eigen -> double 把状态转成double数组交给ceres进行优化
vector2double(); //给 ParameterBlock 赋值



//-- ---------------------------------- 3 -------------------------添加各种残差 AddResidualBlock

//3.2 添加各种残差——IMU残差 11个窗口有9个imu约束
for (inti=0; i<WINDOW_SIZE; i++)// 0-9 10个
{
intj=i+1;// 1-10 10个
if (pre_integrations[j]->sum_dt>10.0)// 预积分时间太长就不可信了
continue;
IMUFactor*imu_factor=newIMUFactor(pre_integrations[j]);// cost function
// AddResidualBlock 参数 : cost function, loss function(核函数), 相关的参数块
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);

}


//------------------------------------4 ------------------------- 非线性优化 求解

ceres::Solver::Optionsoptions;
options.linear_solver_type=ceres::DENSE_SCHUR;//稠密
//options.num_threads = 2;
options.trust_region_strategy_type=ceres::DOGLEG; // dog-leg
options.max_num_iterations=NUM_ITERATIONS;//8
//options.use_explicit_schur_complement = true;
//options.minimizer_progress_to_stdout = true;
//options.use_nonmonotonic_steps = true;

ceres::Solver::Summarysummary;优化信息
ceres::Solve(options, &problem, &summary); // 求解, 求解的结果仍然放在double数组里面
//cout << summary.BriefReport() << endl;

//-- 把 double数组变回vector 同时 防止优化结果在零空间变化，通过固定第一帧的位姿
double2vector();





//! 第二部分 边缘化处理 这一部分，只边缘化，不求解，求解留给下一轮优化的第一部分来进行