视觉SLAM十四讲CH7代码解析

#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);//定义find_feature_matches函数 输入图像1和图像2，输出特征点集合1、特征点集合2和匹配点对

 

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);定义pose_estimation_2d2d函数 输入特征点集合1、特征点集合2和匹配点对，输出估计的旋转矩阵、估计的平移向量和本质矩阵，平移向量差了一个尺度因子，通常将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

);//定义triangulation函数

 

/// 作图用

//inline表示内联函数

inline cv::Scalar get_color(float depth) //depth表示输入深度，get_color表示返回颜色信息

{

  float up_th = 50, low_th = 10, th_range = up_th - low_th;//这里相当于定义阈值

  if (depth > up_th) depth = up_th;//depth > 50 depth = 50

  if (depth < low_th) depth = low_th;//depth < 10 depth = 10

  return cv::Scalar(255 * depth / th_range, 0, 255 * (1 - depth / th_range)); //Scalar()中的颜色顺序为BGR

  //B = 255 * depth / th_range =  255 * depth / 40，G = 0，R = 255 * (1 - depth / th_range) = 255 * (1 - depth /40 ) 

  //反映出的深度信息就是图像里的像素点越远，对应的特征点颜色越红

}

 

// 像素坐标转相机归一化坐标

Point2f pixel2cam(const Point2d &p, const Mat &K);

 

int main(int argc, char **argv) {

  if (argc != 3) {

    cout << "usage: trianguendl imag1 imag2:"<<endl;

    return 1;

  }//在屏幕上显示命令行用法

  //-- 读取图像

  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//计时开始

  

  Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);//读取图像1  CV_LOAD_IMAGE_COLOR表示返回一张彩色图像

  Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);//读取图像2  CV_LOAD_IMAGE_COLOR表示返回一张彩色图像

 

  vector<KeyPoint> keypoints_1, keypoints_2;//keypoints_1->图像1 keypoints_2->图像2

  vector<DMatch> matches;//匹配matches

  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);//调用find_feature_matches函数

  cout << "一共找到了" << matches.size() << "组匹配点" << endl;//输出匹配点数

 

  //-- 估计两张图像间运动

  Mat R, t;//R,t表示旋转矩阵和平移

  pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);//调用函数 pose_estimation_2d2d

 

  //-- 三角化得到特征点在相机1的相机坐标系下的坐标

  vector<Point3d> points;//相机1中的特征点在其相机坐标系中的坐标

  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;  //图像1深度信息

    cout << "depth: " << depth1 << endl;//输出深度信息

    Mat pt1_cam = K * (Mat_<double>(3, 1) << points[i].x / points[i].z, points[i].y / points[i].z, 1); //pt1_cam表示的是归一化坐标(u, v, 1) 具体形式可参考视觉slam十四讲p168的式7.11

    //Point2d pt1_cam = pixel2cam(keypoints_1[matches[i].queryIdx].pt, K);//像素坐标转换为相机坐标

    Point2f pixel1 = keypoints_1[matches[i].queryIdx].pt;

    cv::circle(img1_plot, keypoints_1[matches[i].queryIdx].pt, 2, get_color(depth1), 2);//画圆

    

    Point2f residual1;//图像1残差

    residual1.x = pt1_cam.at<double>(0, 0) - pixel1.x;

    residual1.y = pt1_cam.at<double>(1, 0) - pixel1.y;

    cout << "图像1像素点的深度为" << depth1 << "平移单位 "<< endl;

    cout<<"图像1的残差为: " << residual1.x << ", " << residual1.y << ")像素单位 " << endl;

 

    // 第二个图

    Mat pt2_trans = R * (Mat_<double>(3, 1) << points[i].x, points[i].y, points[i].z) + t;//相当于x2 = R * x1 + t的表达式 //pt2_trans表示的是归一化坐标(u, v, 1) 具体形式可参考视觉slam十四讲p168的式7.11

    float depth2 = pt2_trans.at<double>(2, 0); //图像2的深度信息

    Point2f pixel2 = keypoints_2[matches[i].trainIdx].pt;

    cv::circle(img2_plot, keypoints_2[matches[i].trainIdx].pt, 2, get_color(depth2), 2);//画圆

    Point2f residual2;//图像2残差

    residual2.x = pt2_trans.at<double>(0, 0) - pixel2.x;

    residual2.y = pt2_trans.at<double>(1, 0) - pixel2.y;

    cout << "图像2像素点的深度为" << depth2 << "平移单位 "<< endl;

    cout<<"图像2的残差为: " << residual2.x << ", " << residual2.y << ")像素单位 " << endl;

 

  }

  cv::imshow("img 1", img1_plot);//界面展示图像1的深度信息结果 红圈

  cv::imshow("img 2", img2_plot);//界面展示图像2的深度信息结果 红圈

  cv::waitKey();

  

  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;//描述子和描述子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);//检测图像1的 Oriented FAST 角点

  detector->detect(img_2, keypoints_2);//检测图像2的 Oriented FAST 角点

 

  //-- 第二步:根据角点位置计算 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++)//不同的结果可以在这里设置

    //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误 30.0为经验值

    {

    if (match[i].distance <= max(2 * min_dist, 30.0)) {

      matches.push_back(match[i]);

    }

  }

  Mat img_match;

  drawMatches(img_1, keypoints_1, img_2, keypoints_2, matches, img_match, Scalar::all(-1));

  imshow("good matches", img_match);

  waitKey(0);  //程序暂停执行，等待一个按键输入

}

 

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

}

 

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

  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;//归一化平面上的点，triangulatePoints()函数的输入参数

  for (DMatch m:matches) {

    // 将像素坐标转换至相机坐标

    pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));

    pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));

  }

 

  Mat pts_4d;

  cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);

 

  // 转换成非齐次坐标

  for (int i = 0; i < pts_4d.cols; i++) {

    Mat x = pts_4d.col(i);//取第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)

    );

}

 

./triangulation 1.png 2.png 

-- Max dist : 94.000000 

-- Min dist : 4.000000 

一共找到了79组匹配点

depth: 14.4036

图像1像素点的深度为14.4036平移单位 

图像1的残差为: -0.0148561, -0.150038)像素单位 

图像2像素点的深度为14.8448平移单位 

图像2的残差为: -323.06, -113.005)像素单位 

depth: 9.63635

图像1像素点的深度为9.63635平移单位 

图像1的残差为: -0.019322, -0.644624)像素单位 

图像2像素点的深度为10.0757平移单位 

图像2的残差为: -233.409, -220.17)像素单位 

depth: 7.88393

图像1像素点的深度为7.88393平移单位 

图像1的残差为: -0.00437226, 0.114155)像素单位 

图像2像素点的深度为8.41888平移单位 

图像2的残差为: -69.2036, -307.06)像素单位 

depth: 8.35771

图像1像素点的深度为8.35771平移单位 

图像1的残差为: 5.11942e-05, -0.0027093)像素单位 

图像2像素点的深度为8.79158平移单位 

图像2的残差为: -187.365, -291.286)像素单位 

depth: 7.75093

图像1像素点的深度为7.75093平移单位 

图像1的残差为: -0.00163884, 0.0728191)像素单位 

图像2像素点的深度为8.29532平移单位 

图像2的残差为: -65.2057, -286.407)像素单位 

depth: 12.5495

图像1像素点的深度为12.5495平移单位 

图像1的残差为: 0.00141988, 0.0284572)像素单位 

图像2像素点的深度为12.786平移单位 

图像2的残差为: -411.818, -189.515)像素单位 

depth: 7.71705

图像1像素点的深度为7.71705平移单位 

图像1的残差为: -0.00137752, 0.149456)像素单位 

图像2像素点的深度为8.29455平移单位 

图像2的残差为: -39.6193, -266.727)像素单位 

depth: 7.93482

图像1像素点的深度为7.93482平移单位 

图像1的残差为: 0.0346043, -1.0847)像素单位 

图像2像素点的深度为8.47505平移单位 

图像2的残差为: -68.2496, -300.149)像素单位 

depth: 7.57168

图像1像素点的深度为7.57168平移单位 

图像1的残差为: 7.85694e-05, -0.0130595)像素单位 

图像2像素点的深度为8.14345平移单位 

图像2的残差为: -41.504, -263.776)像素单位 

depth: 8.46969

图像1像素点的深度为8.46969平移单位 

图像1的残差为: -0.00246999, -0.356362)像素单位 

图像2像素点的深度为8.59691平移单位 

图像2的残差为: -506.949, -265.725)像素单位 

depth: 9.78479

图像1像素点的深度为9.78479平移单位 

图像1的残差为: -0.00553395, -0.27674)像素单位 

图像2像素点的深度为10.1623平移单位 

图像2的残差为: -279.9, -236.262)像素单位 

depth: 8.88268

图像1像素点的深度为8.88268平移单位 

图像1的残差为: -0.0144425, -0.259716)像素单位 

图像2像素点的深度为9.43465平移单位 

图像2的残差为: -138.443, -183.222)像素单位 

depth: 9.02949

图像1像素点的深度为9.02949平移单位 

图像1的残差为: -0.00919512, -0.389588)像素单位 

图像2像素点的深度为9.32731平移单位 

图像2的残差为: -349.947, -231.917)像素单位 

depth: 8.87554

图像1像素点的深度为8.87554平移单位 

图像1的残差为: -0.000924774, -0.107203)像素单位 

图像2像素点的深度为9.00642平移单位 

图像2的残差为: -496.976, -262.768)像素单位 

depth: 9.66984

图像1像素点的深度为9.66984平移单位 

图像1的残差为: -0.00427046, -0.142931)像素单位 

图像2像素点的深度为10.1099平移单位 

图像2的残差为: -233.415, -220.181)像素单位 

depth: 12.2194

图像1像素点的深度为12.2194平移单位 

图像1的残差为: -0.00253309, -0.0511311)像素单位 

图像2像素点的深度为12.447平移单位 

图像2的残差为: -418.904, -189.863)像素单位 

depth: 14.8655

图像1像素点的深度为14.8655平移单位 

图像1的残差为: 0.0416501, 0.427027)像素单位 

图像2像素点的深度为15.3079平移单位 

图像2的残差为: -324.033, -115.67)像素单位 

depth: 15.0627

图像1像素点的深度为15.0627平移单位 

图像1的残差为: 0.0816033, 0.852222)像素单位 

图像2像素点的深度为15.4822平移单位 

图像2的残差为: -336.842, -118.057)像素单位 

depth: 7.7117

图像1像素点的深度为7.7117平移单位 

图像1的残差为: -0.00318172, 0.235026)像素单位 

图像2像素点的深度为8.27117平移单位 

图像2的残差为: -54.7616, -273.224)像素单位 

depth: 8.78825

图像1像素点的深度为8.78825平移单位 

图像1的残差为: -0.00246132, -0.290719)像素单位 

图像2像素点的深度为8.9215平移单位 

图像2的残差为: -496.218, -262.571)像素单位 

depth: 9.10896

图像1像素点的深度为9.10896平移单位 

图像1的残差为: -0.0259298, -1.09092)像素单位 

图像2像素点的深度为9.40757平移单位 

图像2的残差为: -349.472, -230.729)像素单位 

depth: 9.62574

图像1像素点的深度为9.62574平移单位 

图像1的残差为: -0.00712829, -0.361729)像素单位 

图像2像素点的深度为10.0024平移单位 

图像2的残差为: -279.297, -236.649)像素单位 

depth: 8.78611

图像1像素点的深度为8.78611平移单位 

图像1的残差为: -0.00323575, -0.38321)像素单位 

图像2像素点的深度为8.9158平移单位 

图像2的残差为: -499.759, -262.569)像素单位 

depth: 12.5336

图像1像素点的深度为12.5336平移单位 

图像1的残差为: 0.0264397, 0.525318)像素单位 

图像2像素点的深度为12.7705平移单位 

图像2的残差为: -411.821, -189.525)像素单位 

depth: 7.88369

图像1像素点的深度为7.88369平移单位 

图像1的残差为: 0.00499926, -0.207999)像素单位 

图像2像素点的深度为8.42969平移单位 

图像2的残差为: -66.6515, -288.558)像素单位 

depth: 8.45255

图像1像素点的深度为8.45255平移单位 

图像1的残差为: 0.000167288, -0.00856084)像素单位 

图像2像素点的深度为8.88793平移单位 

图像2的残差为: -187.39, -291.278)像素单位 

depth: 9.65039

图像1像素点的深度为9.65039平移单位 

图像1的残差为: 0.0113906, 0.380628)像素单位 

图像2像素点的深度为10.0905平移单位 

图像2的残差为: -233.364, -220.049)像素单位 

depth: 8.64867

图像1像素点的深度为8.64867平移单位 

图像1的残差为: -0.00476583, -1.02933)像素单位 

图像2像素点的深度为8.77996平移单位 

图像2的残差为: -498.154, -267.517)像素单位 

depth: 12.2082

图像1像素点的深度为12.2082平移单位 

图像1的残差为: -0.0144103, -0.288414)像素单位 

图像2像素点的深度为12.4375平移单位 

图像2的残差为: -418.203, -188.685)像素单位 

depth: 12.1836

图像1像素点的深度为12.1836平移单位 

图像1的残差为: -0.0067673, -0.1348)像素单位 

图像2像素点的深度为12.4232平移单位 

图像2的残差为: -411.177, -188.687)像素单位 

depth: 9.18492

图像1像素点的深度为9.18492平移单位 

图像1的残差为: -0.0117828, -0.486061)像素单位 

图像2像素点的深度为9.48384平移单位 

图像2的残差为: -349.468, -230.743)像素单位 

depth: 9.52295

图像1像素点的深度为9.52295平移单位 

图像1的残差为: 4.27868e-06, 0.000192284)像素单位 

图像2像素点的深度为9.89965平移单位 

图像2的残差为: -278.817, -236.417)像素单位 

depth: 8.39902

图像1像素点的深度为8.39902平移单位 

图像1的残差为: 0.0106431, -0.57336)像素单位 

图像2像素点的深度为8.83456平移单位 

图像2的残差为: -186.708, -290.173)像素单位 

depth: 8.78013

图像1像素点的深度为8.78013平移单位 

图像1的残差为: -0.00851937, -0.155527)像素单位 

图像2像素点的深度为9.32845平移单位 

图像2的残差为: -138.758, -184.074)像素单位 

depth: 7.80552

图像1像素点的深度为7.80552平移单位 

图像1的残差为: -0.00571924, 0.163689)像素单位 

图像2像素点的深度为8.35257平移单位 

图像2的残差为: -57.1661, -301.558)像素单位 

depth: 8.57948

图像1像素点的深度为8.57948平移单位 

图像1的残差为: -0.00305312, -0.73087)像素单位 

图像2像素点的深度为8.71138平移单位 

图像2的残差为: -498.177, -269.224)像素单位 

depth: 8.53905

图像1像素点的深度为8.53905平移单位 

图像1的残差为: -0.00644693, -0.82554)像素单位 

图像2像素点的深度为8.67311平移单位 

图像2的残差为: -499.889, -262.427)像素单位 

depth: 14.3688

图像1像素点的深度为14.3688平移单位 

图像1的残差为: -0.00555154, -0.0547732)像素单位 

图像2像素点的深度为14.8153平移单位 

图像2的残差为: -321.513, -109.51)像素单位 

depth: 9.03321

图像1像素点的深度为9.03321平移单位 

图像1的残差为: -0.0188211, -0.77313)像素单位 

图像2像素点的深度为9.33315平移单位 

图像2的残差为: -348.627, -230.167)像素单位 

depth: 12.3013

图像1像素点的深度为12.3013平移单位 

图像1的残差为: -0.00555085, -0.112271)像素单位 

图像2像素点的深度为12.5303平移单位 

图像2的残差为: -417.624, -189.825)像素单位 

depth: 12.3386

图像1像素点的深度为12.3386平移单位 

图像1的残差为: 0.0156202, 0.308888)像素单位 

图像2像素点的深度为12.5778平移单位 

图像2的残差为: -411.151, -188.716)像素单位 

depth: 14.5313

图像1像素点的深度为14.5313平移单位 

图像1的残差为: 4.78995e-06, 6.16349e-05)像素单位 

图像2像素点的深度为14.9694平移单位 

图像2的残差为: -324.871, -114.589)像素单位 

depth: 7.71837

图像1像素点的深度为7.71837平移单位 

图像1的残差为: 0.00950829, -0.744489)像素单位 

图像2像素点的深度为8.28189平移单位 

图像2的残差为: -50.517, -273.209)像素单位 

depth: 7.95137

图像1像素点的深度为7.95137平移单位 

图像1的残差为: -0.00520178, 0.398407)像素单位 

图像2像素点的深度为8.5142平移单位 

图像2的残差为: -59.7056, -272.649)像素单位 

depth: 9.54909

图像1像素点的深度为9.54909平移单位 

图像1的残差为: -0.019034, -0.633304)像素单位 

图像2像素点的深度为9.98727平移单位 

图像2的残差为: -233.346, -220.024)像素单位 

depth: 8.44807

图像1像素点的深度为8.44807平移单位 

图像1的残差为: -1.71908e-05, 0.000849519)像素单位 

图像2像素点的深度为8.88461平移单位 

图像2的残差为: -186.721, -290.178)像素单位 

depth: 9.13752

图像1像素点的深度为9.13752平移单位 

图像1的残差为: -0.000702037, 0.275408)像素单位 

图像2像素点的深度为9.57372平移单位 

图像2的残差为: -207.827, -267.512)像素单位 

depth: 9.51844

图像1像素点的深度为9.51844平移单位 

图像1的残差为: -0.0124249, -0.605402)像素单位 

图像2像素点的深度为9.89509平移单位 

图像2的残差为: -279.099, -235.276)像素单位 

depth: 9.60796

图像1像素点的深度为9.60796平移单位 

图像1的残差为: -0.00195225, -0.112312)像素单位 

图像2像素点的深度为10.1347平移单位 

图像2的残差为: -151.76, -236.421)像素单位 

depth: 9.20528

图像1像素点的深度为9.20528平移单位 

图像1的残差为: -0.000143149, -0.00869456)像素单位 

图像2像素点的深度为9.72495平移单位 

图像2的残差为: -150.207, -235.282)像素单位 

depth: 8.86045

图像1像素点的深度为8.86045平移单位 

图像1的残差为: 0.00071856, -1.15286)像素单位 

图像2像素点的深度为9.29351平移单位 

图像2的残差为: -207.425, -265.121)像素单位 

depth: 9.33464

图像1像素点的深度为9.33464平移单位 

图像1的残差为: -0.0106116, -0.434357)像素单位 

图像2像素点的深度为9.63318平移单位 

图像2的残差为: -349.97, -230.523)像素单位 

depth: 9.40554

图像1像素点的深度为9.40554平移单位 

图像1的残差为: -0.0272679, -0.874236)像素单位 

图像2像素点的深度为9.84166平移单位 

图像2的残差为: -233.998, -218.316)像素单位 

depth: 12.4849

图像1像素点的深度为12.4849平移单位 

图像1的残差为: -0.0133288, -0.264876)像素单位 

图像2像素点的深度为12.7155平移单位 

图像2的残差为: -416.578, -188.157)像素单位 

depth: 8.26873

图像1像素点的深度为8.26873平移单位 

图像1的残差为: -0.000708732, 0.038394)像素单位 

图像2像素点的深度为8.7021平移单位 

图像2的残差为: -187.238, -289.626)像素单位 

depth: 9.41741

图像1像素点的深度为9.41741平移单位 

图像1的残差为: -0.00454888, -0.311985)像素单位 

图像2像素点的深度为9.93379平移单位 

图像2的残差为: -154.686, -240.707)像素单位 

depth: 9.40039

图像1像素点的深度为9.40039平移单位 

图像1的残差为: 0.00999438, 0.398571)像素单位 

图像2像素点的深度为9.69934平移单位 

图像2的残差为: -349.966, -230.54)像素单位 

depth: 9.31937

图像1像素点的深度为9.31937平移单位 

图像1的残差为: -0.000107027, -0.00345081)像素单位 

图像2像素点的深度为9.75495平移单位 

图像2的残差为: -233.168, -219.547)像素单位 

depth: 9.22272

图像1像素点的深度为9.22272平移单位 

图像1的残差为: -0.0154454, -0.881064)像素单位 

图像2像素点的深度为9.75295平移单位 

图像2的残差为: -140.383, -236.624)像素单位 

depth: 10.0239

图像1像素点的深度为10.0239平移单位 

图像1的残差为: 0.00811372, 0.407089)像素单位 

图像2像素点的深度为10.4009平移单位 

图像2的残差为: -282.057, -236.664)像素单位 

depth: 7.69828

图像1像素点的深度为7.69828平移单位 

图像1的残差为: -0.0266667, 0.48813)像素单位 

图像2像素点的深度为7.94662平移单位 

图像2的残差为: -343.108, -362.549)像素单位 

depth: 9.09606

图像1像素点的深度为9.09606平移单位 

图像1的残差为: -0.011282, -0.812761)像素单位 

图像2像素点的深度为9.60653平移单位 

图像2的残差为: -152.542, -241.995)像素单位 

depth: 8.77566

图像1像素点的深度为8.77566平移单位 

图像1的残差为: -0.0434209, -1.71614)像素单位 

图像2像素点的深度为9.07921平移单位 

图像2的残差为: -346.004, -227.303)像素单位 

depth: 9.33225

图像1像素点的深度为9.33225平移单位 

图像1的残差为: -0.0173464, -0.559388)像素单位 

图像2像素点的深度为9.76659平移单位 

图像2的残差为: -234.636, -218.562)像素单位 

depth: 10.8048

图像1像素点的深度为10.8048平移单位 

图像1的残差为: -0.0297315, -0.634992)像素单位 

图像2像素点的深度为11.294平移单位 

图像2的残差为: -220.299, -195.281)像素单位 

depth: 11.7904

图像1像素点的深度为11.7904平移单位 

图像1的残差为: 2.89686e-05, 0.000424129)像素单位 

图像2像素点的深度为12.3007平移单位 

图像2的残差为: -221.478, -188.113)像素单位 

depth: 7.37231

图像1像素点的深度为7.37231平移单位 

图像1的残差为: 0.0625925, -1.08867)像素单位 

图像2像素点的深度为7.61585平移单位 

图像2的残差为: -349.007, -365.542)像素单位 

depth: 9.43498

图像1像素点的深度为9.43498平移单位 

图像1的残差为: -0.0269321, -1.35977)像素单位 

图像2像素点的深度为9.80737平移单位 

图像2的残差为: -281.52, -236.128)像素单位 

depth: 7.26135

图像1像素点的深度为7.26135平移单位 

图像1的残差为: 0.019889, -0.359244)像素单位 

图像2像素点的深度为7.50204平移单位 

图像2的残差为: -354.309, -363.812)像素单位 

depth: 7.231

图像1像素点的深度为7.231平移单位 

图像1的残差为: -1.00576e-05, 0.0001053)像素单位 

图像2像素点的深度为7.48103平移单位 

图像2的残差为: -343.714, -363.822)像素单位 

depth: 9.42683

图像1像素点的深度为9.42683平移单位 

图像1的残差为: -0.0501229, -1.51601)像素单位 

图像2像素点的深度为9.86365平移单位 

图像2的残差为: -234.653, -215.621)像素单位 

depth: 9.14959

图像1像素点的深度为9.14959平移单位 

图像1的残差为: -0.0146069, -0.541718)像素单位 

图像2像素点的深度为9.45331平移单位 

图像2的残差为: -347.161, -226.166)像素单位 

depth: 10.1792

图像1像素点的深度为10.1792平移单位 

图像1的残差为: -0.00940457, -0.47708)像素单位 

图像2像素点的深度为10.5547平移单位 

图像2的残差为: -283.923, -236.748)像素单位 

depth: 9.09463

图像1像素点的深度为9.09463平移单位 

图像1的残差为: 0.0219701, 1.378)像素单位 

图像2像素点的深度为9.61948平移单位 

图像2的残差为: -143.166, -236.758)像素单位 

depth: 9.35573

图像1像素点的深度为9.35573平移单位 

图像1的残差为: 0.0147491, 1.30236)像素单位 

图像2像素点的深度为9.8708平移单位 

图像2的残差为: -153.801, -243.794)像素单位 

depth: 9.51033

图像1像素点的深度为9.51033平移单位 

图像1的残差为: 0.000456967, 0.0429056)像素单位 

图像2像素点的深度为9.8855平移单位 

图像2的残差为: -273.323, -250.802)像素单位 

depth: 8.4322

图像1像素点的深度为8.4322平移单位 

图像1的残差为: 0.00538225, -0.297219)像素单位 

图像2像素点的深度为8.86664平移单位 

图像2的残差为: -188.688, -289.543)像素单位 

depth: 7.89044

图像1像素点的深度为7.89044平移单位 

图像1的残差为: 0.0250301, -0.638023)像素单位 

图像2像素点的深度为8.14523平移单位 

图像2的残差为: -347.218, -335.447)像素单位 

depth: 9.86828

图像1像素点的深度为9.86828平移单位 

图像1的残差为: -0.0297473, -1.69913)像素单位 

图像2像素点的深度为10.1808平移单位 

图像2的残差为: -333.078, -240.229)像素单位 

 

执行程序所花费的时间为：44.147秒！