1 /*M/////////////////////////////////////////////////////////////////////////////////////// 2 // 3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 // 5 // By downloading, copying, installing or using the software you agree to this license. 6 // If you do not agree to this license, do not download, install, 7 // copy or use the software. 8 // 9 // 10 // License Agreement 11 // For Open Source Computer Vision Library 12 // 13 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved. 14 // Copyright (C) 2009, Willow Garage Inc., all rights reserved. 15 // Third party copyrights are property of their respective owners. 16 // 17 // Redistribution and use in source and binary forms, with or without modification, 18 // are permitted provided that the following conditions are met: 19 // 20 // * Redistribution's of source code must retain the above copyright notice, 21 // this list of conditions and the following disclaimer. 22 // 23 // * Redistribution's in binary form must reproduce the above copyright notice, 24 // this list of conditions and the following disclaimer in the documentation 25 // and/or other materials provided with the distribution. 26 // 27 // * The name of the copyright holders may not be used to endorse or promote products 28 // derived from this software without specific prior written permission. 29 // 30 // This software is provided by the copyright holders and contributors "as is" and 31 // any express or implied warranties, including, but not limited to, the implied 32 // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 // In no event shall the Intel Corporation or contributors be liable for any direct, 34 // indirect, incidental, special, exemplary, or consequential damages 35 // (including, but not limited to, procurement of substitute goods or services; 36 // loss of use, data, or profits; or business interruption) however caused 37 // and on any theory of liability, whether in contract, strict liability, 38 // or tort (including negligence or otherwise) arising in any way out of 39 // the use of this software, even if advised of the possibility of such damage. 40 // 41 //M*/ 42 43 #include "test_precomp.hpp" 44 #include "test_chessboardgenerator.hpp" 45 46 #include <vector> 47 #include <iterator> 48 #include <algorithm> 49 50 using namespace cv; 51 using namespace std; 52 53 ChessBoardGenerator::ChessBoardGenerator(const Size& _patternSize) : sensorWidth(32), sensorHeight(24), 54 squareEdgePointsNum(200), min_cos(std::sqrt(2.f)*0.5f), cov(0.5), 55 patternSize(_patternSize), rendererResolutionMultiplier(4), tvec(Mat::zeros(1, 3, CV_32F)) 56 { 57 Rodrigues(Mat::eye(3, 3, CV_32F), rvec); 58 } 59 60 void cv::ChessBoardGenerator::generateEdge(const Point3f& p1, const Point3f& p2, vector<Point3f>& out) const 61 { 62 Point3f step = (p2 - p1) * (1.f/squareEdgePointsNum); 63 for(size_t n = 0; n < squareEdgePointsNum; ++n) 64 out.push_back( p1 + step * (float)n); 65 } 66 67 Size cv::ChessBoardGenerator::cornersSize() const 68 { 69 return Size(patternSize.width-1, patternSize.height-1); 70 } 71 72 struct Mult 73 { 74 float m; 75 Mult(int mult) : m((float)mult) {} 76 Point2f operator()(const Point2f& p)const { return p * m; } 77 }; 78 79 void cv::ChessBoardGenerator::generateBasis(Point3f& pb1, Point3f& pb2) const 80 { 81 RNG& rng = theRNG(); 82 83 Vec3f n; 84 for(;;) 85 { 86 n[0] = rng.uniform(-1.f, 1.f); 87 n[1] = rng.uniform(-1.f, 1.f); 88 n[2] = rng.uniform(-1.f, 1.f); 89 float len = (float)norm(n); 90 n[0]/=len; 91 n[1]/=len; 92 n[2]/=len; 93 94 if (n[2] > min_cos) 95 break; 96 } 97 98 Vec3f n_temp = n; n_temp[0] += 100; 99 Vec3f b1 = n.cross(n_temp); 100 Vec3f b2 = n.cross(b1); 101 float len_b1 = (float)norm(b1); 102 float len_b2 = (float)norm(b2); 103 104 pb1 = Point3f(b1[0]/len_b1, b1[1]/len_b1, b1[2]/len_b1); 105 pb2 = Point3f(b2[0]/len_b1, b2[1]/len_b2, b2[2]/len_b2); 106 } 107 108 109 Mat cv::ChessBoardGenerator::generateChessBoard(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, 110 const Point3f& zero, const Point3f& pb1, const Point3f& pb2, 111 float sqWidth, float sqHeight, const vector<Point3f>& whole, 112 vector<Point2f>& corners) const 113 { 114 vector< vector<Point> > squares_black; 115 for(int i = 0; i < patternSize.width; ++i) 116 for(int j = 0; j < patternSize.height; ++j) 117 if ( (i % 2 == 0 && j % 2 == 0) || (i % 2 != 0 && j % 2 != 0) ) 118 { 119 vector<Point3f> pts_square3d; 120 vector<Point2f> pts_square2d; 121 122 Point3f p1 = zero + (i + 0) * sqWidth * pb1 + (j + 0) * sqHeight * pb2; 123 Point3f p2 = zero + (i + 1) * sqWidth * pb1 + (j + 0) * sqHeight * pb2; 124 Point3f p3 = zero + (i + 1) * sqWidth * pb1 + (j + 1) * sqHeight * pb2; 125 Point3f p4 = zero + (i + 0) * sqWidth * pb1 + (j + 1) * sqHeight * pb2; 126 generateEdge(p1, p2, pts_square3d); 127 generateEdge(p2, p3, pts_square3d); 128 generateEdge(p3, p4, pts_square3d); 129 generateEdge(p4, p1, pts_square3d); 130 131 projectPoints(Mat(pts_square3d), rvec, tvec, camMat, distCoeffs, pts_square2d); 132 squares_black.resize(squares_black.size() + 1); 133 vector<Point2f> temp; 134 approxPolyDP(Mat(pts_square2d), temp, 1.0, true); 135 transform(temp.begin(), temp.end(), back_inserter(squares_black.back()), Mult(rendererResolutionMultiplier)); 136 } 137 138 /* calculate corners */ 139 corners3d.clear(); 140 for(int j = 0; j < patternSize.height - 1; ++j) 141 for(int i = 0; i < patternSize.width - 1; ++i) 142 corners3d.push_back(zero + (i + 1) * sqWidth * pb1 + (j + 1) * sqHeight * pb2); 143 corners.clear(); 144 projectPoints(Mat(corners3d), rvec, tvec, camMat, distCoeffs, corners); 145 146 vector<Point3f> whole3d; 147 vector<Point2f> whole2d; 148 generateEdge(whole[0], whole[1], whole3d); 149 generateEdge(whole[1], whole[2], whole3d); 150 generateEdge(whole[2], whole[3], whole3d); 151 generateEdge(whole[3], whole[0], whole3d); 152 projectPoints(Mat(whole3d), rvec, tvec, camMat, distCoeffs, whole2d); 153 vector<Point2f> temp_whole2d; 154 approxPolyDP(Mat(whole2d), temp_whole2d, 1.0, true); 155 156 vector< vector<Point > > whole_contour(1); 157 transform(temp_whole2d.begin(), temp_whole2d.end(), 158 back_inserter(whole_contour.front()), Mult(rendererResolutionMultiplier)); 159 160 Mat result; 161 if (rendererResolutionMultiplier == 1) 162 { 163 result = bg.clone(); 164 drawContours(result, whole_contour, -1, Scalar::all(255), FILLED, LINE_AA); 165 drawContours(result, squares_black, -1, Scalar::all(0), FILLED, LINE_AA); 166 } 167 else 168 { 169 Mat tmp; 170 resize(bg, tmp, bg.size() * rendererResolutionMultiplier); 171 drawContours(tmp, whole_contour, -1, Scalar::all(255), FILLED, LINE_AA); 172 drawContours(tmp, squares_black, -1, Scalar::all(0), FILLED, LINE_AA); 173 resize(tmp, result, bg.size(), 0, 0, INTER_AREA); 174 } 175 176 return result; 177 } 178 179 Mat cv::ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, vector<Point2f>& corners) const 180 { 181 cov = std::min(cov, 0.8); 182 double fovx, fovy, focalLen; 183 Point2d principalPoint; 184 double aspect; 185 calibrationMatrixValues( camMat, bg.size(), sensorWidth, sensorHeight, 186 fovx, fovy, focalLen, principalPoint, aspect); 187 188 RNG& rng = theRNG(); 189 190 float d1 = static_cast<float>(rng.uniform(0.1, 10.0)); 191 float ah = static_cast<float>(rng.uniform(-fovx/2 * cov, fovx/2 * cov) * CV_PI / 180); 192 float av = static_cast<float>(rng.uniform(-fovy/2 * cov, fovy/2 * cov) * CV_PI / 180); 193 194 Point3f p; 195 p.z = cos(ah) * d1; 196 p.x = sin(ah) * d1; 197 p.y = p.z * tan(av); 198 199 Point3f pb1, pb2; 200 generateBasis(pb1, pb2); 201 202 float cbHalfWidth = static_cast<float>(norm(p) * sin( std::min(fovx, fovy) * 0.5 * CV_PI / 180)); 203 float cbHalfHeight = cbHalfWidth * patternSize.height / patternSize.width; 204 205 float cbHalfWidthEx = cbHalfWidth * ( patternSize.width + 1) / patternSize.width; 206 float cbHalfHeightEx = cbHalfHeight * (patternSize.height + 1) / patternSize.height; 207 208 vector<Point3f> pts3d(4); 209 vector<Point2f> pts2d(4); 210 for(;;) 211 { 212 pts3d[0] = p + pb1 * cbHalfWidthEx + cbHalfHeightEx * pb2; 213 pts3d[1] = p + pb1 * cbHalfWidthEx - cbHalfHeightEx * pb2; 214 pts3d[2] = p - pb1 * cbHalfWidthEx - cbHalfHeightEx * pb2; 215 pts3d[3] = p - pb1 * cbHalfWidthEx + cbHalfHeightEx * pb2; 216 217 /* can remake with better perf */ 218 projectPoints(Mat(pts3d), rvec, tvec, camMat, distCoeffs, pts2d); 219 220 bool inrect1 = pts2d[0].x < bg.cols && pts2d[0].y < bg.rows && pts2d[0].x > 0 && pts2d[0].y > 0; 221 bool inrect2 = pts2d[1].x < bg.cols && pts2d[1].y < bg.rows && pts2d[1].x > 0 && pts2d[1].y > 0; 222 bool inrect3 = pts2d[2].x < bg.cols && pts2d[2].y < bg.rows && pts2d[2].x > 0 && pts2d[2].y > 0; 223 bool inrect4 = pts2d[3].x < bg.cols && pts2d[3].y < bg.rows && pts2d[3].x > 0 && pts2d[3].y > 0; 224 225 if (inrect1 && inrect2 && inrect3 && inrect4) 226 break; 227 228 cbHalfWidth*=0.8f; 229 cbHalfHeight = cbHalfWidth * patternSize.height / patternSize.width; 230 231 cbHalfWidthEx = cbHalfWidth * ( patternSize.width + 1) / patternSize.width; 232 cbHalfHeightEx = cbHalfHeight * (patternSize.height + 1) / patternSize.height; 233 } 234 235 Point3f zero = p - pb1 * cbHalfWidth - cbHalfHeight * pb2; 236 float sqWidth = 2 * cbHalfWidth/patternSize.width; 237 float sqHeight = 2 * cbHalfHeight/patternSize.height; 238 239 return generateChessBoard(bg, camMat, distCoeffs, zero, pb1, pb2, sqWidth, sqHeight, pts3d, corners); 240 } 241 242 243 Mat cv::ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, 244 const Size2f& squareSize, vector<Point2f>& corners) const 245 { 246 cov = std::min(cov, 0.8); 247 double fovx, fovy, focalLen; 248 Point2d principalPoint; 249 double aspect; 250 calibrationMatrixValues( camMat, bg.size(), sensorWidth, sensorHeight, 251 fovx, fovy, focalLen, principalPoint, aspect); 252 253 RNG& rng = theRNG(); 254 255 float d1 = static_cast<float>(rng.uniform(0.1, 10.0)); 256 float ah = static_cast<float>(rng.uniform(-fovx/2 * cov, fovx/2 * cov) * CV_PI / 180); 257 float av = static_cast<float>(rng.uniform(-fovy/2 * cov, fovy/2 * cov) * CV_PI / 180); 258 259 Point3f p; 260 p.z = cos(ah) * d1; 261 p.x = sin(ah) * d1; 262 p.y = p.z * tan(av); 263 264 Point3f pb1, pb2; 265 generateBasis(pb1, pb2); 266 267 float cbHalfWidth = squareSize.width * patternSize.width * 0.5f; 268 float cbHalfHeight = squareSize.height * patternSize.height * 0.5f; 269 270 float cbHalfWidthEx = cbHalfWidth * ( patternSize.width + 1) / patternSize.width; 271 float cbHalfHeightEx = cbHalfHeight * (patternSize.height + 1) / patternSize.height; 272 273 vector<Point3f> pts3d(4); 274 vector<Point2f> pts2d(4); 275 for(;;) 276 { 277 pts3d[0] = p + pb1 * cbHalfWidthEx + cbHalfHeightEx * pb2; 278 pts3d[1] = p + pb1 * cbHalfWidthEx - cbHalfHeightEx * pb2; 279 pts3d[2] = p - pb1 * cbHalfWidthEx - cbHalfHeightEx * pb2; 280 pts3d[3] = p - pb1 * cbHalfWidthEx + cbHalfHeightEx * pb2; 281 282 /* can remake with better perf */ 283 projectPoints(Mat(pts3d), rvec, tvec, camMat, distCoeffs, pts2d); 284 285 bool inrect1 = pts2d[0].x < bg.cols && pts2d[0].y < bg.rows && pts2d[0].x > 0 && pts2d[0].y > 0; 286 bool inrect2 = pts2d[1].x < bg.cols && pts2d[1].y < bg.rows && pts2d[1].x > 0 && pts2d[1].y > 0; 287 bool inrect3 = pts2d[2].x < bg.cols && pts2d[2].y < bg.rows && pts2d[2].x > 0 && pts2d[2].y > 0; 288 bool inrect4 = pts2d[3].x < bg.cols && pts2d[3].y < bg.rows && pts2d[3].x > 0 && pts2d[3].y > 0; 289 290 if ( inrect1 && inrect2 && inrect3 && inrect4) 291 break; 292 293 p.z *= 1.1f; 294 } 295 296 Point3f zero = p - pb1 * cbHalfWidth - cbHalfHeight * pb2; 297 298 return generateChessBoard(bg, camMat, distCoeffs, zero, pb1, pb2, 299 squareSize.width, squareSize.height, pts3d, corners); 300 } 301 302 Mat cv::ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, 303 const Size2f& squareSize, const Point3f& pos, vector<Point2f>& corners) const 304 { 305 cov = std::min(cov, 0.8); 306 Point3f p = pos; 307 Point3f pb1, pb2; 308 generateBasis(pb1, pb2); 309 310 float cbHalfWidth = squareSize.width * patternSize.width * 0.5f; 311 float cbHalfHeight = squareSize.height * patternSize.height * 0.5f; 312 313 float cbHalfWidthEx = cbHalfWidth * ( patternSize.width + 1) / patternSize.width; 314 float cbHalfHeightEx = cbHalfHeight * (patternSize.height + 1) / patternSize.height; 315 316 vector<Point3f> pts3d(4); 317 vector<Point2f> pts2d(4); 318 319 pts3d[0] = p + pb1 * cbHalfWidthEx + cbHalfHeightEx * pb2; 320 pts3d[1] = p + pb1 * cbHalfWidthEx - cbHalfHeightEx * pb2; 321 pts3d[2] = p - pb1 * cbHalfWidthEx - cbHalfHeightEx * pb2; 322 pts3d[3] = p - pb1 * cbHalfWidthEx + cbHalfHeightEx * pb2; 323 324 /* can remake with better perf */ 325 projectPoints(Mat(pts3d), rvec, tvec, camMat, distCoeffs, pts2d); 326 327 Point3f zero = p - pb1 * cbHalfWidth - cbHalfHeight * pb2; 328 329 return generateChessBoard(bg, camMat, distCoeffs, zero, pb1, pb2, 330 squareSize.width, squareSize.height, pts3d, corners); 331 } 332
