ch7视觉里程计

视觉里程计

2D-2D：对极约束

（1）公式推导

公式推倒我就不打了【参考这篇】

主要思路

1. 针孔相机模型

2. 代入归一化平面坐标
   

3. 左乘t的反对称矩阵
   此时的目的是将上图中t化简
   外积：a*b=|a||b|sin,自身的外积为0

4. 左乘x2的转置
   引入x2,便于寻找x1，x2的关系

左式相当于求内积

5. 重新代入p1，p2
   
6. 定义基础矩阵（F）和本质矩阵（E）简化
   
   此时已经将姿态优化问题转化为以下两步骤：
   （1）求基础矩阵（F，fundamental_matrix）或本质矩阵（E，essential_matrix）
   （2）根据基础矩阵（F）或本质矩阵（E）求出R，t

根据本质矩阵（E）求出R，t

（1）利用线性性质求本质矩阵（E）
最少使用五点，一般使用八点法

（2）利用内在性质本质矩阵（E）

（3）使用本质矩阵（E）恢复R，t
奇异点分解

（2）在opencv中对极约束

1） findFundamentalMat

Mat findFundamentalMat( InputArray points1, InputArray points2,
                       int method, double ransacReprojThreshold, double confidence,
                       int maxIters, OutputArray mask = noArray() );
1
2
3

• points1：第一张图中的点集

• points2：第二张图中与第一张图中对应的点集

• method：计算基础矩阵的方式

  CV_FM_7POINT for a 7-point algorithm.
  CV_FM_8POINT for an 8-point algorithm.
  CV_FM_RANSAC for the RANSAC algorithm.
  CV_FM_LMEDS for the LMedS algorithm.

//todo;点集中的点大于8？

但是匹配子和vector的数据结构不统一

struct DMatch
{
    DMatch() : queryIdx(-1), trainIdx(-1), imgIdx(-1),
               distance(std::numeric_limits<float>::max()) {}
    DMatch( int _queryIdx, int _trainIdx, float _distance ) :
            queryIdx(_queryIdx), trainIdx(_trainIdx), imgIdx(-1),
            distance(_distance) {}
    DMatch( int _queryIdx, int _trainIdx, int _imgIdx, float _distance ) :
            queryIdx(_queryIdx), trainIdx(_trainIdx), imgIdx(_imgIdx),
            distance(_distance) {}

    int queryIdx; // query descriptor index
    int trainIdx; // train descriptor index
    int imgIdx;   // train image index

    float distance;

    // less is better
    bool operator<( const DMatch &m ) const;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

因此在进行计算之前需要把匹配点转换为vector的形式

 for ( int i = 0; i < ( int ) matches.size(); i++ )
    {
        points1.push_back ( keypoints_1[matches[i].queryIdx].pt );
        points2.push_back ( keypoints_2[matches[i].trainIdx].pt );
    }
1
2
3
4
5

2）findEssentialMat

包含两个重载形式，《14讲》的话用的是第二个，所以我只讲第二个。第一个的话，可以参考基础函数findEssentialMat

Mat findEssentialMat( InputArray points1, InputArray points2,
                                 InputArray cameraMatrix, int method = RANSAC,
                                 double prob = 0.999, double threshold = 1.0,
                                 OutputArray mask = noArray() );                        

Mat 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() );
1
2
3
4
5
6
7
8
9

• focal：相机的焦距
• pp：相机光心

3）findHomography单应矩阵

Mat findHomography( InputArray srcPoints, InputArray dstPoints,
                                 int method = 0, double ransacReprojThreshold = 3,
                                 OutputArray mask=noArray(), const int maxIters = 2000,
                                 const double confidence = 0.995);
1
2
3
4

参数上也差不多

recoverPose 从本质矩阵中恢复旋转和平移信息

int recoverPose( InputArray E, InputArray points1, InputArray points2,
                            OutputArray R, OutputArray t,
                            double focal = 1.0, Point2d pp = Point2d(0, 0),
                            InputOutputArray mask = noArray() );
1
2
3
4

• E：本质矩阵
• points1：第一张图中的点集
• points2：第二张图中与第一张图中对应的点集
• R：旋转矩阵
• t:平移向量
• focal：相机的焦距
• pp：相机光心
• mask：点1和点2中的内联线的输入/输出掩码。如果它不是空的，则它标记对于给定的基本矩阵E，点S1和点S2中的内联线。只有这些内联线将用于恢复姿势。在输出掩码中，只有通过cheirality检查的内联线。

三角测量

使用的函数

void triangulatePoints( 
InputArray projMatr1, InputArray projMatr2,
InputArray projPoints1, InputArray projPoints2,
OutputArray points4D );
1
2
3
4

• projMatr1：第一个摄像机位姿的的3x4投影矩阵，即该矩阵将世界坐标系中给定的3D点投影到第一图像中。
• projMatr2：第二个摄像机位姿的的3x4投影矩阵，即该矩阵将世界坐标系中给定的3D点投影到第二图像中。
• projPoints1：第一幅图像中的2xN特征点阵列。对于c++版本，它也可以是特征点向量或大小为1xN或Nx1的双通道矩阵。
• projPoints2：第二图像中对应点的2xN阵列。对于c++版本，它也可以是特征点向量或大小为1xN或Nx1的双通道矩阵。
• 齐次坐标中重建点的4xN阵列。这些点返回到世界坐标系中。

3D-2D：PnP

（1）PnP常见api

solvePnP

bool solvePnP( InputArray objectPoints, InputArray imagePoints,
                            InputArray cameraMatrix, InputArray distCoeffs,
                            OutputArray rvec, OutputArray tvec,
                            bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE );
1
2
3
4

• objectPoints：对象坐标空间中的对象点阵列，Nx3 1通道或1xN/Nx1 3通道，其中N是点的数量。
• imagePoints：对应图像点阵列，Nx2 1通道或1xN/Nx1 2通道，其中N是点的数量。

这个还算是比较常用

Rodrigues

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

（2）bundleAdjustment

使用G2O进行优化，详见【】

3D-3D：ICP

仅考虑3d点之间的变化，和相机没有关系
ICP的求解包含两种方式：

1. 线性代数的求解（主要是SVD）
2. 非线性优化的求解（类似于BA）

（1）SVD

void pose_estimation_3d3d (
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t
)
{
    Point3f p1, p2;     // center of mass
    int N = pts1.size();
    for ( int i=0; i<N; i++ )
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    //---------求质心------------------
    p1 = Point3f( Vec3f(p1) /  N);
    p2 = Point3f( Vec3f(p2) / N);
    
    //------------得到去质心坐标---------------
    vector<Point3f>     q1 ( N ), q2 ( N ); // remove the center
    for ( int i=0; i<N; i++ )
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }
    
    //-----------误差项只需要优化最后的q1*q2^T----------------------

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for ( int i=0; i<N; i++ )
    {
        W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
    }
    cout<<"W="<<W<<endl;
    //---------------------------

    //-------------SVD on W-------------- 
    Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    
    if (U.determinant() * V.determinant() < 0)
	{
        for (int x = 0; x < 3; ++x)
        {
            U(x, 2) *= -1;
        }
	}
    //--------------求R，t-------------
    Eigen::Matrix3d R_ = U* ( V.transpose() );
    Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

svd的代码实现

Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
1
2
3