ROS-3DSLAM(12):视觉部分visual estimator第六节:initial5

本文链接：https://blog.csdn.net/m0_49088083/article/details/121733476

这篇博客分析了ROS-3DSLAM中initial_sfm的视觉部分，包括ceres库在非线性优化中的应用。介绍了full BA的概念及其在结构从运动(SFM)计算中的重要性，用于最小化重投影误差。通过学习，理解了矩阵、几何和优化算法等数学知识，并完成了initial文件夹的代码分析，为后续的factor文件夹分析做准备。

摘要由CSDN通过智能技术生成

2021@SDUSC

2021年11月29日星期一——2021年12月2日星期四

附:12月1日typora更新了,新版本要收费,我回退到旧的0.9.8版本之后,发现旧的版本有个致命的bug,会丢失对代码的注释…本来已经读完了代码的,结果下午打开一看注释全没了。

幸好时间不算长,还能挽救。

一、背景简介:

这篇博客分为三部分,第一部分是对于上次的代码分析的补全,结束initial_sfm中最后一部分的问题。

第二部分是对于数学知识的补充,类似于上上一次对于代码的分析一样,这一次是对于数学部分的原理加深理解。

最后就是对于initial文件夹的小结,借助于这部分的内容结束,开启下一步的计划。

二、代码分析:

说明:

ceres 同OpenCV、Eigen一样,也是一款第三方的库函数,他的作用是用来处理非线性优化。

该库由Google开发,在Google的SLAM项目cartographer中被大量使用。

官网中给出了详细说明。
full BA 是

在SFM(structure from motion)的计算中BA(Bundle Adjustment)作为最后一步优化具有很重要的作用,在近几年兴起的基于图的SLAM(simultaneous localization and mapping)算法里面使用了图优化替代了原来的滤波器,这里所谓的图优化其实也是指BA。

为了学习BA,需要有以下知识储备:
- 射影相机集合模型
- 对极几何
- 凸优化
- 矩阵理论
除了凸优化似乎都稍微学过了…
凸优化:这个概念实际上机器学习中经常提到的,不过没有指出来这个名词而已。

总的来说,凸优化是最优化问题中重要的组成部分之一。

凸优化是来解决全局最小值的,他对于问题加以限制,对于目标函数限定为凸函数;对于优化变量的可行域限定为凸集。

这样就能够使得的局部最优解一定是全局最优解。

凸函数的概念好说,凸集的概念我还是第一次见到:

在凸几何中,凸集(convex set)是在凸组合下闭合的仿射空间的子集。更具体地说,在欧氏空间中,凸集是对于集合内的每一对点,连接该对点的直线段上的每个点也在该集合内。例如,立方体是凸集,但是任何中空的或具有凹痕的例如月牙形都不是凸集。 [1]

特别的,凸集,实数R上(或复数C上)的向量空间中,如果集合S中任两点的连线上的点都在S内,则称集合S为凸集。

其中,对于凸函数而言,也有一定的扩展。

对于多元函数,如果它是凸函数,则其Hessian矩阵为半正定矩阵。如果Hessian矩阵是正定的,则函数是严格凸函数。

//full BA
//首先是参数的初始化,调用了ceres中的方法
	ceres::Problem problem;
	ceres::LocalParameterization* local_parameterization 
				= new ceres::QuaternionParameterization();
	//cout << " begin full BA " << endl;
   //向矩阵当中填入对应的数值,包括位置信息、旋转信息
	for (int i = 0; i < frame_num; i++)
	{
		//double array for ceres
		c_translation[i][0] = c_Translation[i].x();
		c_translation[i][1] = c_Translation[i].y();
		c_translation[i][2] = c_Translation[i].z();
		c_rotation[i][0] = c_Quat[i].w();
		c_rotation[i][1] = c_Quat[i].x();
		c_rotation[i][2] = c_Quat[i].y();
		c_rotation[i][3] = c_Quat[i].z();
		problem.AddParameterBlock(c_rotation[i], 4, local_parameterization);
		problem.AddParameterBlock(c_translation[i], 3);
		if (i == l)
		{
			problem.SetParameterBlockConstant(c_rotation[i]);
		}
		if (i == l || i == frame_num - 1)
		{
			problem.SetParameterBlockConstant(c_translation[i]);
		}
	}
//正式的计算开始
	for (int i = 0; i < feature_num; i++)
	{	
    //要先检查sfm的状态,是否已经被计算过了
		if (sfm_f[i].state != true)
			continue;
		for (int j = 0; j < int(sfm_f[i].observation.size()); j++)
		{
        //声明损失函数,下面的是添加误差项
			int l = sfm_f[i].observation[j].first;
			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);	 
		}

	}
    //配置并来运行求解器
	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);
	//std::cout << summary.BriefReport() << "\n";
	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;
	}
    //结束对于重投影误差最小化的计算
   //这里得到的是第l帧坐标系到各帧的变换矩阵,应将其转变为各帧在第l帧坐标系上的位姿
	
    for (int i = 0; i < frame_num; i++)
	{
		q[i].w() = c_rotation[i][0]; 
		q[i].x() = c_rotation[i][1]; 
		q[i].y() = c_rotation[i][2]; 
		q[i].z() = c_rotation[i][3]; 
		q[i] = q[i].inverse();
		//cout << "final  q" << " i " << i <<"  " <<q[i].w() << "  " << q[i].vec().transpose() << endl;
	}
	for (int i = 0; i < frame_num; i++)
	{

		T[i] = -1 * (q[i] * Vector3d(c_translation[i][0], c_translation[i][1], c_translation[i][2]));
		//cout << "final  t" << " i " << i <<"  " << T[i](0) <<"  "<< T[i](1) <<"  "<< T[i](2) << endl;
	}
	for (int i = 0; i < (int)sfm_f.size(); i++)
	{
		if(sfm_f[i].state)
			sfm_tracked_points[sfm_f[i].id] = Vector3d(sfm_f[i].position[0], sfm_f[i].position[1], sfm_f[i].position[2]);
	}
	return true;

}

三、问题及解答:

1 full BA

光束法平差,我们之前也介绍过。但是这里是用代码借助于第三方库ceres来实现的。

总的来说 ,这个部分的本质就是就是一个优化,没什么好说的。但是这部分的功能值得分析。

他的目的是最小化重投影误差。

投影是在相机拍照的时候,空间点投射到图像上的过程。

利用这些图像(投影点),我们可以进行三角化,来得到三维空间中点的位置。

最后将计算得到的三维点坐标和计算得到相机矩阵进行二次投影,就是重投影了。

重投影误差就是真实的空间点和重投影的差值,前者是真实的,后者是我们计算得来的,虚拟的。

因为我们计算总会受到各种各样的因素影响,与实际情况的误差总是存在的。

所以此处的功能就是进行最小化这个误差。

2 full BA 2

虽然知道了最小化重投影误差的功能,但是问题也随之而来了,这个最小化误差有什么用处呢?

这个问题就需要回到我们的最初目的上来。

固定住l帧的位置和姿态,固定住最后一帧的位置。因为这时候的图像位姿和点的位置都不太准,所以用ceres统一一起优化图像位姿和三维点位置,优化重投影误差。优化的测量值是,特征点在每帧中被观察到的位置,可以转成重投影误差约束。有关的自变量是,每帧图像的位姿,特征点的三维坐标。

优化完成之后,即用ceres优化出这些关键帧的位姿和地图点后,再用pnp算出在这段时间区域内的所有图像的位姿。每个图像的计算都用下一个关键帧的位姿来当pnp的初值

所谓的优化就是让我们的计算值尽可能近似实际。能够进行下一步的计算。

四、数学知识小结:

本来打算是在把这些数学知识再深入学习一遍的,但是想到自己终归不是数学专业学生,项目的目的也不是专门学习相关的数学知识,所以调整了对于数学部分的重视程度,现在还是先以理解为主,等到结束了这学期的其他学习任务之后再找时间巩固一下:

数学知识总结(initial部分):

矩阵部分:SVD分解、本质矩阵、特征矩阵、五点法、ldlt法、
几何部分:对极约束、BA、pnp算法、三角化、欧拉角、四元数、旋转矩阵
算法部分:凸优化、线性优化

另外,要注意线性代数的基础最好也先在回顾一遍。

五、initial小结:

经过这漫长的将近二十天的学习,我终于解决完了initial文件夹下的全部代码。虽然还没有彻底弄明白相关的知识,关于文件里的问题也还有没理解透彻的部分,但是总的说来,起码把整个代码的基本思路和主干过了一遍,把视觉部分的“初始化”原理和实现弄明白了。

下面还有两个文件夹:factor和utility,第一个是负责“非线性优化”的部分,第二个应该是辅助函数的样子,显而易见,我们应该先分析factor文件夹下相关代码。

factor文件夹下一共有八个文件,同initial一样,其中也是四个cpp文件,四个头文件。
分别是:

imu_factor
integration_base
marginalization_factor
pose_local_parameterization
projection_factor
projection_td_factor

六、参考资料:

一文助你Ceres 入门——Ceres Solver新手向全攻略_福尔摩睿的工作站-CSDN博客_ceres solver

Ceres Solver — A Large Scale Non-linear Optimization Library (ceres-solver.org)

理解凸优化 - 知乎 (zhihu.com)

凸集_百度百科 (baidu.com)

很好理解的讲解: