程序员经验分享-hwhale

PnP 问题

2023-11-14

欢迎访问我的博客首页。

  PnP（Perspective-n-Point）是求解 3D 到 2D 点对运动的方法。即，知道了 n 个 3D 空间点及其投影位置，求相机的位姿变换。如下图，已知一些点在左边相机坐标系中的三维坐标，和在右边相机坐标系中的像素坐标，求这两个相机坐标系间的位姿变换，就是 PnP 问题。
3D-2D

图 3 PnP 问题

  下面介绍四种求解 PnP 问题的方法。DLT、P3P 和 EPnP 利用若干对匹配点即可求得位姿变换，非线性优化可以利用更多观测值构造最小二乘问题。 所以，可以用前三者求得一个初始值，再用非线性优化在此初始值的基础上迭代调整。由于非线性优化通常不需要迭代很多次，所以实际使用时，可以直接使用非线性优化，在旋转矩阵为单位矩阵、平移向量为零向量的基础上直接迭代调整。

  EPnP 的具体原理在这里。

1. DLT

  DLT（Direct Linear Transform）即直接线性变换法，用 12 个参数表示增广矩阵 [R|t]，因此需要至少 6 对匹配点就可以求得该增广矩阵。然后使用 QR 分解求得位姿。

2. P3P

  P3P 使用 3 对匹配点求得四个解，再使用 1 对验证点得到一个合理的解。

#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;

double distance(const Point2f& p1, const Point2f& p2) {
	return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2));
}

int main() {
	// 1.相机内参与畸变参数。
	Mat intrinsics = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
	Mat distortion = (Mat_<double>(4, 1) << 0, 0, 0, 0);
	// 2.三维点。
	vector<Point3f> pts_3d;
	pts_3d.push_back(Point3f(-4, 2, 1));
	pts_3d.push_back(Point3f(1, 2, 3));
	pts_3d.push_back(Point3f(1, 3, 2));
	// 3.二维点。
	vector<Point2f> pts_2d;
	pts_2d.push_back(cam2pix(Point3f(2, 3, 1), intrinsics));
	pts_2d.push_back(cam2pix(Point3f(2, -2, 3), intrinsics));
	pts_2d.push_back(cam2pix(Point3f(3, -2, 2), intrinsics));
	// 4.P3P 求解。
	vector<Mat> rotation_vec, translation_vec;
	solveP3P(pts_3d, pts_2d, intrinsics, distortion, rotation_vec, translation_vec, SOLVEPNP_P3P);
	// 5.准备一对测试点。
	Mat observed_mat = (Mat_<double>(3, 1) << 2, 1, 1);			// 观测值。
	Point3f true_3d(1, -3, 1);
	Point2f true_2d = cam2pix(true_3d, intrinsics);				// 真实值。
	// 6.测试。
	Mat rotation_mat = Mat::zeros(3, 3, CV_64FC1);
	for(int i = 0; i < rotation_vec.size(); i++) {
		Rodrigues(rotation_vec[i], rotation_mat);				// 旋转向量转旋转矩阵。
		Mat measured_mat = rotation_mat * observed_mat + translation_vec[i];
		Point3f measured_3d = Point3f(measured_mat.at<double>(0, 0),
									  measured_mat.at<double>(0, 1),
									  measured_mat.at<double>(0, 2));
		Point2f measured_2d = cam2pix(measured_3d, intrinsics);	// 测量值。
		cout << distance(true_2d, measured_2d) << endl;
	}
}

  传入 solveP3P 的点对只能是 3 对或 4 对，否则会报错。无论传入的是 3 对点还是 4 对点，函数 solveP3P 都可能得出多解，需要我们自己选择正确的解。
  由于我们研究的是 3D-2D 问题，所以使用函数 cam2pix 把一组 3D 点变换成了 2D 点。相机参数可以是任意的，只要在函数 cam2pix 和函数函数 solveP3P 中保持一致。

3. G2O 求解 PnP

  使用非线性优化求解 PnP 问题常被称为 BA，因此 G2O 把为 PnP 问题定义的边和顶点存放在 g2o/types/sba。G2O 定义了多个求解 PnP 问题的边，下面介绍常用的 5 个边。

class G2O_TYPES_SBA_API EdgeProjectXYZ2UV
	: public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{...};

class EdgeSE3ProjectXYZ
	: public BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap> {...};
class EdgeSE3ProjectXYZOnlyPose
	: public BaseUnaryEdge<2, Vector2D, VertexSE3Expmap> {...};
class EdgeStereoSE3ProjectXYZ
	: public BaseBinaryEdge<3, Vector3D, VertexSBAPointXYZ, VertexSE3Expmap> {...};
class EdgeStereoSE3ProjectXYZOnlyPose
	: public BaseUnaryEdge<3, Vector3D, VertexSE3Expmap> {...};

  第一个边与第二个边都用于单目。它们的区别主要在于设置相机内参的方法不同：第一个边的用法在下面 3.1 节，它不包含相机内参，需要使用 optimizer.addParameter(camera) 设置相机内参；第二个边的用法参考 3.2 节，可以直接为其 fx, fy, cx, cy, bf 成员赋值。

  第二个边和第三个边的区别在于是否优化三维点，所以在第二个边中固定三维点就能实现和第三个边一样的作用。第四、第五个边可以用于双目摄像头获取的二维图像，具体用法见 3.2 节。

  第二个边与第四个边经常一起使用，第三个边与第五个边经常一起使用。这是因为在双目相机中，有的物方点只在其中一个相机中成像，有的物方点在两个相机中都成像。

3.1 单目

  下面是使用 EdgeProjectXYZ2UV 求解 PnP 的例子。该程序只能使用 G2O 20170730 及以前版本，因为 G2O 20200410 及以后的版本定义优化器需要使用智能指针，G2O 20201223 及以后的版本用 VertexPointXYZ 替代了 VertexSBAPointXYZ。

#include <iostream>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_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 bundleAdjustment(const vector<Eigen::Vector3d> points_3d,
                      const vector<Eigen::Vector2d> points_2d,
                      const Eigen::Matrix3d &K,
                      const Eigen::Matrix3d &R, const Eigen::Vector3d &t) {
    // 1.定义优化器。
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block;
    Block::LinearSolverType *linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>();
    Block *solver_ptr = new Block(linearSolver);
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

    // 2.位姿顶点（随意设置的初始值）。
    int vertex_idx = 0;
    g2o::VertexSE3Expmap *pose = new g2o::VertexSE3Expmap();
    pose->setId(vertex_idx++);
    pose->setEstimate(g2o::SE3Quat(R, t));
    optimizer.addVertex(pose);

    // 3.路标顶点。
    for (const Eigen::Vector3d p : points_3d) {
        g2o::VertexSBAPointXYZ *point = new g2o::VertexSBAPointXYZ();
        point->setId(vertex_idx++);
        point->setEstimate(p);
        point->setMarginalized(true);
        point->setFixed(true);
        optimizer.addVertex(point);
    }

    // 4.添加参数（相机内参）。
    int params_idx = 0;
    g2o::CameraParameters *camera =
        new g2o::CameraParameters(K(0, 0), Eigen::Vector2d(K(0, 2), K(1, 2)), 0);
    camera->setId(params_idx);
    optimizer.addParameter(camera);

    // 5.边。
    int edge_idx = 0;
    for (const Eigen::Vector2d p : points_2d) {
        g2o::EdgeProjectXYZ2UV *edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId(edge_idx);
        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(edge_idx + 1)));
        edge->setVertex(1, pose);
        edge->setMeasurement(p);
        edge->setParameterId(0, params_idx);
        edge->setInformation(Eigen::Matrix2d::Identity());
        optimizer.addEdge(edge);
        edge_idx++;
    }

    // 6.开始迭代。
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(50);

    // 7.优化后的位姿。
    cout << Eigen::Isometry3d(pose->estimate()).matrix() << 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> pts_3d;
    vector<Eigen::Vector2d> pts_2d;

    pts_2d.push_back(cam2pix(Eigen::Vector3d(2, 3, 1), K));
    pts_2d.push_back(cam2pix(Eigen::Vector3d(2, -2, 3), K));
    pts_2d.push_back(cam2pix(Eigen::Vector3d(3, -2, 2), K));
    pts_2d.push_back(cam2pix(Eigen::Vector3d(1, -3, 1), K));
    pts_2d.push_back(cam2pix(Eigen::Vector3d(4, 0, 2), K));
    pts_2d.push_back(cam2pix(Eigen::Vector3d(0, -8, 3), K));
    pts_3d.push_back(Eigen::Vector3d(-4, 2, 1));
    pts_3d.push_back(Eigen::Vector3d(1, 2, 3));
    pts_3d.push_back(Eigen::Vector3d(1, 3, 2));
    pts_3d.push_back(Eigen::Vector3d(2, 1, 1));
    pts_3d.push_back(Eigen::Vector3d(-1, 4, 2));
    pts_3d.push_back(Eigen::Vector3d(7, 0, 3));

    Eigen::Matrix3d R;
    R << 1, 0, 0, 0, 1, 0, 0, 0, 1;
    Eigen::Vector3d t(0, 0, 0);
    bundleAdjustment(pts_3d, pts_2d, K, R, t);
}

  在函数 bundleAdjustment 中，我们先定义了优化器，然后向其中添加顶点、参数、边：

  • 添加顶点：第 25 行开始添加一个位姿顶点，编号为 0。第 32 行开始添加 6 个路标顶点，编号从 1 开始。
  • 添加参数：第 42 行开始添加一个参数，编号为 0。
  • 添加边：第 49 行开始添加 6 个边，编号从 0 开始。

  每个边对应一个路标顶点，由于路标顶点编号从 1 开始，边编号从 0 开始，所以在第 54 行使用的是 edge_idx + 1。

  第 54 行和第 55 行让一个二元边连接两个顶点：0 号顶点是路标 VertexSBAPointXYZ，1 号顶点是位姿 VertexSE3Expmap。顺序不能颠倒，因为边 EdgeProjectXYZ2UV 的模板参数决定了这个顺序：

class G2O_TYPES_SBA_API EdgeProjectXYZ2UV
	: public  BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>{...};

  第 57 行把编号为 params_idx 的参数存储在索引为 0 的参数容器中。0 号顶点会根据参数的编号从优化器中取出该参数：

// OptimizableGraph 是 EdgeProjectXYZ2UV 的基类。
bool OptimizableGraph::Edge::setParameterId(int argNum, int paramId) {
	if ((int)_parameters.size() <= argNum) return false;
	if (argNum < 0) return false;
	*_parameters[argNum] = 0;
	_parameterIds[argNum] = paramId;
	return true;
}

void EdgeProjectXYZ2UV::computeError() {
	// 省略。
    const CameraParameters *cam = static_cast<const CameraParameters *>(parameter(0));
    // 利用该参数计算投影误差。
}

  下面是 CMakeLists.txt 的内容。其中，路径 D:/MinGW/libraries/g2o_20170730 是安装 G2O 的位置，包含以下文件：

bin
include
lib
FindCSparse.cmake
FindG2O.cmake

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}
    D:/MinGW/libraries/include
)
link_directories(${G2O_ROOT}/lib)

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

# cmake -G "MinGW Makefiles" ..

  程序运行结果如下面的左图。

G2O

  分析：可以看到计算的结果有一定的误差，尤其是平移向量 (0.19,-0.57,0.02) 与实际值 (0,-1,0) 相差较大。这是因为我们在使用 G2O 优化时同时调整了位姿与 3D 点。vertex 1 到 vertex 6 都被调整了，所以不能得到实际的位姿。但这样做在工程中却是有意义的。因为 3D 点的坐标准确度不那么可信，所以有必要把它们和位姿一块调整。如果能确定这些 3D 点的坐标是准确的，就可以得到准确的位姿，比如去掉对 point->setFixed(true) 的注释，就可以得到右图的结果。

3.2 双目

  双目摄像头有视差，根据视差公式可以得到深度信息，所以使用 G2O 定义的边 EdgeStereoSE3ProjectXYZ 可以利用更多约束求解 PnP 问题。

#include <iostream>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_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 bundleAdjustment(const vector<Eigen::Vector3d> &points_3d,
                      const vector<Eigen::Vector2d> &points_2d,
                      const vector<int> &zs, const Eigen::Matrix3d &K,
                      const Eigen::Matrix3d &R, const Eigen::Vector3d &t) {
    // 1.定义优化器。
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block;
    Block::LinearSolverType *linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>();
    Block *solver_ptr = new Block(linearSolver);
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

    // 2.位姿顶点（随意设置的初始值）。
    int vertex_idx = 0;
    g2o::VertexSE3Expmap *pose = new g2o::VertexSE3Expmap();
    pose->setId(vertex_idx++);
    pose->setEstimate(g2o::SE3Quat(R, t));
    optimizer.addVertex(pose);

    // 3.路标顶点。
    for (const Eigen::Vector3d p : points_3d) {
        g2o::VertexSBAPointXYZ *point = new g2o::VertexSBAPointXYZ();
        point->setId(vertex_idx++);
        point->setEstimate(p);
        point->setMarginalized(true);
        point->setFixed(true);
        optimizer.addVertex(point);
    }

    // 4.边。
    int edge_idx = 0;
    double bfx = K(0, 0); // baseline * fx。
    double parallax;
    for (const Eigen::Vector2d p : points_2d) {
        g2o::EdgeStereoSE3ProjectXYZ *edge = new g2o::EdgeStereoSE3ProjectXYZ();
        edge->fx = K(0, 0);
        edge->fy = K(1, 1);
        edge->cx = K(0, 2);
        edge->cy = K(1, 2);
        edge->bf = bfx;
        edge->setId(edge_idx);
        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(edge_idx + 1)));
        edge->setVertex(1, pose);
        parallax = bfx / zs[edge_idx];
        edge->setMeasurement(Eigen::Vector3d(p(0), p(1), p(0) - parallax));
        edge->setInformation(Eigen::Matrix3d::Identity());
        optimizer.addEdge(edge);
        edge_idx++;
    }

    // 5.开始迭代。
    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(50);

    // 6.优化后的位姿。
    cout << Eigen::Isometry3d(pose->estimate()).matrix() << 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> pts_3d;
    vector<Eigen::Vector2d> pts_2d;
    vector<int> zs = {1, 3, 2, 1, 2, 3};

    pts_2d.push_back(cam2pix(Eigen::Vector3d(2, 3, 1), K));
    pts_2d.push_back(cam2pix(Eigen::Vector3d(2, -2, 3), K));
    pts_2d.push_back(cam2pix(Eigen::Vector3d(3, -2, 2), K));
    pts_2d.push_back(cam2pix(Eigen::Vector3d(1, -3, 1), K));
    pts_2d.push_back(cam2pix(Eigen::Vector3d(4, 0, 2), K));
    pts_2d.push_back(cam2pix(Eigen::Vector3d(0, -8, 3), K));
    pts_3d.push_back(Eigen::Vector3d(-4, 2, 1));
    pts_3d.push_back(Eigen::Vector3d(1, 2, 3));
    pts_3d.push_back(Eigen::Vector3d(1, 3, 2));
    pts_3d.push_back(Eigen::Vector3d(2, 1, 1));
    pts_3d.push_back(Eigen::Vector3d(-1, 4, 2));
    pts_3d.push_back(Eigen::Vector3d(7, 0, 3));

    Eigen::Matrix3d R;
    R << 1, 0, 0, 0, 1, 0, 0, 0, 1;
    Eigen::Vector3d t(0, 0, 0);
    bundleAdjustment(pts_3d, pts_2d, zs, K, R, t);
}

  如果有双目相机，视差是可以直接获取的。由于没有双目相机，我们在第 44 行假设基线是 1 米，在第 56 行利用像素点的深度信息生成视差信息。第 77 行是为每个像素点准备的深度信息，用在第 56 行生成视差。

  从上面的程序可以看到，双目 PnP 边和单目 PnP 边用法几乎完全一样，只是测量值除了像素坐标 p p p 外，多了一个维度表示 p x p_x px​ 与视差的差值。

4. 自定义顶点与边优化内参

  下面我们将自定义顶点与边，在求解 PnP 的问题的过程中，把相机内参也当作待优化项。我们不考虑桶形畸变和枕形畸变，且只针对单目相机。如果考虑畸变，只需再加两个畸变参数，实现起来很容易。

4.1 二元边

  《视觉 SLAM 十四讲 第 2 版》第 9 章的示例程序演示了怎么优化畸变参数，本小节就是参考该实例程序而来。

#include <iostream>
#include <sophus/se3.hpp>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
using namespace std;

struct SE3Vector4 {
	SE3Vector4() {
		R = Sophus::SO3d::exp(Eigen::Vector3d(0, 0, 0));
		t = Eigen::Vector3d(0, 0, 0);
		K = Eigen::Vector4d(0, 0, 0, 0);
	}
	explicit SE3Vector4(const Eigen::Matrix3d &_R, const Eigen::Vector3d &_t, const Eigen::Matrix3d &_K) {
		R = Sophus::SO3d(_R);
		t = _t;
		K = Eigen::Vector4d(_K(0, 0), _K(1, 1), _K(0, 2), _K(1, 2));
	}
	Sophus::SO3d R;
	Eigen::Vector3d t;
	Eigen::Vector4d K;
};

class VertexSE3Vector4 : public g2o::BaseVertex<10, SE3Vector4> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
	VertexSE3Vector4() {}
	virtual void setToOriginImpl() override {
		_estimate = SE3Vector4();
	}
	virtual void oplusImpl(const double *update) override {
		Sophus::SO3d delta_R = Sophus::SO3d::exp(Eigen::Vector3d(update[0], update[1], update[2]));
		Eigen::Vector3d delta_t = Eigen::Vector3d(update[3], update[4], update[5]);
		Eigen::Vector4d delta_K = Eigen::Vector4d(update[6], update[7], update[8], update[9]);
		_estimate.R = delta_R * _estimate.R;
		_estimate.t = delta_t + _estimate.t;
		_estimate.K = delta_K + _estimate.K;
	}
	Eigen::Vector2d project(const Eigen::Vector3d &point) {
		Eigen::Vector3d pc = _estimate.R * point + _estimate.t;
		double u = pc(0) * _estimate.K(0) / pc(2) + _estimate.K(2);
		double v = pc(1) * _estimate.K(1) / pc(2) + _estimate.K(3);
		return Eigen::Vector2d(u, v);
	}
	virtual bool read(istream &in) {}
	virtual bool write(ostream &out) const {}
};

class EdgeProjectXYZKUV : public g2o::BaseBinaryEdge<2, Eigen::Vector2d, g2o::VertexSBAPointXYZ, VertexSE3Vector4> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
	virtual void computeError() override {
		auto v0 = (g2o::VertexSBAPointXYZ *)_vertices[0];
		auto v1 = (VertexSE3Vector4 *)_vertices[1];
		auto proj = v1->project(v0->estimate());
		_error = proj - _measurement;
	}
	virtual bool read(istream &in) {}
	virtual bool write(ostream &out) const {}
};

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 bundleAdjustment(const vector<Eigen::Vector3d> points_3d,
					  const vector<Eigen::Vector2d> points_2d,
					  const Eigen::Matrix3d &K,
					  const Eigen::Matrix3d &R, const Eigen::Vector3d &t) {
	// 1.定义优化器。
	typedef g2o::BlockSolverX Block;
	Block::LinearSolverType *linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>();
	Block *solver_ptr = new Block(linearSolver);
	g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
	g2o::SparseOptimizer optimizer;
	optimizer.setAlgorithm(solver);

	int vertex_idx = 0;

	// 2.位姿内参顶点。
	VertexSE3Vector4 *pose = new VertexSE3Vector4();
	pose->setId(vertex_idx++);
	pose->setEstimate(SE3Vector4(R, t, K));
	pose->setFixed(false);
	optimizer.addVertex(pose);

	// 3.路标顶点。
	for (const Eigen::Vector3d p : points_3d) {
		g2o::VertexSBAPointXYZ *point = new g2o::VertexSBAPointXYZ();
		point->setId(vertex_idx++);
		point->setEstimate(p);
		point->setMarginalized(true);
		// point->setFixed(true);
		optimizer.addVertex(point);
	}

	// 4.边。
	int edge_idx = 0;
	for (const Eigen::Vector2d p : points_2d) {
		EdgeProjectXYZKUV *edge = new EdgeProjectXYZKUV();
		edge->setId(edge_idx);
		edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(edge_idx + 1)));
		edge->setVertex(1, pose);
		edge->setMeasurement(p);
		edge->setInformation(Eigen::Matrix2d::Identity());
		optimizer.addEdge(edge);
		edge_idx++;
	}

	// 5.开始迭代。
	optimizer.setVerbose(true);
	optimizer.initializeOptimization();
	optimizer.optimize(50);

	// 6.优化后的位姿和内参。
	cout << pose->estimate().R.matrix() << endl;
	cout << pose->estimate().t.transpose() << endl;
	cout << pose->estimate().K.transpose() << 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> pts_3d;
	vector<Eigen::Vector2d> pts_2d;

	pts_2d.push_back(cam2pix(Eigen::Vector3d(2, 3, 1), K));
	pts_2d.push_back(cam2pix(Eigen::Vector3d(2, -2, 3), K));
	pts_2d.push_back(cam2pix(Eigen::Vector3d(3, -2, 2), K));
	pts_2d.push_back(cam2pix(Eigen::Vector3d(1, -3, 1), K));
	pts_2d.push_back(cam2pix(Eigen::Vector3d(4, 0, 2), K));
	pts_2d.push_back(cam2pix(Eigen::Vector3d(0, -8, 3), K));
	pts_3d.push_back(Eigen::Vector3d(-4, 2, 1));
	pts_3d.push_back(Eigen::Vector3d(1, 2, 3));
	pts_3d.push_back(Eigen::Vector3d(1, 3, 2));
	pts_3d.push_back(Eigen::Vector3d(2, 1, 1));
	pts_3d.push_back(Eigen::Vector3d(-1, 4, 2));
	pts_3d.push_back(Eigen::Vector3d(7, 0, 3));

	Eigen::Matrix3d R;
	R << 1, 0, 0, 0, 1, 0, 0, 0, 1;
	Eigen::Vector3d t(0, 0, 0);
	bundleAdjustment(pts_3d, pts_2d, K, R, t);
}

  结构体 SE3Vector4 用于存放自定义顶点 VertexSE3Vector4 的待优化的量：位姿和内参。类似于 g2o 为顶点 VertexSE3Expmap 定义的 SE3Quat。

  自定义边 VertexSE3Vector4 时使用的模板参数是 <10, SE3Vector4>，前者指定该边的成员函数 oplusImpl 的参数数组长度，暂且称为增量维度，后者指定该边的成员变量 _estimate 的类型。

  配置文件 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}
    D:/MinGW/libraries/Sophus220401/include
    D:/MinGW/libraries/fmt800/include
)
link_directories(
    ${G2O_ROOT}/lib
    D:/MinGW/libraries/fmt800/lib
)

add_executable(main main.cpp)

target_link_libraries(main
    g2o_core
    g2o_stuff
    g2o_types_sba
    g2o_types_slam3d
    g2o_types_sim3
    fmt
)

# cmake -G "MinGW Makefiles" ..

4.2 三元边

  下面定义三元边实现 4.1 节的功能，边连接的三个顶点是三维路标、位姿、相机内参。

#include <iostream>
#include <float.h>
#include <sophus/se3.hpp>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
using namespace std;

class VertexIntrinsics : public g2o::BaseVertex<4, Eigen::Matrix3d> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
	virtual void setToOriginImpl() override {
		_estimate = Eigen::Matrix3d::Zero();
	}
	virtual void oplusImpl(const double *update) override {
		_estimate(0, 0) += update[0];
		_estimate(1, 1) += update[1];
		_estimate(0, 2) += update[2];
		_estimate(1, 2) += update[3];
	}
	Eigen::Vector2d project(const Eigen::Vector3d &point, const g2o::SE3Quat &T) {
		Eigen::Vector3d pc = T * point;
		double u = pc(0) * _estimate(0, 0) / pc(2) + _estimate(0, 2);
		double v = pc(1) * _estimate(1, 1) / pc(2) + _estimate(1, 2);
		return Eigen::Vector2d(u, v);
	}
	virtual bool read(istream &in) {}
	virtual bool write(ostream &out) const {}
};

class EdgeProjectPTK : public g2o::BaseMultiEdge<2, Eigen::Vector2d> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
	EdgeProjectPTK() {
		this->resize(3);
	}
	virtual void computeError() override {
		auto v0 = (g2o::VertexSBAPointXYZ *)_vertices[0];
		auto v1 = (g2o::VertexSE3Expmap *)_vertices[1];
		auto v2 = (VertexIntrinsics *)_vertices[2];
		auto proj = v2->project(v0->estimate(), v1->estimate());
		_error = proj - _measurement;
	}
	virtual bool read(istream &in) {}
	virtual bool write(ostream &out) const {}
};

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 bundleAdjustment(const vector<Eigen::Vector3d> points_3d,
					  const vector<Eigen::Vector2d> points_2d,
					  const Eigen::Matrix3d &K,
					  const Eigen::Matrix3d &R, const Eigen::Vector3d &t) {
	// 1.定义优化器。
	typedef g2o::BlockSolverX Block;
	Block::LinearSolverType *linearSolver = new g2o::LinearSolverEigen<Block::PoseMatrixType>();
	Block *solver_ptr = new Block(linearSolver);
	g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
	g2o::SparseOptimizer optimizer;
	optimizer.setAlgorithm(solver);

	int vertex_idx = 0;

	// 2.路标顶点。
	for (const Eigen::Vector3d p : points_3d) {
		g2o::VertexSBAPointXYZ *point = new g2o::VertexSBAPointXYZ();
		point->setId(vertex_idx++);
		point->setEstimate(p);
		point->setMarginalized(true);
		// point->setFixed(true);
		optimizer.addVertex(point);
	}

	// 3.位姿顶点。
	g2o::VertexSE3Expmap *pose = new g2o::VertexSE3Expmap();
	pose->setId(vertex_idx++);
	pose->setEstimate(g2o::SE3Quat(R, t));
	pose->setFixed(false);
	optimizer.addVertex(pose);

	// 4.内参顶点。
	VertexIntrinsics *intrinsics = new VertexIntrinsics();
	intrinsics->setId(vertex_idx++);
	intrinsics->setEstimate(K);
	// intrinsics->setFixed(true);
	optimizer.addVertex(intrinsics);

	// 5.边。
	int edge_idx = 0;
	for (const Eigen::Vector2d p : points_2d) {
		EdgeProjectPTK *edge = new EdgeProjectPTK();
		edge->setId(edge_idx);
		edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(edge_idx)));
		edge->setVertex(1, pose);
		edge->setVertex(2, intrinsics);
		edge->setMeasurement(p);
		edge->setInformation(Eigen::Matrix2d::Identity());
		optimizer.addEdge(edge);
		edge_idx++;
	}

	// 6.开始迭代。
	optimizer.setVerbose(true);
	optimizer.initializeOptimization();
	optimizer.optimize(50);

	// 7.优化后的位姿和内参。
	cout << Eigen::Isometry3d(pose->estimate()).matrix() << endl;
	cout << intrinsics->estimate() << 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> pts_3d;
	vector<Eigen::Vector2d> pts_2d;

	pts_2d.push_back(cam2pix(Eigen::Vector3d(2, 3, 1), K));
	pts_2d.push_back(cam2pix(Eigen::Vector3d(2, -2, 3), K));
	pts_2d.push_back(cam2pix(Eigen::Vector3d(3, -2, 2), K));
	pts_2d.push_back(cam2pix(Eigen::Vector3d(1, -3, 1), K));
	pts_2d.push_back(cam2pix(Eigen::Vector3d(4, 0, 2), K));
	pts_2d.push_back(cam2pix(Eigen::Vector3d(0, -8, 3), K));
	pts_3d.push_back(Eigen::Vector3d(-4, 2, 1));
	pts_3d.push_back(Eigen::Vector3d(1, 2, 3));
	pts_3d.push_back(Eigen::Vector3d(1, 3, 2));
	pts_3d.push_back(Eigen::Vector3d(2, 1, 1));
	pts_3d.push_back(Eigen::Vector3d(-1, 4, 2));
	pts_3d.push_back(Eigen::Vector3d(7, 0, 3));

	Eigen::Matrix3d R;
	R << 1, 0, 0, 0, 1, 0, 0, 0, 1;
	Eigen::Vector3d t(0, 0, 0);
	bundleAdjustment(pts_3d, pts_2d, K, R, t);
}

  从第 35 行可以看出，定义三元边时继承了 BaseMultiEdge。和定义一元边、二元边不同的是，定义三元边时的模板参数只有增量维度和测量值类型，没有各顶点类型。在 39 行使用 resize 函数指定该边是三元边。

4.3 总结

  从上面的两个程序可以看出，在位姿不固定的情况下：如果固定三维的路标顶点，无论相机内参的四个初始值与真实值相差多大，都能收敛到真实值；如果不固定路标顶点，相机内参就难以收敛到真实值，而是在初始值附近。这个原因很好解释：路标、像素坐标、位姿、内参这几项，确定的项越多，优化得到的不确定项就越准确。因此在 SLAM 中，标定相机内参很重要。

  求解 PnP 问题的方法有很多，比如本文介绍的 DLT、P3P、EPnP 以及第 3、4 节使用 G2O 的非线性优化。第 3、4 节把空间点变换到相机坐标系，再投影得到像素坐标，优化的目标是使像素坐标与观测值的误差趋向 0，这就是 BA（bundle adjustment）算法。

5. 参考

  1. PnP，博客园。
  2. OpenCV 中的 9 种 PnP 算法，CSDN。
  3. G2O 路标顶点的 setMarginalized 函数，知乎专栏。
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

Slam

多视图几何

PnP 问题 的相关文章

  • SLAM方法汇总

    原文 http blog csdn net smartxxyx article details 53068855 目录 SLAM概述 SLAM一般处理流程包括track和map两部分 所谓的track是用来估计相机的位姿 也叫front e

  • SLAM笔记（四）运动恢复结构的几何数学(本征矩阵、单应矩阵、基础矩阵)

    1 间接法进行运动恢复的前提假设 对于结构与运动或视觉三维重建中 通常假设已经通过特征匹配等方法获取了匹配好的点对 先求出匹配点对再获取结构和运动信息的方法称作间接法 间接法最重要的三个假设是 1 拥有一系列两帧之间的匹配点对 但同时假设匹

  • 各向异性（anisotropic）浅提

    文章目录 各向异性 anisotropic 定义 哪种物体具有各向异性反射 什么导致各向异性反射 总结 各向异性 anisotropic 定义 它指一种存在方向依赖性 这意味着在不同的方向不同的特性 相对于该属性各向同性 当沿不同轴测量时

  • np.meshgrid()函数 以及 三维空间中的坐标位置生成 以及 numpy.repeat()函数介绍

    一 np meshgrid 函数 1 np meshgrid 介绍 X Y np meshgrid x y 代表的是将x中每一个数据和y中每一个数据组合生成很多点 然后将这些点的x坐标放入到X中 y坐标放入Y中 并且相应位置是对应的 下面是

  • 速腾聚创雷达最新驱动安装（包含ring和timestamp）运行lio-sam

    记录一下搞slam的过程 ring和timestamp 最近想跑lio sam 需要用到ring和timestamp两个参数 lio sam作者用的velodyne雷达是带这两个参数的 但是rs雷达的老版驱动录制的点云包没有这两个参数 在g

  • Sophus使用记录

    sophus库是一个基于Eigen的C 李群李代数库 可以用来方便地进行李群李代数的运算 头文件 主要用到以下两个头文件 include

  • SLAM评估工具evo的使用

    evo官方指南 参考博客 lt 官方手册 这篇参考博客 完全可以掌握evo的基本操作 gt Then 实践出真知 1 安装evo sudo apt install python pip pip install evo upgrade no

  • 单目视觉里程记代码

    在Github上发现了一个简单的单目vo 有接近500星 链接如下 https github com avisingh599 mono vo 这个单目里程计主要依靠opencv实现 提取fast角点并进行光流跟踪 然后求取本质矩阵并恢复两帧

  • LeGO-LOAM论文翻译（内容精简)

    LeGO LOAM是一种在LOAM之上进行改进的激光雷达建图方法 建图效果比LOAM要好 但是建图较为稀疏 计算量也更小了 本文原地址 wykxwyc的博客 github注释后LeGO LOAM源码 LeGO LOAM NOTED 关于代码

  • 【SLAM】libQGLViewer：VS 2019 + Qt 5.14.2 + Win 10 配置

    libQGLViewer 2 7 2 VS 2019 Qt 5 14 2 Win 10 配置 注意 这次配置没有完全成功 编译25个成功 一个失败 失败的是 qglviewerplugin qglviewerplugin 是一个可选控件 不

  • [SLAM四元数基础系列一] 四元数定义 Hamilton vs JPL

    四元数定义 Hamilton vs JPL 简介 四种区分方式 Hamilton vs JPL 引用 不管是卡尔曼滤波或者BA优化形式的SLAM或者VIO系统中 都需要用到单位四元数 Quaternion 来表示旋转 主要是单位四元数表示旋

  • vscode配置eigen3

    目录 1 头文件包含 2 c cpp properties json 3 CMakeList txt 4 完整代码 1 头文件包含 Eigen 核心部分 include

  • 图像匹配算法

    图像匹配算法分为3类 基于灰度的匹配算法 基于特征的匹配算法 基于关系的匹配算法 1 基于灰度的模板匹配算法 模板匹配 Blocking Matching 是根据已知模板图像到另一幅图像中寻找与模板图像相似的子图像 基于灰度的匹配算法也称作

  • 关于GPS、惯导、视觉里程计的几个定义

    1 首先写几个定义 惯性导航系统 Inertial Navigation System INS 全球定位卫星系统 Global Navigation Satellite System GNSS GNSS 包括全球定位系统 Global Po

  • 视觉SLAM技术及其应用（章国锋--复杂环境下的鲁棒SfM与SLAM）

    SLAM 同时定位与地图构建 机器人和计算机视觉领域的基本问题 在未知环境中定位自身方位并同时构建环境三维地图 应用广泛 增强现实 虚拟现实 机器人 无人驾驶 SLAM常用的传感器 红外传感器 较近距离感应 常用与扫地机器人 激光雷达 单线

  • Ubuntu20.04安装各种库----简洁版

    目录 Eigen3 Sophus Pangolin Ceres g2o 建议先装anaconda再装ros python opencv啥该有的都有了 下面仅仅安装ros没有的库 Eigen3 作用 线性代数开源库 提供了有关线性代数 矩阵和

  • docker dbus-x11

    本来想用terminator启动nvidia docker 显示出图形界面的 结果发现启动的时候出问题了 terminator 1 dbind WARNING 07 31 53 725 Couldn t connect to accessi

  • LeGO-LOAM中的数学公式推导

    LeGO LOAM是一种在LOAM之上进行改进的激光雷达建图方法 建图效果比LOAM要好 但是建图较为稀疏 计算量也更小了 本文原地址 wykxwyc的博客 github注释后LeGO LOAM源码 LeGO LOAM NOTED 关于代码

  • Ubuntu18.04安装pcl(过程/坑记录式教程)

    Ubuntu18 04安装pcl 1 下载pcl 2 安装依赖项 3 编译 4 安装 5 网上教程说要安装QT5和VTK 但按照本文的 本文记录了安装时出现的问题 出错的安装命令也记录了 建议浏览一遍再参考 不要错用了错误的指令 1 下载p

  • ORB-SLAM2：基于可识别特征的自主导航与地图构建

    ORB SLAM2 基于可识别特征的自主导航与地图构建 ORB SLAM Tracking and Mapping Recognizable Features 转自 http blog csdn net cicibabe article d

随机推荐

  • poium测试库介绍

    poium测试库前身为selenium page objects测试库 我在以前的文章中也有介绍过 这可能是最简单的Page Object库 项目的核心是基于Page Objects实现元素定位的封装 该项目由我个人在维护 目前在公司项目中

  • 使用ChatGPT的方式与在其他地方使用它的方式基本相同。以下是一些步骤：

    在中国使用ChatGPT的方式与在其他地方使用它的方式基本相同 以下是一些步骤 访问OpenAI的官方网站 OpenAI 在网站上找到GPT 3或ChatGPT的相关信息 OpenAI提供了详细的API文档 可以帮助你理解如何使用它们 你需

  • mysql数据库之跨表复制

    背景说明 目标库 target db 目标数据表 target tb 将目标库的制定表复制到当前数据库中 包括一下几个方面 一 表结构复制 仅仅复制了表的结构 没有数据 create table current db new tb like

  • Logitech G系鼠标脚本编程，实现鼠标自动定位控制

    利用罗技官方提供的API来写一个鼠标自动定位移动脚本 点击脚本编辑器中的帮助选项 查看罗技官方提供的API说明 有很多实现好的鼠标功能 G series Lua API V8 45 Overview and Reference 下面是我写的

  • 深入解析SpringBoot启动原理

    1 启动类中的SpringApplication run方法会创建一个SpringApplication的实例 并做一些初始化工作 SpringBootApplication Slf4j public class HuotuUserServ

  • Linux C编程基础：获取时间

    1 前言 对于linux下的编程 无论是用户态还是内核态 时间获取都是经常需要使用到的 以下分别从用户态和内核态整理了几个常用的时间获取接口 供编写代码时快速查阅 linux时间子系统的发展历史及详细介绍 可以参考 深入理解Linux时间子

  • stm32 机械周期_STM32定时器周期计算

    STM32定时器周期计算 公式是 1 TIM Prescaler 时钟 1 TIM Period F103配置生成1ms的时钟 1 35 36M 1 999 1MS TIM TimeBaseInitTypeDef TIM TimeBaseS

  • LeNet-5识别数字

    LeNet识别数字 前言 环境 实现 结果 前言 实现经典卷积神经网络LeNet LeNet 5 识别数字 这里将激活函数从sigmoid换成ReLU 参考资料 动手学深度学习 环境 python pytorch 实现 import tor

  • 设计模式八大原则知多少

    设计模式是一种通用的解决问题的经验 可以帮助我们设计出可重用 可维护和可扩展的软件 在设计模式中 有八个常见的原则 它们是 单一职责原则 SRP Single Responsibility Principle 一个类应该只有一个引起变化的原

  • AlexNet(深度学习模型)详解

    AlexNet是一种深度卷积神经网络 由Alex Krizhevsky Ilya Sutskever和Geoffrey Hinton于2012年在ImageNet图像分类竞赛中首次引入 这项竞赛是一个庞大的数据集 其中包含超过100万张图像

  • TP5学习（十三）：其他

    一 缓存 设置 缓存支持采用驱动方式 所以缓存在使用之前 需要进行连接操作 也就是缓存初始化操作 options 缓存类型为File type gt File 缓存有效期为永久有效 expire gt 0 缓存前缀 prefix gt th

  • 快速排序和快速选择

    用同样的划分 完成不同的目的 快速排序在 的平均时间复杂度完成排序 快速选择在 的平均时间复杂度找出第 k 大的元素 因为 quickSelect 只需要对划分的其中一边递归 int partition int a int l int r

  • 如何在visio2010的框图中插入公式？

    如何在visio2010的框图中插入公式 其实很简单 也没必要下载Mathtype 先在world中用自带的公式编辑器将公式写好 如下图 此时选中公式 博文已经迁移到个人主页https guangmujun cn archives 143

  • SpringAOP+自定义注解模拟shiro框架实现

    一 概念 权限管理就是管理用户对于资源的操作 本 CRM 系统的权限 也称作资源 是基于角色操作权限来实现的 即RBAC Role Based Access Control 基于角色的访问控制 就是用户通过角色与权限进行关联 简单地说 一个

  • iOSWebview与js交互之调整字体大小

    先看效果图 iOS开发经常会跟Webview交互 主要调用的就是stringByEvaluatingJavaScriptFromString这个方法 也可以在 BOOL webView UIWebView webView shouldSta

  • NAT网关和NAT穿越原理

    转自 https blog csdn net chance yin article details 37913963 一 原理图 1 背景信息 1 我们模拟的情形是位于网络A下的内网主机UserA 想要和位于网络B下的内网主机UserB进行

  • 网页HTML5制作flex布局骰子,CSS3的Flexbox骰子布局的实现及分析

    这篇文章主要介绍了关于CSS3的Flexbox骰子布局的实现及分析 有着一定的参考价值 现在分享给大家 有需要的朋友可以参考一下 骰子布局顾名思义 就是好比骰子的一面最多可以放置9个点 而每个面放置的点数正好就是一个布局的模型图 这里我们就

  • 网站漏洞怎么修复代码漏洞

    jeecms 最近被爆出高危网站漏洞 可以导致网站被上传webshell木马文件 受影响的版本是jeecms V6 0版本到jeecmsV7 0版本 该网站系统采用的是JAVA语言开发 数据库使用的是oracle mysql sql数据库

  • python 类self详解

    class Box object def init this boxname size color this boxname boxname this size size this color color self就是用于存储对象属性的集合

  • PnP 问题

    欢迎访问我的博客首页 PnP 问题 1 DLT 2 P3P 3 G2O 求解 PnP 3 1 单目 3 2 双目 4 自定义顶点与边优化内参 4 1 二元边 4 2 三元边 4 3 总结 5 参考 PnP Perspective n Poi

热门标签