opencv三角测量一种代码实现

opencv三角测量一种代码实现，帮助理解三角测量的算法

#include “extra.h“
#include 
using  namespace std;

void decomposeEssentialMat（ InputArray _E OutputArray _R1 OutputArray _R2 OutputArray _t ）
{
    Mat E = _E.getMat（）.reshape（1 3）;
    CV_Assert（E.cols == 3 && E.rows == 3）;

    Mat D U Vt;
    SVD::compute（E D U Vt）;

    if （determinant（U） < 0） U *= -1.;
    if （determinant（Vt） < 0） Vt *= -1.;

    Mat W = （Mat_（3 3） << 0 1 0 -1 0 0 0 0 1）;
    W.convertTo（W E.type（））;

    Mat R1 R2 t;
    R1 = U * W * Vt;
    R2 = U * W.t（） * Vt;
    t = U.col（2） * 1.0;

    R1.copyTo（_R1）;
    R2.copyTo（_R2）;
    t.copyTo（_t）;
}

int recoverPose（ InputArray E InputArray _points1 InputArray _points2 OutputArray _R
                     OutputArray _t double focal Point2d pp InputOutputArray _mask）
{
    Mat points1 points2 cameraMatrix;
    cameraMatrix = （Mat_（33） << focal 0 pp.x 0 focal pp.y 0 0 1）;
    _points1.getMat（）.convertTo（points1 CV_64F）;
    _points2.getMat（）.convertTo（points2 CV_64F）;
    
    int npoints = points1.checkVector（2）;
    CV_Assert（ npoints >= 0 && points2.checkVector（2） == npoints &&
                              points1.type（） == points2.type（））;

    CV_Assert（cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels（） == 1）;

    if （points1.channels（） > 1）
    {
        points1 = points1.reshape（1 npoints）;
        points2 = points2.reshape（1 npoints）;
    }

    double fx = cameraMatrix.at（00）;
    double fy = cameraMatrix.at（11）;
    double cx = cameraMatrix.at（02）;
    double cy = cameraMatrix.at（12）;

    points1.col（0） = （points1.col（0） - cx） / fx;
    points2.col（0） = （points2.col（0） - cx） / fx;
    points1.col（1） = （points1.col（1） - cy） / fy;
    points2.col（1） = （points2.col（1） - cy） / fy;

    points1 = points1.t（）;
    points2 = points2.t（）;

    Mat R1 R2 t;
    decomposeEssentialMat（E R1 R2 t）;
    Mat P0 = Mat::eye（3 4 R1.type（））;
    Mat P1（3 4 R1.type（）） P2（3 4 R1.type（）） P3（3 4 R1.type（）） P4（3 4 R1.type（））;
    P1（Range::all（） Range（0 3）） = R1 * 1.0; P1.col（3） = t * 1.0;
    P2（Range::all（） Range（0 3）） = R2 * 1.0; P2.col（3） = t * 1.0;
    P3（Range::all（） Range（0 3）） = R1 * 1.0; P3.col（3） = -t * 1.0;
    P4（Range::all（） Range（0 3）） = R2 * 1.0; P4.col（3） = -t * 1.0;

    // Do the cheirality check.
    // Notice here a threshold dist is used to filter
    // out far away points （i.e. infinite points） since
    // there depth may vary between postive and negtive.
    double dist = 50.0;
    Mat Q;
    triangulatePoints（P0 P1 points1 points2 Q）;
    Mat mask1 = Q.row（2）.mul（Q.row（3）） > 0;
    Q.row（0） /= Q.row（3）;
    Q.row（1） /= Q.row（3）;
    Q.row（2） /= Q.row（3）;
    Q.row（3） /= Q.row（3）;
    mask1 = （Q.row（2） < dist） & mask1;
    Q = P1 * Q;
    mask1 = （Q.row（2） > 0） & mask1;
    mask1 = （Q.row（2） < dist） & mask1;

    triangulatePoints（P0 P2 points1 points2 Q）;
    Mat mask2 = Q.row（2）.mul（Q.row（3）） > 0;
    

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2017-11-28 02:24  FeatureMethod-master\
     目录           0  2017-11-28 02:24  FeatureMethod-master\.idea\
     文件           3  2017-11-28 02:24  FeatureMethod-master\.idea\.name
     文件          97  2017-11-28 02:24  FeatureMethod-master\.idea\FeatureExtraction.iml
     文件         137  2017-11-28 02:24  FeatureMethod-master\.idea\misc.xml
     文件         286  2017-11-28 02:24  FeatureMethod-master\.idea\modules.xml
     文件       28552  2017-11-28 02:24  FeatureMethod-master\.idea\workspace.xml
     文件      529319  2017-11-28 02:24  FeatureMethod-master\1.png
     文件      122848  2017-11-28 02:24  FeatureMethod-master\1_depth.png
     文件      532217  2017-11-28 02:24  FeatureMethod-master\2.png
     文件      122985  2017-11-28 02:24  FeatureMethod-master\2_depth.png
     文件        1482  2017-11-28 02:24  FeatureMethod-master\CMakeLists.txt
     文件         449  2017-11-28 02:24  FeatureMethod-master\README.md
     目录           0  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\
     文件       36638  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeCache.txt
     目录           0  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\
     目录           0  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\3.7.1\
     文件        2003  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\3.7.1\CMakeCCompiler.cmake
     文件        4557  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\3.7.1\CMakeCXXCompiler.cmake
     文件        8192  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\3.7.1\CMakeDetermineCompilerABI_C.bin
     文件        8208  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\3.7.1\CMakeDetermineCompilerABI_CXX.bin
     文件         410  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\3.7.1\CMakeSystem.cmake
     目录           0  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\3.7.1\CompilerIdC\
     文件       16826  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\3.7.1\CompilerIdC\CMakeCCompilerId.c
     文件        8352  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\3.7.1\CompilerIdC\a.out
     目录           0  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\3.7.1\CompilerIdCXX\
     文件       16397  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\3.7.1\CompilerIdCXX\CMakeCXXCompilerId.cpp
     文件        8360  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\3.7.1\CompilerIdCXX\a.out
     文件         657  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\CMakeDirectoryInformation.cmake
     文件       41999  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\CMakeOutput.log
     文件       10491  2017-11-28 02:24  FeatureMethod-master\cmake-build-debug\CMakeFiles\Makefile.cmake
............此处省略66个文件信息