视觉SLAM笔记（37） 求解 PnP

---

1. 使用 EPnP 求解位姿

首先，用 OpenCV 提供的 EPnP 求解 PnP 问题，然后通过 g2o 对结果进行优化

创建/VSLAM_note/037/pose_estimation_3d2d文件
由于 PnP 需要使用 3D 点，为了避免初始化带来的麻烦
使用了 RGB-D 相机中的深度（1_depth.png），作为特征点的 3D位置

首先来看 OpenCV 提供的 PnP 函数：

int main(int argc, char** argv)
{
    if (argc != 4)
    {
        cout << "usage: pose_estimation_3d2d img1 img2 depth1" << endl;
        return 1;
    }

    //-- 读取图像
    Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);

    //-- 特征提取
    vector<KeyPoint> keypoints_1, keypoints_2;
    vector<DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    cout << "一共找到了" << matches.size() << "组匹配点" << endl;

    // 建立3D点
    Mat d1 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);  // 深度图为16位无符号数，单通道图像
    Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1); // 相机内参
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    for (DMatch m : matches)
    {
        ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
        if (d == 0)   // bad depth
            continue;
        float dd = d / 5000.0;
        Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K); // 像素坐标转相机归一化坐标
        pts_3d.push_back(Point3f(p1.x*dd, p1.y*dd, dd));       // 添加第一个图的特征位置 3D 点
        pts_2d.push_back(keypoints_2[m.trainIdx].pt);          // 添加第二个图的特征位置 2D 点
    }

    cout << "3d-2d pairs: " << pts_3d.size() << endl;

    // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    Mat r, t;
    solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false); 

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

    cout << "R=" << endl << R << endl;
    cout << "t=" << endl << t << endl;
}

得到配对特征点后，在第一个图的深度图中寻找它们的深度，并求出空间位置，为 3D 点
再以第二个图像的像素位置为 2D 点，调用 EPnP 求解 PnP 问题

程序输出如下：

可以对比先前 2D-2D 情况下求解的 R， t

可以看到，在有 3D 信息时，估计的 R 几乎是相同的，而 t 相差的较多
这是由于引入了新的深度信息所致

不过，由于 Kinect 采集的深度图本身会有一些误差，所以这里的 3D 点也不是准确的
以此，希望可以把位姿 ξ 和所有三维特征点 P 同时优化

---

2. 使用 BA 优化

将使用前一步的估计值作为初始值
优化可以使用 Ceres 或 g2o 库实现，这里采用 g2o 作为例子

在使用 g2o 之前，要把问题建模成一个最小二乘的图优化问题

在这个图优化中，节点和边的选择为：

1. 节点：第二个相机的位姿节点 ξ $\in$ se(3)，以及所有特征点的空间位置 P $\in$ R³
2. 边：每个 3D 点在第二个相机中的投影，以观测方程来描述

由于第一个相机位姿固定为零，没有把它写到优化变量里
但在这里，希望能够把第一个相机的位姿与观测也考虑进来

现在根据一组 3D 点和第二个图像中的 2D 投影，估计第二个相机的位姿
所以把第一个相机画成虚线，表明不希望考虑它

g2o 提供了许多关于 BA 的节点和边，不必自己从头实现所有的计算
在 g2o/types/sba/types_six_dof_expmap.h 中则提供了李代数表达的节点和边

打开这个文件，找到三个类：

• VertexSE3Expmap（李代数位姿）
• VertexSBAPointXYZ（空间点位置）
• EdgeProjectXYZ2UV（投影方程边）

来简单看一下它们的类定义

---

2.1 李代数位姿

请注意它的模板参数

/**
 * \brief SE3 Vertex parameterized internally with a transformation matrix
 and externally with its exponential map
 */
class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  VertexSE3Expmap();

  bool read(std::istream& is);

  bool write(std::ostream& os) const;

  virtual void setToOriginImpl() {
    _estimate = SE3Quat();
  }

  virtual void oplusImpl(const number_t* update_)  {
    Eigen::Map<const Vector6> update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());
  }
};

第一个参数 6 表示它 内部存储的优化变量维度，可以看到这是一个 6 维的李代数
第二参数是 优化变量的类型，这里使用了 g2o 定义的相机位姿： SE3Quat
这个类内部使用了四元数加位移向量来存储位姿，但同时也支持李代数上的运算
例如对数映射（log 函数）和李代数上增量（update 函数）等操作

可以对照它的实现代码，看看 g2o 对李代数是如何操作的：

---

2.2 空间点位置

就不把整个类定义都搬过来了

 class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, Vector3D>

从模板参数可以看到，空间点位置类的维度为 3，类型是 Eigen 的 Vector3D

---

2.3 投影方程边

投影方程边 EdgeProjectXYZ2UV 连接了两个前面说的两个顶点

class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public  BaseBinaryEdge<2, Vector2, VertexSBAPointXYZ, VertexSE3Expmap>{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    EdgeProjectXYZ2UV();

    bool read(std::istream& is);

    bool write(std::ostream& os) const;

    void computeError()  {
      const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
      const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
      const CameraParameters * cam
        = static_cast<const CameraParameters *>(parameter(0));
      Vector2 obs(_measurement);
      _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
    }

    virtual void linearizeOplus();

    CameraParameters * _cam;
};

它的观测值为 2 维，由 Vector2D 表示，实际上就是空间点的像素坐标
它的误差计算函数表达了投影方程的误差计算方法，也就是前面提到的 $z − h(ξ, P )$ 的方式

进一步观察 EdgeProjectXYZ2UV 的 linearizeOplus 函数的实现
这里用到了前面推导的雅可比矩阵：

void EdgeProjectXYZ2UV::linearizeOplus() {
  VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
  SE3Quat T(vj->estimate());
  VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
  Vector3 xyz = vi->estimate();
  Vector3 xyz_trans = T.map(xyz);

  number_t x = xyz_trans[0];
  number_t y = xyz_trans[1];
  number_t z = xyz_trans[2];
  number_t z_2 = z*z;

  const CameraParameters * cam = static_cast<const CameraParameters *>(parameter(0));

  Matrix<number_t,2,3,Eigen::ColMajor> tmp;
  tmp(0,0) = cam->focal_length;
  tmp(0,1) = 0;
  tmp(0,2) = -x/z*cam->focal_length;

  tmp(1,0) = 0;
  tmp(1,1) = cam->focal_length;
  tmp(1,2) = -y/z*cam->focal_length;

  _jacobianOplusXi =  -1./z * tmp * T.rotation().toRotationMatrix();

  _jacobianOplusXj(0,0) =  x*y/z_2 *cam->focal_length;
  _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *cam->focal_length;
  _jacobianOplusXj(0,2) = y/z *cam->focal_length;
  _jacobianOplusXj(0,3) = -1./z *cam->focal_length;
  _jacobianOplusXj(0,4) = 0;
  _jacobianOplusXj(0,5) = x/z_2 *cam->focal_length;

  _jacobianOplusXj(1,0) = (1+y*y/z_2) *cam->focal_length;
  _jacobianOplusXj(1,1) = -x*y/z_2 *cam->focal_length;
  _jacobianOplusXj(1,2) = -x/z *cam->focal_length;
  _jacobianOplusXj(1,3) = 0;
  _jacobianOplusXj(1,4) = -1./z *cam->focal_length;
  _jacobianOplusXj(1,5) = y/z_2 *cam->focal_length;
}

仔细研究此段代码，会发现它与在 视觉SLAM笔记（36） 3D-2D: PnP 的公式：

是一致的

成员变量“_jacobianOplusXi ”是误差到空间点的导数
成员变量“_jacobianOplusXj”是误差到相机位姿的导数
以李代数的左乘扰动表达

稍有差别的是， g2o 的相机里用 $f$ 统一描述 $f$_x, $f$_y
并且李代数定义顺序不同（g2o 是旋转在前，平移在后；定义是平移在前，旋转在后）
所以矩阵前三列和后三列与定义是颠倒的
此外都是一致的

值得一提的是，我们亦可自己实现相机位姿节点，并使用 Sophus::SE3 来表达位姿，提供类似的求导过程
然而，既然 g2o 已经提供了这样的类，在没有额外要求的情况下，自己重新实现就没有必要了

---

2.4 Bundle Adjustment

程序大体上和 视觉SLAM笔记（29） g2o 类似
首先声明了 g2o 图优化，配置优化求解器和梯度下降方法
然后根据估计到的特征点，将位姿和空间点放到图中
最后调用优化函数进行求解

void bundleAdjustment(
    const vector< Point3f > points_3d,
    const vector< Point2f > points_2d,
    const Mat& K,
    Mat& R, Mat& t)
{
    // 构建图优化，先设定g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> > Block;  // pose 维度为 6, landmark 维度为 3
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linearSolver));         // 矩阵块求解器

    // 梯度下降方法，从GN, LM, DogLeg 中选
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr));
    g2o::SparseOptimizer optimizer;   // 图模型
    optimizer.setAlgorithm(solver);   // 设置求解器
    optimizer.setVerbose(true);       // 打开调试输出

    // 增加顶点
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    Eigen::Matrix3d R_mat;
    R_mat <<
        R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
        R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
        R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2);
    pose->setId(0);
    pose->setEstimate(g2o::SE3Quat(
        R_mat,
        Eigen::Vector3d(t.at<double>(0, 0), t.at<double>(1, 0), t.at<double>(2, 0))
    ));
    optimizer.addVertex(pose);

    int index = 1;
    for (const Point3f p : points_3d)   // landmarks
    {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId(index++);
        point->setEstimate(Eigen::Vector3d(p.x, p.y, p.z));
        point->setMarginalized(true); // g2o 中必须设置 marg
        optimizer.addVertex(point);
    }

    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters(
        K.at<double>(0, 0), Eigen::Vector2d(K.at<double>(0, 2), K.at<double>(1, 2)), 0
    );
    camera->setId(0);
    optimizer.addParameter(camera);


    // 增加边
    index = 1;
    for (const Point2f p : points_2d)
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId(index);
        edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(index)));
        edge->setVertex(1, pose);
        edge->setMeasurement(Eigen::Vector2d(p.x, p.y));
        edge->setParameterId(0, 0);
        edge->setInformation(Eigen::Matrix2d::Identity());
        optimizer.addEdge(edge);
        index++;
    }

    // 执行优化
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.initializeOptimization();
    optimizer.optimize(100);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> (t2 - t1);
    cout << "optimization costs time: " << time_used.count() << " seconds." << endl;

    // 输出优化值
    cout << endl << "after optimization:" << endl;
    cout << "T=" << endl << Eigen::Isometry3d(pose->estimate()).matrix() << endl;
}

在主函数最后添加调用：

// 使用 BA 优化
cout << "calling bundle adjustment" << endl;
bundleAdjustment(pts_3d, pts_2d, K, R, t);

再编译执行，可以看到优化的结果：

迭代 11 轮后， LM 发现优化目标函数接近不变，于是停止了优化
输出了最后得到位姿变换矩阵 T，对比之前直接做 PnP 的结果，大约在小数点后第三位发生了一些变化
这主要是由于同时优化了特征点和相机位姿导致的

Bundle Adjustment 是一种通用的做法
它可以不限于两个图像
完全可以放入多个图像匹配到的位姿和空间点进行迭代优化，甚至可以把整个 SLAM 过程放进来
那种做法规模较大，主要在后端使用，会在后续重新遇到这个问题
在前端，通常考虑局部相机位姿和特征点的小型 Bundle Adjustment 问题
希望实时对它进行求解和优化

---

参考：

《视觉SLAM十四讲》

---

相关推荐：

视觉SLAM笔记（36） 3D-2D: PnP
视觉SLAM笔记（35） 三角化求特征点的空间位置
视觉SLAM笔记（34） 三角测量
视觉SLAM笔记（33） 对极约束求解相机运动
视觉SLAM笔记（32） 2D-2D: 对极几何

---