PnP算法

 个人博客：http://www.chenjianqu.com/

原文链接：http://www.chenjianqu.com/show-95.html

 PnP（Perspective-n-Point）是求解 3D 到 2D 点对运动的方法。如果两张图像中其中一张特征点的 3D 位置已知，那么最少只需 3 个点对就可以使用PnP估计相机运动。特征点的 3D 位置可以由三角化或者 RGB-D 相机的深度图确定。PnP 问题有很多种求解方法：用3对点估计位姿的 P3P、直接线性变换(DLT)、EPnP(Efficient PnP)、UPnP等。此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，即Bundle Adjustment。

 

DLT

    某个空间点P 在世界坐标系的齐次坐标为P=(X,Y,Z,1)，在相机O1的归一化坐标为x1=(u1,v1,1)，归一化坐标可由像素坐标p1转换而来：x1=K-1 p1。此时相机的位姿R,t未知，定义变换矩阵T=[R|t]为一个3x4的矩阵，包含旋转和平移，根据刚体运动的三维变换关系，得：

    其中s是P点在相机坐标系的三维坐标的Z轴的值，即P点的深度。把s消去，得到两个约束：

    为简化表示，定义变换矩阵T的行向量：t1=(t1,t2,t3,t4)T，t2=(t5,t6,t7,t8)T，t3=(t9,t10,t11,t12)T。于是有：

t1TP - t3T Pu1 = 0,  t2TP - t3T Pv1 = 0

    t 是待求变量，每个特征点提供两个关于t的线性约束。设共有N个特征点，则列得如下方程组：

    

    T有 12 参数，每对点提供两个约束，因此最少通过 6 对3D-2D匹配点即可实现矩阵 T 的线性求解，这种方法称为直接线性变换（Direct Linear Transform，DLT）。当匹配点大于 6 对时，也可以使用 SVD 等方法对超定方程求最小二乘解。

    在 DLT 求解中，我们直接将变换矩阵T看成了 12 个未知数，忽略了它们之间的联系。因为旋转矩阵 R ∈ SO(3)，用 DLT 求出的解不一定满足该约束，是一个一般矩阵。对于旋转矩阵 R，必须针对 DLT 估计的 T左边 3 × 3 的矩阵块，寻找一个最好的旋转矩阵对它进行近似。这可以由 QR 分解完成，相当于把结果从矩阵空间重新投影到 SE(3)流形上，转换成旋转和平移两部分。

 

P3P

    解 PnP 的另一种方法是P3P，该方法并不直接求解运动，而是通过求解特征点的世界坐标的相机坐标，把2D-3D问题转换为3D-3D问题，然后再使用ICP求解。P3P使用 3 对匹配点进行计算，还需要一对验证点D−d，从可能的解中选出正确的。记 3D 点为A, B, C，2D 点为 a, b, c，其中小写字母代表的点为对应大写字母代表的点在相机成像平面上的投影。注意，A, B, C 在世界坐标系中的3维坐标，而不是在相机坐标系。如下：

    三角形之间的对应关系：∆Oab − ∆OAB, ∆Obc − ∆OBC, ∆Oac − ∆OAC. 对于利用余弦定理，有：

    对以上公式除以 OC2，并记 x = OA/OC, y = OB/OC，得：

    记 v = AB2/OC2, u = BC2/AB2, w = AC2/AB2,则uv = BC2/OC2, wv = AC2/OC2，有：

    把第一个式子中的 v 放到等式一边，并代入其后两式，得：

    已经知道2D 点的图像位置，则3个余弦角 cos〈a, b〉, cos〈b, c〉, cos〈a, c〉是已知的，u,w可以通过 A,B,C 在世界坐标系下的坐标算出。剩下的 x, y 是未知的，随着相机移动会发生变化。因此，该方程组是关于 x, y 的一个二元二次方程（多项式方程）。解析地求解该方程组是一个复杂的过程，需要用吴消元法。

    该方程最多可能得到 4 个解，可以用验证点来计算最可能的解，得到 A, B, C在相机坐标系下的 3D 坐标。然后，根据 3D−3D 的点对，使用ICP计算相机的运动 R, t。

    存在的问题：1. P3P 只利用 3 个点的信息。当给定的配对点多于 3 组时，难以利用更多的信息。2. 如果 3D 点或 2D 点受噪声影响，或者存在误匹配，则算法失效。

 

Bundle Adjustment

    在PnP中，Bundle Adjustment 是一个最小化重投影误差(Reprojection error)的问题。

重投影误差

    考虑 n 个三维空间点 P 及其投影 p，我们希望计算相机的位姿 R, t，变换矩阵的李代数表示为 ξ。假设个空间点的世界坐标为 Pi = [Xi, Yi, Zi]，其投影的像素坐标为 ui = [ui, vi]，根据三维变换和李代数，有：

    矩阵形式：si ui = K exp (ξ^) Pi，由于相机位姿未知及观测点的噪声，该等式左右两边存在一个误差。即我们刚开始不知道确切的相机位姿exp (ξ^)，可能会随机猜测一个，也可能使用DLT或P3P求解得到的位姿T(但由于噪声的存在并不精确)，然后通过最小二乘优化，最终得到更加精确的位姿。即我们把误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使它最小化：

    该问题的误差项，是将像素坐标(观测到的投影位置)与 3D 坐标按照当前估计的位姿进行投影得到的位置相比较得到的误差，所以称为重投影误差。使用齐次坐标时，这个误差有 3 维。不过由于像素齐次坐标ui最后一维为 1，该维度的误差一直为零，因而我们更多时候使用非齐次坐标，这样误差就只有2 维。其示意图如下：

   

    如上图所示，我们通过特征匹配知道了 p1 和 p2 是同一个空间点 P 的投影，但是不知道相机的位姿。在初始值中，P 的投影 p2帽 与实际的 p2 之间有一定的距离。于是我们调整相机的位姿，使得这个距离变小。不过，由于这个调整需要考虑很多个点，所以最后每个点的误差通常都不会精确为零。

关于位姿的雅可比矩阵推导

    上面已经构建好了最小二乘问题，根据<非线性最小二乘求解方法>，首先要知道每个误差项关于优化变量的导数，即线性化：e(x + ∆x) ≈ e(x) + J∆x。当误差项e为像素坐标误差(2维)，x为相机位姿(6维)时，雅可比矩阵J将是一个2×6的矩阵。雅可比矩阵 J 的形式如下：

    已知空间点P的世界坐标，记P变换到相机坐标系下的空间点坐标为 P′，将其前3维(齐次坐标，共有4维)取出来，得：P’=(exp(ξ^)P)1:3 = [X’,Y’,Z’]，注意此时ξ并未精确的知道。由相机模型有：su=KP’，注意这里的u是像素坐标，下面的u,v是像素坐标的值，展开得：

    利用第 3 行消去 s（即 P′ 的距离），得：

    即上面的像素坐标u=(u,v)是由空间点 P 根据 ξ 投影得到像素平面的坐标。u=(u,v)是我们定义的中间变量，下面开始求导。我们还知道P投影点的真实值ui，因此可以把重投影坐标与真实坐标进行比较， 然后求差，也就是最小二乘的公式，e=

    该公式的自变量是位姿李代数ξ，因变量P投影到相机坐标系的P'。根据<李群与李代数>，对 ξ^左乘扰动量 δξ，然后考虑 e 的变化关于扰动量的导数。利用链式法则，可以列写如下(⊕ 指李代数上的左乘扰动)：

    第一项是误差关于投影点的导数，且：

    第二项为变换后的点关于李代数的导数，根据<李群与李代数>中的推导，有：

    因为P' = (TP)1:3，即取出了前三维，故得：

    第一项和第二项相乘，得到雅可比矩阵 J 如下：

    雅可比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系。保留前面的负号是因为误差是由观测值减预测值定义的。

关于空间点的雅可比矩阵推导

    除了优化位姿，我们还希望优化特征点的空间位置。 e 关于空间点P 的导数如下：

    第一项前面推导过了，第二项，按照定义：P’=(exp(ξ^)P)1:3 = RP+t，P'对P求导只剩下R，因此得雅可比矩阵如下：

    有了雅可比矩阵，就可以使用<非线性最小二乘求解方法>进行优化了。

 

代码实现

    OpenCV提供了求解PnP的函数，如下：

bool solvePnP( 
    InputArray objectPoints, //匹配的3D空间点，第一张图片
    InputArray imagePoints,//匹配的2D特征点，第二张图片
    InputArray cameraMatrix, //内参矩阵
    InputArray distCoeffs,//畸变系数的输入变量
    OutputArray rvec, //旋转矩阵
    OutputArray tvec,//平移向量
    bool useExtrinsicGuess = false, //是否进行非线性优化
    int flags = SOLVEPNP_ITERATIVE //求解算法的选择
);
flag参数如下：
-   **SOLVEPNP_ITERATIVE** Iterative method is based on Levenberg-Marquardt optimization. In
this case the function finds such a pose that minimizes reprojection error, that is the sum
of squared distances between the observed projections imagePoints and the projected (using
projectPoints ) objectPoints .

-   **SOLVEPNP_P3P** Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang
"Complete Solution Classification for the Perspective-Three-Point Problem" (@cite gao2003complete).
In this case the function requires exactly four object and image points.

-   **SOLVEPNP_AP3P** Method is based on the paper of T. Ke, S. Roumeliotis
"An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (@cite Ke17).
In this case the function requires exactly four object and image points.

-   **SOLVEPNP_EPNP** Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the
paper "EPnP: Efficient Perspective-n-Point Camera Pose Estimation" (@cite lepetit2009epnp).

-   **SOLVEPNP_DLS** Method is based on the paper of Joel A. Hesch and Stergios I. Roumeliotis.
"A Direct Least-Squares (DLS) Method for PnP" (@cite hesch2011direct).

-   **SOLVEPNP_UPNP** Method is based on the paper of A.Penate-Sanchez, J.Andrade-Cetto,
F.Moreno-Noguer. "Exhaustive Linearization for Robust Camera Pose and Focal Length
Estimation" (@cite penate2013exhaustive). In this case the function also estimates the parameters \f$f_x\f$ and \f$f_y\f$
assuming that both have the same value. Then the cameraMatrix is updated with the estimated
focal length.

-   **SOLVEPNP_AP3P** Method is based on the paper of Tong Ke and Stergios I. Roumeliotis.
"An Efficient Algebraic Solution to the Perspective-Three-Point Problem" (@cite Ke17). In this case the
function requires exactly four object and image points.

    旋转矩阵和旋转向量使用Rodrigues()转换：

void Rodrigues( 
    InputArray src, //输入，rotation vector (3x1 or 1x3) or rotation matrix (3x3).
    OutputArray dst, //输出，rotation matrix (3x3) or rotation vector (3x1 or 1x3)
    OutputArray jacobian = noArray() //Optional output Jacobian matrix, 3x9 or 9x3, which is a matrix of partial derivatives of the output array components with respect to the input array components.
);

 

    下面的代码先使用OpenCV的solvePnP()求解出运动，然后再使用高斯牛顿法进行BA优化。

CMakeLists.txt

cmake_minimum_required(VERSION 2.6)
project(epnp)

set(CMAKE_BUILD_TYPE Release)
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

#添加CMAKE_MODULE_PATH路径
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

# OpenCV
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

# Eigen
include_directories("/home/chenjianqu/software/eigen-3.3.5")

find_package( Sophus REQUIRED )
include_directories( ${Sophus_INCLUDE_DIRS} )

add_executable(epnp main.cpp)
target_link_libraries(epnp ${OpenCV_LIBS})
install(TARGETS epnp RUNTIME DESTINATION bin)

main.cpp

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <sophus/se3.hpp>

using namespace std;
using namespace cv;

typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;

void find_feature_matches(
  const Mat &img_1, const Mat &img_2,
  std::vector<KeyPoint> &keypoints_1,
  std::vector<KeyPoint> &keypoints_2,
  std::vector<DMatch> &matches);

  // BA by gauss-newton
void bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose
);

// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &p, const Mat &K);


int main(int argc, char **argv) 
{
  //读取图像
  Mat img_1 = imread("1.png", CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread("2.png", CV_LOAD_IMAGE_COLOR);
  assert(img_1.data && img_2.data && "Can not load images!");

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

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

  Mat r, t;
  //求解PnP
  solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false); 

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

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


  VecVector3d pts_3d_eigen;
  VecVector2d pts_2d_eigen;
  for (size_t i = 0; i < pts_3d.size(); ++i) {
    pts_3d_eigen.push_back(Eigen::Vector3d(pts_3d[i].x, pts_3d[i].y, pts_3d[i].z));
    pts_2d_eigen.push_back(Eigen::Vector2d(pts_2d[i].x, pts_2d[i].y));
  }
  
  cout << "使用高斯牛顿法进行bundle adjustment" << endl;
  Sophus::SE3d pose_gn;
  bundleAdjustmentGaussNewton(pts_3d_eigen, pts_2d_eigen, K, pose_gn);

  
 
  return 0;
}


void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  //-- 初始化
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  //-- 第一步:检测 Oriented FAST 角点位置
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);
  //-- 第二步:根据角点位置计算 BRIEF 描述子
  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);
  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
  vector<DMatch> match;
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);
  //-- 第四步:匹配点对筛选
  double min_dist = 10000, max_dist = 0;
  //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }
  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);
  //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}

Point2d pixel2cam(const Point2d &p, const Mat &K) {
  return Point2d
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}


void bundleAdjustmentGaussNewton(
		    const VecVector3d &points_3d,
		    const VecVector2d &points_2d,
		    const Mat &K,
		    Sophus::SE3d &pose)
{
  
    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();
	Vector6d b = Vector6d::Zero();
	cost = 0;
	//所有特征点的代价计算
	for (int i = 0; i < points_3d.size(); i++) 
	{
	    //计算P'
	    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*dx=b，H=J^2，b=J*e
	    H += J.transpose() * J;
	    b += -J.transpose() * e;
	}

	//解方程：H*dx=b，其中dx是李代数的增量
	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;
	}

	//更新姿态
	pose = Sophus::SE3d::exp(dx) * pose;
	lastCost = cost;

	cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;
	//迭代已收敛
	if (dx.norm() < 1e-6) {//norm()表示绝对值
	    break;
	}
    }

    cout << "pose by g-n: \n" << pose.matrix() << endl;
}

 

参考文献

[0]高翔.视觉SLAM14讲