samples/cpp/calibration_artificial.cpp

   1 #include "opencv2/calib3d/calib3d.hpp"
   2 #include "opencv2/imgproc/imgproc.hpp"
   3 #include "opencv2/highgui/highgui.hpp"
   4
   5 #include <iostream>
   6 #include <vector>
   7 #include <algorithm>
   8 #include <iterator>
   9 #include <stdio.h>
  10
  11 using namespace cv;
  12 using namespace std;
  13
  14 static void help()
  15 {
  16     printf( "\nThis code generates an artificial camera and artificial chessboard images,\n"
  17             "and then calibrates. It is basically test code for calibration that shows\n"
  18             "how to package calibration points and then calibrate the camera.\n"
  19             "Usage:\n"
  20             "./calibration_artificial\n\n");
  21 }
  22 namespace cv
  23 {
  24
  25 /* copy of class defines int tests/cv/chessboardgenerator.h */
  26 class ChessBoardGenerator
  27 {
  28 public:
  29     double sensorWidth;
  30     double sensorHeight;
  31     size_t squareEdgePointsNum;
  32     double min_cos;
  33     mutable double cov;
  34     Size patternSize;
  35     int rendererResolutionMultiplier;
  36
  37     ChessBoardGenerator(const Size& patternSize = Size(8, 6));
  38     Mat operator()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, vector<Point2f>& corners) const;
  39     Size cornersSize() const;
  40 private:
  41     void generateEdge(const Point3f& p1, const Point3f& p2, vector<Point3f>& out) const;
  42     Mat generageChessBoard(const Mat& bg, const Mat& camMat, const Mat& distCoeffs,
  43         const Point3f& zero, const Point3f& pb1, const Point3f& pb2,
  44         float sqWidth, float sqHeight, const vector<Point3f>& whole, vector<Point2f>& corners) const;
  45     void generateBasis(Point3f& pb1, Point3f& pb2) const;
  46     Point3f generateChessBoardCenter(const Mat& camMat, const Size& imgSize) const;
  47     Mat rvec, tvec;
  48 };
  49 }
  50
  51
  52
  53 const Size imgSize(800, 600);
  54 const Size brdSize(8, 7);
  55 const size_t brds_num = 20;
  56
  57 template<class T> ostream& operator<<(ostream& out, const Mat_<T>& mat)
  58 {
  59     for(int j = 0; j < mat.rows; ++j)
  60         for(int i = 0; i < mat.cols; ++i)
  61             out << mat(j, i) << " ";
  62     return out;
  63 }
  64
  65
  66
  67 int main()
  68 {
  69     help();
  70     cout << "Initializing background...";
  71     Mat background(imgSize, CV_8UC3);
  72     randu(background, Scalar::all(32), Scalar::all(255));
  73     GaussianBlur(background, background, Size(5, 5), 2);
  74     cout << "Done" << endl;
  75
  76     cout << "Initializing chess board generator...";
  77     ChessBoardGenerator cbg(brdSize);
  78     cbg.rendererResolutionMultiplier = 4;
  79     cout << "Done" << endl;
  80
  81     /* camera params */
  82     Mat_<double> camMat(3, 3);
  83     camMat << 300., 0., background.cols/2., 0, 300., background.rows/2., 0., 0., 1.;
  84
  85     Mat_<double> distCoeffs(1, 5);
  86     distCoeffs << 1.2, 0.2, 0., 0., 0.;
  87
  88     cout << "Generating chessboards...";
  89     vector<Mat> boards(brds_num);
  90     vector<Point2f> tmp;
  91     for(size_t i = 0; i < brds_num; ++i)
  92         cout << (boards[i] = cbg(background, camMat, distCoeffs, tmp), i) << " ";
  93     cout << "Done" << endl;
  94
  95     vector<Point3f> chessboard3D;
  96     for(int j = 0; j < cbg.cornersSize().height; ++j)
  97         for(int i = 0; i < cbg.cornersSize().width; ++i)
  98             chessboard3D.push_back(Point3i(i, j, 0));
  99
 100     /* init points */
 101     vector< vector<Point3f> > objectPoints;
 102     vector< vector<Point2f> > imagePoints;
 103
 104     cout << endl << "Finding chessboards' corners...";
 105     for(size_t i = 0; i < brds_num; ++i)
 106     {
 107         cout << i;
 108         namedWindow("Current chessboard"); imshow("Current chessboard", boards[i]); waitKey(100);
 109         bool found = findChessboardCorners(boards[i], cbg.cornersSize(), tmp);
 110         if (found)
 111         {
 112             imagePoints.push_back(tmp);
 113             objectPoints.push_back(chessboard3D);
 114             cout<< "-found ";
 115         }
 116         else
 117             cout<< "-not-found ";
 118
 119         drawChessboardCorners(boards[i], cbg.cornersSize(), Mat(tmp), found);
 120         imshow("Current chessboard", boards[i]); waitKey(1000);
 121     }
 122     cout << "Done" << endl;
 123     destroyAllWindows();
 124
 125     Mat camMat_est;
 126     Mat distCoeffs_est;
 127     vector<Mat> rvecs, tvecs;
 128
 129     cout << "Calibrating...";
 130     double rep_err = calibrateCamera(objectPoints, imagePoints, imgSize, camMat_est, distCoeffs_est, rvecs, tvecs);
 131     cout << "Done" << endl;
 132
 133     cout << endl << "Average Reprojection error: " << rep_err/brds_num/cbg.cornersSize().area() << endl;
 134     cout << "==================================" << endl;
 135     cout << "Original camera matrix:\n" << camMat << endl;
 136     cout << "Original distCoeffs:\n" << distCoeffs << endl;
 137     cout << "==================================" << endl;
 138     cout << "Estimated camera matrix:\n" << (Mat_<double>&)camMat_est << endl;
 139     cout << "Estimated distCoeffs:\n" << (Mat_<double>&)distCoeffs_est << endl;
 140
 141     return 0;
 142 }
 143
 144
 145 /////////////////////////////////////////////////////////////////////////////////////////////////
 146 /////////////////////////////////////////////////////////////////////////////////////////////////
 147 /////////////////////////////////////////////////////////////////////////////////////////////////
 148
 149 // Copy of  tests/cv/src/chessboardgenerator code. Just do not want to add dependency.
 150
 151
 152 ChessBoardGenerator::ChessBoardGenerator(const Size& _patternSize) : sensorWidth(32), sensorHeight(24),
 153     squareEdgePointsNum(200), min_cos(std::sqrt(2.f)*0.5f), cov(0.5),
 154     patternSize(_patternSize), rendererResolutionMultiplier(4), tvec(Mat::zeros(1, 3, CV_32F))
 155 {
 156     Rodrigues(Mat::eye(3, 3, CV_32F), rvec);
 157 }
 158
 159 void cv::ChessBoardGenerator::generateEdge(const Point3f& p1, const Point3f& p2, vector<Point3f>& out) const
 160 {
 161     Point3f step = (p2 - p1) * (1.f/squareEdgePointsNum);
 162     for(size_t n = 0; n < squareEdgePointsNum; ++n)
 163         out.push_back( p1 + step * (float)n);
 164 }
 165
 166 Size cv::ChessBoardGenerator::cornersSize() const
 167 {
 168     return Size(patternSize.width-1, patternSize.height-1);
 169 }
 170
 171 struct Mult
 172 {
 173     float m;
 174     Mult(int mult) : m((float)mult) {}
 175     Point2f operator()(const Point2f& p)const { return p * m; }
 176 };
 177
 178 void cv::ChessBoardGenerator::generateBasis(Point3f& pb1, Point3f& pb2) const
 179 {
 180     RNG& rng = theRNG();
 181
 182     Vec3f n;
 183     for(;;)
 184     {
 185         n[0] = rng.uniform(-1.f, 1.f);
 186         n[1] = rng.uniform(-1.f, 1.f);
 187         n[2] = rng.uniform(-1.f, 1.f);
 188         float len = (float)norm(n);
 189         n[0]/=len;
 190         n[1]/=len;
 191         n[2]/=len;
 192
 193         if (fabs(n[2]) > min_cos)
 194             break;
 195     }
 196
 197     Vec3f n_temp = n; n_temp[0] += 100;
 198     Vec3f b1 = n.cross(n_temp);
 199     Vec3f b2 = n.cross(b1);
 200     float len_b1 = (float)norm(b1);
 201     float len_b2 = (float)norm(b2);
 202
 203     pb1 = Point3f(b1[0]/len_b1, b1[1]/len_b1, b1[2]/len_b1);
 204     pb2 = Point3f(b2[0]/len_b1, b2[1]/len_b2, b2[2]/len_b2);
 205 }
 206
 207 Mat cv::ChessBoardGenerator::generageChessBoard(const Mat& bg, const Mat& camMat, const Mat& distCoeffs,
 208                                                 const Point3f& zero, const Point3f& pb1, const Point3f& pb2,
 209                                                 float sqWidth, float sqHeight, const vector<Point3f>& whole,
 210                                                 vector<Point2f>& corners) const
 211 {
 212     vector< vector<Point> > squares_black;
 213     for(int i = 0; i < patternSize.width; ++i)
 214         for(int j = 0; j < patternSize.height; ++j)
 215             if ( (i % 2 == 0 && j % 2 == 0) || (i % 2 != 0 && j % 2 != 0) )
 216             {
 217                 vector<Point3f> pts_square3d;
 218                 vector<Point2f> pts_square2d;
 219
 220                 Point3f p1 = zero + (i + 0) * sqWidth * pb1 + (j + 0) * sqHeight * pb2;
 221                 Point3f p2 = zero + (i + 1) * sqWidth * pb1 + (j + 0) * sqHeight * pb2;
 222                 Point3f p3 = zero + (i + 1) * sqWidth * pb1 + (j + 1) * sqHeight * pb2;
 223                 Point3f p4 = zero + (i + 0) * sqWidth * pb1 + (j + 1) * sqHeight * pb2;
 224                 generateEdge(p1, p2, pts_square3d);
 225                 generateEdge(p2, p3, pts_square3d);
 226                 generateEdge(p3, p4, pts_square3d);
 227                 generateEdge(p4, p1, pts_square3d);
 228
 229                 projectPoints( Mat(pts_square3d), rvec, tvec, camMat, distCoeffs, pts_square2d);
 230                 squares_black.resize(squares_black.size() + 1);
 231                 vector<Point2f> temp;
 232                 approxPolyDP(Mat(pts_square2d), temp, 1.0, true);
 233                 transform(temp.begin(), temp.end(), back_inserter(squares_black.back()), Mult(rendererResolutionMultiplier));
 234             }
 235
 236     /* calculate corners */
 237     vector<Point3f> corners3d;
 238     for(int j = 0; j < patternSize.height - 1; ++j)
 239         for(int i = 0; i < patternSize.width - 1; ++i)
 240             corners3d.push_back(zero + (i + 1) * sqWidth * pb1 + (j + 1) * sqHeight * pb2);
 241     corners.clear();
 242     projectPoints( Mat(corners3d), rvec, tvec, camMat, distCoeffs, corners);
 243
 244     vector<Point3f> whole3d;
 245     vector<Point2f> whole2d;
 246     generateEdge(whole[0], whole[1], whole3d);
 247     generateEdge(whole[1], whole[2], whole3d);
 248     generateEdge(whole[2], whole[3], whole3d);
 249     generateEdge(whole[3], whole[0], whole3d);
 250     projectPoints( Mat(whole3d), rvec, tvec, camMat, distCoeffs, whole2d);
 251     vector<Point2f> temp_whole2d;
 252     approxPolyDP(Mat(whole2d), temp_whole2d, 1.0, true);
 253
 254     vector< vector<Point > > whole_contour(1);
 255     transform(temp_whole2d.begin(), temp_whole2d.end(),
 256         back_inserter(whole_contour.front()), Mult(rendererResolutionMultiplier));
 257
 258     Mat result;
 259     if (rendererResolutionMultiplier == 1)
 260     {
 261         result = bg.clone();
 262         drawContours(result, whole_contour, -1, Scalar::all(255), FILLED, LINE_AA);
 263         drawContours(result, squares_black, -1, Scalar::all(0), FILLED, LINE_AA);
 264     }
 265     else
 266     {
 267         Mat tmp;
 268         resize(bg, tmp, bg.size() * rendererResolutionMultiplier);
 269         drawContours(tmp, whole_contour, -1, Scalar::all(255), FILLED, LINE_AA);
 270         drawContours(tmp, squares_black, -1, Scalar::all(0), FILLED, LINE_AA);
 271         resize(tmp, result, bg.size(), 0, 0, INTER_AREA);
 272     }
 273     return result;
 274 }
 275
 276 Mat cv::ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, vector<Point2f>& corners) const
 277 {
 278     cov = std::min(cov, 0.8);
 279     double fovx, fovy, focalLen;
 280     Point2d principalPoint;
 281     double aspect;
 282     calibrationMatrixValues( camMat, bg.size(), sensorWidth, sensorHeight,
 283         fovx, fovy, focalLen, principalPoint, aspect);
 284
 285     RNG& rng = theRNG();
 286
 287     float d1 = static_cast<float>(rng.uniform(0.1, 10.0));
 288     float ah = static_cast<float>(rng.uniform(-fovx/2 * cov, fovx/2 * cov) * CV_PI / 180);
 289     float av = static_cast<float>(rng.uniform(-fovy/2 * cov, fovy/2 * cov) * CV_PI / 180);
 290
 291     Point3f p;
 292     p.z = cos(ah) * d1;
 293     p.x = sin(ah) * d1;
 294     p.y = p.z * tan(av);
 295
 296     Point3f pb1, pb2;
 297     generateBasis(pb1, pb2);
 298
 299     float cbHalfWidth = static_cast<float>(norm(p) * sin( std::min(fovx, fovy) * 0.5 * CV_PI / 180));
 300     float cbHalfHeight = cbHalfWidth * patternSize.height / patternSize.width;
 301
 302     vector<Point3f> pts3d(4);
 303     vector<Point2f> pts2d(4);
 304     for(;;)
 305     {
 306         pts3d[0] = p + pb1 * cbHalfWidth + cbHalfHeight * pb2;
 307         pts3d[1] = p + pb1 * cbHalfWidth - cbHalfHeight * pb2;
 308         pts3d[2] = p - pb1 * cbHalfWidth - cbHalfHeight * pb2;
 309         pts3d[3] = p - pb1 * cbHalfWidth + cbHalfHeight * pb2;
 310
 311         /* can remake with better perf */
 312         projectPoints( Mat(pts3d), rvec, tvec, camMat, distCoeffs, pts2d);
 313
 314         bool inrect1 = pts2d[0].x < bg.cols && pts2d[0].y < bg.rows && pts2d[0].x > 0 && pts2d[0].y > 0;
 315         bool inrect2 = pts2d[1].x < bg.cols && pts2d[1].y < bg.rows && pts2d[1].x > 0 && pts2d[1].y > 0;
 316         bool inrect3 = pts2d[2].x < bg.cols && pts2d[2].y < bg.rows && pts2d[2].x > 0 && pts2d[2].y > 0;
 317         bool inrect4 = pts2d[3].x < bg.cols && pts2d[3].y < bg.rows && pts2d[3].x > 0 && pts2d[3].y > 0;
 318
 319         if ( inrect1 && inrect2 && inrect3 && inrect4)
 320             break;
 321
 322         cbHalfWidth*=0.8f;
 323         cbHalfHeight = cbHalfWidth * patternSize.height / patternSize.width;
 324     }
 325
 326     cbHalfWidth  *= static_cast<float>(patternSize.width)/(patternSize.width + 1);
 327     cbHalfHeight *= static_cast<float>(patternSize.height)/(patternSize.height + 1);
 328
 329     Point3f zero = p - pb1 * cbHalfWidth - cbHalfHeight * pb2;
 330     float sqWidth  = 2 * cbHalfWidth/patternSize.width;
 331     float sqHeight = 2 * cbHalfHeight/patternSize.height;
 332
 333     return generageChessBoard(bg, camMat, distCoeffs, zero, pb1, pb2, sqWidth, sqHeight,  pts3d, corners);
 334 }