第7讲2 --- 求解相机的运动

最新推荐文章于 2022-03-23 17:09:03 发布

置顶不服输的小白

最新推荐文章于 2022-03-23 17:09:03 发布

阅读量3.1k

点赞数 4

分类专栏：视觉SLAM

本文链接：https://blog.csdn.net/llfjcmx/article/details/82555934

版权

视觉SLAM 专栏收录该内容

14 篇文章 18 订阅

订阅专栏

上一篇博客中学习了特征提取和匹配的概念，并且调用OpenCV库实现了ORB特征的提取和匹配。

找到了匹配点后，我们希望能够根据匹配的点对来估计相机的运动。一共有三种方法可以用来估计相机的运动，分别为对极几何约束、PnP、ICP。这三种方法分别可以用来处理不同情况下的相机的位姿估计。

当相机为单目的时候，我们只知道2D的像素坐标，因而问题是根据两组2D点估计运动。该问题用对极几何来解决。
如果有3D点及其在相机的投影位置，即3D-2D问题，也能估计相机的运动，该问题通过PnP来求解。
当相机为双目、RGB-D时，或者通过某种方法得到了距离信息，那么问题就是根据两组3D点估计运动，即3D-3D问题。该问题通常用ICP来解决。

1、对极约束

下面先来看一下两个相邻帧之间的匹配点有什么关系？

以上图为例，我们设从 $I_{1}$ 到 $I_{2}$ 的运动为 $R,\vec{t}$ ，两个相机的中心分别为 $O_{1},O_{2}$ 。

现在，假设 $I_{1}$ 中有一个特征点 $p_{1}$ ，它在 $I_{2}$ 中对应的点为 $p_{2}$

（我们知道这是通过特征匹配得到的，如果匹配结果正确的话，就可以认为 $p_{1},p_{2}$ 是同一个空间点在两个成像平面上投影）

现在，我们假设匹配结果是正确的，然后就可以开始下面的数学推导了：

在 $I_{1}$ 的坐标系下，设点的空间位置为 $P=[X,Y,Z]^{T}$ ，

根据针孔相机模型可以知道，两个像素点 $p_{1},p_{2}$ 的像素位置满足：

$z_{1}p_{1}=KP$ ， $z_{2}p_{2}=K(RP+t)$

因为使用的是齐次坐标，所以在上式的左边乘以任意一个非零常数也是成立的，则有：

$p_{1}=KP$ ， $p_{2}=K(RP+t)$

现在取两个像素点在归一化平面上的坐标： $x_{1}=K^{-1}p_{1}$ ， $x_{2}=K^{-1}p_{2}$ ，代入上式中得：

$x_{2}=Rx_{1}+t$

然后两边同时左乘 $t^{\Lambda }$ ，注意： $t^{\Lambda }t=t\times t=0$ ，得：

$t^{\Lambda }x_{2}=t^{\Lambda }Rx_{1}$

然后，两式同时左乘 $x_{2}^{T}$ ，得到：

$x_{2}^{T}t^{\Lambda }x_{2}=x_{2}^{T}t^{\Lambda }Rx_{1}$

因为 $t^{\Lambda }x_{2}=t\times x_{2}$ 是一个和 $t,x_{2}$ 都垂直的向量，所以： $x_{2}^{T}t^{\Lambda }x_{2}$ =0，则有：

$x_{2}^{T}t^{\Lambda }Rx_{1}=0$

将 $x_{1},x_2$ 的值重新代入得到：

${\color{Red} p_{2}^{T}K^{-T}t^{\Lambda }RK^{-1}p_{1}=0}$

上式就称之为对极约束，它的几何意义是 O_1,O_2,P 共面。

我们把上式的中间两个部分记为两个矩阵：基础矩阵（Fundamental matrix）F、本质矩阵（Essential matrix）E

${\color{Red} E=t^{\Lambda }R}$ ， ${\color{Red} F=K^{-T}t^{\Lambda }RK^{-1}=K^{-T}EK^{-1}}$

于是对极约束进一步简化为下式：

有了上面的基础之后，相机的位姿估计问题就可以分解为下面两步：

（1）根据相邻帧配对点的像素位置求出E或者F

（2）根据E或者F求出R, t

注：由对极几何的表达式也可以知道利用对极几何仅根据配对的像素点位置就可以求相机运动，记为 2D-2D 问题（瞎说的）

------------------------------------------------------本质矩阵E------------------------------------------------

上面说了，根据 E 或者 F 都可以求出R, t，E 和 F 之间相差的就是相机内参K，而相机的内参矩阵在SLAM问题中一般是已知的，所以我们经常使用形式更加简单的本质矩阵E 来求解相机的运动。

根据定义，本质矩阵 $E=t^{\Lambda}R$ ，它是一个3*3的矩阵，内有9个未知数。另一方面，由于旋转和平移各有3个自由度，故 $E=t^{\Lambda}R$ 一共有6个自由度，但由于尺度等价性，实际上只有5个自由度。具有5个自由度的事实，表明我们最少可以用5对点来求解。但是的内在性质是一种非线性性质，在求解线性方程时会带来麻烦，因此，也可以只考虑它的尺度等价性，使用8对点来估计---------这就是经典的八点法。（ps: 我对这段话不是很理解）

接下来的问题就是如何根据估计到的本质矩阵，来分解出R和t了。这个过程是通过奇异值分解得到的。

------------------------------------------------------单应矩阵H------------------------------------------------

单应矩阵H，描述了两个平面之间的映射关系。若场景中的特征点都落在同一个平面上（比如墙、地面），则可以通过单应性来进行运动估计。这种情况在无人携带的俯视相机或者扫地机携带的顶视相机中比较常见。

下面从数学的角度说明一下什么是单应矩阵：

考虑在图像 $I_{1},I_{2}$ 上有一对匹配好的特征点 $p_{1},p_{2}$ ，这些特征点落在平面上，设这个平面的方程为：

$n^{T}P+d=0$

整理一下，就是：

$-\frac{n^{T}P}{d}=1$

又因为： $p_{2}=K(RP+t)$ ，可以得到：

$p_{2}=K(RP+t(-\frac{n^{T}P}{d}))=K(R-\frac{tn^{T}}{d})P=K(R-\frac{tn^{T}}{d})K^{-1}p_{1}$

令 $H=K(R-\frac{tn^{T}}{d})K^{-1}$ ，于是 $p_{2}=Hp_{1}$ （H就是所谓的单应矩阵）

自由度为8的单应矩阵可以通过4对匹配特征点算出（这些特征点不能有三点共线的情况），然后通过H分解得到R，t。

计算出两幅图像之间的单应矩阵H之后，可以有如下用途：

（1）根据H分解出相机的运动R、t

（2）应用 $p_{2}=Hp_{1}$ 这个关系可以将一个视图中的所有点变换到另一个图的视角下的图片。

实践部分：下面看一下如何使用OpenCV计算基础矩阵、本质矩阵、单应矩阵以及分解R、T和利用单应矩阵变换图像的视角。

feature_extract_match.hpp

（这个文件中定义了特征提取和特征匹配函数，之后所有关于ORB特征提取和匹配的操作都调用这个头文件里面的代码）

#ifndef FEATURE_EXTRACT_MATCH_H
#define FEATURE_EXTRACT_MATCH_H

#include <iostream>
using namespace std;

//opencv
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace cv;

/*
实现两张图像的特征提取和匹配：
使用引用的目的是为了直接将提取和匹配的结果保存在key_point_1，key_point_2，matches中
*/
void feature_extract_match(Mat img1, Mat img2, vector<KeyPoint> &key_point_1, vector<KeyPoint> &key_point_2, vector<DMatch> &matches)
{

    //创建ORB提取器、描述子提取器、描述子匹配器
    Ptr<FeatureDetector> orb_detectore = ORB::create(); 
    Ptr<DescriptorExtractor> orb_descriptor = ORB::create();
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");

    //定义变量
    Mat descriptor_1, descriptor_2;
    vector<DMatch> matches_before_filter;
    Mat img_matches_before_filter, img_good_matches;

    //提取ORB关键点
    orb_detectore->detect(img1, key_point_1);
    orb_detectore->detect(img2, key_point_2);

    //计算描述子
    orb_descriptor->compute(img1, key_point_1, descriptor_1);
    orb_descriptor->compute(img2, key_point_2, descriptor_2);

    //特征点匹配，匹配结果中会包含一些误匹配的点
    matcher->match(descriptor_1, descriptor_2, matches_before_filter);

    //剔除误匹配的点：汉明距离大于两倍最小距离的点剔除
    double min_distance = 9999;
    for(int i = 0; i < descriptor_1.rows; i++)
    {
        if(matches_before_filter[i].distance < min_distance)
        {
            min_distance = matches_before_filter[i].distance;
        }
    }
    cout << "min_distance = " << min_distance << endl;
    
    //有的时候最小距离非常小，这个情况下取最小距离为一个经验值30
    for( int i = 0; i < descriptor_1.rows; i++ )
    {
        if(matches_before_filter[i].distance <= max(2*min_distance, 30.0))
        {
            matches.push_back( matches_before_filter[i] );
        }
    }

    drawMatches(img1, key_point_1, img2, key_point_2, matches, img_good_matches);
    drawMatches(img1, key_point_1, img2, key_point_2, matches_before_filter, img_matches_before_filter);
    // imshow("matches_before_filter", img_matches_before_filter);
    // imshow("goog_matches", img_good_matches);
    // waitKey(0);
}

#endif

estimation.hpp:

/*
在这里，要接触到OpenCV的两个重要的模块：
（1）features2d模块：进行特征提取和匹配要用到的模块
（2）calib3d模块：这是一个之前没有接触过的模块，它是OpenCV实现的一个相机校准和姿态估计模块
*/

#ifndef ESTIMATION_H
#define ESTIMATION_H

#include "feature_extract_match.hpp"

//opencv
#include <opencv2/calib3d/calib3d.hpp>

//估计相机的运动：返回R、t
void pose_estimation_2d2d(vector<KeyPoint> key_points_1, vector<KeyPoint> key_points_2, vector<DMatch> matches, Mat &R, Mat &t)
{
    //内参矩阵
    double fx, fy, cx, cy;
    Mat K = Mat::eye(3, 3, CV_64FC1);   //Mat::eye()返回一个指定大小和类型的恒定矩阵
    fx = 520.9;
    fy = 521.0;
    cx = 325.1;
    cy = 249.7;
    K.at<double>(0, 0) = fx;
    K.at<double>(0, 2) = cx;
    K.at<double>(1, 1) = fy;
    K.at<double>(1, 2) = cy;
    cout << "internal reference matrix = " << endl;
    cout << K << endl;

    //将所有的KeyPoint转化为Point2f
    vector<Point2f> points_1;
    vector<Point2f> points_2;
    for(int i = 0; i < matches.size(); i++)
    {
        points_1.push_back(key_points_1[matches[i].queryIdx].pt);   //pt属性是KeyPoint的坐标
        points_2.push_back(key_points_2[matches[i].trainIdx].pt);
    }

    //计算基础矩阵
    Mat fundamental_matrix;
    fundamental_matrix = findFundamentalMat(points_1, points_2, CV_FM_8POINT);  //最后一个参数CV_FM_8POINT表示使用8点法计算基础矩阵
    cout << "fundamental_matrix = " << endl;
    cout << fundamental_matrix << endl;

    //直接调用findEssentialMat()函数来求本质矩阵，findEssentialMat()有两种原型，本次要调用的为下面这一种
    //findEssentialMat(InputArray points1, InputArray points2, double focal = 1.0, Point2d pp = Point2d(0, 0), int method = RANSAC, double prob = 0.999, double threshold = 1.0, OutputArray mask = noArray());
    double focal = 521;
    Point2d pp(325.1, 249.7);
    Mat essential_matrix;
    essential_matrix = findEssentialMat(points_1, points_2, focal, pp);
    cout << "essential_matrix = " << endl;
    cout << essential_matrix << endl;

    //从本质矩阵恢复R、t
    //recoverPose(InputArray E, InputArray points1, InputArray points2, OutputArray R, OutputArray t, double focal = 1.0, Point2d pp = Point2d(0, 0), InputOutputArray = noArray());
    recoverPose(essential_matrix, points_1, points_2, R, t, focal, pp);
}

//计算单应矩阵
void homograph_estimation(vector<KeyPoint> key_points_1, vector<KeyPoint> key_points_2, vector<DMatch> matches, Mat &H)
{
    //将所有的KeyPoint转换为Point2f
    vector<Point2f> points_1, points_2;
    for(int i = 0; i < matches.size(); i++)
    {
        points_1.push_back(key_points_1[matches[i].queryIdx].pt);
        points_2.push_back(key_points_2[matches[i].trainIdx].pt);
    }

    //计算单应矩阵
    H = findHomography(points_1, points_2, RANSAC);
    cout << "H = " << endl;
    cout << H <<endl;
}

#endif

main.hpp:

#include "estimation.hpp"
#include "feature_extract_match.hpp"

//opencv，warpPerspective()在这个头文件中
#include "opencv2/imgproc.hpp"

int main(int argc, char ** argv)
{
    Mat img1, img2;
    img1 = imread("../datas/1.png");
    img2 = imread("../datas/2.png");

    vector<KeyPoint> key_points_1, key_points_2;
    vector<DMatch> matches;
    feature_extract_match(img1, img2, key_points_1, key_points_2, matches);

    cout << "一共找到了： " << matches.size() << "个匹配点" << endl;

    Mat R, t;
    pose_estimation_2d2d(key_points_1, key_points_2, matches, R, t);
    cout << "R = " << endl;
    cout << R << endl;
    cout << "t = " << endl;
    cout << t << endl;

    Mat H;
    Mat img_warp_perspective;
    homograph_estimation(key_points_1, key_points_2, matches, H);
    warpPerspective(img1, img_warp_perspective, H, img2.size());    //利用单应矩阵进行转换
    imshow("img1", img1);
    imshow("img2", img2);
    imshow("img_warp_perspective", img_warp_perspective);
    waitKey(0);

    return 0;
}

2、3D-2D --- PnP

PnP也是求解相机运动的一种方法，它描述了当知道了n个3D空间点以及它们对应的像素点（在成像平面上的投影点）时，如何估计相机的位姿。

在PnP方法中，最少只需要知道3个点对（额外还至少需要一个点来验证结果）就可以估计相机运动。特征点的3D位置可以由三角测量或者rgb-d相机的深度图给出。因此，在双目或者rgb-d的视觉里程计中，我们可以直接用PnP方法来估计相机运动。但是在单目视觉里程计中，必须先进行初始化，然后通过三角测量的方法得到深度才能使用PnP。

由于3D-2D方法不需要使用对极约束，并且只需要很少的匹配点就可以较好的估计相机的运动，因此它也是最重要的一种姿态估计方法。

PnP问题有很多种求解方法，例如：P3P、DLT、EPnP、UPnP等等，此外还可以用非线性优化的方式，构建最小二乘问题迭代求解，也就是BA。这里暂时只学习其中的P3P方法。

给出一个博客，很好的参考资料：https://www.cnblogs.com/mafuqiang/p/8302663.html#commentform

P3P需要利用给定的3对点的几何关系，它的输入为3对3D-2D匹配点。此外，它还需要一对验证点，为了从可能的解中选出正确的那一个。

下面看一下解决该问题的简单思路：

如上图一样，A、B、C是已知的3D点，对应的2D点为a、b、c，相机光心为O。

（首先需要清楚的一点是P3P并不是直接根据3D-2D点求出相机的位姿矩阵，而是先求出对应的2D点在当前相机坐标系下的3D坐标，然后根据世界坐标系下的3D坐标和当前相机坐标系下的3D坐标来求解相机姿态的。-这应该是后面要讲的ICP问题）

首先，从余弦定理出发，可以得到下面的关系式：

$OA^{2}+OB^{2}-2*OA*OB\cos<a,b>=AB^{2}$

$OB^{2}+OC^{2}-2*OB*OC\cos<b,c>=BC^{2}$

$OA^{2}+OC^{2}-2*OA*OC\cos<a,c>=AC^{2}$

上面的三个式子同时除以 $OC^{2}$ ，并令 $x=\frac{OA}{OC},y=\frac{OB}{OC}$ ，得：

$x^{2}+y^{2}-2xy\cos<a,b>=\frac{AB^{2}}{OC^{2}}$

$y^{2}+1-2y\cos<b,c>=\frac{BC^{2}}{OC^{2}}$

$x^{2}+1-2x\cos<a,c>=\frac{AC^2}{OC^2}$

下面做一下换元： $v=\frac{AB^2}{OC^2},uv=\frac{BC^2}{OC^2},wv=\frac{AC^2}{OC^2}$ ，得到：

$x^{2}+y^{2}-2xy\cos<a,b>=v$

$y^{2}+1-2y\cos<b,c>=uv$

$x^{2}+1-2x\cos<a,c>=wv$

然后把第一个式子带入下面两个式子中可以得到：

$(1-u)y^2-ux^2-2\cos<b,c>y+2uxycox<a,b>+1=0$

$(1-w)x^2-wy^2-2\cos<a,c>+2wxy\cos<a,b>+1=0$

我们现在要弄清楚上面的两个式子中，哪些变量是已知的：

（1） $u=\frac{BC^2}{AB^2},w=\frac{AC^2}{AB^2}$ 可以通过A、B、C的3D坐标点直接算出来

（2） $\cos<a,b>,\cos<b,c>,\cos<a,c>$ 是可以算出来，上面给出来的博客中给出了计算方法

$x,y$ 是未知的，随着相机的移动会发生变化。解析的求上面这个二元二次方程是非常复杂的，需要用“吴消元法”求解。与分解本质矩阵E的情况类似，这个方程最多可能会得到四组解，但是我们会用验证点（这就是前面为什么说需要至少一对额外的点）来计算最可能的解，得到A、B、C在相机坐标系下的3D坐标，然后根据3D-3D点对，求解R、t。

P3P中还存在的问题：

（1）P3P只利用了3对点的信息，当配对的点比较多的时候，难以利用更多的信息（我认为这既是优点也是缺点）。

（2）如果3D-2D点存在无匹配的情况，那么P3P算法就会失效。

在SLAM中，通常的做法是先使用P3P、EPnP等方法估计相机的位姿，然后构建最小二乘优化问题对估计值进行优化。

实践部分：利用OpenCV已经集成好的求解方法进行求解。

先介绍两个方法：

（1）solve() --- calib3d.hpp

(2)rodrigues() --- calib3d.hpp --- 将旋转向量转换为选装矩阵

/*
P3P要使用3D点，为了避免单目初始化带来的麻烦，使用深度图提供的深度信息来构建3D点
*/

#include "feature_extract_match.hpp"
//opencv
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

//像素坐标转换为归一化坐标
Point2d pixel_to_cam(Point2d p, 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)
        );
}

int main(int argc, char **argv)
{
    Mat rgb_img1 = imread("../datas/1.png");
    Mat rgb_img2 = imread("../datas/2.png");

    vector<KeyPoint> key_points_1, key_points_2;
    vector<DMatch> matches;

    //进行特征提取和匹配
    feature_extract_match(rgb_img1, rgb_img2, key_points_1, key_points_2, matches);

    Mat depth_img = imread("../datas/1_depth.png", CV_LOAD_IMAGE_UNCHANGED);    //深度图为16位无符号数，单通道图像
    vector<Point3d> p_3d;
    vector<Point2d> p_2d;

    //内参矩阵
    Mat K = (Mat_<double>(3, 3) <<
        529.0, 0, 325.1,
        0, 521.0, 249.9,
        0, 0, 1
    );

    for(int i=0; i<matches.size(); i++)
    {
        //取出深度信息
        ushort d = depth_img.ptr<unsigned short>( (int)(key_points_1[matches[i].queryIdx].pt.y) ) [ (int)(key_points_1[matches[i].queryIdx].pt.x) ];
        if(d == 0)
        {
            continue;
        }
        float dd = d/1000.0;
        Point2d normalized_p = pixel_to_cam(key_points_1[matches[i].queryIdx].pt, K);

        p_3d.push_back( Point3d(normalized_p.x*dd, normalized_p.y*dd, dd) );
        p_2d.push_back( key_points_2[matches[i].trainIdx].pt );
    }

    cout << "the number of 3D-2D pairs = " << p_3d.size() << endl;

    //调用OpenCV中的函数求解P3P
    Mat r, t, R;
    solvePnP(p_3d, p_2d, K, Mat(), r, t, false, cv::SOLVEPNP_EPNP);//使用EPNP方法求解，返回的r是旋转向量
    Rodrigues(r, R);//将旋转向量转为旋转矩阵

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

    return 0;
}

3、3D-3D --- ICP

看一下ICP的问题描述：

假设我们有一组匹配好的3D点： $P=\begin{Bmatrix} p_{1},p_{2},...,p_{n} \end{Bmatrix} ,P^{'}=\begin{Bmatrix} p^{'}_{1},p^{'}_{2},...,p^{'}_{n} \end{Bmatrix}$ ，然后需要找一个欧式变换R、t，使得：

$\forall i,p_{i}=Rp^{'}_{i}+t$ （注意：按照这样的推导方式，得到是从第二帧到第一帧的R、t，如果要求从第一帧到第二帧的，则要进行逆变换）。（从对问题的描述中可以看到，3D-3D问题和相机是没有关系的，因为在激光SLAM中也会用到ICP来求解位姿）

和PnP类似，ICP的求解也有两种方式：（1）利用线性代数的求解方法（主要是SVD）；（2）利用非线性优化方式求解（类似BA，暂时还没有学，所以在这里关注的是第一种方法）

-----------------------------------------------------------SVD求解ICP----------------------------------------------

根据上面描述的ICP问题，我们可以定义第 $i$ 对点的误差项：

$e_{i}=p_{i}-(Rp_{i}^{'}+t)$

然后，构建最小二乘问题，求使得误差平方和达到极小的 $R,t$ :

$\min_{R,t} J=\frac{1}{2}\sum_{i=1}^{n}\begin{Vmatrix} (p_{i}-(Rp^{'}_{i}+t)) \end{Vmatrix}^{2}$

下面来推导上式的求解方法：

首先，定义两组点的质心：

$p=\frac{1}{n}\sum_{i=1}^{n}(p_{i}),p^{'}=\frac{1}{n}\sum_{i=1}^{n}(p_{i}^{'})$

然后，对上面的误差函数左如下处理：

注意到一个现象： $\sum_{i=1}^{n}(p_{i}-p)-R(p^{'}_{i}-p&{'})=0$ ，所以上面的这个误差函数就变为：

注意到上式中的左右两项：左边这一项只和R有关，右边这一项和R、t都有关，并且它只和质心有关。因此，可以想到：只要我们通过第一项式子求出了R，令第二项式子为0就可以求出t了。因此，可以总结出ICP的求解步骤：

1、计算两组点的质心位置 $p.p^{'}$ ，然后计算每个点的去质心坐标：

$q_{i}=p_{i}-p$

$q_{i}^{'}=p_{i}^{'}-p^{'}$

2、然后根据一下优化问题计算旋转矩阵：

将计算好的去质心坐标代入得到：

3、根据2中的R计算t：

下面看一下如何计算R：

注意：这里省去了《视觉SLAM十四讲》中的部分推导。

定义一个矩阵：，W是一个3*3的矩阵，需要对W进行SVD分解：

下面这个博客详细的讲了SVD：https://blog.csdn.net/zhongkejingwang/article/details/43053513

对W进行SVD分解得到：

是一个由奇异值组成的对角矩阵，对角线元素从大到小排列，而 $U,V$ 也均为对角矩阵。

当W为满秩矩阵时， $R=UV^{T}$ ，然后利用R就可以求出t了。

下面用OpenCV实践一下用SVD求解ICP的程序：

1）common_include.h中包含了公用的源文件，这样可以避免每次都写一大堆的include命令。

//一些都要include的头文件放在common_include.h中，以免要写很多次include指令

#ifndef COMMON_INCLUDE_H
#define COMMON_INCLUDE_H

#include <iostream>
using namespace std;

//opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
using namespace cv;

//eigen3
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>

#endif

2）pose_estimation.hpp定义了3d3d位姿估计的函数，可以把上面的几个2d2d、3d2d的位姿估计都合到这个头文件中来。

#ifndef POSE_EXTIMATION_H
#define POSE_EXTIMATION_H

#include "common_include.h"

//3D-3D 位姿估计
void pose_estimation_3d3d(vector<Point3f> points_1, vector<Point3f> points_2, Mat &R, Mat &t)
{
    //求质心
    Point3f center_points_1, center_points_2;
    int N = points_1.size();

    for(int i = 0; i < N; i++)
    {
        center_points_1 += points_1[i];
        center_points_2 += points_2[i];
    }

    center_points_1 /= N;
    center_points_2 /= N;

    //求每个点的去质心坐标
    vector<Point3f> q_1, q_2;
    for(int i = 0; i < N; i++)
    {
        q_1.push_back(points_1[i] - center_points_1);
        q_2.push_back(points_2[i] - center_points_2);
    }

    //计算W
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for(int i = 0; i < N; i++)
    {
        W += Eigen::Vector3d(q_1[i].x, q_1[i].y, q_1[i].z) * Eigen::Vector3d(q_2[i].x, q_2[i].y, q_2[i].z).transpose();
    }
    cout << "W = " << endl;
    cout << W << endl;

    //对W进行SVD分解
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU|Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    cout << "U = " << endl;
    cout << U << endl;
    cout << "V = " << endl;
    cout << V << endl;

    Eigen::Matrix3d R_ = U * (V.transpose());
    Eigen::Vector3d t_ = Eigen::Vector3d(center_points_1.x, center_points_1.y, center_points_1.z) - R_ * Eigen::Vector3d(center_points_2.x, center_points_2.y, center_points_2.z);
    cout << "R_ = " << endl;
    cout << R << endl;
    cout << "t_ = " << endl;
    cout << t_ << endl;

    //Eigen::Matrix3d和opencv中的矩阵数据格式不是一致的
    R = (Mat_<double>(3, 3) <<
        R_(0,0), R_(0,1), R_(0,2),
        R_(1,0), R_(1,1), R_(1,2),
        R_(2,0), R_(2,1), R_(2,2)
    );

    t = (Mat_<double>(3, 1) <<
        t_(0, 0),
        t_(1, 0),
        t_(2, 0)
    );
}

#endif

3）test_estimation.cpp定义了测试程序，3d点是读取深度图得到的。

#include "common_include.h"
#include "pose_estimation.hpp"
#include "feature_extract_match.hpp"

//像素坐标转换为归一化坐标
Point2d pixel_to_cam(Point2d p, Mat K);

int main ( int argc, char** argv )
{
    //-- 读取图像
    Mat rgb_img_1 = imread ("../datas/1.png");
    Mat rgb_img_2 = imread ("../datas/2.png");

    vector<KeyPoint> key_points_1, key_points_2;
    vector<DMatch> matches;
    feature_extract_match(rgb_img_1, rgb_img_2, key_points_1, key_points_2, matches);
    //cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    // 建立3D点
    Mat depth_img_1 = imread ("../datas/1_depth.png", CV_LOAD_IMAGE_UNCHANGED);       // 深度图为16位无符号数，单通道图像
    Mat depth_img_2 = imread ("../datas/2_depth.png", 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> pts1, pts2;

    for (int i = 0; i < matches.size(); i++)
    {
        ushort d1 = depth_img_1.ptr<unsigned short> (int(key_points_1[matches[i].queryIdx].pt.y))[int(key_points_1[matches[i].queryIdx].pt.x)];
        ushort d2 = depth_img_2.ptr<unsigned short> (int(key_points_2[matches[i].trainIdx].pt.y))[int(key_points_2[matches[i].trainIdx].pt.x)];
        if ( d1==0 || d2==0 )   // bad depth
            continue;

        Point2d p1 = pixel_to_cam (key_points_1[matches[i].queryIdx].pt, K);
        Point2d p2 = pixel_to_cam (key_points_2[matches[i].trainIdx].pt, K);
        float dd1 = float ( d1 ) /5000.0;
        float dd2 = float ( d2 ) /5000.0;
        pts1.push_back ( Point3f(p1.x*dd1, p1.y*dd1, dd1) );
        pts2.push_back ( Point3f(p2.x*dd2, p2.y*dd2, dd2) );
    }

    cout<<"3d-3d pairs: "<<pts1.size() <<endl;
    
    Mat R, t;
    pose_estimation_3d3d ( pts1, pts2, R, t );
    cout << "ICP via SVD results: " << endl;
    cout << "R = " << R << endl;
    cout << "t = " << t << endl;

    return 0;
}

//像素坐标转换为归一化坐标
Point2d pixel_to_cam(Point2d p, 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)
        );
}

上面就是要学习的2d-2d、3d-2d、3d-3d的位姿估计方法，求解PNP、ICP也是可以用优化的方法来求的，下一节会学习优化的内容，采用的优化库是G2O库。

不服输的小白

关注

4
点赞
踩
19

收藏

觉得还不错? 一键收藏
3
评论
第7讲2 --- 求解相机的运动

上一篇博客中学习了特征提取和匹配的概念，并且调用OpenCV库实现了ORB特征的提取和匹配。找到了匹配点后，我们希望能够根据匹配的点对来估计相机的运动。一共有三种方法可以用来估计相机的运动，分别为对极几何约束、PnP、ICP。这三种方法分别可以用来处理不同情况下的相机的位姿估计。当相机为单目的时候，我们只知道2D的像素坐标，因而问题是根据两组2D点估计运动。该问题用对极几何来解决。...
复制链接

扫一扫