从 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*>¶meter_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();
//! 第二部分 边缘化处理 这一部分,只边缘化,不求解,求解留给下一轮优化的第一部分来进行
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)