视觉SLAM前端（间接法）——位姿求解（g2o）

目录：

• 对极约束
• PnP问题
• ICP问题
• Gauss-Newton法
• g2o求解最小二乘问题

对极约束

1.求解基础矩阵/本质矩阵

基础矩阵F

Mat fundamental_matrix;
	fundamental_matrix = findFundamentalMat(points1, points2, FM_8POINT);

注意findFundamentalMat（）函数的参数：
参数1，2：points_1；cv类型的中的特征点1，2容器类，像素坐标。
参数3：为求解方法，有8点法等等，为一参数。
返回值为mat矩阵。

本质矩阵E

Point2d principal_point(325.1, 249.7);  //相机光心, TUM dataset标定值
double focal_length = 521;      
//相机焦距, TUM dataset标定值
Mat essential_matrix; 
essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);

参数1，2与基础矩阵求解一样。
参数3，4分别为相机光心与焦距值。
返回值为mat矩阵。

2.求解R，t

recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
//求解R，t

recoverPose(函数有7个参数)：
参数1：本质矩阵E；
参数2，3：特征点1，2的容器类，像素坐标
参数4，5：R,t矩阵，mat类型
参数6，7：焦距，光心值

3.三角测量（三角化）
Mat pts_4d;
cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);

	// 转换成非齐次坐标
	for (int i = 0; i < pts_4d.cols; i++) {
		Mat x = pts_4d.col(i);
		x /= x.at<float>(3, 0); // 归一化
		Point3d p(
			x.at<float>(0, 0),
			x.at<float>(1, 0),
			x.at<float>(2, 0)
		);
		points.push_back(p);
	}	

注：triangulatePoints（）一共有5个参数
参数1，2：为变换矩阵，一般T1为单位矩阵，因为没有变换，Mat类型
参数3，4: 为特征点，为Point2d类，为归一化坐标系的坐标
参数5：存储求解的结果，为一个4×n的矩阵，每一列存储一个点的结果，但存储的是其次坐标，需要归一化得到相机坐标（X,Y,Z），实际使用时，把Z提取出来即可。

PnP问题

Mat r, t;
solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false); 
Mat R;
cv::Rodrigues(r, R); 
// r为旋转向量形式，用Rodrigues公式转换为矩阵

// 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法

solvePnP（）有7个参数：
参数1：三维点的P的坐标（X,Y,Z）相机坐标系，实际值，不是归一化坐标系，point容器类型。
参数2：二维点的像素坐标，point容器类型。
参数3：相机内参矩阵，mat类
参数4：畸变参数，没有的化设置问哦Mat()即可
参数5，6：r旋转向量，t平移向量；mat类
参数7：默认false吧

ICP问题

1.求解特征点的质心

Point3f p1, p2;     // 质心坐标
	int N = pts1.size();
	for (int i = 0; i < N; i++) {
		p1 += pts1[i];
		p2 += pts2[i];
	}
	p1 = Point3f(Vec3f(p1) / N);
	p2 = Point3f(Vec3f(p2) / N);

2.去质心化

vector<Point3f> q1(N), q2(N); 
// 去质心化
	for (int i = 0; i < N; i++) {
		q1[i] = pts1[i] - p1;
		q2[i] = pts2[i] - p2;
	}

3.计算W矩阵

	// compute q1*q2^T
	Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
	for (int i = 0; i < N; i++) {
		W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x, q2[i].y, q2[i].z).transpose();
	}
	cout << "W=" << W << endl;

4.对W进行SVD分解

Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d U = svd.matrixU();
Eigen::Matrix3d V = svd.matrixV();

5.求解R，t

	Eigen::Matrix3d R_ = U * (V.transpose());
	if (R_.determinant() < 0) {
		R_ = -R_;
	}
	Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

Gauss-Newton法

以下是PnP问题的gauss-newton优化方法：

void bundleAdjustmentGaussNewton(
	const VecVector3d &points_3d,
	const VecVector2d &points_2d,
	const Mat &K,
	Sophus::SE3d &pose) {
	typedef Eigen::Matrix<double, 6, 1> Vector6d;
	const int iterations = 10;
	double cost = 0, lastCost = 0;
	double fx = K.at<double>(0, 0);
	double fy = K.at<double>(1, 1);
	double cx = K.at<double>(0, 2);
	double cy = K.at<double>(1, 2);

	for (int iter = 0; iter < iterations; iter++) {
		Eigen::Matrix<double, 6, 6> H = Eigen::Matrix<double, 6, 6>::Zero();
		//定义H矩阵
		Vector6d b = Vector6d::Zero();

		cost = 0;
		// 计算cost
		for (int i = 0; i < points_3d.size(); i++) {
			Eigen::Vector3d pc = pose * points_3d[i];
			double inv_z = 1.0 / pc[2];
			double inv_z2 = inv_z * inv_z;
			Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);

			Eigen::Vector2d e = points_2d[i] - proj;

			cost += e.squaredNorm();
			Eigen::Matrix<double, 2, 6> J;
			J << -fx * inv_z,
				0,
				fx * pc[0] * inv_z2,
				fx * pc[0] * pc[1] * inv_z2,
				-fx - fx * pc[0] * pc[0] * inv_z2,
				fx * pc[1] * inv_z,
				0,
				-fy * inv_z,
				fy * pc[1] * inv_z2,
				fy + fy * pc[1] * pc[1] * inv_z2,
				-fy * pc[0] * pc[1] * inv_z2,
				-fy * pc[0] * inv_z;

			H += J.transpose() * J;   //计算H,注意J是2×6的矩阵 J.transpose()*J为6×6的矩阵
			b += -J.transpose() * e;  //计算b，b为2×1的矩阵
		}

		Vector6d dx;
		dx = H.ldlt().solve(b);      //求解增量方程

		if (isnan(dx[0])) {
			cout << "result is nan!" << endl;
			break;
		}//判断迭代值是否为无穷


		if (iter > 0 && cost >= lastCost) {
			cout << "cost: " << cost << ", last cost: " << lastCost << endl;
			break;
		}//判断cost的增加方向，理论上是越来越小；

		
		pose = Sophus::SE3d::exp(dx) * pose; //更新位姿，从而更新误差e
		lastCost = cost;

		cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;
		if (dx.norm() < 1e-6) {
			// converge
			break;
		}//当增增量很小便停止迭代
	}

g2o求解最小二乘问题

  g2o求解最小二乘法问题最小分为三个部分：

• 编写Vertex类（优化变量）
• 编写Edge类（误差函数）
• 配置optimazer

以求解PnP问题为例：
Vertex类的主要工作有：

1.编写setToOriginImpl()函数：初始化变量_estimate.
2.编写oplusImpl()函数：设置增量的更新表达式。

class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

	virtual void setToOriginImpl() override {
		_estimate = Sophus::SE3d();
	}

	/// left multiplication on SE3
	virtual void oplusImpl(const double *update) override {
		Eigen::Matrix<double, 6, 1> update_eigen;
		update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
		_estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
	}

	virtual bool read(istream &in) override { return true; }

	virtual bool write(ostream &out) const override { return true; }
};

注意：
1.BaseVertex<6,Sophus::SE3d>代表优化变量的维度为6，优化变量的类型为Sophs::SE3d;SE3d的维度可以看作是6维
2.oplusImpl(const double *update)中的update应该是一个数组指针，存储着更新量，实际使用的时候需要转换为相应的类型。（不能之间与数组进行运算）

Edge类的主要工作有：
1.编写 computeError()函数：误差计算表达式
2.编写linearizeOplus()函数：输入J矩阵

class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

	EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}

	virtual void computeError() override {
		const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
		Sophus::SE3d T = v->estimate();
		Eigen::Vector3d pos_pixel = _K * (T * _pos3d);
		pos_pixel /= pos_pixel[2];
		_error = _measurement - pos_pixel.head<2>();
	}

	virtual void linearizeOplus() override {
		const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);
		Sophus::SE3d T = v->estimate();
		Eigen::Vector3d pos_cam = T * _pos3d;
		double fx = _K(0, 0);
		double fy = _K(1, 1);
		double cx = _K(0, 2);
		double cy = _K(1, 2);
		double X = pos_cam[0];
		double Y = pos_cam[1];
		double Z = pos_cam[2];
		double Z2 = Z * Z;
		_jacobianOplusXi
			<< -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z,
			0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z;
	}



	virtual bool read(istream &in) override { return true; }

	virtual bool write(ostream &out) const override { return true; }

private:
	Eigen::Vector3d _pos3d;
	Eigen::Matrix3d _K;
};

注:
  1.computeError()函数中，_error,_measurement变量均为继承的变量，类型为Eigen的向量。其中，_measurement为测量值，需要类外输入。
  2.linearizeOplus()的核心是给_jacobianOplusXi矩阵赋值，关于_jacobianOplusXi矩阵的结构如下：
设误差$e(x)=\left[\begin{array}{c}{e}_1\\ e_2\end{array}\right]$优化变量$x=\left[\begin{array}{c}u\\ v\\ w\end{array}\right]$

则$J_1=\left[\begin{array}{c}\frac{\mathrm{d}{e}_1}{\mathrm{d}u}\\ \frac{\mathrm{d}{e}_2}{\mathrm{d}u}\end{array}\right]$ $J_2=\left[\begin{array}{c}\frac{\mathrm{d}{e}_1}{\mathrm{d}v}\\ \frac{\mathrm{d}{e}_2}{\mathrm{d}v}\end{array}\right]$$J_1=\left[\begin{array}{c}\frac{\mathrm{d}{e}_1}{\mathrm{d}w}\\ \frac{\mathrm{d}{e}_2}{\mathrm{d}w}\end{array}\right]$

$J=\left[\begin{array}{ccc}{J}_1& J_2& J_3\end{array}\right]$

  因此，整个J矩阵的排布是列向量安装一定顺序排列的，所以对于实际的_jacobianOplusXi矩阵也常常安装这种规律依次赋值，在g2o中常用采用以下分块矩阵的形式：

_jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();
<3,3>代表分块的大小，（0，x）代表从第x列分块的分界点。
_jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans);
或者像以下形式：
_jacobianOplusXi.block<2,3>(0,0) = dp_dE;
_jacobianOplusXi.block<2,3>(0,3) = de_dp;  
_jacobianOplusXi.block<2,1>(0,6) = de_df; 
_jacobianOplusXi.block<2,1>(0,7) = de_dk1;     
_jacobianOplusXi.block<2,1>(0,8) = de_dk2;

  3.对于class EdgeProjection : publicg2o::BaseUnaryEdge<2,Eigen::Vector2d,VertexPose>；

2表示error的维度，
Eigen::Vector2d表示error的类型，
VertexPose表示定义边所连接的顶点（有可能有多个）

配置optimazer
具体工作如下：
1.定义求解器（solver）：设置求解类型，变量个数，求解算法等等
2.定义优化器（optimazer）：定义一个optimazer
3.设置Vertex：类初始化，赋初值，设置顶点的个数等
4.设置Edge：类初始化，传递_measurement参数等等。
5.设置optimazer
代码如下：

void bundleAdjustmentG2O(
	const VecVector3d &points_3d,
	const VecVector2d &points_2d,
	const Mat &K,
	Sophus::SE3d &pose) {

	// 构建图优化，先设定g2o
	typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 2>> BlockSolverType;  // 优化变量6个，误差e的维度为2
	typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
	// 梯度下降方法，可以从GN, LM, DogLeg 中选
	auto solver = new g2o::OptimizationAlgorithmGaussNewton(
		g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
	g2o::SparseOptimizer optimizer;     // 图模型
	optimizer.setAlgorithm(solver);   // 设置求解器
	optimizer.setVerbose(true);       // 打开调试输出

	// 设置Vetex
	VertexPose *vertex_pose = new VertexPose(); // 空的构造函数
	vertex_pose->setId(0);        //设置Vertex的个数
	vertex_pose->setEstimate(Sophus::SE3d());  //初始化
	optimizer.addVertex(vertex_pose);

	// K
	Eigen::Matrix3d K_eigen;
	K_eigen <<
		K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2),
		K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2),
		K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2);

	// 设置Edge
	int index = 1;
	for (size_t i = 0; i < points_2d.size(); ++i) {
		auto p2d = points_2d[i];
		auto p3d = points_3d[i];
		EdgeProjection *edge = new EdgeProjection(p3d, K_eigen); //构造函数初始化类
		edge->setId(index);                //Edge的个数               
		edge->setVertex(0, vertex_pose); //与顶点的连接
		edge->setMeasurement(p2d);        //赋值给
		edge->setInformation(Eigen::Matrix2d::Identity()); //初始化
		optimizer.addEdge(edge);  
		index++;
	}

	 
	optimizer.setVerbose(true);    // 打开调试输出
	optimizer.initializeOptimization(); //optimazer的初始化
	optimizer.optimize(10);       //iteration的数量
	
	pose = vertex_pose->estimate();  //返回位姿
}

补充：

1.当具有多个类型的vertex时，g2o中必须把每个vertex添加到edge中，即顶点与边连接，一般做法是定义vertex容器。

2.每次设置vertex的指针必须保存起来，一般是放到前面定义的容器中，因为后面要把这个指针添加到edge中。

3.g2o中的vertex是连续的，每一个vertex只能占用一个Id，一般是把一类顶点放在一串Id中。

4.g2o中每一条edge中都有一个vertice容器，里面存储这对条边所连接顶点的指针。注意区分2中的容器。

	vector<camera_vertex*> cameravertex;
	vector<point_vertex*> pointvertex;
	//上面是储存vertex的容器，这里一共有两类顶点：camera_vertex和point_vertex
	//设置camera_vertex，因为有很多vertex和edge，所以用for语句
	for (int i = 0; i < bal_problem.num_cameras(); i++)
	{
		camera_vertex* v = new camera_vertex(); //必须要加上read和write那几个虚函数
		v->setId(i);
		//从0-bal_problem.num_cameras()-1都是camera_vertex；
		double* camera = cameras + camera_block_size * i;
		v->setEstimate(pose_and_intrincis(camera));
		optimazer.addVertex(v);
		cameravertex.push_back(v);  //保存好指针
	}
	
	for (int i = 0; i < bal_problem.num_points(); i++)
	{
		point_vertex* v = new point_vertex();
		v->setId(i+bal_problem.num_cameras());
		//从 bal_problem.num_cameras()至bal_problem.num_cameras()+bal_problem.num_points()都是point_vertex。
		double* point = points + point_block_size * i;
		v->setEstimate(Vector3d(point[0],point[1],point[2]));
		optimazer.addVertex(v);
		pointvertex.push_back(v); //保存好指针
		v->setMarginalized(true);

	}

	//注意，vertex中的指针v必须存储起来，然后添加到后面的edg里面去


	//以下是添加边
	for (int i = 0; i < bal_problem.num_observations(); i++)
	{
		edg_projection* edge = new edg_projection();
		edge->setVertex(0, cameravertex[bal_problem.camera_index()[i]]);
		edge->setVertex(1, pointvertex[bal_problem.point_index()[i]]);
		//上面是把保存的指针添加到对于的edge中，
		//0，1代表定义edge是vertex的顺序，在这里，0代表camera_vertex,1代表point_vertex
		edge->setMeasurement(Vector2d(observations[2 * i + 0], observations[2 * i + 1]));
		edge->setInformation(Matrix2d::Identity());
		edge->setRobustKernel(new g2o::RobustKernelHuber());
		optimazer.addEdge(edge);
		
	}

给出g2o设置的结构图（仅仅求解单顶点的非线性最小二乘问题）

补充： 关于g2o定义求解器的三步：

1.定义BlockSolver
2.定义求解算法类型
3.定义optimazer

//第一步：定义blocksover
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType;
//这是定义BlockSolver的类型

typedef g2o::LinearSolverEigen <BlockSolverType::PoseMatrixType> LinearSolverType;
//这是定义在定义类型的基础上，定义一种求解器的类型

//第二步：定义求解算法
auto solver = new g2o::OptimizationAlgorithmLevenberg(
		g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>())
	);//这里使用L-M算法，注意看图，算法是继承于第一步所中定义各种类型

//第三步：定义优化器
g2o::SparseOptimizer optimazer;
optimazer.setAlgorithm(solver);//同样，注意看图，优化器继承于算法

这三步有明显的继承关系，从图中可以看出，3继承2继承1。

参考文献：
[1]视觉SLAM十四讲，高翔等
[2]https://blog.csdn.net/johnnyyeh/article/details/82315543
[3]https://blog.csdn.net/qq_34935373/article/details/93898626