当前位置：首页 > news >正文

视觉SLAM：使用 Sophus 的 SE3 类，自己设计 g2o 的节点与边，实现 PnP 和 ICP 的优化

news 2025/7/17 15:04:14

下面我将基于 Sophus 的 SE3 类，实现完整的 PnP（Perspective-n-Point）和 ICP（Iterative Closest Point）优化代码，使用 g2o 自定义顶点和边。代码包含关键数学推导、雅可比计算及完整的优化流程。

一、实现思路

自定义顶点：继承 g2o::BaseVertex，用 Sophus::SE3d 表示位姿。
自定义边：
- PnP边：继承 g2o::BaseUnaryEdge，计算 3D 点投影到 2D 像素的误差。
- ICP边：继承 g2o::BaseBinaryEdge，计算两点云匹配点对的欧氏距离误差。
雅可比矩阵：基于李代数扰动模型推导（见后续推导过程）。
优化流程：构建图 → 添加顶点/边 → 配置优化器 → 执行优化。

二、代码实现

1. 自定义顶点（SE3 位姿）

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Sophus/se3.hpp>// 自定义顶点：SE3 位姿
class VertexSE3Sophus : public g2o::BaseVertex<6, Sophus::SE3d> {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWVertexSE3Sophus() {}virtual void setToOriginImpl() override { _estimate = Sophus::SE3d(); }virtual void oplusImpl(const double* update) override {// 李代数更新：前6维为 [平移, 旋转]（注意顺序）Eigen::Map<const Eigen::Matrix<double, 6, 1>> delta(update);_estimate = Sophus::SE3d::exp(delta) * _estimate;}virtual bool read(std::istream&) override { return false; }virtual bool write(std::ostream&) const override { return false; }
};

2. PnP 边（3D→2D 投影误差）

// PnP 边：计算 3D 点投影到 2D 的误差
class EdgePnPSophus : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexSE3Sophus> {
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWEdgePnPSophus(const Eigen::Vector3d& point_3d, const Eigen::Matrix3d& K) : _point3d(point_3d), _K(K) {}virtual void computeError() override {const VertexSE3Sophus* v = static_cast<VertexSE3Sophus*>(_vertices[0]);Sophus::SE3d pose = v->estimate();// 将 3D 点变换到相机坐标系Eigen::Vector3d pc = pose * _point3d;// 投影到像素坐标Eigen::Vector3d proj = _K * pc;proj /= proj.z(); // 归一化_error = _measurement - proj.head<2>();}// 雅可比计算：使用链式法则 + 李代数扰动virtual void linearizeOplus() override {const VertexSE3Sophus* v = static_cast<VertexSE3Sophus*>(_vertices[0]);Sophus::SE3d pose = v->estimate();Eigen::Vector3d pc = pose * _point3d;Eigen::Vector3d proj = _K * pc;double inv_z = 1.0 / proj.z();double inv_z2 = inv_z * inv_z;// 投影雅可比 (2x3)Eigen::Matrix<double, 2, 3> J_proj;J_proj << inv_z, 0, -proj.x() * inv_z2,0, inv_z, -proj.y() * inv_z2;J_proj = _K.block<2, 3>(0, 0) * J_proj; // 内参影响// 点变换雅可比 (3x6)：∂(T*p)/∂δ = [I, - (R*p)^∧]Eigen::Matrix<double, 3, 6> J_point;J_point.leftCols