singleImageCalibration.hpp

#include <iostream>
#include <opencv2/opencv.hpp>
#include <vector>
#include <string>

using namespace cv;
using namespace std;

void showUsage() {
  cout << "this.out [image name]" << endl;
}

void drawLine(Mat& image, Point2f pt1, Point2f pt2, Scalar s) {
  Point2f p, q;
  p.x = 0; q.x = image.cols;
  float m = (pt1.y - pt2.y) / (pt1.x - pt2.x);
  float b = pt1.y - m*pt1.x;
  p.y = m * p.x + b;
  q.y = m * q.x + b;
  line(image, p, q, s, 3, 4);
}

void showLinesforCalibration(vector<Point2f> points_on_corners, Mat& image){
  drawLine(image, points_on_corners[0], points_on_corners[4], Scalar(0,0,200));
  drawLine(image, points_on_corners[4], points_on_corners[19], Scalar(100,0,100));
  drawLine(image, points_on_corners[19], points_on_corners[15], Scalar(200,0,0));
  drawLine(image, points_on_corners[15], points_on_corners[0], Scalar(0,200,0));
}

  /* points-index
   *  0  1
   *  2  3
   */

vector< pair<Mat, Mat> > get4linesParams(vector<Point2f> points_on_corners){
  vector<Mat> pts(4);
  for(int i = 0; i < 4; i++) {
    pts[i] = (Mat_<float>(3, 1) << points_on_corners[i].x, points_on_corners[i].y, 1.0);
  }

  Mat line0to1 = pts[0].cross(pts[1]);
  Mat line2to3 = pts[2].cross(pts[3]);
  Mat line0to2 = pts[0].cross(pts[2]);
  Mat line1to3 = pts[1].cross(pts[3]);
  pair<Mat, Mat> pair1(line0to1, line2to3);
  pair<Mat, Mat> pair2(line0to2, line1to3);
  vector< pair<Mat, Mat> > pairs{pair1, pair2};

  return  pairs;
}

vector<Mat> getIntersectionPoints(const vector< pair<Mat, Mat> > pairs) {
  vector<Mat> intersectionPts(2);
  for(int i = 0; i < 2; i++) {
    intersectionPts[i] = (pairs[i].first).cross(pairs[i].second);
  }
  
  return intersectionPts;
}

Mat singleImageCalibration(Mat image, vector<Point2f> points_on_lines) {
  float W = image.rows;
  float H = image.cols;
  vector< pair<Mat, Mat> > twoPairsofTwoLines =  get4linesParams(points_on_lines);
  vector<Mat> intersectionPts = getIntersectionPoints(twoPairsofTwoLines);
  vector<float> p{intersectionPts[0].at<float>(0),
                  intersectionPts[0].at<float>(1),
                  intersectionPts[0].at<float>(2)};
  vector<float> q{intersectionPts[1].at<float>(0),
                  intersectionPts[1].at<float>(1),
                  intersectionPts[1].at<float>(2)};

  float f = -( (2*p[0] - p[2]*W)*(2*q[0] - q[2]*W) + 
               (2*p[1] - p[2]*H)*(2*q[1] - q[2]*H) ) / (2 * p[2] * q[2]);

  Mat K = (Mat_<float>(3, 3) <<  f, 0.0,  W/2.0,
                                0.0,  f,  H/2.0,
                                0.0, 0.0,   1.0);

  return K;
}