ROS-3DSLAM（11）：视觉部分visual_estimator第四节：initial 4

2021@SDUSC
2021年11月22日星期一——2021年11月25日星期四

一、背景简介：

继续对于initial文件夹内的代码：initial_sfm进行分析。

这是initial中的最后一部分代码。

不过，在分析之前，首先要明确它的主要功能，以及在系统中的意义。

initial文件夹内存放的是用来初始化系统参数的函数，包括了估计旋转外参数、利用sfm求取特征点和位姿在参考坐标下的值，利用相机旋转约束来标定IMU角速度偏差、以及利用平移来估计重力和各个bk帧的尺度、优化标定重力，确定世界坐标系，确定各点在世界坐标系下的参数等等。

结合我们之前分析的三部分代码：

initial_aligment：优化，消除偏差：利用相机的旋转来计算bias、利用gw优化gc0。

initial_ex_rotation：求解相机坐标系到IMU坐标系的相对旋转。

solve_5pts：利用五点法恢复尺度。

我们可以通过再次阅读论文以及initial的功能得出结论，initial_sfm的功能是：求解出来特征点和位姿在c0坐标系下的值。

二、前置知识：

第六次学习中曾经简单提到过sfm。

说明：这里就是上面所说的”尺度参数“

SFM的意思是Structure From Motion，

通过相机的移动来确定目标的空间和几何关系，是三维重建的一种常见方法。 它与Kinect这种3D摄像头最大的不同在于，它只需要普通的RGB摄像头即可，因此成本更低廉，且受环境约束较小， 在室内和室外均能使用。

这里借助了ros中的：

vi_sfm 是一个 ROS 包，用于从运动问题中解决视觉惯性结构。 该软件包在封闭形式的解决方案中融合了视觉和惯性数据。 封闭形式的解决方案由单个方程组成，该方程仅使用惯性测量单元 (IMU) 和在短时间间隔（~4 秒）内获取的单目相机特征，线性确定初始速度、到点特征的距离和重力 . 假设相机已校准，并且相机和 IMU 之间的转换已知并且可以使用 TF 转换来确定。

在这部分代码中，系统利用了sfm来求解滑窗内的所有帧的位姿（参考坐标系c0就是第一帧的参考）和所有路标点的三维位置。

三、Initial_sfm分析：

非常奇怪的是，这里和上上一篇一样，都没有大量的使用cv库函数，而是选择了Eigen库函数等，并利用他们重写了opencv库函数里本来就有的函数，这令我十分不理解。😆

1、头文件

//定义了SFM结构
struct SFMFeature
{
   
    bool state;
    int id;
    vector<pair<int,Vector2d>> observation;
    double position[3];
    double depth;
};
//定义重投影误差的数据结构
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);
		p[0] += camera_T[0]; p[1] += camera_T[1]; p[2] += camera_T[2];
		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;
};
//非常重要的类，定义了核心函数construct。
class GlobalSFM
{
   
public:
	GlobalSFM();
	bool construct(int frame_num, Quaterniond* q, Vector3d* T, int l,
			  const Matrix3d relative_R, const Vector3d relative_T,
			  vector<SFMFeature> &sfm_f, map<int, Vector3d> &sfm_tracked_points);

private:
    //pnp算法实现对于位姿的计算
	bool solveFrameByPnP(Matrix3d &R_initial, Vector3d &P_initial, int i, vector<SFMFeature> &sfm_f);
	//三角化点，三角化两帧的基础
	void triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,Vector2d &point0, Vector2d &point1, Vector3d &point_3d);
	//三角化两帧
    void triangulateTwoFrames(int frame0, Eigen::Matrix<double, 3, 4> &Pose0, 
							  int frame1, Eigen::Matrix<double, 3, 4> &Pose1,
							  vector<SFMFeature> &sfm_f);
	//定义了特征点的数目
	int feature_num;
};

2、initial_sfm 代码

#include "initial_sfm.h"

GlobalSFM::GlobalSFM(){
   }
//点的三角化，利用了矩阵
void GlobalSFM::triangulatePoint(Eigen::Matrix<double