Sim3 问题

欢迎访问我的博客首页。

---

  相似变换用于单目 SLAM。

1. 相似变换算法

---

void Sim3Solver::ComputeSim3(...);

2. 使用 G2O 求相似变换

---

  这里介绍用 G2O 求解相似变换的方法。G2O 定义了三条用于相似变换问题的边：用两个坐标系的相对位姿优化这两个坐标系在世界坐标系中的位姿，用 EdgeSim3；已知两个坐标系内的三维点坐标和相机内参，求这两个坐标系内三维坐标的相似变换，用 EdgeSim3ProjectXYZ 和 EdgeInverseSim3ProjectXYZ。后两条边是配合在一起使用的。这三条边的定义如下：

class EdgeSim3
	: public BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap> {...};
	
class EdgeSim3ProjectXYZ
	: public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSim3Expmap> {...};
class EdgeInverseSim3ProjectXYZ
	: public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSim3Expmap> {...};

  很明显，固定相似变换的尺度系数为 1，上面的这些边就可以用于欧氏变换 SE3 问题。

2.1 用相对位姿优化两个坐标系的位姿

---

  本节介绍用两个坐标系的相对位姿优化这两个坐标系在世界坐标系中的位姿。下图是世界坐标系 $O_w$、相机坐标系 $O_1$、$O_2$ 之间的位姿关系。假设世界坐标系 $O_w$ 到相机坐标系 $O_1$ 和 $O_2$ 的相似变换分别是 $S_{1w}$ 和 $S_{2w}$，相机坐标系 $O_1$ 到相机坐标系 $O_2$ 的相似变换是 $S_{21}$。边 EdgeSim3 以 $S_{21}$ 为测量值，优化两个顶点 $S_{1w}$ 和 $S_{2w}$。

图 2.1 三个坐标系之间的位姿关系

  通常会固定一个顶点优化另一个顶点，我们固定 $S_{1w}$ 优化 $S_{2w}$。假设从 $O_w$ 到 $O_1$ 的变换 $S_{1w}$ 是绕 Z 轴顺时针旋转 90 °，再沿旋转后的 Y 轴负方向平移 1 个单位，即

$$S_{1w}=\left[\begin{array}{cccc}0& 1& 0& 0\\ -1& 0& 0& -1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].\text{\hspace{1em}(2.1)}$$

假设从 $O_1$ 到 $O_2$ 的变换 $S_{21}$ 是绕 Z 轴顺时针旋转 30 °，再沿旋转后的 Z 轴正方向平移 0.5 个单位，即

$$S_{21}=\left[\begin{array}{cccc}0.87& 0.5& 0& 0\\ -0.5& 0.87& 0& 0\\ 0& 0& 1& 0.5\\ 0& 0& 0& 1\end{array}\right].\text{\hspace{1em}(2.2)}$$

根据位姿变换的传递性，$O_w$ 到 $O_2$ 的变换

$$S_{2w}=S_{21}\cdot S_{1w}=\left[\begin{array}{cccc}-0.5& 0.87& 0& -0.5\\ -0.87& -0.5& 0& -0.87\\ 0& 0& 1& 0.5\\ 0& 0& 0& 1\end{array}\right].\text{\hspace{1em}(2.3)}$$

下面我们就固定 $S_{1w}$，用 $S_{21}$ 优化 $S_{2w}$，并验证得到的 $S_{2w}$ 与公式 (2.3) 是否一致。

#include <iostream>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/sim3/types_seven_dof_expmap.h>
using namespace std;

int main() {
	cout << setiosflags(ios::fixed) << setiosflags(ios::right) << setprecision(2);
	// 1.定义优化器。
	g2o::BlockSolverX::LinearSolverType *linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
	g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);
	g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
	g2o::SparseOptimizer optimizer;
	optimizer.setAlgorithm(solver);
	optimizer.setVerbose(true);

	// 2.两个相机的初始位姿和相对位姿。
	// S1w。
	Eigen::Matrix3d R1w;
	R1w << 0, 1, 0, -1, 0, 0, 0, 0, 1;
	Eigen::Vector3d t1w(0, -1, 0);
	g2o::Sim3 S1w(R1w, t1w, 1.0);
	// S2w（随意设置的初始值）。
	Eigen::Matrix3d R2w;
	R2w << 1, 0, 0, 0, 1, 0, 0, 0, 1;
	Eigen::Vector3d t2w(0, 0, 0);
	g2o::Sim3 S2w(R2w, t2w, 1.0);
	// S21。
	Eigen::Matrix3d R21;
	R21 << 0.87, 0.5, 0, -0.5, 0.87, 0, 0, 0, 1;
	Eigen::Vector3d t21(0, 0, 0.5);
	g2o::Sim3 S21(R21, t21, 1.0);

	// 3.添加两个位姿顶点。
	// 位姿顶点 1。
	g2o::VertexSim3Expmap *pose1w = new g2o::VertexSim3Expmap();
	pose1w->setId(0);
	pose1w->setEstimate(S1w);
	pose1w->setFixed(true);
	optimizer.addVertex(pose1w);
	// 位姿顶点 2。
	g2o::VertexSim3Expmap *pose2w = new g2o::VertexSim3Expmap();
	pose2w->setId(1);
	pose2w->setEstimate(S2w);
	pose2w->setFixed(false);
	optimizer.addVertex(pose2w);

	// 4.添加边。
	g2o::EdgeSim3 *edge = new g2o::EdgeSim3();
	edge->setVertex(0, pose1w);
	edge->setVertex(1, pose2w);
	edge->setMeasurement(S21);
	edge->setInformation(Eigen::Matrix<double, 7, 7>::Identity());
	optimizer.addEdge(edge);

	// 5.开始迭代。
	optimizer.initializeOptimization();
	optimizer.optimize(100);

	// 6.优化后的位姿和路标。
	g2o::Sim3 S2w_res = pose2w->estimate();
	cout << S2w_res.rotation().matrix() << endl
		 << S2w_res.translation().transpose() << endl
		 << S2w_res.scale() << endl;
}

2.2 三维点之间的相似变换

---

  本节介绍已知两个坐标系内的三维点的空间坐标和相机内参，求这两个坐标系内三维坐标的相似变换的方法。

#include <iostream>
#include <Eigen/StdVector>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/sim3/types_seven_dof_expmap.h>
using namespace std;

Eigen::Vector2d cam2pix(const Eigen::Vector3d &p, const Eigen::Matrix3d &K) {
	return Eigen::Vector2d((p(0) * K(0, 0)) / p(2) + K(0, 2),
						   (p(1) * K(1, 1)) / p(2) + K(1, 2));
}

void Sim3(const vector<Eigen::Vector3d> points0, const vector<Eigen::Vector3d> points1,
		  const Eigen::Matrix3d &K0, const Eigen::Matrix3d &K1) {
	// 1.定义优化器。
	g2o::BlockSolverX::LinearSolverType *linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
	g2o::BlockSolverX *solver_ptr = new g2o::BlockSolverX(linearSolver);
	g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
	g2o::SparseOptimizer optimizer;
	optimizer.setAlgorithm(solver);
	optimizer.setVerbose(true);

	// 2.相机 1 到相机 0 的相似变换（随意设置的初始值）。
	Eigen::Matrix3d R;
	R << 1, 0, 0, 0, 1, 0, 0, 0, 1;
	Eigen::Vector3d t(0, 0, 0);
	double s = 3.0;
	g2o::Sim3 S01(R, t, s);

	// 3.添加一个位姿顶点。
	int vertex_idx = 0;
	g2o::VertexSim3Expmap *pose01 = new g2o::VertexSim3Expmap();
	pose01->_fix_scale = false;
	pose01->_principle_point1[0] = K0(0, 2);
	pose01->_principle_point1[1] = K0(1, 2);
	pose01->_focal_length1[0] = K0(0, 0);
	pose01->_focal_length1[1] = K0(1, 1);
	pose01->_principle_point2[0] = K1(0, 2);
	pose01->_principle_point2[1] = K1(1, 2);
	pose01->_focal_length2[0] = K1(0, 0);
	pose01->_focal_length2[1] = K1(1, 1);
	pose01->setId(vertex_idx++);
	pose01->setEstimate(S01);
	pose01->setMarginalized(false);
	pose01->setFixed(false);
	optimizer.addVertex(pose01);

	// 4.添加 12 个地图点顶点和 12 条边。
	for (size_t i = 0; i < points0.size(); i++) {
		// 4.1.1 添加顶点（相机坐标系 1 中的空间坐标）。
		g2o::VertexSBAPointXYZ *point1i = new g2o::VertexSBAPointXYZ();
		point1i->setEstimate(points1[i]);
		point1i->setId(vertex_idx++);
		point1i->setFixed(true);
		optimizer.addVertex(point1i);
		// 4.1.2 添加边（相机坐标系 1 中的空间坐标，位姿），观测值是像素坐标系 0 中的坐标。
		g2o::EdgeSim3ProjectXYZ *edge12 = new g2o::EdgeSim3ProjectXYZ();
		edge12->setVertex(0, point1i);
		edge12->setVertex(1, pose01);
		edge12->setMeasurement(cam2pix(points0[i], K0));
		edge12->setInformation(Eigen::Matrix2d::Identity());
		optimizer.addEdge(edge12);
		// 4.2.1 添加顶点（相机坐标系 0 中的空间坐标）。
		g2o::VertexSBAPointXYZ *point0i = new g2o::VertexSBAPointXYZ();
		point0i->setEstimate(points0[i]);
		point0i->setId(vertex_idx++);
		point0i->setFixed(true);
		optimizer.addVertex(point0i);
		// 4.2.2 添加边（相机坐标系 0 中的空间坐标，位姿），观测值是像素坐标系 1 中的坐标。
		g2o::EdgeInverseSim3ProjectXYZ *edge21 = new g2o::EdgeInverseSim3ProjectXYZ();
		edge21->setVertex(0, point0i);
		edge21->setVertex(1, pose01);
		edge21->setMeasurement(cam2pix(points1[i], K1));
		edge21->setInformation(Eigen::Matrix2d::Identity());
		optimizer.addEdge(edge21);
	}

	// 5.开始迭代。
	optimizer.initializeOptimization();
	optimizer.optimize(100);

	// 6.优化后的位姿。
	g2o::Sim3 Sres = pose01->estimate();
	cout << Sres.rotation().matrix() << endl
		 << Sres.translation().transpose() << endl
		 << Sres.scale() << endl;
}

int main(int argc, char **argv) {
	Eigen::Matrix3d K;
	K << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;
	vector<Eigen::Vector3d> points0, points1;
	double scale = 2.0;

	points0.push_back(Eigen::Vector3d(2, 3, 1));
	points0.push_back(Eigen::Vector3d(2, -2, 3));
	points0.push_back(Eigen::Vector3d(3, -2, 2));
	points0.push_back(Eigen::Vector3d(1, -3, 1));
	points0.push_back(Eigen::Vector3d(4, 0, 2));
	points0.push_back(Eigen::Vector3d(0, -8, 3));
	points1.push_back(Eigen::Vector3d(-4, 2, 1) / scale);
	points1.push_back(Eigen::Vector3d(1, 2, 3) / scale);
	points1.push_back(Eigen::Vector3d(1, 3, 2) / scale);
	points1.push_back(Eigen::Vector3d(2, 1, 1) / scale);
	points1.push_back(Eigen::Vector3d(-1, 4, 2) / scale);
	points1.push_back(Eigen::Vector3d(7, 0, 3) / scale);

	Sim3(points0, points1, K, K);
}

  配置文件 CMakeLists.txt 的写法：

cmake_minimum_required(VERSION 3.2)
project(test)

add_compile_options(-std=c++14)

set(Eigen3_DIR D:/MinGW/libraries/share/eigen3/cmake)
find_package(Eigen3 REQUIRED)

set(G2O_ROOT D:/MinGW/libraries/g2o_20170730)
set(CMAKE_MODULE_PATH ${G2O_ROOT})
find_package(G2O REQUIRED)
find_package(CSPARSE REQUIRED)

include_directories(
    ${EIGEN3_INCLUDE_DIRS}
    ${G2O_INCLUDE_DIR}
    ${CSPARSE_INCLUDE_DIR}
)
link_directories(${G2O_ROOT}/lib)

add_executable(main simXYZ.cpp)
target_link_libraries(main
    g2o_core
    g2o_stuff
    g2o_types_sba
    g2o_types_slam3d
    g2o_types_sim3
)

# cmake -G "MinGW Makefiles" ..

3. 参考

---

1. 相似变换算法，论文，1987。
2. sim3求解过程推导以及代码对应，知乎专栏。