视觉SLAM学习打卡【7-2】-视觉里程计·特征点法程序详解

  • 开发
  • 14

视觉SLAM学习打卡【7-2】-视觉里程计·特征点法程序详解

1.opencv特征提取和匹配 orb_cv.cpp

该blog写的较为详细：点击跳转

2.手写ORB特征 orb_self.cpp

#include <opencv2/opencv.hpp>
#include <string>
#include <nmmintrin.h>
#include <chrono>
 
using namespace std;
 
// global variables定义图片路径全局变量
string first_file = "./1.png";
string second_file = "./2.png";
 
// 32 bit unsigned int, will have 8, 8x32=256
typedef vector<uint32_t> DescType; // Descriptor type
 
/**
 * compute descriptor of orb keypoints
 * @param img input image
 * @param keypoints detected fast keypoints
 * @param descriptors descriptors
 *
 * NOTE: if a keypoint goes outside the image boundary (8 pixels), descriptors will not be computed and will be left as
 * empty
 */
void ComputeORB(const cv::Mat &img, vector<cv::KeyPoint> &keypoints, vector<DescType> &descriptors);
 
/**
 * brute-force match two sets of descriptors
 * @param desc1 the first descriptor
 * @param desc2 the second descriptor
 * @param matches matches of two images
 */
void BfMatch(const vector<DescType> &desc1, const vector<DescType> &desc2, vector<cv::DMatch> &matches);
 
int main(int argc, char **argv) {
 
    // 加载图片
    //从全局变量路径中读取图片
    cv::Mat first_image = cv::imread(first_file, 0);                  
    cv::Mat second_image = cv::imread(second_file, 0);
    //检查图片指针是否为空
    assert(first_image.data != nullptr && second_image.data != nullptr);          
 
    //ORB-Oriented FAST and Rotated BRIEF
 
 
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    //ORB使用FAST算法检测特征点
    //OpenCV中的ORB采用了图像金字塔来解决尺度变换一致性
    //****自定义ComputeORB函数来描述ORB特征点，并旋转使其具备旋转尺度不变性
 
    // ORB提取图1特征threshold=40
    vector<cv::KeyPoint> keypoints1;
    cv::FAST(first_image, keypoints1, 40);    //利用FAST从图1中提取关键点keypoints1
    vector<DescType> descriptor1;                               //定义图1的描述子
    //根据图1和FAST提取的关键点，通过ORB设置描述子descriptor1
    ComputeORB(first_image, keypoints1, descriptor1);     
    //ORB提取图2特征
    vector<cv::KeyPoint> keypoints2;
    vector<DescType> descriptor2;
    cv::FAST(second_image, keypoints2, 40);
    ComputeORB(second_image, keypoints2, descriptor2);
 
    //计算特征提取耗时
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "extract ORB cost = " << time_used.count() << " seconds. " << endl;
 
    // 进行匹配
    vector<cv::DMatch> matches;
    t1 = chrono::steady_clock::now();
    BfMatch(descriptor1, descriptor2, matches);
    t2 = chrono::steady_clock::now();
    time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "match ORB cost = " << time_used.count() << " seconds. " << endl;
    cout << "matches: " << matches.size() << endl;
 
    // plot the matches
    cv::Mat image_show;
    cv::drawMatches(first_image, keypoints1, second_image, keypoints2, matches, image_show);
    cv::imshow("matches", image_show);
    cv::imwrite("matches.png", image_show);
    cv::waitKey(0);
 
    cout << "done." << endl;
    return 0;
}
 
#pragma region ORB_pattern[256 * 4]相当于在以关键点为中心[-13,12]的范围内,随机选点对p,q;进行关键点的向量构建
//这个变量里的数字，在ORBSLAM的代码中总共是256行，代表了256个点对儿，也就是每一个都代表了一对点的坐标，
//如第一行表示点q1(8,-3) 和点 q2(9,5), 接下来就是要对比这两个坐标对应的像素值的大小；
int ORB_pattern[256 * 4] = {
        8, -3, 9, 5/*mean (0), correlation (0)*/,
        4, 2, 7, -12/*mean (1.12461e-05), correlation (0.0437584)*/,
        -11, 9, -8, 2/*mean (3.37382e-05), correlation (0.0617409)*/,
        7, -12, 12, -13/*mean (5.62303e-05), correlation (0.0636977)*/,
        2, -13, 2, 12/*mean (0.000134953), correlation (0.085099)*/,
        1, -7, 1, 6/*mean (0.000528565), correlation (0.0857175)*/,
        -2, -10, -2, -4/*mean (0.0188821), correlation (0.0985774)*/,
        -13, -13, -11, -8/*mean (0.0363135), correlation (0.0899616)*/,
        -13, -3, -12, -9/*mean (0.121806), correlation (0.099849)*/,
        10, 4, 11, 9/*mean (0.122065), correlation (0.093285)*/,
        -13, -8, -8, -9/*mean (0.162787), correlation (0.0942748)*/,
        -11, 7, -9, 12/*mean (0.21561), correlation (0.0974438)*/,
        7, 7, 12, 6/*mean (0.160583), correlation (0.130064)*/,
        -4, -5, -3, 0/*mean (0.228171), correlation (0.132998)*/,
        -13, 2, -12, -3/*mean (0.00997526), correlation (0.145926)*/,
        -9, 0, -7, 5/*mean (0.198234), correlation (0.143636)*/,
        12, -6, 12, -1/*mean (0.0676226), correlation (0.16689)*/,
        -3, 6, -2, 12/*mean (0.166847), correlation (0.171682)*/,
        -6, -13, -4, -8/*mean (0.101215), correlation (0.179716)*/,
        11, -13, 12, -8/*mean (0.200641), correlation (0.192279)*/,
        4, 7, 5, 1/*mean (0.205106), correlation (0.186848)*/,
        5, -3, 10, -3/*mean (0.234908), correlation (0.192319)*/,
        3, -7, 6, 12/*mean (0.0709964), correlation (0.210872)*/,
        -8, -7, -6, -2/*mean (0.0939834), correlation (0.212589)*/,
        -2, 11, -1, -10/*mean (0.127778), correlation (0.20866)*/,
        -13, 12, -8, 10/*mean (0.14783), correlation (0.206356)*/,
        -7, 3, -5, -3/*mean (0.182141), correlation (0.198942)*/,
        -4, 2, -3, 7/*mean (0.188237), correlation (0.21384)*/,
        -10, -12, -6, 11/*mean (0.14865), correlation (0.23571)*/,
        5, -12, 6, -7/*mean (0.222312), correlation (0.23324)*/,
        5, -6, 7, -1/*mean (0.229082), correlation (0.23389)*/,
        1, 0, 4, -5/*mean (0.241577), correlation (0.215286)*/,
        9, 11, 11, -13/*mean (0.00338507), correlation (0.251373)*/,
        4, 7, 4, 12/*mean (0.131005), correlation (0.257622)*/,
        2, -1, 4, 4/*mean (0.152755), correlation (0.255205)*/,
        -4, -12, -2, 7/*mean (0.182771), correlation (0.244867)*/,
        -8, -5, -7, -10/*mean (0.186898), correlation (0.23901)*/,
        4, 11, 9, 12/*mean (0.226226), correlation (0.258255)*/,
        0, -8, 1, -13/*mean (0.0897886), correlation (0.274827)*/,
        -13, -2, -8, 2/*mean (0.148774), correlation (0.28065)*/,
        -3, -2, -2, 3/*mean (0.153048), correlation (0.283063)*/,
        -6, 9, -4, -9/*mean (0.169523), correlation (0.278248)*/,
        8, 12, 10, 7/*mean (0.225337), correlation (0.282851)*/,
        0, 9, 1, 3/*mean (0.226687), correlation (0.278734)*/,
        7, -5, 11, -10/*mean (0.00693882), correlation (0.305161)*/,
        -13, -6, -11, 0/*mean (0.0227283), correlation (0.300181)*/,
        10, 7, 12, 1/*mean (0.125517), correlation (0.31089)*/,
        -6, -3, -6, 12/*mean (0.131748), correlation (0.312779)*/,
        10, -9, 12, -4/*mean (0.144827), correlation (0.292797)*/,
        -13, 8, -8, -12/*mean (0.149202), correlation (0.308918)*/,
        -13, 0, -8, -4/*mean (0.160909), correlation (0.310013)*/,
        3, 3, 7, 8/*mean (0.177755), correlation (0.309394)*/,
        5, 7, 10, -7/*mean (0.212337), correlation (0.310315)*/,
        -1, 7, 1, -12/*mean (0.214429), correlation (0.311933)*/,
        3, -10, 5, 6/*mean (0.235807), correlation (0.313104)*/,
        2, -4, 3, -10/*mean (0.00494827), correlation (0.344948)*/,
        -13, 0, -13, 5/*mean (0.0549145), correlation (0.344675)*/,
        -13, -7, -12, 12/*mean (0.103385), correlation (0.342715)*/,
        -13, 3, -11, 8/*mean (0.134222), correlation (0.322922)*/,
        -7, 12, -4, 7/*mean (0.153284), correlation (0.337061)*/,
        6, -10, 12, 8/*mean (0.154881), correlation (0.329257)*/,
        -9, -1, -7, -6/*mean (0.200967), correlation (0.33312)*/,
        -2, -5, 0, 12/*mean (0.201518), correlation (0.340635)*/,
        -12, 5, -7, 5/*mean (0.207805), correlation (0.335631)*/,
        3, -10, 8, -13/*mean (0.224438), correlation (0.34504)*/,
        -7, -7, -4, 5/*mean (0.239361), correlation (0.338053)*/,
        -3, -2, -1, -7/*mean (0.240744), correlation (0.344322)*/,
        2, 9, 5, -11/*mean (0.242949), correlation (0.34145)*/,
        -11, -13, -5, -13/*mean (0.244028), correlation (0.336861)*/,
        -1, 6, 0, -1/*mean (0.247571), correlation (0.343684)*/,
        5, -3, 5, 2/*mean (0.000697256), correlation (0.357265)*/,
        -4, -13, -4, 12/*mean (0.00213675), correlation (0.373827)*/,
        -9, -6, -9, 6/*mean (0.0126856), correlation (0.373938)*/,
        -12, -10, -8, -4/*mean (0.0152497), correlation (0.364237)*/,
        10, 2, 12, -3/*mean (0.0299933), correlation (0.345292)*/,
        7, 12, 12, 12/*mean (0.0307242), correlation (0.366299)*/,
        -7, -13, -6, 5/*mean (0.0534975), correlation (0.368357)*/,
        -4, 9, -3, 4/*mean (0.099865), correlation (0.372276)*/,
        7, -1, 12, 2/*mean (0.117083), correlation (0.364529)*/,
        -7, 6, -5, 1/*mean (0.126125), correlation (0.369606)*/,
        -13, 11, -12, 5/*mean (0.130364), correlation (0.358502)*/,
        -3, 7, -2, -6/*mean (0.131691), correlation (0.375531)*/,
        7, -8, 12, -7/*mean (0.160166), correlation (0.379508)*/,
        -13, -7, -11, -12/*mean (0.167848), correlation (0.353343)*/,
        1, -3, 12, 12/*mean (0.183378), correlation (0.371916)*/,
        2, -6, 3, 0/*mean (0.228711), correlation (0.371761)*/,
        -4, 3, -2, -13/*mean (0.247211), correlation (0.364063)*/,
        -1, -13, 1, 9/*mean (0.249325), correlation (0.378139)*/,
        7, 1, 8, -6/*mean (0.000652272), correlation (0.411682)*/,
        1, -1, 3, 12/*mean (0.00248538), correlation (0.392988)*/,
        9, 1, 12, 6/*mean (0.0206815), correlation (0.386106)*/,
        -1, -9, -1, 3/*mean (0.0364485), correlation (0.410752)*/,
        -13, -13, -10, 5/*mean (0.0376068), correlation (0.398374)*/,
        7, 7, 10, 12/*mean (0.0424202), correlation (0.405663)*/,
        12, -5, 12, 9/*mean (0.0942645), correlation (0.410422)*/,
        6, 3, 7, 11/*mean (0.1074), correlation (0.413224)*/,
        5, -13, 6, 10/*mean (0.109256), correlation (0.408646)*/,
        2, -12, 2, 3/*mean (0.131691), correlation (0.416076)*/,
        3, 8, 4, -6/*mean (0.165081), correlation (0.417569)*/,
        2, 6, 12, -13/*mean (0.171874), correlation (0.408471)*/,
        9, -12, 10, 3/*mean (0.175146), correlation (0.41296)*/,
        -8, 4, -7, 9/*mean (0.183682), correlation (0.402956)*/,
        -11, 12, -4, -6/*mean (0.184672), correlation (0.416125)*/,
        1, 12, 2, -8/*mean (0.191487), correlation (0.386696)*/,
        6, -9, 7, -4/*mean (0.192668), correlation (0.394771)*/,
        2, 3, 3, -2/*mean (0.200157), correlation (0.408303)*/,
        6, 3, 11, 0/*mean (0.204588), correlation (0.411762)*/,
        3, -3, 8, -8/*mean (0.205904), correlation (0.416294)*/,
        7, 8, 9, 3/*mean (0.213237), correlation (0.409306)*/,
        -11, -5, -6, -4/*mean (0.243444), correlation (0.395069)*/,
        -10, 11, -5, 10/*mean (0.247672), correlation (0.413392)*/,
        -5, -8, -3, 12/*mean (0.24774), correlation (0.411416)*/,
        -10, 5, -9, 0/*mean (0.00213675), correlation (0.454003)*/,
        8, -1, 12, -6/*mean (0.0293635), correlation (0.455368)*/,
        4, -6, 6, -11/*mean (0.0404971), correlation (0.457393)*/,
        -10, 12, -8, 7/*mean (0.0481107), correlation (0.448364)*/,
        4, -2, 6, 7/*mean (0.050641), correlation (0.455019)*/,
        -2, 0, -2, 12/*mean (0.0525978), correlation (0.44338)*/,
        -5, -8, -5, 2/*mean (0.0629667), correlation (0.457096)*/,
        7, -6, 10, 12/*mean (0.0653846), correlation (0.445623)*/,
        -9, -13, -8, -8/*mean (0.0858749), correlation (0.449789)*/,
        -5, -13, -5, -2/*mean (0.122402), correlation (0.450201)*/,
        8, -8, 9, -13/*mean (0.125416), correlation (0.453224)*/,
        -9, -11, -9, 0/*mean (0.130128), correlation (0.458724)*/,
        1, -8, 1, -2/*mean (0.132467), correlation (0.440133)*/,
        7, -4, 9, 1/*mean (0.132692), correlation (0.454)*/,
        -2, 1, -1, -4/*mean (0.135695), correlation (0.455739)*/,
        11, -6, 12, -11/*mean (0.142904), correlation (0.446114)*/,
        -12, -9, -6, 4/*mean (0.146165), correlation (0.451473)*/,
        3, 7, 7, 12/*mean (0.147627), correlation (0.456643)*/,
        5, 5, 10, 8/*mean (0.152901), correlation (0.455036)*/,
        0, -4, 2, 8/*mean (0.167083), correlation (0.459315)*/,
        -9, 12, -5, -13/*mean (0.173234), correlation (0.454706)*/,
        0, 7, 2, 12/*mean (0.18312), correlation (0.433855)*/,
        -1, 2, 1, 7/*mean (0.185504), correlation (0.443838)*/,
        5, 11, 7, -9/*mean (0.185706), correlation (0.451123)*/,
        3, 5, 6, -8/*mean (0.188968), correlation (0.455808)*/,
        -13, -4, -8, 9/*mean (0.191667), correlation (0.459128)*/,
        -5, 9, -3, -3/*mean (0.193196), correlation (0.458364)*/,
        -4, -7, -3, -12/*mean (0.196536), correlation (0.455782)*/,
        6, 5, 8, 0/*mean (0.1972), correlation (0.450481)*/,
        -7, 6, -6, 12/*mean (0.199438), correlation (0.458156)*/,
        -13, 6, -5, -2/*mean (0.211224), correlation (0.449548)*/,
        1, -10, 3, 10/*mean (0.211718), correlation (0.440606)*/,
        4, 1, 8, -4/*mean (0.213034), correlation (0.443177)*/,
        -2, -2, 2, -13/*mean (0.234334), correlation (0.455304)*/,
        2, -12, 12, 12/*mean (0.235684), correlation (0.443436)*/,
        -2, -13, 0, -6/*mean (0.237674), correlation (0.452525)*/,
        4, 1, 9, 3/*mean (0.23962), correlation (0.444824)*/,
        -6, -10, -3, -5/*mean (0.248459), correlation (0.439621)*/,
        -3, -13, -1, 1/*mean (0.249505), correlation (0.456666)*/,
        7, 5, 12, -11/*mean (0.00119208), correlation (0.495466)*/,
        4, -2, 5, -7/*mean (0.00372245), correlation (0.484214)*/,
        -13, 9, -9, -5/*mean (0.00741116), correlation (0.499854)*/,
        7, 1, 8, 6/*mean (0.0208952), correlation (0.499773)*/,
        7, -8, 7, 6/*mean (0.0220085), correlation (0.501609)*/,
        -7, -4, -7, 1/*mean (0.0233806), correlation (0.496568)*/,
        -8, 11, -7, -8/*mean (0.0236505), correlation (0.489719)*/,
        -13, 6, -12, -8/*mean (0.0268781), correlation (0.503487)*/,
        2, 4, 3, 9/*mean (0.0323324), correlation (0.501938)*/,
        10, -5, 12, 3/*mean (0.0399235), correlation (0.494029)*/,
        -6, -5, -6, 7/*mean (0.0420153), correlation (0.486579)*/,
        8, -3, 9, -8/*mean (0.0548021), correlation (0.484237)*/,
        2, -12, 2, 8/*mean (0.0616622), correlation (0.496642)*/,
        -11, -2, -10, 3/*mean (0.0627755), correlation (0.498563)*/,
        -12, -13, -7, -9/*mean (0.0829622), correlation (0.495491)*/,
        -11, 0, -10, -5/*mean (0.0843342), correlation (0.487146)*/,
        5, -3, 11, 8/*mean (0.0929937), correlation (0.502315)*/,
        -2, -13, -1, 12/*mean (0.113327), correlation (0.48941)*/,
        -1, -8, 0, 9/*mean (0.132119), correlation (0.467268)*/,
        -13, -11, -12, -5/*mean (0.136269), correlation (0.498771)*/,
        -10, -2, -10, 11/*mean (0.142173), correlation (0.498714)*/,
        -3, 9, -2, -13/*mean (0.144141), correlation (0.491973)*/,
        2, -3, 3, 2/*mean (0.14892), correlation (0.500782)*/,
        -9, -13, -4, 0/*mean (0.150371), correlation (0.498211)*/,
        -4, 6, -3, -10/*mean (0.152159), correlation (0.495547)*/,
        -4, 12, -2, -7/*mean (0.156152), correlation (0.496925)*/,
        -6, -11, -4, 9/*mean (0.15749), correlation (0.499222)*/,
        6, -3, 6, 11/*mean (0.159211), correlation (0.503821)*/,
        -13, 11, -5, 5/*mean (0.162427), correlation (0.501907)*/,
        11, 11, 12, 6/*mean (0.16652), correlation (0.497632)*/,
        7, -5, 12, -2/*mean (0.169141), correlation (0.484474)*/,
        -1, 12, 0, 7/*mean (0.169456), correlation (0.495339)*/,
        -4, -8, -3, -2/*mean (0.171457), correlation (0.487251)*/,
        -7, 1, -6, 7/*mean (0.175), correlation (0.500024)*/,
        -13, -12, -8, -13/*mean (0.175866), correlation (0.497523)*/,
        -7, -2, -6, -8/*mean (0.178273), correlation (0.501854)*/,
        -8, 5, -6, -9/*mean (0.181107), correlation (0.494888)*/,
        -5, -1, -4, 5/*mean (0.190227), correlation (0.482557)*/,
        -13, 7, -8, 10/*mean (0.196739), correlation (0.496503)*/,
        1, 5, 5, -13/*mean (0.19973), correlation (0.499759)*/,
        1, 0, 10, -13/*mean (0.204465), correlation (0.49873)*/,
        9, 12, 10, -1/*mean (0.209334), correlation (0.49063)*/,
        5, -8, 10, -9/*mean (0.211134), correlation (0.503011)*/,
        -1, 11, 1, -13/*mean (0.212), correlation (0.499414)*/,
        -9, -3, -6, 2/*mean (0.212168), correlation (0.480739)*/,
        -1, -10, 1, 12/*mean (0.212731), correlation (0.502523)*/,
        -13, 1, -8, -10/*mean (0.21327), correlation (0.489786)*/,
        8, -11, 10, -6/*mean (0.214159), correlation (0.488246)*/,
        2, -13, 3, -6/*mean (0.216993), correlation (0.50287)*/,
        7, -13, 12, -9/*mean (0.223639), correlation (0.470502)*/,
        -10, -10, -5, -7/*mean (0.224089), correlation (0.500852)*/,
        -10, -8, -8, -13/*mean (0.228666), correlation (0.502629)*/,
        4, -6, 8, 5/*mean (0.22906), correlation (0.498305)*/,
        3, 12, 8, -13/*mean (0.233378), correlation (0.503825)*/,
        -4, 2, -3, -3/*mean (0.234323), correlation (0.476692)*/,
        5, -13, 10, -12/*mean (0.236392), correlation (0.475462)*/,
        4, -13, 5, -1/*mean (0.236842), correlation (0.504132)*/,
        -9, 9, -4, 3/*mean (0.236977), correlation (0.497739)*/,
        0, 3, 3, -9/*mean (0.24314), correlation (0.499398)*/,
        -12, 1, -6, 1/*mean (0.243297), correlation (0.489447)*/,
        3, 2, 4, -8/*mean (0.00155196), correlation (0.553496)*/,
        -10, -10, -10, 9/*mean (0.00239541), correlation (0.54297)*/,
        8, -13, 12, 12/*mean (0.0034413), correlation (0.544361)*/,
        -8, -12, -6, -5/*mean (0.003565), correlation (0.551225)*/,
        2, 2, 3, 7/*mean (0.00835583), correlation (0.55285)*/,
        10, 6, 11, -8/*mean (0.00885065), correlation (0.540913)*/,
        6, 8, 8, -12/*mean (0.0101552), correlation (0.551085)*/,
        -7, 10, -6, 5/*mean (0.0102227), correlation (0.533635)*/,
        -3, -9, -3, 9/*mean (0.0110211), correlation (0.543121)*/,
        -1, -13, -1, 5/*mean (0.0113473), correlation (0.550173)*/,
        -3, -7, -3, 4/*mean (0.0140913), correlation (0.554774)*/,
        -8, -2, -8, 3/*mean (0.017049), correlation (0.55461)*/,
        4, 2, 12, 12/*mean (0.01778), correlation (0.546921)*/,
        2, -5, 3, 11/*mean (0.0224022), correlation (0.549667)*/,
        6, -9, 11, -13/*mean (0.029161), correlation (0.546295)*/,
        3, -1, 7, 12/*mean (0.0303081), correlation (0.548599)*/,
        11, -1, 12, 4/*mean (0.0355151), correlation (0.523943)*/,
        -3, 0, -3, 6/*mean (0.0417904), correlation (0.543395)*/,
        4, -11, 4, 12/*mean (0.0487292), correlation (0.542818)*/,
        2, -4, 2, 1/*mean (0.0575124), correlation (0.554888)*/,
        -10, -6, -8, 1/*mean (0.0594242), correlation (0.544026)*/,
        -13, 7, -11, 1/*mean (0.0597391), correlation (0.550524)*/,
        -13, 12, -11, -13/*mean (0.0608974), correlation (0.55383)*/,
        6, 0, 11, -13/*mean (0.065126), correlation (0.552006)*/,
        0, -1, 1, 4/*mean (0.074224), correlation (0.546372)*/,
        -13, 3, -9, -2/*mean (0.0808592), correlation (0.554875)*/,
        -9, 8, -6, -3/*mean (0.0883378), correlation (0.551178)*/,
        -13, -6, -8, -2/*mean (0.0901035), correlation (0.548446)*/,
        5, -9, 8, 10/*mean (0.0949843), correlation (0.554694)*/,
        2, 7, 3, -9/*mean (0.0994152), correlation (0.550979)*/,
        -1, -6, -1, -1/*mean (0.10045), correlation (0.552714)*/,
        9, 5, 11, -2/*mean (0.100686), correlation (0.552594)*/,
        11, -3, 12, -8/*mean (0.101091), correlation (0.532394)*/,
        3, 0, 3, 5/*mean (0.101147), correlation (0.525576)*/,
        -1, 4, 0, 10/*mean (0.105263), correlation (0.531498)*/,
        3, -6, 4, 5/*mean (0.110785), correlation (0.540491)*/,
        -13, 0, -10, 5/*mean (0.112798), correlation (0.536582)*/,
        5, 8, 12, 11/*mean (0.114181), correlation (0.555793)*/,
        8, 9, 9, -6/*mean (0.117431), correlation (0.553763)*/,
        7, -4, 8, -12/*mean (0.118522), correlation (0.553452)*/,
        -10, 4, -10, 9/*mean (0.12094), correlation (0.554785)*/,
        7, 3, 12, 4/*mean (0.122582), correlation (0.555825)*/,
        9, -7, 10, -2/*mean (0.124978), correlation (0.549846)*/,
        7, 0, 12, -2/*mean (0.127002), correlation (0.537452)*/,
        -1, -6, 0, -11/*mean (0.127148), correlation (0.547401)*/
};
 
#pragma endregion
 
// 计算关键点的描述子向量，
/*
 * 1、因为以关键点为中心，边长为16×16的方形区域灰度计算关键点角度信息；
 *   所以先去掉所有在离图像边缘在8个像素范围内的像素，否则计算关键点角度会出错；
 *2、计算关键点周围16×16的领域质心，并计算关键点角度的cos，和sin
 *3、根据指定随机规则，选择关键点周围的随机点对计算随机点对的亮度强弱，
 *   如果第一个像素pp比第二个qq亮，则为描述符中的相应位分配值 1，否则分配值 0
 *4、将计算的关键点描述子向量加入到描述子向量集合中
 */
void ComputeORB(const cv::Mat &img, vector<cv::KeyPoint> &keypoints, vector<DescType> &descriptors) {
    const int half_patch_size = 8;
    const int half_boundary = 16;
    int bad_points = 0;
    for (auto &kp: keypoints) {
        //剔除边缘关键点，边缘的像素值为16，即将图像上靠近四边16个像素的边框区域内所有特征点都去掉，
        //因为要以关键点为中心，边长为16×16的方形区域灰度计算关键点角度信息；
        if (kp.pt.x < half_boundary || kp.pt.y < half_boundary ||
            kp.pt.x >= img.cols - half_boundary || kp.pt.y >= img.rows - half_boundary) {
            // outside
            bad_points++;
            descriptors.push_back({});
            continue;
        }
 
        //计算灰度质心
        float m01 = 0, m10 = 0;
        //从左到右遍历以关键点为中心的，半径为8的像素点，共256个像素点
        //从左到右遍历以关键点为中心的，半径为8的像素点，共256个像素点
        for (int dx = -half_patch_size; dx < half_patch_size; ++dx)                     
        {
            //从上到下遍历
            for (int dy = -half_patch_size; dy < half_patch_size; ++dy)             
            {
                //img.at(y,x)，参数是点所在的行列而不是点的坐标
                //计算区域内的像素坐标，关键点坐标（x，y）+偏移坐标（dx，dy）
                uchar pixel = img.at<uchar>(kp.pt.y + dy, kp.pt.x + dx); 
                //计算x方向灰度总权重，注意：此处使用dx，非X方向坐标值     
                m01 += dx * pixel;   
                //计算y方向灰度总权重                                               
                m10 += dy * pixel;                                                  
            }
        }
 
        // 计算关键点角度信息 arc tan(m01/m10);
         //这里计算灰度质心与关键点构成的直角三角形的长边，用于后面计算角度；没有通过除总灰度值计算质心位置 avoid divide by zero
        float m_sqrt = sqrt(m01 * m01 + m10 * m10) + 1e-18;                      
        float sin_theta = m01 / m_sqrt;             //计算关键点角度信息sin
        float cos_theta = m10 / m_sqrt;             //计算关键点角度信息cos
 
        // 计算关键点描述子向量，原理参考https://www.cnblogs.com/alexme/p/11345701.html
        //8个描述子向量，每个向量中的元素占据32位，初始化为0；每个描述子使用256位二进制数进行描述
        DescType desc(8, 0);                
        for (int i = 0; i < 8; i++) {               //处理每个向量
            uint32_t d = 0;
            for (int k = 0; k < 32; k++) {          //处理每一位
                //在256*4的随机数中随机选一行作为p(idx1,idx2),q(idx3,idx4),，i，k递增，所以所有点选择特征向量的规制一致，才能比较
                int idx_pq = i * 8 + k;             
                //ORB_pattern含义是在16*16图像块中按高斯分布选取点对，选出来的p、q是未经过旋转的
                //相当于在以关键点为中心[-13,12]的范围内,随机选点对p,q;进行关键点的向量构建
                cv::Point2f p(ORB_pattern[idx_pq * 4], ORB_pattern[idx_pq * 4 + 1]);
                cv::Point2f q(ORB_pattern[idx_pq * 4 + 2], ORB_pattern[idx_pq * 4 + 3]);
 
                // 计算关键点随机选择的特征点对旋转后的位置
                //pp和qq利用了特征点的方向，计算了原始随机选出的p，q点旋转后的位置pp，qq，体现了ORB的旋转不变性
                cv::Point2f pp = cv::Point2f(cos_theta * p.x - sin_theta * p.y, sin_theta * p.x + cos_theta * p.y)
                                 + kp.pt;  
                 //之所以此处需要+ kp.pt的原因是上述随机点对选择是以关键点为中心选择的，需要加关键点坐标获取按照指定规则随机选择后的随机点对绝对图像坐标
                cv::Point2f qq = cv::Point2f(cos_theta * q.x - sin_theta * q.y, sin_theta * q.x + cos_theta * q.y)
                                 + kp.pt;
                //计算两个旋转后的关键点对应随机点对的灰度值大小，构建关键点的特征向量
                if (img.at<uchar>(pp.y, pp.x) < img.at<uchar>(qq.y, qq.x)) {
                    //如果第一个像素pp比第二个qq亮，则为描述符中的相应位分配值 1，否则分配值 0
                    d |= 1 << k;            
                }
            }
            desc[i] = d;
        }
        //将获取的关键点描述子向量添加进描述子集合中
        descriptors.push_back(desc);        
    }
 
    cout << "bad/total: " << bad_points << "/" << keypoints.size() << endl;
}
 
// brute-force matching
void BfMatch(const vector<DescType> &desc1, const vector<DescType> &desc2, vector<cv::DMatch> &matches) {
    const int d_max = 40;
 
    for (size_t i1 = 0; i1 < desc1.size(); ++i1) {
        if (desc1[i1].empty()) continue;
        cv::DMatch m{i1, 0, 256};
        for (size_t i2 = 0; i2 < desc2.size(); ++i2) {
            if (desc2[i2].empty()) continue;
            int distance = 0;
            //此处k8×下行u32=256个二进制描述子，即一个关键点的描述子
            for (int k = 0; k < 8; k++) {   
                //将选择的两个随机特征的描述子按位进行异或运算，从而计算两个描述子之间的汉明顿距离
                distance += _mm_popcnt_u32(desc1[i1][k] ^ desc2[i2][k]);
            }
            if (distance < d_max && distance < m.distance) {
                m.distance = distance;
                m.trainIdx = i2;
            }
        }
        if (m.distance < d_max) {
            matches.push_back(m);
        }
    }
}

3.对极约束求解相机运动 pose_estimation_2d2d.cpp

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include<chrono>
// #include "extra.h" // use this if in OpenCV2
 
using namespace std;
using namespace cv;
 
/****************************************************
 * 本程序演示了如何使用2D-2D的特征匹配估计相机运动
 * **************************************************/
//一些自定义函数的声明
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);
 
void pose_estimation_2d2d(
  std::vector<KeyPoint> keypoints_1,
  std::vector<KeyPoint> keypoints_2,
  std::vector<DMatch> matches,
  Mat &R, Mat &t);
 
// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &p, const Mat &K);
 
int main(int argc, char **argv) {
  // if (argc != 3) {
  //   cout << "usage: pose_estimation_2d2d img1 img2" << endl;
  //   return 1;
  // }
  //-- 读取图像
 
  chrono::steady_clock::time_point t1=chrono::steady_clock::now();//计时函数
  Mat img_1 = imread("/home/rxz/slambook2/ch7/1.png", CV_LOAD_IMAGE_COLOR); //前面引号里的是图片的位置
  Mat img_2 = imread("/home/rxz/slambook2/ch7/2.png", CV_LOAD_IMAGE_COLOR);
  assert(img_1.data && img_2.data && "Can not load images!");//assert()为断言函数，条件为假则停止执行
 
  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 R, t;
  pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);
 
  //-- 验证E=t^R*scale
  //t_x把t向量写成矩阵形式 反对称矩阵，以下代码是构造一个3*3的矩阵
  Mat t_x =
    (Mat_<double>(3, 3) << 0, -t.at<double>(2, 0), t.at<double>(1, 0),
      t.at<double>(2, 0), 0, -t.at<double>(0, 0),
      -t.at<double>(1, 0), t.at<double>(0, 0), 0);
 
  cout << "t^R=" << endl << t_x * R << endl;
 
  //-- 验证对极约束
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1); //相机内参
  for (DMatch m: matches) {
    //queryIdx是关键点匹配对在第一幅图像上的的索引,trainIdx是另一副图像上的索引
    Point2d pt1 = pixel2cam(keypoints_1[m.queryIdx].pt, K); //将像素坐标转化成归一化坐标
    Mat y1 = (Mat_<double>(3, 1) << pt1.x, pt1.y, 1);   //将归一化坐标转化成3*1的列向量
    Point2d pt2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
    Mat y2 = (Mat_<double>(3, 1) << pt2.x, pt2.y, 1);
    Mat d = y2.t() * t_x * R * y1;   //y2.t()指的是对y2向量进行转置。验证书P167页，式（7.8）是否为零
    cout << "匹配点对的对极几何残差（epipolar constraint） = " << d << endl;
  }
  chrono::steady_clock::time_point t2=chrono::steady_clock::now();
  chrono::duration<double>time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1);
  cout<<"程序所用时间："<<time_used.count()<<"秒"<<endl;
    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();
  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 = 100, 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]);
    }
  }
  // Mat image1;
  // drawMatches(img_1,keypoints_1,img_2,keypoints_2,matches,image1);
  // imshow("all match",image1);
  // waitKey(0);
  
}
//将像素点坐标转化成归一化坐标
Point2d pixel2cam(const Point2d &p, const Mat &K) {
  return Point2d
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),//  x/z=（p.x-cx）/fx
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1) //  y/z=(p.y-cy)/fy
    );
}
 
void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,
                          std::vector<KeyPoint> keypoints_2,
                          std::vector<DMatch> matches,
                          Mat &R, Mat &t) {
  // 相机内参,TUM Freiburg2
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
 
  //-- 把匹配点转换为vector<Point2f>的形式
  vector<Point2f> points1;
  vector<Point2f> points2;
 
  for (int i = 0; i < (int) matches.size(); i++) {
    //queryIdx是一组关键点的索引,trainIdx是另一组关键点的索引.
    points1.push_back(keypoints_1[matches[i].queryIdx].pt);  //把匹配对中属于第一副图像的关键点给points1
    points2.push_back(keypoints_2[matches[i].trainIdx].pt); //同上
  }
 
  //-- 计算基础矩阵
  Mat fundamental_matrix;
  fundamental_matrix = findFundamentalMat(points1, points2, CV_FM_8POINT);//CV_FM_8POINT采用八点法，采用8点法求解F = K^(-T) * E * K^(-1)
  cout << "fundamental_matrix is " << endl << fundamental_matrix << endl;
 
  //-- 计算本质矩阵
  Point2d principal_point(325.1, 249.7);  //相机光心, TUM dataset标定值
  double focal_length = 521;      //相机焦距, TUM dataset标定值
  Mat essential_matrix;
  essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);//E = t(^) * R
  cout << "essential_matrix is " << endl << essential_matrix << endl;
 
  //-- 计算单应矩阵
  //-- 但是本例中场景不是平面，单应矩阵意义不大
  Mat homography_matrix;
  homography_matrix = findHomography(points1, points2, RANSAC, 3);
  cout << "homography_matrix is " << endl << homography_matrix << endl;
 
  //-- 从本质矩阵中恢复旋转和平移信息.
  // 此函数仅在Opencv3中提供
  recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
  cout << "R is " << endl << R << endl;
  cout << "t is " << endl << t << endl;
 
}

4.三角测量 triangulation.cpp

#include <iostream>
#include <opencv2/opencv.hpp>
#include<chrono>
// #include "extra.h" // used in opencv2
using namespace std;
using namespace cv;
 
//自定义的函数声明
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);
 
void pose_estimation_2d2d(
  const std::vector<KeyPoint> &keypoints_1,
  const std::vector<KeyPoint> &keypoints_2,
  const std::vector<DMatch> &matches,
  Mat &R, Mat &t);
 
void triangulation(
  const vector<KeyPoint> &keypoint_1,
  const vector<KeyPoint> &keypoint_2,
  const std::vector<DMatch> &matches,
  const Mat &R, const Mat &t,
  vector<Point3d> &points
);
 
/// 作图用
inline cv::Scalar get_color(float depth) {
  float up_th = 50, low_th = 10, th_range = up_th - low_th;
  if (depth > up_th) depth = up_th;
  if (depth < low_th) depth = low_th;
  return cv::Scalar(255 * depth / th_range, 0, 255 * (1 - depth / th_range));
}
 
// 像素坐标转相机归一化坐标
Point2f pixel2cam(const Point2d &p, const Mat &K);
 
int main(int argc, char **argv) {
  if (argc != 3) {
    cout << "usage: triangulation img1 img2" << endl;
    return 1;
  }
  chrono::steady_clock::time_point t1=chrono::steady_clock::now();
  //-- 读取图像
  Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR); //前面引号里的是图片的位置
  Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
 
  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 R, t;
  pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);
 
  //-- 三角化
  vector<Point3d> points;
  triangulation(keypoints_1, keypoints_2, matches, R, t, points);
 
  //-- 验证三角化点与特征点的重投影关系
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  Mat img1_plot = img_1.clone();
  Mat img2_plot = img_2.clone();
  for (int i = 0; i < matches.size(); i++) {
    // 第一个图
    float depth1 = points[i].z;
    cout <<"第"<<i<<"个匹配点的depth: "<< depth1 << endl;
    Point2d pt1_cam = pixel2cam(keypoints_1[matches[i].queryIdx].pt, K);
    cv::circle(img1_plot, keypoints_1[matches[i].queryIdx].pt, 2, get_color(depth1), 2,8,0);
 
    // 第二个图
    Mat pt2_trans = R * (Mat_<double>(3, 1) << points[i].x, points[i].y, points[i].z) + t;
    float depth2 = pt2_trans.at<double>(2, 0);
    cv::circle(img2_plot, keypoints_2[matches[i].trainIdx].pt, 2, get_color(depth2), 2);
  }
  cv::imshow("img 1", img1_plot);
  cv::imshow("img 2", img2_plot);
  chrono::steady_clock::time_point t2=chrono::steady_clock::now();
  chrono::duration<double>time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1);
  cout<<"time spent by project: "<<time_used.count()<<"second"<<endl;
  cv::waitKey();
 
  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]);
    }
  }
}
 
void pose_estimation_2d2d(
  const std::vector<KeyPoint> &keypoints_1,
  const std::vector<KeyPoint> &keypoints_2,
  const std::vector<DMatch> &matches,
  Mat &R, Mat &t) {
  // 相机内参,TUM Freiburg2
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);//内参矩阵
 
  //-- 把匹配点转换为vector<Point2f>的形式
  vector<Point2f> points1;
  vector<Point2f> points2;
 
  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);
  }
 
  //-- 计算本质矩阵
  Point2d principal_point(325.1, 249.7);        //相机主点, TUM dataset标定值
  int focal_length = 521;            //相机焦距, TUM dataset标定值
  Mat essential_matrix;
  essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
 
  //-- 从本质矩阵中恢复旋转和平移信息.
  recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
  cout<<"R 矩阵:"<<endl<< R <<endl;
}
 
void triangulation(
  const vector<KeyPoint> &keypoint_1,
  const vector<KeyPoint> &keypoint_2,
  const std::vector<DMatch> &matches,
  const Mat &R, const Mat &t,
  vector<Point3d> &points) {
//T1和T2矩阵是相机位姿矩阵
  Mat T1 = (Mat_<float>(3, 4) <<
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0);
  Mat T2 = (Mat_<float>(3, 4) <<
    R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
    R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
    R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0)
  );
 
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
  vector<Point2f> pts_1, pts_2;
  for(int i=0;i<matches.size();i++)
  {
    DMatch m=matches[i];
     // 将像素坐标转换至归一化坐标
    pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));
    pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));
  }
  // for (DMatch m:matches) {
   
  // }
 
  Mat pts_4d;
  cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);//输出三角化后的特征点的3D坐标，是齐次坐标系，是四维的。因此需要将前三个维度除以第四个维度以得到非齐次坐标xyz
 
  // 转换成非齐次坐标
  for (int i = 0; i < pts_4d.cols; i++) {
    Mat x = pts_4d.col(i);
    x /= x.at<float>(3, 0); // 归一化 同时除以最后一维数
    Point3d p(
      x.at<float>(0, 0),
      x.at<float>(1, 0),
      x.at<float>(2, 0)
    );
    points.push_back(p);
  }
}
 
Point2f pixel2cam(const Point2d &p, const Mat &K) {
  return Point2f
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

5.PnP pose_estimation_3d2d.cpp

#include <iostream>
#include <opencv2/core/core.hpp>//opencv核心模块
#include <opencv2/features2d/features2d.hpp>//opencv特征点
#include <opencv2/highgui/highgui.hpp>//opencv gui
#include <opencv2/calib3d/calib3d.hpp>//求解器头文件
#include <Eigen/Core>//eigen核心模块
#include <g2o/core/base_vertex.h>//g2o顶点（Vertex）头文件 视觉slam十四讲p141用顶点表示优化变量，用边表示误差项
#include <g2o/core/base_unary_edge.h>//g2o边（edge）头文件
#include <g2o/core/sparse_optimizer.h>//稠密矩阵求解
#include <g2o/core/block_solver.h>//求解器头文件
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>//高斯牛顿算法头文件
#include <g2o/solvers/dense/linear_solver_dense.h>//线性求解
#include <sophus/se3.hpp>//李群李代数se3
#include <chrono>
 
 
using namespace std;
using namespace cv;
 
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);
 
// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &p, const Mat &K);
 
// BA by g2o
typedef vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> VecVector2d;
typedef vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> VecVector3d;
 
void bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose
);
 
// BA by gauss-newton
void bundleAdjustmentGaussNewton(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose
);
 
int main(int argc, char **argv) {
  if (argc != 4) {
    cout << "usage: pose_estimation_3d2d img1 img2 depth1 depth2" << endl;
    return 1;
  }
  //-- 读取图像
  Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
  Mat img_2 = imread(argv[2], 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;
 
  // 建立3D点 在第一张图的深度图中寻找它们的深度，并求出空间位置，得到3D点
  Mat d1 = imread(argv[3], 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> pts_3d;
  vector<Point2f> pts_2d;
  for (DMatch m:matches) {
    ushort d = d1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];
    if (d == 0)   // bad depth 排除深度为0的关键点
      continue;
    float dd = d / 5000.0;
    Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K); //像素坐标转为相机坐标
    pts_3d.push_back(Point3f(p1.x * dd, p1.y * dd, dd)); //得到像素点在相机坐标下的3D信息
    pts_2d.push_back(keypoints_2[m.trainIdx].pt);  //以第二个图像的特征点位置作为2D点
  }
 
  cout << "3d-2d pairs: " << pts_3d.size() << endl;
 
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  Mat r, t;
  //1.PnP求解  opencv求解
  solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
  Mat R;
  cv::Rodrigues(r, R); // r为旋转向量形式，用Rodrigues公式转换为矩阵 见书上P53 式（3.15）
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp in opencv cost time: " << time_used.count() << " seconds." << endl;
 
  cout << "R=" << endl << R << endl;
  cout << "t=" << endl << t << endl;
  //2.最小化重投影误差求最优解
  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));
  }
//（1）利用高斯牛顿法获取相机位资R和T
  cout << "calling bundle adjustment by gauss newton" << endl;
  Sophus::SE3d pose_gn;
  t1 = chrono::steady_clock::now();
  bundleAdjustmentGaussNewton(pts_3d_eigen, pts_2d_eigen, K, pose_gn);
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp by gauss newton cost time: " << time_used.count() << " seconds." << endl;
//（2）通过g2o图优化算法
  cout << "calling bundle adjustment by g2o" << endl;
  Sophus::SE3d pose_g2o;
  t1 = chrono::steady_clock::now();
  bundleAdjustmentG2O(pts_3d_eigen, pts_2d_eigen, K, pose_g2o);
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve pnp by g2o cost time: " << time_used.count() << " seconds." << endl;
  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) {
  typedef Eigen::Matrix<double, 6, 1> Vector6d;
  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();//定义一个6*6的零矩阵
    Vector6d b = Vector6d::Zero(); //定义6*1的列向量，元素全为0
 
    cost = 0;
    // compute cost
    for (int i = 0; i < points_3d.size(); i++) {
      Eigen::Vector3d pc = pose * points_3d[i]; //P' = TP =[X',Y',Z'] 视觉slam十四讲p186式7.38
      double inv_z = 1.0 / pc[2];//相当于1 / Z'
      double inv_z2 = inv_z * inv_z;//相当于( 1 / Z' ) * ( 1 / Z' )
      Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);//转化为像素坐标 u = fx(X' / z') +cx,v = fy(Y' / z') +cy 视觉slam十四讲p186式7.41
      Eigen::Vector2d e = points_2d[i] - proj;   //比较两个向量，一个是之前利用特征提取得出来的2d像素坐标，一个是现在将3D点投影到2D坐标上
 
      cost += e.squaredNorm(); //squaredNorm（）矩阵/向量所有元素的平方和
      Eigen::Matrix<double, 2, 6> J;//2*6雅克比矩阵 视觉slam十四讲p186式7.46
      //雅克比矩阵表达式见 视觉slam十四讲p186式7.46  
       //导数矩阵 
      J << -fx * inv_z, // -fx * inv_z 相当于-fx / Z'
        0,//0
        fx * pc[0] * inv_z2, //fx * pc[0] * inv_z2相当于fx * X' / ( Z' * Z' )
        fx * pc[0] * pc[1] * inv_z2,
        -fx - fx * pc[0] * pc[0] * inv_z2,//-fx - fx * pc[0] * pc[0] * inv_z2相当于fx * X' * Y' / ( Z' * Z')
        fx * pc[1] * inv_z,    //fx * pc[1] * inv_z相当于fx * Y' / Z'
        0,//0
        -fy * inv_z,  //-fy * inv_z相当于-fy / Z'
        fy * pc[1] * inv_z2,    //fy * pc[1] * inv_z2相当于fy * Y' / (Z' * Z')
        fy + fy * pc[1] * pc[1] * inv_z2,    //fy + fy * pc[1] * pc[1] * inv_z2相当于fy + fy * Y'*Y' / (Z' * Z')
        -fy * pc[0] * pc[1] * inv_z2,    //-fy * pc[0] * pc[1] * inv_z2相当于fy * X' * Y' / ( Z' * Z')
        -fy * pc[0] * inv_z;//-fy * pc[0] * inv_z相当于-fy * X' / Z'
 
      H += J.transpose() * J;
      b += -J.transpose() * e;
    }
 
    Vector6d dx;
    dx = H.ldlt().solve(b);
 
    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }
 
    if (iter > 0 && cost >= lastCost) {
      // cost increase, update is not good
      cout << "cost: " << cost << ", last cost: " << lastCost << endl;
      break;
    }
 
    // update your estimation
    pose = Sophus::SE3d::exp(dx) * pose;
    lastCost = cost;
 
    cout << "iteration " << iter << " cost=" << std::setprecision(12) << cost << endl;
    if (dx.norm() < 1e-6) {
      // converge
      break;
    }
  }
 
  cout << "pose by g-n: \n" << pose.matrix() << endl;
}
 
/// vertex and edges used in g2o ba
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 
  virtual void setToOriginImpl() override {
    _estimate = Sophus::SE3d();
  }
 
  /// left multiplication on SE3
  virtual void oplusImpl(const double *update) override {
    Eigen::Matrix<double, 6, 1> update_eigen;
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;
  }
 
  virtual bool read(istream &in) override {}
 
  virtual bool write(ostream &out) const override {}
};
 
class EdgeProjection : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, VertexPose> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 
  EdgeProjection(const Eigen::Vector3d &pos, const Eigen::Matrix3d &K) : _pos3d(pos), _K(K) {}
 
 // 重写computeError函数，用于计算误差  
virtual void computeError() override {  
    // 将_vertices[0]强制转换为VertexPose类型指针，并赋值给v  
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);  
    // 获取顶点v的估计值，即相机的位姿  
    Sophus::SE3d T = v->estimate();  
    // 计算3D点_pos3d在相机坐标系下的像素坐标  
    Eigen::Vector3d pos_pixel = _K * (T * _pos3d);  
    // 对齐像素坐标（归一化）  
    pos_pixel /= pos_pixel[2];  
    // 计算误差，即观测值_measurement与计算出的像素坐标之间的差  
    _error = _measurement - pos_pixel.head<2>();  
}  
  
// 重写linearizeOplus函数，用于线性化误差模型  
virtual void linearizeOplus() override {  
    // 将_vertices[0]强制转换为VertexPose类型指针，并赋值给v  
    const VertexPose *v = static_cast<VertexPose *> (_vertices[0]);  
    // 获取顶点v的估计值，即相机的位姿  
    Sophus::SE3d T = v->estimate();  
    // 计算3D点_pos3d在相机坐标系下的坐标  
    Eigen::Vector3d pos_cam = T * _pos3d;  
    // 从相机内参矩阵_K中提取焦距和主点坐标  
    double fx = _K(0, 0);  
    double fy = _K(1, 1);  
    double cx = _K(0, 2);  
    double cy = _K(1, 2);  
    // 提取相机坐标系下的x, y, z坐标  
    double X = pos_cam[0];  
    double Y = pos_cam[1];  
    double Z = pos_cam[2];  
    // 计算Z的平方  
    double Z2 = Z * Z;  
    // 计算雅可比矩阵_jacobianOplusXi，用于描述误差关于相机位姿的线性近似  
    _jacobianOplusXi  
      << -fx / Z, 0, fx * X / Z2, fx * X * Y / Z2, -fx - fx * X * X / Z2, fx * Y / Z, // 雅可比矩阵的第一行  
        0, -fy / Z, fy * Y / (Z * Z), fy + fy * Y * Y / Z2, -fy * X * Y / Z2, -fy * X / Z; // 雅可比矩阵的第二行  
}
 
  virtual bool read(istream &in) override {}
 
  virtual bool write(ostream &out) const override {}
 
private:
  Eigen::Vector3d _pos3d;
  Eigen::Matrix3d _K;
};
 
void bundleAdjustmentG2O(
  const VecVector3d &points_3d,
  const VecVector2d &points_2d,
  const Mat &K,
  Sophus::SE3d &pose) {
 
  // 构建图优化，先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  // pose is 6, landmark is 3
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出
 
  // vertex
  // 创建相机位姿顶点  
VertexPose *vertex_pose = new VertexPose(); // camera vertex_pose  
// 为相机位姿顶点设置ID，通常设置为0表示这是一个固定或主要的顶点  
vertex_pose->setId(0);  
// 初始化相机位姿顶点的估计值为单位SE3变换，即没有旋转和位移  
vertex_pose->setEstimate(Sophus::SE3d());  
// 将相机位姿顶点添加到优化器中  
optimizer.addVertex(vertex_pose);  
  
// 创建一个3x3的Eigen矩阵，用于存储相机内参矩阵K  
Eigen::Matrix3d K_eigen;  
// 使用OpenCV矩阵K的元素来初始化Eigen矩阵K_eigen  
// 注意：这里假设K是一个已经存在的OpenCV矩阵，且其类型为CV_64F（即double类型）  
K_eigen <<  
    K.at<double>(0, 0), K.at<double>(0, 1), K.at<double>(0, 2), // 第一行  
    K.at<double>(1, 0), K.at<double>(1, 1), K.at<double>(1, 2), // 第二行  
    K.at<double>(2, 0), K.at<double>(2, 1), K.at<double>(2, 2); // 第三行
 
  // edges  
// 遍历二维点集，为每一个二维点创建一个边，并与相机位姿顶点连接  
int index = 1; // 边的ID从1开始，避免与相机位姿顶点的ID冲突  
for (size_t i = 0; i < points_2d.size(); ++i) {  
  // 获取当前的二维点  
  auto p2d = points_2d[i];  
  // 获取对应的三维点  
  auto p3d = points_3d[i];  
  // 创建投影边，连接相机位姿顶点，并关联三维点坐标和相机内参  
  EdgeProjection *edge = new EdgeProjection(p3d, K_eigen);  
  // 设置边的ID  
  edge->setId(index);  
  // 将边与相机位姿顶点连接  
  edge->setVertex(0, vertex_pose);  
  // 设置观测值为当前的二维点坐标  
  edge->setMeasurement(p2d);  
  // 设置信息矩阵，这里使用单位矩阵  
  edge->setInformation(Eigen::Matrix2d::Identity());  
  // 将边加入优化器  
  optimizer.addEdge(edge);  
  index++; // 边的ID递增  
}  
  
// 记录优化开始时间  
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();  
  
// 打开优化器的调试输出  
optimizer.setVerbose(true);  
  
// 初始化优化  
optimizer.initializeOptimization();  
  
// 执行优化，最多迭代10次  
optimizer.optimize(10);  
  
// 记录优化结束时间  
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();  
  
// 计算优化用时  
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);  
  
// 输出优化用时  
cout << "optimization costs time: " << time_used.count() << " seconds." << endl;  
  
// 输出优化后的相机位姿  
cout << "pose estimated by g2o =\n" << vertex_pose->estimate().matrix() << endl;  
  
// 将优化后的相机位姿赋值给pose变量  
pose = vertex_pose->estimate();

6.ICP pose_estimation_3d3d.cpp

// 引入iostream库，用于输入输出流操作  
#include <iostream>  
  
// 引入OpenCV的core模块，提供基础的数据结构和功能  
#include <opencv2/core/core.hpp>  
  
// 引入OpenCV的features2d模块，用于特征检测和描述  
#include <opencv2/features2d/features2d.hpp>  
  
// 引入OpenCV的highgui模块，用于图像的显示和高级GUI功能  
#include <opencv2/highgui/highgui.hpp>  
  
// 引入OpenCV的calib3d模块，用于相机标定和3D重建  
#include <opencv2/calib3d/calib3d.hpp>  
  
// 引入Eigen库，用于线性代数、矩阵和向量运算、数值分析和相关的数学算法  
#include <Eigen/Core>  
#include <Eigen/Dense>  
#include <Eigen/Geometry>  
#include <Eigen/SVD>  
  
// 引入g2o库，用于通用图优化  
#include <g2o/core/base_vertex.h>  
#include <g2o/core/base_unary_edge.h>  
#include <g2o/core/block_solver.h>  
#include <g2o/core/optimization_algorithm_gauss_newton.h>  
#include <g2o/core/optimization_algorithm_levenberg.h>  
  
// 引入g2o的dense求解器  
#include <g2o/solvers/dense/linear_solver_dense.h>  
  
// 引入chrono库，用于高精度时间测量  
#include <chrono>  
  
// 引入Sophus库，用于李群和李代数的操作  
#include <sophus/se3.hpp>  
  
// 使用std命名空间，简化代码书写  
using namespace std;  
  
// 使用cv命名空间，OpenCV的相关功能都在这个命名空间下  
using namespace cv;  
  
// 定义函数，用于在两个图像之间找到特征匹配  
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         // 匹配的结果  
);  
  
// 定义函数，将像素坐标转换为相机归一化坐标  
Point2d pixel2cam(const Point2d &p, const Mat &K);  
  
// 定义函数，用于估计3D-3D之间的变换矩阵  
void pose_estimation_3d3d(  
  const vector<Point3f> &pts1,  // 第一组3D点  
  const vector<Point3f> &pts2,  // 第二组3D点  
  Mat &R, Mat &t                // 输出的旋转矩阵和平移向量  
);  
  
// 定义函数，用于进行Bundle Adjustment优化  
void bundleAdjustment(  
  const vector<Point3f> &points_3d,  // 3D点  
  const vector<Point3f> &points_2d,  // 2D点  
  Mat &R, Mat &t                    // 输出的旋转矩阵和平移向量  
);  
  
// 定义g2o中使用的顶点和边  
/// vertex and edges used in g2o ba  
class VertexPose : public g2o::BaseVertex<6, Sophus::SE3d> {  
public:  
  // 使得类的new操作符按照Eigen的对齐方式进行内存分配  
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;  
  
  // 设置顶点为原点  
  virtual void setToOriginImpl() override {  
    _estimate = Sophus::SE3d();  
  }  
  
  // 使用左乘更新顶点  
  /// left multiplication on SE3  
  virtual void oplusImpl(const double *update) override {  
    // 将update数组转换为Eigen的向量  
    Eigen::Matrix<double, 6, 1> update_eigen;  
    update_eigen << update[0], update[1], update[2], update[3], update[4], update[5];  
    // 使用Sophus的exp函数将更新量转换为李代数，然后使用左乘更新_estimate  
    _estimate = Sophus::SE3d::exp(update_eigen) * _estimate;  
  }  
  
  // 读盘存盘：留空  
  virtual bool read(istream &in) override {}
  virtual bool write(ostream &out) const override {}
};
// 定义g2o中的边，用于RGB-D的投影误差  
class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexPose> {  
public:  
  // 使得类的new操作符按照Eigen的对齐方式进行内存分配  
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;  
  
  // 构造函数，接收一个3D点作为观测值  
  EdgeProjectXYZRGBDPoseOnly(const Eigen::Vector3d &point) : _point(point) {}  
  
  // 计算误差  
  virtual void computeError() override {  
    // 将第一个顶点转换为VertexPose类型，并获取其估计的位姿  
    const VertexPose *pose = static_cast<const VertexPose *> ( _vertices[0] );  
    // 计算观测值与估计值之间的误差  
    _error = _measurement - pose->estimate() * _point;  
  }  
  
  // 线性化误差函数  
  virtual void linearizeOplus() override {  
    // 将第一个顶点转换为VertexPose类型，并获取其估计的位姿  
    VertexPose *pose = static_cast<VertexPose *>(_vertices[0]);  
    // 获取位姿的SE3表示  
    Sophus::SE3d T = pose->estimate();  
    // 计算变换后的3D点  
    Eigen::Vector3d xyz_trans = T * _point;  
    // 设置雅可比矩阵的前3x3块为单位矩阵的负值  
    _jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix3d::Identity();  
    // 设置雅可比矩阵的后3x3块为xyz_trans对应的李代数表示的叉乘矩阵  
    _jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(xyz_trans);  
  }  
  
  // 存盘读盘：留空  
  bool read(istream &in) {}   
  bool write(ostream &out) const {}  
  
protected:  
  // 存储观测的3D点  
  Eigen::Vector3d _point;  
};  
  
// 主函数入口  
int main(int argc, char **argv) {  
  // 如果命令行参数数量不等于5，则输出使用方法并退出  
  if (argc != 5) {  
    cout << "usage: pose_estimation_3d3d img1 img2 depth1 depth2" << endl;  
    return 1;  
  }  
  
  // 读取第一张图像  
  Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);  
  // 读取第二张图像  
  Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);  
  
  // 定义关键点向量和匹配向量  
  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 depth1 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);  
  // 读取第二张图像的深度图  
  Mat depth2 = imread(argv[4], CV_LOAD_IMAGE_UNCHANGED);  
  
  // 定义相机内参矩阵K  
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);  
  
  // 定义存储3D点的向量  
  vector<Point3f> pts1, pts2;  
  
  // 遍历匹配点，将匹配点转换为3D点  
  for (DMatch m:matches) {  
    // 获取第一张图像中关键点的深度值  
    ushort d1 = depth1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)];  
    // 获取第二张图像中关键点的深度值  
   ushort d2 = depth2.ptr<unsigned short>(int(keypoints_2[m.trainIdx].pt.y))[int(keypoints_2[m.trainIdx].pt.x)];
   // 如果深度值为0（表示无效的深度），则跳过当前循环的迭代  
if (d1 == 0 || d2 == 0) {  
  continue;  
}  
  
// 将像素坐标转换为相机坐标  
Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);  
Point2d p2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);  
  
// 将深度值从毫米转换为米  
float dd1 = float(d1) / 5000.0;  
float dd2 = float(d2) / 5000.0;  
  
// 将相机坐标与深度值结合，得到3D点  
pts1.push_back(Point3f(p1.x * dd1, p1.y * dd1, dd1));  
pts2.push_back(Point3f(p2.x * dd2, p2.y * dd2, dd2));  
  
// 输出成功转换的3D点对的数量  
cout << "3d-3d pairs: " << pts1.size() << endl;  
  
// 定义旋转矩阵R和平移向量t  
Mat R, t;  
  
// 调用pose_estimation_3d3d函数，使用SVD进行迭代最近点（ICP）求解，得到旋转矩阵R和平移向量t  
pose_estimation_3d3d(pts1, pts2, R, t);  
  
// 输出SVD求解得到的旋转矩阵和平移向量  
cout << "ICP via SVD results: " << endl;  
cout << "R = " << R << endl;  
cout << "t = " << t << endl;  
  
// 输出旋转矩阵的逆和平移向量的逆  
cout << "R_inv = " << R.t() << endl;  
cout << "t_inv = " << -R.t() * t << endl;  
  
// 调用bundleAdjustment函数进行捆集调整优化  
cout << "calling bundle adjustment" << endl;  
bundleAdjustment(pts1, pts2, R, t);  
  
// 验证优化后的位姿变换是否正确  
cout << "verifying p1 = R * p2 + t" << endl;  
for (int i = 0; i < 5; i++) {  
  // 输出第i个3D点  
  cout << "p1 = " << pts1[i] << endl;  
  cout << "p2 = " << pts2[i] << endl;  
  // 计算变换后的p2，并与p1比较  
  cout << "(R*p2+t) = " <<  
       R * (Mat_<double>(3, 1) << pts2[i].x, pts2[i].y, pts2[i].z) + t  
       << endl;  
  cout << endl;  
 }
}
// 声明函数，输入参数为两幅图像img_1和img_2，以及它们的特征点集合keypoints_1和keypoints_2，输出匹配点对matches  
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对象，用于存储描述子  
  Mat descriptors_1, descriptors_2;  
  
  // 创建一个ORB特征检测器（用于OpenCV3版本）  
  Ptr<FeatureDetector> detector = ORB::create();  
  // 创建一个ORB描述子提取器（用于OpenCV3版本）  
  Ptr<DescriptorExtractor> descriptor = ORB::create();  
  
  // 如果你的环境是OpenCV2，你可以使用下面的方式创建检测器和提取器  
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );  
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );  
  
  // 创建一个基于Hamming距离的暴力匹配器  
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");  
  
  // 第一步: 在两幅图像中检测Oriented FAST角点  
  // 在img_1中检测角点，并将结果存储在keypoints_1中  
  detector->detect(img_1, keypoints_1);  
  // 在img_2中检测角点，并将结果存储在keypoints_2中  
  detector->detect(img_2, keypoints_2);  
  
  // 第二步: 根据检测到的角点位置计算BRIEF描述子  
  // 在img_1的角点位置计算描述子，并将结果存储在descriptors_1中  
  descriptor->compute(img_1, keypoints_1, descriptors_1);  
  // 在img_2的角点位置计算描述子，并将结果存储在descriptors_2中  
  descriptor->compute(img_2, keypoints_2, descriptors_2);  
  
  // 第三步: 使用Hamming距离对两幅图像中的描述子进行匹配  
  vector<DMatch> match; // 临时变量，用于存储匹配结果  
  // 对descriptors_1和descriptors_2中的描述子进行匹配，并将结果存储在match中  
  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(  
    // 使用内参矩阵K的fx和cx进行归一化  
    (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),  
    // 使用内参矩阵K的fy和cy进行归一化  
    (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)  
  );  
}  
  
// 估计3D到3D点的刚体变换  
void pose_estimation_3d3d(const vector<Point3f> &pts1,  
                           const vector<Point3f> &pts2,  
                           Mat &R, Mat &t) {  
  Point3f p1, p2;     // 初始化质心  
  int N = pts1.size(); // 获取点的数量  
  
  // 计算两组点的质心  
  for (int i = 0; i < N; i++) {  
    p1 += pts1[i];  
    p2 += pts2[i];  
  }  
  // 计算质心并赋值给p1和p2  
  p1 = Point3f(Vec3f(p1) / N);  
  p2 = Point3f(Vec3f(p2) / N);  
  
  vector<Point3f> q1(N), q2(N); // 去除质心的点集  
  
  // 去除质心，得到新的点集q1和q2  
  for (int i = 0; i < N; i++) {  
    q1[i] = pts1[i] - p1;  
    q2[i] = pts2[i] - p2;  
  }  
  
  // 初始化W矩阵，用于存储q1和q2之间的叉积  
  Eigen::Matrix3d W = Eigen::Matrix3d::Zero();  
  
  // 计算q1和q2的叉积，并累加到W  
  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();  
  }  
  // 输出W矩阵  
  cout << "W=" << W << endl;  
  
  // 对W进行SVD分解  
  Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);  
  // 获取U和V矩阵  
  Eigen::Matrix3d U = svd.matrixU();  
  Eigen::Matrix3d V = svd.matrixV();  
  
  // 输出U和V矩阵  
  cout << "U=" << U << endl;  
  cout << "V=" << V << endl;  
  
  // 计算旋转矩阵R_  
  Eigen::Matrix3d R_ = U * (V.transpose());  
  
  // 如果R_的行列式为负，则取反，确保是旋转而不是反射  
  if (R_.determinant() < 0) {  
    R_ = -R_;  
  }  
  // 计算平移向量t_  
  Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);  
  
  // 将Eigen的矩阵和向量转换为OpenCV的Mat格式  
  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));  
}
// bundleAdjustment函数：使用图优化方法进行3D点对的相机位姿优化  
void bundleAdjustment(  
  const vector<Point3f> &pts1,       // 第一组3D点  
  const vector<Point3f> &pts2,       // 第二组3D点  
  Mat &R, Mat &t) {                   // 输出旋转矩阵和平移向量  
  
  // 构建图优化模型，首先设定g2o  
  // 定义块求解器类型  
  typedef g2o::BlockSolverX BlockSolverType;  
  // 定义线性求解器类型  
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType;  
  
  // 梯度下降方法选择Levenberg-Marquardt方法  
  auto solver = new g2o::OptimizationAlgorithmLevenberg(  
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));  
  
  // 创建图优化器对象  
  g2o::SparseOptimizer optimizer;  
  // 设置优化器使用的求解器  
  optimizer.setAlgorithm(solver);  
  // 打开调试输出  
  optimizer.setVerbose(true);  
  
  // 顶点：相机位姿  
  VertexPose *pose = new VertexPose();  
  pose->setId(0); // 设置顶点ID  
  pose->setEstimate(Sophus::SE3d()); // 初始化为单位SE3  
  optimizer.addVertex(pose); // 将相机位姿顶点加入优化器  
  
  // 边：观测方程  
  for (size_t i = 0; i < pts1.size(); i++) {  
    // 创建边，连接相机位姿顶点与观测值  
    EdgeProjectXYZRGBDPoseOnly *edge = new EdgeProjectXYZRGBDPoseOnly(  
      Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z));  
    edge->setVertex(0, pose); // 设置边的连接顶点  
    // 设置观测值  
    edge->setMeasurement(Eigen::Vector3d(  
      pts1[i].x, pts1[i].y, pts1[i].z));  
    // 设置信息矩阵，这里使用单位矩阵  
    edge->setInformation(Eigen::Matrix3d::Identity());  
    optimizer.addEdge(edge); // 将边加入优化器  
  }  
  
  // 记录优化开始时间  
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();  
  // 初始化优化  
  optimizer.initializeOptimization();  
  // 执行优化，最多迭代10次  
  optimizer.optimize(10);  
  // 记录优化结束时间  
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();  
  // 计算优化用时  
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);  
  cout << "optimization costs time: " << time_used.count() << " seconds." << endl;  
  
  // 输出优化后的结果  
  cout << endl << "after optimization:" << endl;  
  cout << "T=\n" << pose->estimate().matrix() << endl;  
  
  // 将Sophus的SE3转换为OpenCV的Mat格式  
  Eigen::Matrix3d R_ = pose->estimate().rotationMatrix();  
  Eigen::Vector3d t_ = pose->estimate().translation();  
  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));  
}
阅读全部

相关推荐

  1. 视觉SLAM学习打卡【7-2】-视觉里程计·特征点法程序详解

    2024-04-08 02:28:02       15 阅读

  10. 视觉SLAM

    2024-04-08 02:28:02       8 阅读

最近更新

  4. TCP协议是安全的吗?

    2024-04-08 02:28:02       18 阅读

  12. 阿里云服务器执行yum，一直下载docker-ce-stable失败

    2024-04-08 02:28:02       19 阅读

  13. 【Python教程】压缩PDF文件大小

    2024-04-08 02:28:02       18 阅读

  18. 通过文章id递归查询所有评论（xml）

    2024-04-08 02:28:02       20 阅读

热门阅读

  1. 集成swagger2的时候swagger-ui.html页面的v2/api-docs接口报404

    2024-04-08 02:28:02       14 阅读
  2. DFS序列

    DFS序列

    2024-04-08 02:28:02      9 阅读

  3. linux中常见的后台进程

    2024-04-08 02:28:02       16 阅读

  5. Docker搭建私有镜像仓库

    2024-04-08 02:28:02       13 阅读

  7. 洛谷的练习（天梯赛）

    2024-04-08 02:28:02       15 阅读

  8. localStorage封装代码

    2024-04-08 02:28:02       17 阅读

  9. es6:set()和weakset()

    2024-04-08 02:28:02       16 阅读

  12. docker一键部署GPU版ChatGLM3

    2024-04-08 02:28:02       15 阅读

  20. c#编程基础学习之数组

    2024-04-08 02:28:02       16 阅读

  21. ubuntu c++调用python编译成的so库并获取返回值

    2024-04-08 02:28:02       18 阅读

  23. 服务器硬件基础知识202404

    2024-04-08 02:28:02       24 阅读

  24. 【python】Flask Web框架

    2024-04-08 02:28:02       23 阅读

  28. 【00150】金融理论与实务-2023年10月自考真题

    2024-04-08 02:28:02       21 阅读

  29. python格式化输出

    2024-04-08 02:28:02       21 阅读