opencv/samples/c/calibration_artificial.cpp

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