双目三维定位

双目三维定位

一、基本原理

1. 几何关系与数学模型

假设有一个点p，沿着垂直于相机中心连线方向上下移动，则其在左右相机上的成像点的位置会不断变化，即d=x1-x2的大小不断变化，并且点p和相机之间的距离Z跟视差d存在着反比关系。上式中视差d可以通过两个相机中心距T减去p点分别在左右图像上的投影点偏离中心点的值获得，所以只要获取到了两个相机的中心距T,就可以评估出p点距离相机的距离，这个中心距T也是双目标定中需要确立的参数之一。

2. 双目定位的基本流程：

• 需要分别获取左右相机的内外参数（标定）
• 通过立体标定对左右两幅图像进行立体校准和对齐
• 确定两个相机的相对位置关系，即中心距
• 计算坐标

二、制作相机（最好使用买的高精度双目）

三、相机标定与图像校正对齐

1. 基本原理

• 两个相机成像上定位到同一个点p上，就是要把左右两个图片的点匹配起来，这就涉及到双目校正的动作。如果通过一幅图片上一个点的特征在另一个二维图像空间上匹配对应点，这个过程会非常耗时。为了减少匹配搜索的运算量，我们可以利用极限约束使得对应点的匹配由二维搜索空间降到一维搜索空间。

• 这时候要用双目校正把消除畸变后的两幅图像在水平方向严格的对齐，使得两幅图像的对极线恰好在同一水平线上，这样一幅图像上任意一点与其在另一幅图像上的匹配点就必然具有相同的行号，只需要在该行进行一维搜索就可匹配到对应点。

2. 基于OpenCV的双目标定

标定板：

使用到的2个重要函数：

cv::findChessboardCorners，cv::stereoCalibrate

• cv::findChessboardCorners()棋盘格角点检测

  函数定义如下：

  	bool cv::findChessboardCorners( // 如果找到角点则返回true
  		cv::InputArray image, // 输入的棋盘格图像（8UC1或8UC3）
  		cv::Size patternSize, // 棋盘格内部角点的行、列数
  		cv::OutputArray corners, // 输出的棋盘格角点
  		int flags = cv::CALIB_CB_ADAPTIVE_THRESH 
  		| cv::CALIB_CB_NORMALIZE_IMAGE
  	);
  

• cv::stereoCalibrate获取相机参数

  函数定义如下：

  double stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1,
  　　　　　　　　　　　　　InputArrayOfArrays imagePoints2, InputOutputArray cameraMatrix1,InputOutputArray distCoeffs1, 
  　　　　　　　　　　　　　InputOutputArray cameraMatrix2, InputOutputArray distCoeffs2, Size imageSize, OutputArray R,
  　　　　　　　　　　　　　OutputArray T, OutputArray E, OutputArray F, int
  　　　　　　　　　　　　　flags=CALIB_FIX_INTRINSIC , TermCriteria criteria=
  　　　　　　　　　　　　　TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 1e-6))
  

  objectPoints- vector<point3f> 型的数据结构，存储标定角点在世界坐标系中的位置
   
  imagePoints1- vector<vector<point2f>> 型的数据结构，存储标定角点在第一个摄像机下的投影后的亚像素坐标
   
  imagePoints2- vector<vector<point2f>> 型的数据结构，存储标定角点在第二个摄像机下的投影后的亚像素坐标
   
  cameraMatrix1-输入/输出型的第一个摄像机的相机矩阵。如果CV_CALIB_USE_INTRINSIC_GUESS , CV_CALIB_FIX_ASPECT_RATIO ,CV_CALIB_FIX_INTRINSIC , or CV_CALIB_FIX_FOCAL_LENGTH其中的一个或多个标志被设置，该摄像机矩阵的一些或全部参数需要被初始化
   
  distCoeffs1-第一个摄像机的输入/输出型畸变向量。根据矫正模型的不同，输出向量长度由标志决定
   
  cameraMatrix2-输入/输出型的第二个摄像机的相机矩阵。参数意义同第一个相机矩阵相似
   
  distCoeffs2-第一个摄像机的输入/输出型畸变向量。根据矫正模型的不同，输出向量长度由标志决定
   
  imageSize-图像的大小
   
  R-输出型，第一和第二个摄像机之间的旋转矩阵
   
  T-输出型，第一和第二个摄像机之间的平移矩阵
   
  E-输出型，基本矩阵
   
  F-输出型，基础矩阵
   
  flag-
  CV_CALIB_FIX_INTRINSIC 如果该标志被设置，那么就会固定输入的cameraMatrix和distCoeffs不变，只求解R,T,E,F
  CV_CALIB_USE_INTRINSIC_GUESS 根据用户提供的cameraMatrix和distCoeffs为初始值开始迭代
  CV_CALIB_FIX_PRINCIPAL_POINT 迭代过程中不会改变主点的位置
  CV_CALIB_FIX_FOCAL_LENGTH 迭代过程中不会改变焦距
  CV_CALIB_SAME_FOCAL_LENGTH 强制保持两个摄像机的焦距相同
  CV_CALIB_ZERO_TANGENT_DIST 切向畸变保持为零
  CV_CALIB_FIX_K1,...,CV_CALIB_FIX_K6 迭代过程中不改变相应的值。如果设置了CV_CALIB_USE_INTRINSIC_GUESS 将会使用用户提供的初始值，否则设置为零
  CV_CALIB_RATIONAL_MODEL 畸变模型的选择，如果设置了该参数，将会使用更精确的畸变模型，distCoeffs的长度就会变成8
   
  term_crit-迭代优化的终止条件
  

• 最终的标定程序（由书《学习opencv3》的配套代码改制）

  #include 
  #include 
   
  using namespace std;
  using namespace cv;
   
  
  const int maxScale = 1;
  const float squareSize = 1.f;
  int nx = 9, ny =6;
  int N = nx * ny;
  cv::Size board_sz = cv::Size(nx, ny);
  vector<string> imageNames[2];
  vector<cv::Point3f> boardModel;
  vector<vector<cv::Point3f> > objectPoints;
  vector<vector<cv::Point2f> > points[2];
  vector<cv::Point2f> corners[2];
  bool found[2] = {false, false};
  cv::Size imageSize;
  
  
  int main()
  {
   
      for (int i = 0; i < ny; i++)
          for (int j = 0; j < nx; j++)
              boardModel.push_back(cv::Point3f((float)(i * squareSize), (float)(j * squareSize), 0.f));
      
      
      cv::VideoCapture capl(0);
      cv::VideoCapture capr(1);
   
      int i = 0;
   
      cv::Mat src_imgl_i;
      cv::Mat src_imgr_i;
   
      char filename_l[15];
      char filename_r[15];
      int num = 0;
      while(capl.read(src_imgl_i) && capr.read(src_imgr_i))
      {
          cv::Mat src_imgl, src_imgr;
          cv::cvtColor(src_imgl_i, src_imgl, cv::COLOR_BGR2GRAY);
          cv::cvtColor(src_imgr_i, src_imgr, cv::COLOR_BGR2GRAY);
          found[0] = found[1] = false;
          imageSize = src_imgl.size();
          for (int s = 1; s <= maxScale; s++) {
              cv::Mat timl = src_imgl;
              cv::Mat timr = src_imgr;
              resize(src_imgl, timl, cv::Size(), s, s, cv::INTER_CUBIC);
              resize(src_imgr, timr, cv::Size(), s, s, cv::INTER_CUBIC);
              found[0] = cv::findChessboardCorners(timl, board_sz, corners[0]);
              found[1] = cv::findChessboardCorners(timr, board_sz, corners[1]);
  
              if (found[0] && found[1])
                  break;
          }
          cv::Mat cimgr, cimgl;
          cv::cvtColor(src_imgl, cimgl, cv::COLOR_GRAY2BGR);
          cv::cvtColor(src_imgr, cimgr, cv::COLOR_GRAY2BGR);
          // draw chessboard corners works for circle grids too
          cv::drawChessboardCorners(cimgl, cv::Size(nx, ny), corners[0], found[0]);
          cv::drawChessboardCorners(cimgr, cv::Size(nx, ny), corners[1], found[1]);
          cv::imshow("Corners_left", cimgl);
          cv::imshow("Corners_right", cimgr);
          char c = cv::waitKey(1);
          if (c == ' ')
          {
              if (found[0] && found[1]) {
                  objectPoints.push_back(boardModel);
                  points[0].push_back(corners[0]);
                  points[1].push_back(corners[1]);
                  num++;
              }
              cout<<"num: "<<num<<endl;
          }
  
          if (c == 'q' || c =='Q')
          {
              cv::destroyAllWindows();
              break;
          }
      }
       //#####开始标定#######
      // CALIBRATE THE STEREO CAMERAS
      cv::Mat M1 = cv::Mat::eye(3, 3, CV_64F);
      cv::Mat M2 = cv::Mat::eye(3, 3, CV_64F);
      cv::Mat D1, D2, R, T, E, F;
  
      cout << "\nRunning stereo calibration ...\n";
      cv::stereoCalibrate(
          objectPoints, points[0], points[1], M1, D1, M2, D2, imageSize, R, T, E, F,
          cv::CALIB_FIX_ASPECT_RATIO | cv::CALIB_ZERO_TANGENT_DIST |
              cv::CALIB_SAME_FOCAL_LENGTH,
          cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 100,
                          1e-5));
      
      cout <<"image size: \n"<< imageSize<<endl;
      cout<< "Intrinsics M1 for cam1: \n"<<M1 << "\n  Distortion coeffs D1 for cam1: \n"<<D1<<endl;
      cout<< "Intrinsics M2 for cam2: \n"<<M2 << "\n  Distortion coeffs D2 for cam2: \n"<<D2<<endl;
      cout<<"Rotation R: \n" << R<<endl;
      cout<<"对齐Transform T: \n"<<T<<endl;
      //######标定结束##########
      cout << "Done! Press any key to step through images, ESC to exit\n\n";
   
      return 0;
  }
  

3. 基于OpenCV的图像校正对齐

使用到的几个重要函数：

cv::stereoRectify，cv::initUndistortRectifyMap，cv::remap

• cv::stereoRectify图像矫正

  函数定义如下：

  CV_EXPORTS_W void stereoRectify(
      InputArray cameraMatrix1, 第一个相机内参矩阵
      InputArray distCoeffs1,第一个相机畸变参数
      InputArray cameraMatrix2, 第二个相机内参矩阵
      InputArray distCoeffs2, 第二个相机畸变参数
      Size imageSize, 用于相机标定的图像尺寸
      InputArray R, 从第一个相机到第二个相机坐标系系统的旋转矩阵
      InputArray T,从第一个相机到第二个相机的坐标系的平移矩阵
      OutputArray R1, 第一个相机输出3x3的修正矩阵（旋转矩阵）也就是从未矫正的第一个摄像机的坐标系到矫正的第一个摄像机的坐标系
      OutputArray R2, 第二个相机输出3x3的校正变换(旋转矩阵) 也就是从未矫正的第二个摄像机的坐标系到矫正的第二个摄像机的坐标系
      OutputArray P1, 为第一个摄像机输出新的(经校正的)坐标系统中的3x4投影矩阵，即它将经校正的第一个摄像机坐标系统中给定的点投影到被校正的第一个摄像机的图像中
      OutputArray P2,为第二个摄像机输出新的(经校正的)坐标系统中的3x4投影矩阵，即将经校正的第一个摄像机坐标系统中给定的点投影到经校正的第二个摄像机的图像中。
      OutputArray Q, int flags = CALIB_ZERO_DISPARITY,
      double alpha = -1, Size newImageSize =Size(),
      CV_OUT Rect* validPixROI1 = 0, CV_OUT Rect* validPixROI2 = 0 );
  

• cv::initUndistortRectifyMap计算无畸变和修正转换映射

  函数定义如下：

  CV_EXPORTS_W void initUndistortRectifyMap( 
      InputArray cameraMatrix, 相机内参矩阵
      InputArray distCoeffs,相机畸变参数
      InputArray R,修正变换矩阵，是个3*3的矩阵
      InputArray newCameraMatrix, 新的相机矩阵
      Size size, 未畸变的图像尺寸
      int m1type, 第一个输出的映射的类型，可以为 CV_32FC1, CV_32FC2或CV_16SC2
      OutputArray map1, 第一个输出映射
      OutputArray map2 第二个输出映射
  );
  

• cv::remap重映射

  函数定义如下

  void remap(InputArray src, //输入图像
             OutputArraydst, //输出图像
             InputArray map1,//第一个映射
             InputArray map2, //第二个映射
             int interpolation, //插值
             intborderMode=BORDER_CONSTANT, 
             const Scalar& borderValue=Scalar()
             )
  

• 最终图片校正与对齐的代码由书《学习opencv3》的配套代码改制）

  #include 
  #include 
   
  using namespace std;
  using namespace cv;
   
  
  cv::Mat M1 = (cv::Mat_<double>(3,3)<< 
  804.1476522179109, 0, 281.7145109619536,
   0, 804.1476522179109, 234.5954155516309,
   0, 0, 1
   );
  
   cv::Mat D1 = (cv::Mat_<double>(1,5)<<
  -0.1203642252210332, 0.1539717942660204, 0, 0, 0.08485466535340704
  );
  
  cv::Mat M2 = (cv::Mat_<double>(3,3)<<
  804.1476522179109, 0, 278.2822439212751,
   0, 804.1476522179109, 218.5407101920005,
   0, 0, 1
  );
  
  
  cv::Mat D2 = (cv::Mat_<double>(5,1)<<
  -0.1356206020706684, 1.065324592485442, 0, 0, -0.8657253074569891
  );
  
  cv::Mat R = (cv::Mat_<double>(3,3)<< 
  0.9995597611592925, 0.02846513509768539, -0.008367792722625002,
   -0.0286520883431294, 0.9993215077292794, -0.02314264512231314,
   0.00770335672001529, 0.02337221156737922, 0.9996971531527404
  );
  
  cv::Mat T = (cv::Mat_<double>(3,1)<< 
  -5.577096813538549,
   -0.2343373116497023,
   0.5063135791015613
  );
  cv::Size imageSize;
  int main(int argc, char **argv)
  {
      cout<<"T: \n"<<T<<endl;
      cv::VideoCapture capl(0);
      cv::VideoCapture capr(1);
      cv::Mat img, imgt;
      capl.read(img);
      cv::cvtColor(img, imgt, cv::COLOR_BGR2GRAY);
      cout<<"11"<<endl;
      imageSize = imgt.size();
      cv::Mat R1, R2, P1, P2, map11, map12, map21, map22;
      //计算所需的矫正、投影和视差图
      stereoRectify(M1, D1, M2, D2, imageSize, R, T, 
                  R1, R2, P1, P2,
                  cv::noArray(), 0);
      // Precompute maps for cvRemap() 把图像进行即便矫正和裁剪
      initUndistortRectifyMap(M1, D1, R1, P1, imageSize, CV_16SC2, map11,
                              map12);
      initUndistortRectifyMap(M2, D2, R2, P2, imageSize, CV_16SC2, map21,
                              map22);
      cv::Mat src_imgl_i, src_imgr_i;
      while(capl.read(src_imgl_i) && capr.read(src_imgr_i))
      {
          cout<<"22"<<endl;
          cv::Mat src_imgl, src_imgr;
          cv::cvtColor(src_imgl_i, src_imgl, cv::COLOR_BGR2GRAY);
          cv::cvtColor(src_imgr_i, src_imgr, cv::COLOR_BGR2GRAY);
          cv::Mat img1r, img2r;
          cv::remap(src_imgl, img1r, map11, map12, cv::INTER_LINEAR);
          cv::remap(src_imgr, img2r, map21, map22, cv::INTER_LINEAR);
          cout<<"33"<<endl;
          cv::imshow("img left", img1r);
          cv::imshow("img right", img2r);
          cv::waitKey(1);
      }
      return 0;
  }
  

4. 双目测距

使用两张矫正好的左眼与右眼的图片，在其上选择同一个目标物并记录下他们的x轴的坐标。最后使用几何关系求解距离。

		cv::Point2d pointL, pointR;
        pointL.x = 363;
        pointR.x = 263;
        cout<<((pointL.x)- (pointR.x ))<<endl;
        double Z = (M1.at<double>(0,0)*120) / ((pointL.x)- (pointR.x ));
        cout<<"Z: "<<Z<<endl;