视觉里程计

2D-2D：对极约束

（1）公式推导

ch7视觉里程计-LMLPHP

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

主要思路

针孔相机模型
代入归一化平面坐标
左乘t的反对称矩阵
此时的目的是将上图中t化简
外积：a*b=|a||b|sin<a,b>,自身的外积为0
左乘x2的转置
引入x2,便于寻找x1，x2的关系

左式相当于求内积

重新代入p1，p2
定义基础矩阵（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() );

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<Point2f>的数据结构不统一

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;
};

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

 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 );
    }

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() );

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);

参数上也差不多

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() );

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

3D-2D：PnP

（1）PnP算法

solvePnP

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

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

这个还算是比较常用

Rodrigues

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

（2）bundleAdjustment

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

3D-3D：ICP

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

线性代数的求解（主要是SVD）
非线性优化的求解（类似于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 );

}

svd的代码实现

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

weixin_50862344

ch7视觉里程计