From f2a3e7e312153ad259eface7be7bea38c808f1ef Mon Sep 17 00:00:00 2001 From: Vadim Pisarevsky Date: Sun, 28 Nov 2010 19:41:55 +0000 Subject: [PATCH] converted some more samples to C++ --- samples/c/inpaint.cpp | 80 ---- samples/cpp/3calibration.cpp | 16 - samples/cpp/CMakeLists.txt | 20 +- samples/{c => cpp}/calibration_artificial.cpp | 646 +++++++++++++------------- samples/cpp/camshiftdemo.cpp | 225 ++++----- samples/{c => cpp}/fitellipse.cpp | 0 samples/{c => cpp}/image.cpp | 0 samples/cpp/inpaint.cpp | 78 ++++ samples/cpp/lkdemo.cpp | 191 +++----- samples/cpp/stereo_calib.cpp | 550 ++++++++++------------ 10 files changed, 830 insertions(+), 976 deletions(-) delete mode 100644 samples/c/inpaint.cpp rename samples/{c => cpp}/calibration_artificial.cpp (97%) rename samples/{c => cpp}/fitellipse.cpp (100%) rename samples/{c => cpp}/image.cpp (100%) create mode 100644 samples/cpp/inpaint.cpp diff --git a/samples/c/inpaint.cpp b/samples/c/inpaint.cpp deleted file mode 100644 index 302ecfd..0000000 --- a/samples/c/inpaint.cpp +++ /dev/null @@ -1,80 +0,0 @@ -#include "opencv2/highgui/highgui.hpp" -#include "opencv2/imgproc/imgproc_c.h" - -#include - -IplImage* inpaint_mask = 0; -IplImage* img0 = 0, *img = 0, *inpainted = 0; -CvPoint prev_pt = {-1,-1}; - -void on_mouse( int event, int x, int y, int flags, void* ) -{ - if( !img ) - return; - - if( event == CV_EVENT_LBUTTONUP || !(flags & CV_EVENT_FLAG_LBUTTON) ) - prev_pt = cvPoint(-1,-1); - else if( event == CV_EVENT_LBUTTONDOWN ) - prev_pt = cvPoint(x,y); - else if( event == CV_EVENT_MOUSEMOVE && (flags & CV_EVENT_FLAG_LBUTTON) ) - { - CvPoint pt = cvPoint(x,y); - if( prev_pt.x < 0 ) - prev_pt = pt; - cvLine( inpaint_mask, prev_pt, pt, cvScalarAll(255), 5, 8, 0 ); - cvLine( img, prev_pt, pt, cvScalarAll(255), 5, 8, 0 ); - prev_pt = pt; - cvShowImage( "image", img ); - } -} - - -int main( int argc, char** argv ) -{ - char* filename = argc >= 2 ? argv[1] : (char*)"fruits.jpg"; - - if( (img0 = cvLoadImage(filename,-1)) == 0 ) - return 0; - - printf( "Hot keys: \n" - "\tESC - quit the program\n" - "\tr - restore the original image\n" - "\ti or SPACE - run inpainting algorithm\n" - "\t\t(before running it, paint something on the image)\n" ); - - cvNamedWindow( "image", 1 ); - - img = cvCloneImage( img0 ); - inpainted = cvCloneImage( img0 ); - inpaint_mask = cvCreateImage( cvGetSize(img), 8, 1 ); - - cvZero( inpaint_mask ); - cvZero( inpainted ); - cvShowImage( "image", img ); - cvShowImage( "inpainted image", inpainted ); - cvSetMouseCallback( "image", on_mouse, 0 ); - - for(;;) - { - int c = cvWaitKey(0); - - if( (char)c == 27 ) - break; - - if( (char)c == 'r' ) - { - cvZero( inpaint_mask ); - cvCopy( img0, img ); - cvShowImage( "image", img ); - } - - if( (char)c == 'i' || (char)c == ' ' ) - { - cvNamedWindow( "inpainted image", 1 ); - cvInpaint( img, inpaint_mask, inpainted, 3, CV_INPAINT_TELEA ); - cvShowImage( "inpainted image", inpainted ); - } - } - - return 1; -} diff --git a/samples/cpp/3calibration.cpp b/samples/cpp/3calibration.cpp index cada2d4..9f496b5 100644 --- a/samples/cpp/3calibration.cpp +++ b/samples/cpp/3calibration.cpp @@ -17,7 +17,6 @@ enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 }; void help() { - printf( "This is a camera calibration sample that calibrates 3 horizontally placed cameras together.\n" "Usage: 3calibration\n" " -w # the number of inner corners per one of board dimension\n" @@ -32,20 +31,6 @@ void help() } - - - - - - - - - - - - - - static void calcChessboardCorners(Size boardSize, float squareSize, vector& corners) { corners.resize(0); @@ -56,7 +41,6 @@ static void calcChessboardCorners(Size boardSize, float squareSize, vector > imagePoints1, vector > imagePoints2, vector > imagePoints3, diff --git a/samples/cpp/CMakeLists.txt b/samples/cpp/CMakeLists.txt index b669d69..4054a0f 100644 --- a/samples/cpp/CMakeLists.txt +++ b/samples/cpp/CMakeLists.txt @@ -29,16 +29,16 @@ if (BUILD_EXAMPLES) # --------------------------------------------- MACRO(MY_DEFINE_EXAMPLE name srcs) set(the_target "example_${name}") - add_executable(${the_target} ${srcs}) - set_target_properties(${the_target} PROPERTIES - OUTPUT_NAME "${name}" - PROJECT_LABEL "(EXAMPLE) ${name}") - add_dependencies(${the_target} opencv_core opencv_flann opencv_imgproc opencv_highgui - opencv_ml opencv_video opencv_objdetect opencv_features2d - opencv_calib3d opencv_legacy opencv_contrib) - target_link_libraries(${the_target} ${OPENCV_LINKER_LIBS} opencv_core - opencv_flann opencv_imgproc opencv_highgui opencv_ml opencv_video opencv_objdetect - opencv_features2d opencv_calib3d opencv_legacy opencv_contrib) + add_executable(${the_target} ${srcs}) + set_target_properties(${the_target} PROPERTIES + OUTPUT_NAME "${name}" + PROJECT_LABEL "(EXAMPLE) ${name}") + add_dependencies(${the_target} opencv_core opencv_flann opencv_imgproc opencv_highgui + opencv_ml opencv_video opencv_objdetect opencv_features2d + opencv_calib3d opencv_legacy opencv_contrib) + target_link_libraries(${the_target} ${OPENCV_LINKER_LIBS} opencv_core + opencv_flann opencv_imgproc opencv_highgui opencv_ml opencv_video opencv_objdetect + opencv_features2d opencv_calib3d opencv_legacy opencv_contrib) if(WIN32) install(TARGETS ${the_target} diff --git a/samples/c/calibration_artificial.cpp b/samples/cpp/calibration_artificial.cpp similarity index 97% rename from samples/c/calibration_artificial.cpp rename to samples/cpp/calibration_artificial.cpp index 7757dfe..3cea1d6 100644 --- a/samples/c/calibration_artificial.cpp +++ b/samples/cpp/calibration_artificial.cpp @@ -1,323 +1,323 @@ -#include -#include -#include -#include - -#include -#include -#include - -using namespace cv; -using namespace std; - -namespace cv -{ - -/* copy of class defines int tests/cv/chessboardgenerator.h */ -class ChessBoardGenerator -{ -public: - double sensorWidth; - double sensorHeight; - size_t squareEdgePointsNum; - double min_cos; - mutable double cov; - Size patternSize; - int rendererResolutionMultiplier; - - ChessBoardGenerator(const Size& patternSize = Size(8, 6)); - Mat operator()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, vector& corners) const; - Size cornersSize() const; -private: - void generateEdge(const Point3f& p1, const Point3f& p2, vector& out) const; - Mat generageChessBoard(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, - const Point3f& zero, const Point3f& pb1, const Point3f& pb2, - float sqWidth, float sqHeight, const vector& whole, vector& corners) const; - void generateBasis(Point3f& pb1, Point3f& pb2) const; - Point3f generateChessBoardCenter(const Mat& camMat, const Size& imgSize) const; - Mat rvec, tvec; -}; -}; - - - -const Size imgSize(800, 600); -const Size brdSize(8, 7); -const size_t brds_num = 20; - -template ostream& operator<<(ostream& out, const Mat_& mat) -{ - for(int j = 0; j < mat.rows; ++j) - for(int i = 0; i < mat.cols; ++i) - out << mat(j, i) << " "; - return out; -} - -int main() -{ - cout << "Initializing background..."; - Mat background(imgSize, CV_8UC3); - randu(background, Scalar::all(32), Scalar::all(255)); - GaussianBlur(background, background, Size(5, 5), 2); - cout << "Done" << endl; - - cout << "Initializing chess board generator..."; - ChessBoardGenerator cbg(brdSize); - cbg.rendererResolutionMultiplier = 4; - cout << "Done" << endl; - - /* camera params */ - Mat_ camMat(3, 3); - camMat << 300., 0., background.cols/2., 0, 300., background.rows/2., 0., 0., 1.; - - Mat_ distCoeffs(1, 5); - distCoeffs << 1.2, 0.2, 0., 0., 0.; - - cout << "Generating chessboards..."; - vector boards(brds_num); - vector tmp; - for(size_t i = 0; i < brds_num; ++i) - cout << (boards[i] = cbg(background, camMat, distCoeffs, tmp), i) << " "; - cout << "Done" << endl; - - vector chessboard3D; - for(int j = 0; j < cbg.cornersSize().height; ++j) - for(int i = 0; i < cbg.cornersSize().width; ++i) - chessboard3D.push_back(Point3i(i, j, 0)); - - /* init points */ - vector< vector > objectPoints; - vector< vector > imagePoints; - - cout << endl << "Finding chessboards' corners..."; - for(size_t i = 0; i < brds_num; ++i) - { - cout << i; - namedWindow("Current chessboard"); imshow("Current chessboard", boards[i]); waitKey(100); - bool found = findChessboardCorners(boards[i], cbg.cornersSize(), tmp); - if (found) - { - imagePoints.push_back(tmp); - objectPoints.push_back(chessboard3D); - cout<< "-found "; - } - else - cout<< "-not-found "; - - drawChessboardCorners(boards[i], cbg.cornersSize(), Mat(tmp), found); - imshow("Current chessboard", boards[i]); waitKey(1000); - } - cout << "Done" << endl; - cvDestroyAllWindows(); - - Mat camMat_est; - Mat distCoeffs_est; - vector rvecs, tvecs; - - cout << "Calibrating..."; - double rep_err = calibrateCamera(objectPoints, imagePoints, imgSize, camMat_est, distCoeffs_est, rvecs, tvecs); - cout << "Done" << endl; - - cout << endl << "Average Reprojection error: " << rep_err/brds_num/cbg.cornersSize().area() << endl; - cout << "==================================" << endl; - cout << "Original camera matrix:\n" << camMat << endl; - cout << "Original distCoeffs:\n" << distCoeffs << endl; - cout << "==================================" << endl; - cout << "Estiamted camera matrix:\n" << (Mat_&)camMat_est << endl; - cout << "Estiamted distCoeffs:\n" << (Mat_&)distCoeffs_est << endl; - - return 0; -} - - -///////////////////////////////////////////////////////////////////////////////////////////////// -///////////////////////////////////////////////////////////////////////////////////////////////// -///////////////////////////////////////////////////////////////////////////////////////////////// - -// Copy of tests/cv/src/chessboardgenerator code. Just do not want to add dependency. - - -ChessBoardGenerator::ChessBoardGenerator(const Size& _patternSize) : sensorWidth(32), sensorHeight(24), - squareEdgePointsNum(200), min_cos(sqrt(2.f)*0.5f), cov(0.5), - patternSize(_patternSize), rendererResolutionMultiplier(4), tvec(Mat::zeros(1, 3, CV_32F)) -{ - Rodrigues(Mat::eye(3, 3, CV_32F), rvec); -} - -void cv::ChessBoardGenerator::generateEdge(const Point3f& p1, const Point3f& p2, vector& out) const -{ - Point3f step = (p2 - p1) * (1.f/squareEdgePointsNum); - for(size_t n = 0; n < squareEdgePointsNum; ++n) - out.push_back( p1 + step * (float)n); -} - -Size cv::ChessBoardGenerator::cornersSize() const -{ - return Size(patternSize.width-1, patternSize.height-1); -} - -struct Mult -{ - float m; - Mult(int mult) : m((float)mult) {} - Point2f operator()(const Point2f& p)const { return p * m; } -}; - -void cv::ChessBoardGenerator::generateBasis(Point3f& pb1, Point3f& pb2) const -{ - RNG& rng = theRNG(); - - Vec3f n; - for(;;) - { - n[0] = rng.uniform(-1.f, 1.f); - n[1] = rng.uniform(-1.f, 1.f); - n[2] = rng.uniform(-1.f, 1.f); - float len = (float)norm(n); - n[0]/=len; - n[1]/=len; - n[2]/=len; - - if (fabs(n[2]) > min_cos) - break; - } - - Vec3f n_temp = n; n_temp[0] += 100; - Vec3f b1 = n.cross(n_temp); - Vec3f b2 = n.cross(b1); - float len_b1 = (float)norm(b1); - float len_b2 = (float)norm(b2); - - pb1 = Point3f(b1[0]/len_b1, b1[1]/len_b1, b1[2]/len_b1); - pb2 = Point3f(b2[0]/len_b1, b2[1]/len_b2, b2[2]/len_b2); -} - -Mat cv::ChessBoardGenerator::generageChessBoard(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, - const Point3f& zero, const Point3f& pb1, const Point3f& pb2, - float sqWidth, float sqHeight, const vector& whole, - vector& corners) const -{ - vector< vector > squares_black; - for(int i = 0; i < patternSize.width; ++i) - for(int j = 0; j < patternSize.height; ++j) - if ( (i % 2 == 0 && j % 2 == 0) || (i % 2 != 0 && j % 2 != 0) ) - { - vector pts_square3d; - vector pts_square2d; - - Point3f p1 = zero + (i + 0) * sqWidth * pb1 + (j + 0) * sqHeight * pb2; - Point3f p2 = zero + (i + 1) * sqWidth * pb1 + (j + 0) * sqHeight * pb2; - Point3f p3 = zero + (i + 1) * sqWidth * pb1 + (j + 1) * sqHeight * pb2; - Point3f p4 = zero + (i + 0) * sqWidth * pb1 + (j + 1) * sqHeight * pb2; - generateEdge(p1, p2, pts_square3d); - generateEdge(p2, p3, pts_square3d); - generateEdge(p3, p4, pts_square3d); - generateEdge(p4, p1, pts_square3d); - - projectPoints( Mat(pts_square3d), rvec, tvec, camMat, distCoeffs, pts_square2d); - squares_black.resize(squares_black.size() + 1); - vector temp; - approxPolyDP(Mat(pts_square2d), temp, 1.0, true); - transform(temp.begin(), temp.end(), back_inserter(squares_black.back()), Mult(rendererResolutionMultiplier)); - } - - /* calculate corners */ - vector corners3d; - for(int j = 0; j < patternSize.height - 1; ++j) - for(int i = 0; i < patternSize.width - 1; ++i) - corners3d.push_back(zero + (i + 1) * sqWidth * pb1 + (j + 1) * sqHeight * pb2); - corners.clear(); - projectPoints( Mat(corners3d), rvec, tvec, camMat, distCoeffs, corners); - - vector whole3d; - vector whole2d; - generateEdge(whole[0], whole[1], whole3d); - generateEdge(whole[1], whole[2], whole3d); - generateEdge(whole[2], whole[3], whole3d); - generateEdge(whole[3], whole[0], whole3d); - projectPoints( Mat(whole3d), rvec, tvec, camMat, distCoeffs, whole2d); - vector temp_whole2d; - approxPolyDP(Mat(whole2d), temp_whole2d, 1.0, true); - - vector< vector > whole_contour(1); - transform(temp_whole2d.begin(), temp_whole2d.end(), - back_inserter(whole_contour.front()), Mult(rendererResolutionMultiplier)); - - Mat result; - if (rendererResolutionMultiplier == 1) - { - result = bg.clone(); - drawContours(result, whole_contour, -1, Scalar::all(255), CV_FILLED, CV_AA); - drawContours(result, squares_black, -1, Scalar::all(0), CV_FILLED, CV_AA); - } - else - { - Mat tmp; - resize(bg, tmp, bg.size() * rendererResolutionMultiplier); - drawContours(tmp, whole_contour, -1, Scalar::all(255), CV_FILLED, CV_AA); - drawContours(tmp, squares_black, -1, Scalar::all(0), CV_FILLED, CV_AA); - resize(tmp, result, bg.size(), 0, 0, INTER_AREA); - } - return result; -} - -Mat cv::ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, vector& corners) const -{ - cov = min(cov, 0.8); - double fovx, fovy, focalLen; - Point2d principalPoint; - double aspect; - calibrationMatrixValues( camMat, bg.size(), sensorWidth, sensorHeight, - fovx, fovy, focalLen, principalPoint, aspect); - - RNG& rng = theRNG(); - - float d1 = static_cast(rng.uniform(0.1, 10.0)); - float ah = static_cast(rng.uniform(-fovx/2 * cov, fovx/2 * cov) * CV_PI / 180); - float av = static_cast(rng.uniform(-fovy/2 * cov, fovy/2 * cov) * CV_PI / 180); - - Point3f p; - p.z = cos(ah) * d1; - p.x = sin(ah) * d1; - p.y = p.z * tan(av); - - Point3f pb1, pb2; - generateBasis(pb1, pb2); - - float cbHalfWidth = static_cast(norm(p) * sin( min(fovx, fovy) * 0.5 * CV_PI / 180)); - float cbHalfHeight = cbHalfWidth * patternSize.height / patternSize.width; - - vector pts3d(4); - vector pts2d(4); - for(;;) - { - pts3d[0] = p + pb1 * cbHalfWidth + cbHalfHeight * pb2; - pts3d[1] = p + pb1 * cbHalfWidth - cbHalfHeight * pb2; - pts3d[2] = p - pb1 * cbHalfWidth - cbHalfHeight * pb2; - pts3d[3] = p - pb1 * cbHalfWidth + cbHalfHeight * pb2; - - /* can remake with better perf */ - projectPoints( Mat(pts3d), rvec, tvec, camMat, distCoeffs, pts2d); - - bool inrect1 = pts2d[0].x < bg.cols && pts2d[0].y < bg.rows && pts2d[0].x > 0 && pts2d[0].y > 0; - bool inrect2 = pts2d[1].x < bg.cols && pts2d[1].y < bg.rows && pts2d[1].x > 0 && pts2d[1].y > 0; - bool inrect3 = pts2d[2].x < bg.cols && pts2d[2].y < bg.rows && pts2d[2].x > 0 && pts2d[2].y > 0; - bool inrect4 = pts2d[3].x < bg.cols && pts2d[3].y < bg.rows && pts2d[3].x > 0 && pts2d[3].y > 0; - - if ( inrect1 && inrect2 && inrect3 && inrect4) - break; - - cbHalfWidth*=0.8f; - cbHalfHeight = cbHalfWidth * patternSize.height / patternSize.width; - } - - cbHalfWidth *= static_cast(patternSize.width)/(patternSize.width + 1); - cbHalfHeight *= static_cast(patternSize.height)/(patternSize.height + 1); - - Point3f zero = p - pb1 * cbHalfWidth - cbHalfHeight * pb2; - float sqWidth = 2 * cbHalfWidth/patternSize.width; - float sqHeight = 2 * cbHalfHeight/patternSize.height; - - return generageChessBoard(bg, camMat, distCoeffs, zero, pb1, pb2, sqWidth, sqHeight, pts3d, corners); -} - +#include +#include +#include +#include + +#include +#include +#include + +using namespace cv; +using namespace std; + +namespace cv +{ + +/* copy of class defines int tests/cv/chessboardgenerator.h */ +class ChessBoardGenerator +{ +public: + double sensorWidth; + double sensorHeight; + size_t squareEdgePointsNum; + double min_cos; + mutable double cov; + Size patternSize; + int rendererResolutionMultiplier; + + ChessBoardGenerator(const Size& patternSize = Size(8, 6)); + Mat operator()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, vector& corners) const; + Size cornersSize() const; +private: + void generateEdge(const Point3f& p1, const Point3f& p2, vector& out) const; + Mat generageChessBoard(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, + const Point3f& zero, const Point3f& pb1, const Point3f& pb2, + float sqWidth, float sqHeight, const vector& whole, vector& corners) const; + void generateBasis(Point3f& pb1, Point3f& pb2) const; + Point3f generateChessBoardCenter(const Mat& camMat, const Size& imgSize) const; + Mat rvec, tvec; +}; +}; + + + +const Size imgSize(800, 600); +const Size brdSize(8, 7); +const size_t brds_num = 20; + +template ostream& operator<<(ostream& out, const Mat_& mat) +{ + for(int j = 0; j < mat.rows; ++j) + for(int i = 0; i < mat.cols; ++i) + out << mat(j, i) << " "; + return out; +} + +int main() +{ + cout << "Initializing background..."; + Mat background(imgSize, CV_8UC3); + randu(background, Scalar::all(32), Scalar::all(255)); + GaussianBlur(background, background, Size(5, 5), 2); + cout << "Done" << endl; + + cout << "Initializing chess board generator..."; + ChessBoardGenerator cbg(brdSize); + cbg.rendererResolutionMultiplier = 4; + cout << "Done" << endl; + + /* camera params */ + Mat_ camMat(3, 3); + camMat << 300., 0., background.cols/2., 0, 300., background.rows/2., 0., 0., 1.; + + Mat_ distCoeffs(1, 5); + distCoeffs << 1.2, 0.2, 0., 0., 0.; + + cout << "Generating chessboards..."; + vector boards(brds_num); + vector tmp; + for(size_t i = 0; i < brds_num; ++i) + cout << (boards[i] = cbg(background, camMat, distCoeffs, tmp), i) << " "; + cout << "Done" << endl; + + vector chessboard3D; + for(int j = 0; j < cbg.cornersSize().height; ++j) + for(int i = 0; i < cbg.cornersSize().width; ++i) + chessboard3D.push_back(Point3i(i, j, 0)); + + /* init points */ + vector< vector > objectPoints; + vector< vector > imagePoints; + + cout << endl << "Finding chessboards' corners..."; + for(size_t i = 0; i < brds_num; ++i) + { + cout << i; + namedWindow("Current chessboard"); imshow("Current chessboard", boards[i]); waitKey(100); + bool found = findChessboardCorners(boards[i], cbg.cornersSize(), tmp); + if (found) + { + imagePoints.push_back(tmp); + objectPoints.push_back(chessboard3D); + cout<< "-found "; + } + else + cout<< "-not-found "; + + drawChessboardCorners(boards[i], cbg.cornersSize(), Mat(tmp), found); + imshow("Current chessboard", boards[i]); waitKey(1000); + } + cout << "Done" << endl; + cvDestroyAllWindows(); + + Mat camMat_est; + Mat distCoeffs_est; + vector rvecs, tvecs; + + cout << "Calibrating..."; + double rep_err = calibrateCamera(objectPoints, imagePoints, imgSize, camMat_est, distCoeffs_est, rvecs, tvecs); + cout << "Done" << endl; + + cout << endl << "Average Reprojection error: " << rep_err/brds_num/cbg.cornersSize().area() << endl; + cout << "==================================" << endl; + cout << "Original camera matrix:\n" << camMat << endl; + cout << "Original distCoeffs:\n" << distCoeffs << endl; + cout << "==================================" << endl; + cout << "Estiamted camera matrix:\n" << (Mat_&)camMat_est << endl; + cout << "Estiamted distCoeffs:\n" << (Mat_&)distCoeffs_est << endl; + + return 0; +} + + +///////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////////////////////////////////////////////////////// + +// Copy of tests/cv/src/chessboardgenerator code. Just do not want to add dependency. + + +ChessBoardGenerator::ChessBoardGenerator(const Size& _patternSize) : sensorWidth(32), sensorHeight(24), + squareEdgePointsNum(200), min_cos(sqrt(2.f)*0.5f), cov(0.5), + patternSize(_patternSize), rendererResolutionMultiplier(4), tvec(Mat::zeros(1, 3, CV_32F)) +{ + Rodrigues(Mat::eye(3, 3, CV_32F), rvec); +} + +void cv::ChessBoardGenerator::generateEdge(const Point3f& p1, const Point3f& p2, vector& out) const +{ + Point3f step = (p2 - p1) * (1.f/squareEdgePointsNum); + for(size_t n = 0; n < squareEdgePointsNum; ++n) + out.push_back( p1 + step * (float)n); +} + +Size cv::ChessBoardGenerator::cornersSize() const +{ + return Size(patternSize.width-1, patternSize.height-1); +} + +struct Mult +{ + float m; + Mult(int mult) : m((float)mult) {} + Point2f operator()(const Point2f& p)const { return p * m; } +}; + +void cv::ChessBoardGenerator::generateBasis(Point3f& pb1, Point3f& pb2) const +{ + RNG& rng = theRNG(); + + Vec3f n; + for(;;) + { + n[0] = rng.uniform(-1.f, 1.f); + n[1] = rng.uniform(-1.f, 1.f); + n[2] = rng.uniform(-1.f, 1.f); + float len = (float)norm(n); + n[0]/=len; + n[1]/=len; + n[2]/=len; + + if (fabs(n[2]) > min_cos) + break; + } + + Vec3f n_temp = n; n_temp[0] += 100; + Vec3f b1 = n.cross(n_temp); + Vec3f b2 = n.cross(b1); + float len_b1 = (float)norm(b1); + float len_b2 = (float)norm(b2); + + pb1 = Point3f(b1[0]/len_b1, b1[1]/len_b1, b1[2]/len_b1); + pb2 = Point3f(b2[0]/len_b1, b2[1]/len_b2, b2[2]/len_b2); +} + +Mat cv::ChessBoardGenerator::generageChessBoard(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, + const Point3f& zero, const Point3f& pb1, const Point3f& pb2, + float sqWidth, float sqHeight, const vector& whole, + vector& corners) const +{ + vector< vector > squares_black; + for(int i = 0; i < patternSize.width; ++i) + for(int j = 0; j < patternSize.height; ++j) + if ( (i % 2 == 0 && j % 2 == 0) || (i % 2 != 0 && j % 2 != 0) ) + { + vector pts_square3d; + vector pts_square2d; + + Point3f p1 = zero + (i + 0) * sqWidth * pb1 + (j + 0) * sqHeight * pb2; + Point3f p2 = zero + (i + 1) * sqWidth * pb1 + (j + 0) * sqHeight * pb2; + Point3f p3 = zero + (i + 1) * sqWidth * pb1 + (j + 1) * sqHeight * pb2; + Point3f p4 = zero + (i + 0) * sqWidth * pb1 + (j + 1) * sqHeight * pb2; + generateEdge(p1, p2, pts_square3d); + generateEdge(p2, p3, pts_square3d); + generateEdge(p3, p4, pts_square3d); + generateEdge(p4, p1, pts_square3d); + + projectPoints( Mat(pts_square3d), rvec, tvec, camMat, distCoeffs, pts_square2d); + squares_black.resize(squares_black.size() + 1); + vector temp; + approxPolyDP(Mat(pts_square2d), temp, 1.0, true); + transform(temp.begin(), temp.end(), back_inserter(squares_black.back()), Mult(rendererResolutionMultiplier)); + } + + /* calculate corners */ + vector corners3d; + for(int j = 0; j < patternSize.height - 1; ++j) + for(int i = 0; i < patternSize.width - 1; ++i) + corners3d.push_back(zero + (i + 1) * sqWidth * pb1 + (j + 1) * sqHeight * pb2); + corners.clear(); + projectPoints( Mat(corners3d), rvec, tvec, camMat, distCoeffs, corners); + + vector whole3d; + vector whole2d; + generateEdge(whole[0], whole[1], whole3d); + generateEdge(whole[1], whole[2], whole3d); + generateEdge(whole[2], whole[3], whole3d); + generateEdge(whole[3], whole[0], whole3d); + projectPoints( Mat(whole3d), rvec, tvec, camMat, distCoeffs, whole2d); + vector temp_whole2d; + approxPolyDP(Mat(whole2d), temp_whole2d, 1.0, true); + + vector< vector > whole_contour(1); + transform(temp_whole2d.begin(), temp_whole2d.end(), + back_inserter(whole_contour.front()), Mult(rendererResolutionMultiplier)); + + Mat result; + if (rendererResolutionMultiplier == 1) + { + result = bg.clone(); + drawContours(result, whole_contour, -1, Scalar::all(255), CV_FILLED, CV_AA); + drawContours(result, squares_black, -1, Scalar::all(0), CV_FILLED, CV_AA); + } + else + { + Mat tmp; + resize(bg, tmp, bg.size() * rendererResolutionMultiplier); + drawContours(tmp, whole_contour, -1, Scalar::all(255), CV_FILLED, CV_AA); + drawContours(tmp, squares_black, -1, Scalar::all(0), CV_FILLED, CV_AA); + resize(tmp, result, bg.size(), 0, 0, INTER_AREA); + } + return result; +} + +Mat cv::ChessBoardGenerator::operator ()(const Mat& bg, const Mat& camMat, const Mat& distCoeffs, vector& corners) const +{ + cov = min(cov, 0.8); + double fovx, fovy, focalLen; + Point2d principalPoint; + double aspect; + calibrationMatrixValues( camMat, bg.size(), sensorWidth, sensorHeight, + fovx, fovy, focalLen, principalPoint, aspect); + + RNG& rng = theRNG(); + + float d1 = static_cast(rng.uniform(0.1, 10.0)); + float ah = static_cast(rng.uniform(-fovx/2 * cov, fovx/2 * cov) * CV_PI / 180); + float av = static_cast(rng.uniform(-fovy/2 * cov, fovy/2 * cov) * CV_PI / 180); + + Point3f p; + p.z = cos(ah) * d1; + p.x = sin(ah) * d1; + p.y = p.z * tan(av); + + Point3f pb1, pb2; + generateBasis(pb1, pb2); + + float cbHalfWidth = static_cast(norm(p) * sin( min(fovx, fovy) * 0.5 * CV_PI / 180)); + float cbHalfHeight = cbHalfWidth * patternSize.height / patternSize.width; + + vector pts3d(4); + vector pts2d(4); + for(;;) + { + pts3d[0] = p + pb1 * cbHalfWidth + cbHalfHeight * pb2; + pts3d[1] = p + pb1 * cbHalfWidth - cbHalfHeight * pb2; + pts3d[2] = p - pb1 * cbHalfWidth - cbHalfHeight * pb2; + pts3d[3] = p - pb1 * cbHalfWidth + cbHalfHeight * pb2; + + /* can remake with better perf */ + projectPoints( Mat(pts3d), rvec, tvec, camMat, distCoeffs, pts2d); + + bool inrect1 = pts2d[0].x < bg.cols && pts2d[0].y < bg.rows && pts2d[0].x > 0 && pts2d[0].y > 0; + bool inrect2 = pts2d[1].x < bg.cols && pts2d[1].y < bg.rows && pts2d[1].x > 0 && pts2d[1].y > 0; + bool inrect3 = pts2d[2].x < bg.cols && pts2d[2].y < bg.rows && pts2d[2].x > 0 && pts2d[2].y > 0; + bool inrect4 = pts2d[3].x < bg.cols && pts2d[3].y < bg.rows && pts2d[3].x > 0 && pts2d[3].y > 0; + + if ( inrect1 && inrect2 && inrect3 && inrect4) + break; + + cbHalfWidth*=0.8f; + cbHalfHeight = cbHalfWidth * patternSize.height / patternSize.width; + } + + cbHalfWidth *= static_cast(patternSize.width)/(patternSize.width + 1); + cbHalfHeight *= static_cast(patternSize.height)/(patternSize.height + 1); + + Point3f zero = p - pb1 * cbHalfWidth - cbHalfHeight * pb2; + float sqWidth = 2 * cbHalfWidth/patternSize.width; + float sqHeight = 2 * cbHalfHeight/patternSize.height; + + return generageChessBoard(bg, camMat, distCoeffs, zero, pb1, pb2, sqWidth, sqHeight, pts3d, corners); +} + diff --git a/samples/cpp/camshiftdemo.cpp b/samples/cpp/camshiftdemo.cpp index 22096a0..02d741d 100644 --- a/samples/cpp/camshiftdemo.cpp +++ b/samples/cpp/camshiftdemo.cpp @@ -1,24 +1,21 @@ #include +#include #include #include #include -IplImage *image = 0, *hsv = 0, *hue = 0, *mask = 0, *backproject = 0, *histimg = 0; -CvHistogram *hist = 0; - -int backproject_mode = 0; -int select_object = 0; -int track_object = 0; -int show_hist = 1; -CvPoint origin; -CvRect selection; -CvRect track_window; -CvBox2D track_box; -CvConnectedComp track_comp; -int hdims = 16; -float hranges_arr[] = {0,180}; -float* hranges = hranges_arr; +using namespace cv; +using namespace std; + +Mat image; + +bool backprojMode = false; +bool selectObject = false; +int trackObject = 0; +bool showHist = true; +Point origin; +Rect selection; int vmin = 10, vmax = 256, smin = 30; void onMouse( int event, int x, int y, int flags, void* param ) @@ -27,15 +24,10 @@ void onMouse( int event, int x, int y, int flags, void* param ) { selection.x = MIN(x, origin.x); selection.y = MIN(y, origin.y); - selection.width = selection.x + std::abs(x - origin.x); - selection.height = selection.y + std::abs(y - origin.y); - - selection.x = MAX(selection.x, 0); - selection.y = MAX(selection.y, 0); - selection.width = MIN(selection.width, image.cols); - selection.height = MIN(selection.height, image.rows); - selection.width -= selection.x; - selection.height -= selection.y; + selection.width = std::abs(x - origin.x); + selection.height = std::abs(y - origin.y); + + selection &= Rect(0, 0, image.cols, image.rows); } switch( event ) @@ -53,162 +45,131 @@ void onMouse( int event, int x, int y, int flags, void* param ) } } - -Scalar hsv2rgb( float hue ) -{ - int rgb[3], p, sector; - static const int sectorData[][3]= - {{0,2,1}, {1,2,0}, {1,0,2}, {2,0,1}, {2,1,0}, {0,1,2}}; - hue *= 0.033333333333333333333333333333333f; - sector = cvFloor(hue); - p = cvRound(255*(hue - sector)); - p ^= sector & 1 ? 255 : 0; - - rgb[sector_data[sector][0]] = 255; - rgb[sector_data[sector][1]] = 0; - rgb[sector_data[sector][2]] = p; - - return cvScalar(rgb[2], rgb[1], rgb[0],0); -} - int main( int argc, char** argv ) { - CvCapture* capture = 0; + VideoCapture cap; + Rect trackWindow; + RotatedRect trackBox; + CvConnectedComp trackComp; + int hsize = 16; + float hranges[] = {0,180}; + const float* phranges = hranges; if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0]))) - capture = cvCaptureFromCAM( argc == 2 ? argv[1][0] - '0' : 0 ); + cap.open(argc == 2 ? argv[1][0] - '0' : 0); else if( argc == 2 ) - capture = cvCaptureFromAVI( argv[1] ); + cap.open(argv[1]); - if( !capture ) + if( !cap.isOpened() ) { - fprintf(stderr,"Could not initialize capturing...\n"); - return -1; + cout << "Could not initialize capturing...\n"; + return 0; } - printf( "Hot keys: \n" + cout << "Hot keys: \n" "\tESC - quit the program\n" "\tc - stop the tracking\n" "\tb - switch to/from backprojection view\n" "\th - show/hide object histogram\n" - "To initialize tracking, select the object with mouse\n" ); + "To initialize tracking, select the object with mouse\n"; - cvNamedWindow( "Histogram", 1 ); - cvNamedWindow( "CamShiftDemo", 1 ); - cvSetMouseCallback( "CamShiftDemo", on_mouse, 0 ); - cvCreateTrackbar( "Vmin", "CamShiftDemo", &vmin, 256, 0 ); - cvCreateTrackbar( "Vmax", "CamShiftDemo", &vmax, 256, 0 ); - cvCreateTrackbar( "Smin", "CamShiftDemo", &smin, 256, 0 ); + namedWindow( "Histogram", 1 ); + namedWindow( "CamShift Demo", 1 ); + setMouseCallback( "CamShift Demo", onMouse, 0 ); + createTrackbar( "Vmin", "CamShift Demo", &vmin, 256, 0 ); + createTrackbar( "Vmax", "CamShift Demo", &vmax, 256, 0 ); + createTrackbar( "Smin", "CamShift Demo", &smin, 256, 0 ); + Mat hsv, hue, mask, hist, histimg = Mat::zeros(200, 320, CV_8UC3), backproj; + for(;;) { - IplImage* frame = 0; - int i, bin_w, c; - - frame = cvQueryFrame( capture ); - if( !frame ) + Mat frame; + cap >> frame; + if( frame.empty() ) break; - if( !image ) - { - /* allocate all the buffers */ - image = cvCreateImage( cvGetSize(frame), 8, 3 ); - image->origin = frame->origin; - hsv = cvCreateImage( cvGetSize(frame), 8, 3 ); - hue = cvCreateImage( cvGetSize(frame), 8, 1 ); - mask = cvCreateImage( cvGetSize(frame), 8, 1 ); - backproject = cvCreateImage( cvGetSize(frame), 8, 1 ); - hist = cvCreateHist( 1, &hdims, CV_HIST_ARRAY, &hranges, 1 ); - histimg = cvCreateImage( cvSize(320,200), 8, 3 ); - cvZero( histimg ); - } - - cvCopy( frame, image, 0 ); - cvCvtColor( image, hsv, CV_BGR2HSV ); + frame.copyTo(image); + cvtColor(image, hsv, CV_BGR2HSV); - if( track_object ) + if( trackObject ) { int _vmin = vmin, _vmax = vmax; - cvInRangeS( hsv, cvScalar(0,smin,MIN(_vmin,_vmax),0), - cvScalar(180,256,MAX(_vmin,_vmax),0), mask ); - cvSplit( hsv, hue, 0, 0, 0 ); + inRange(hsv, Scalar(0, smin, MIN(_vmin,_vmax)), + Scalar(180, 256, MAX(_vmin, _vmax)), mask); + int ch[] = {0, 0}; + hue.create(hsv.size(), hsv.depth()); + mixChannels(&hsv, 1, &hue, 1, ch, 1); - if( track_object < 0 ) + if( trackObject < 0 ) { - float max_val = 0.f; - cvSetImageROI( hue, selection ); - cvSetImageROI( mask, selection ); - cvCalcHist( &hue, hist, 0, mask ); - cvGetMinMaxHistValue( hist, 0, &max_val, 0, 0 ); - cvConvertScale( hist->bins, hist->bins, max_val ? 255. / max_val : 0., 0 ); - cvResetImageROI( hue ); - cvResetImageROI( mask ); - track_window = selection; - track_object = 1; - - cvZero( histimg ); - bin_w = histimg->width / hdims; - for( i = 0; i < hdims; i++ ) + Mat roi(hue, selection), maskroi(mask, selection); + calcHist(&roi, 1, 0, maskroi, hist, 1, &hsize, &phranges); + normalize(hist, hist, 0, 255, CV_MINMAX); + + trackWindow = selection; + trackObject = 1; + + histimg = Scalar::all(0); + int binW = histimg.cols / hsize; + Mat buf(1, hsize, CV_8UC3); + for( int i = 0; i < hsize; i++ ) + buf.at(i) = Vec3b(saturate_cast(i*180./hsize), 255, 255); + cvtColor(buf, buf, CV_HSV2BGR); + + for( int i = 0; i < hsize; i++ ) { - int val = cvRound( cvGetReal1D(hist->bins,i)*histimg->height/255 ); - CvScalar color = hsv2rgb(i*180.f/hdims); - cvRectangle( histimg, cvPoint(i*bin_w,histimg->height), - cvPoint((i+1)*bin_w,histimg->height - val), - color, -1, 8, 0 ); + int val = saturate_cast(hist.at(i)*histimg.rows/255); + rectangle( histimg, Point(i*binW,histimg.rows), + Point((i+1)*binW,histimg.rows - val), + Scalar(buf.at(i)), -1, 8 ); } } - cvCalcBackProject( &hue, backproject, hist ); - cvAnd( backproject, mask, backproject, 0 ); - cvCamShift( backproject, track_window, - cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ), - &track_comp, &track_box ); - track_window = track_comp.rect; - - if( backproject_mode ) - cvCvtColor( backproject, image, CV_GRAY2BGR ); - if( !image->origin ) - track_box.angle = -track_box.angle; - cvEllipseBox( image, track_box, CV_RGB(255,0,0), 3, CV_AA, 0 ); + calcBackProject(&hue, 1, 0, hist, backproj, &phranges); + backproj &= mask; + RotatedRect trackBox = CamShift(backproj, trackWindow, + TermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 )); + trackBox.angle = 90-trackBox.angle; + + if( backprojMode ) + cvtColor( backproj, image, CV_GRAY2BGR ); + ellipse( image, trackBox, Scalar(0,0,255), 3, CV_AA ); } - if( select_object && selection.width > 0 && selection.height > 0 ) + if( selectObject && selection.width > 0 && selection.height > 0 ) { - cvSetImageROI( image, selection ); - cvXorS( image, cvScalarAll(255), image, 0 ); - cvResetImageROI( image ); + Mat roi(image, selection); + bitwise_not(roi, roi); } - cvShowImage( "CamShiftDemo", image ); - cvShowImage( "Histogram", histimg ); + imshow( "CamShift Demo", image ); + imshow( "Histogram", histimg ); - c = cvWaitKey(10); - if( (char) c == 27 ) + char c = (char)waitKey(10); + if( c == 27 ) break; - switch( (char) c ) + switch(c) { case 'b': - backproject_mode ^= 1; + backprojMode = !backprojMode; break; case 'c': - track_object = 0; - cvZero( histimg ); + trackObject = 0; + histimg = Scalar::all(0); break; case 'h': - show_hist ^= 1; - if( !show_hist ) - cvDestroyWindow( "Histogram" ); + showHist = !showHist; + if( !showHist ) + destroyWindow( "Histogram" ); else - cvNamedWindow( "Histogram", 1 ); + namedWindow( "Histogram", 1 ); break; default: ; } } - cvReleaseCapture( &capture ); - cvDestroyWindow("CamShiftDemo"); - return 0; } diff --git a/samples/c/fitellipse.cpp b/samples/cpp/fitellipse.cpp similarity index 100% rename from samples/c/fitellipse.cpp rename to samples/cpp/fitellipse.cpp diff --git a/samples/c/image.cpp b/samples/cpp/image.cpp similarity index 100% rename from samples/c/image.cpp rename to samples/cpp/image.cpp diff --git a/samples/cpp/inpaint.cpp b/samples/cpp/inpaint.cpp new file mode 100644 index 0000000..844f260 --- /dev/null +++ b/samples/cpp/inpaint.cpp @@ -0,0 +1,78 @@ +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/imgproc/imgproc.hpp" + +#include + +using namespace cv; +using namespace std; + +Mat img, inpaintMask; +Point prevPt(-1,-1); + +void onMouse( int event, int x, int y, int flags, void* ) +{ + if( event == CV_EVENT_LBUTTONUP || !(flags & CV_EVENT_FLAG_LBUTTON) ) + prevPt = Point(-1,-1); + else if( event == CV_EVENT_LBUTTONDOWN ) + prevPt = Point(x,y); + else if( event == CV_EVENT_MOUSEMOVE && (flags & CV_EVENT_FLAG_LBUTTON) ) + { + Point pt(x,y); + if( prevPt.x < 0 ) + prevPt = pt; + line( inpaintMask, prevPt, pt, Scalar::all(255), 5, 8, 0 ); + line( img, prevPt, pt, Scalar::all(255), 5, 8, 0 ); + prevPt = pt; + imshow("image", img); + } +} + + +int main( int argc, char** argv ) +{ + char* filename = argc >= 2 ? argv[1] : (char*)"fruits.jpg"; + Mat img0 = imread(filename, -1); + if(img0.empty()) + { + cout << "Usage: inpaint \n"; + return 0; + } + + cout << "Hot keys: \n" + "\tESC - quit the program\n" + "\tr - restore the original image\n" + "\ti or SPACE - run inpainting algorithm\n" + "\t\t(before running it, paint something on the image)\n"; + + namedWindow( "image", 1 ); + + img = img0.clone(); + inpaintMask = Mat::zeros(img.size(), CV_8U); + + imshow("image", img); + setMouseCallback( "image", onMouse, 0 ); + + for(;;) + { + char c = (char)waitKey(); + + if( c == 27 ) + break; + + if( c == 'r' ) + { + inpaintMask = Scalar::all(0); + img0.copyTo(img); + imshow("image", img); + } + + if( c == 'i' || c == ' ' ) + { + Mat inpainted; + inpaint(img, inpaintMask, inpainted, 3, CV_INPAINT_TELEA); + imshow("inpainted image", inpainted); + } + } + + return 0; +} diff --git a/samples/cpp/lkdemo.cpp b/samples/cpp/lkdemo.cpp index 53f4e0f..a482656 100644 --- a/samples/cpp/lkdemo.cpp +++ b/samples/cpp/lkdemo.cpp @@ -1,134 +1,98 @@ #include "opencv2/video/tracking.hpp" +#include "opencv2/imgproc/imgproc.hpp" #include "opencv2/highgui/highgui.hpp" +#include #include -#include -IplImage *image = 0, *grey = 0, *prev_grey = 0, *pyramid = 0, *prev_pyramid = 0, *swap_temp; +using namespace cv; +using namespace std; -int win_size = 10; -const int MAX_COUNT = 500; -CvPoint2D32f* points[2] = {0,0}, *swap_points; -char* status = 0; -int count = 0; -int need_to_init = 0; -int night_mode = 0; -int flags = 0; -int add_remove_pt = 0; -CvPoint pt; +Point2f pt; +bool addRemovePt = false; - -void on_mouse( int event, int x, int y, int flags, void* param ) +void onMouse( int event, int x, int y, int flags, void* param ) { - if( !image ) - return; - - if( image->origin ) - y = image->height - y; - if( event == CV_EVENT_LBUTTONDOWN ) { - pt = cvPoint(x,y); - add_remove_pt = 1; + pt = Point2f((float)x,(float)y); + addRemovePt = true; } } - int main( int argc, char** argv ) { - CvCapture* capture = 0; - + VideoCapture cap; + TermCriteria termcrit(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03); + Size winSize(10,10); + + const int MAX_COUNT = 500; + bool needToInit = false; + bool nightMode = false; + if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0]))) - capture = cvCaptureFromCAM( argc == 2 ? argv[1][0] - '0' : 0 ); + cap.open(argc == 2 ? argv[1][0] - '0' : 0); else if( argc == 2 ) - capture = cvCaptureFromAVI( argv[1] ); + cap.open(argv[1]); - if( !capture ) + if( !cap.isOpened() ) { - fprintf(stderr,"Could not initialize capturing...\n"); - return -1; + cout << "Could not initialize capturing...\n"; + return 0; } - /* print a welcome message, and the OpenCV version */ - printf ("Welcome to lkdemo, using OpenCV version %s (%d.%d.%d)\n", - CV_VERSION, - CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION); + // print a welcome message, and the OpenCV version + cout << "Welcome to lkdemo, using OpenCV version %s\n" << CV_VERSION; - printf( "Hot keys: \n" + cout << "\nHot keys: \n" "\tESC - quit the program\n" "\tr - auto-initialize tracking\n" "\tc - delete all the points\n" "\tn - switch the \"night\" mode on/off\n" - "To add/remove a feature point click it\n" ); + "To add/remove a feature point click it\n"; - cvNamedWindow( "LkDemo", 0 ); - cvSetMouseCallback( "LkDemo", on_mouse, 0 ); + namedWindow( "LK Demo", 1 ); + setMouseCallback( "LK Demo", onMouse, 0 ); + Mat gray, prevGray, image; + vector points[2]; + for(;;) { - IplImage* frame = 0; - int i, k, c; - - frame = cvQueryFrame( capture ); - if( !frame ) + Mat frame; + cap >> frame; + if( frame.empty() ) break; - if( !image ) - { - /* allocate all the buffers */ - image = cvCreateImage( cvGetSize(frame), 8, 3 ); - image->origin = frame->origin; - grey = cvCreateImage( cvGetSize(frame), 8, 1 ); - prev_grey = cvCreateImage( cvGetSize(frame), 8, 1 ); - pyramid = cvCreateImage( cvGetSize(frame), 8, 1 ); - prev_pyramid = cvCreateImage( cvGetSize(frame), 8, 1 ); - points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0])); - points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0])); - status = (char*)cvAlloc(MAX_COUNT); - flags = 0; - } - - cvCopy( frame, image, 0 ); - cvCvtColor( image, grey, CV_BGR2GRAY ); + frame.copyTo(image); + cvtColor(image, gray, CV_BGR2GRAY); - if( night_mode ) - cvZero( image ); + if( nightMode ) + image = Scalar::all(0); - if( need_to_init ) + if( needToInit ) { - /* automatic initialization */ - IplImage* eig = cvCreateImage( cvGetSize(grey), 32, 1 ); - IplImage* temp = cvCreateImage( cvGetSize(grey), 32, 1 ); - double quality = 0.01; - double min_distance = 10; - - count = MAX_COUNT; - cvGoodFeaturesToTrack( grey, eig, temp, points[1], &count, - quality, min_distance, 0, 3, 0, 0.04 ); - cvFindCornerSubPix( grey, points[1], count, - cvSize(win_size,win_size), cvSize(-1,-1), - cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03)); - cvReleaseImage( &eig ); - cvReleaseImage( &temp ); - - add_remove_pt = 0; + // automatic initialization + goodFeaturesToTrack(gray, points[1], MAX_COUNT, 0.01, 10, Mat(), 3, 0, 0.04); + cornerSubPix(gray, points[1], winSize, Size(-1,-1), termcrit); + addRemovePt = false; } - else if( count > 0 ) + else if( !points[0].empty() ) { - cvCalcOpticalFlowPyrLK( prev_grey, grey, prev_pyramid, pyramid, - points[0], points[1], count, cvSize(win_size,win_size), 3, status, 0, - cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), flags ); - flags |= CV_LKFLOW_PYR_A_READY; - for( i = k = 0; i < count; i++ ) + vector status; + vector err; + if(prevGray.empty()) + gray.copyTo(prevGray); + calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, winSize, + 3, termcrit, 0); + size_t i, k; + for( i = k = 0; i < points[1].size(); i++ ) { - if( add_remove_pt ) + if( addRemovePt ) { - double dx = pt.x - points[1][i].x; - double dy = pt.y - points[1][i].y; - - if( dx*dx + dy*dy <= 25 ) + if( norm(pt - points[1][i]) <= 5 ) { - add_remove_pt = 0; + addRemovePt = false; continue; } } @@ -137,51 +101,44 @@ int main( int argc, char** argv ) continue; points[1][k++] = points[1][i]; - cvCircle( image, cvPointFrom32f(points[1][i]), 3, CV_RGB(0,255,0), -1, 8,0); + circle( image, points[1][i], 3, Scalar(0,255,0), -1, 8); } - count = k; + points[1].resize(k); } - if( add_remove_pt && count < MAX_COUNT ) + if( addRemovePt && points[1].size() < MAX_COUNT ) { - points[1][count++] = cvPointTo32f(pt); - cvFindCornerSubPix( grey, points[1] + count - 1, 1, - cvSize(win_size,win_size), cvSize(-1,-1), - cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03)); - add_remove_pt = 0; + vector tmp; + tmp.push_back(pt); + cornerSubPix( gray, tmp, winSize, cvSize(-1,-1), termcrit); + points[1].push_back(tmp[0]); + addRemovePt = false; } - CV_SWAP( prev_grey, grey, swap_temp ); - CV_SWAP( prev_pyramid, pyramid, swap_temp ); - CV_SWAP( points[0], points[1], swap_points ); - need_to_init = 0; - cvShowImage( "LkDemo", image ); + needToInit = false; + imshow("LK Demo", image); - c = cvWaitKey(10); - if( (char)c == 27 ) + char c = (char)waitKey(10); + if( c == 27 ) break; - switch( (char) c ) + switch( c ) { case 'r': - need_to_init = 1; + needToInit = true; break; case 'c': - count = 0; + points[1].clear(); break; case 'n': - night_mode ^= 1; + nightMode = !nightMode; break; default: ; } + + std::swap(points[1], points[0]); + swap(prevGray, gray); } - cvReleaseCapture( &capture ); - cvDestroyWindow("LkDemo"); - return 0; } - -#ifdef _EiC -main(1,"lkdemo.c"); -#endif diff --git a/samples/cpp/stereo_calib.cpp b/samples/cpp/stereo_calib.cpp index 1e20e7b..77106c1 100644 --- a/samples/cpp/stereo_calib.cpp +++ b/samples/cpp/stereo_calib.cpp @@ -27,14 +27,18 @@ #include "opencv2/calib3d/calib3d.hpp" #include "opencv2/highgui/highgui.hpp" -#include "opencv2/imgproc/imgproc_c.h" +#include "opencv2/imgproc/imgproc.hpp" #include #include #include +#include +#include #include +#include #include +using namespace cv; using namespace std; // @@ -45,344 +49,294 @@ using namespace std; // rectified results along with the computed disparity images. // static void -StereoCalib(const char* path, const char* imageList, int useUncalibrated) +StereoCalib(const vector& imagelist, Size boardSize, bool useCalibrated=true, bool showRectified=true) { - CvRect roi1, roi2; - int nx = 0, ny = 0; - int displayCorners = 1; - int showUndistorted = 1; - bool isVerticalStereo = false;//OpenCV can handle left-right - //or up-down camera arrangements - const int maxScale = 1; - const float squareSize = 1.f; //Set this to your actual square size - FILE* f = fopen(imageList, "rt"); - int i, j, lr, nframes = 0, n, N = 0; - vector imageNames[2]; - vector objectPoints; - vector points[2]; - vector temp_points[2]; - vector npoints; -// vector active[2]; - int is_found[2] = {0, 0}; - vector temp; - CvSize imageSize = {0,0}; - // ARRAY AND VECTOR STORAGE: - double M1[3][3], M2[3][3], D1[5], D2[5]; - double R[3][3], T[3], E[3][3], F[3][3]; - double Q[4][4]; - CvMat _M1 = cvMat(3, 3, CV_64F, M1 ); - CvMat _M2 = cvMat(3, 3, CV_64F, M2 ); - CvMat _D1 = cvMat(1, 5, CV_64F, D1 ); - CvMat _D2 = cvMat(1, 5, CV_64F, D2 ); - CvMat matR = cvMat(3, 3, CV_64F, R ); - CvMat matT = cvMat(3, 1, CV_64F, T ); - CvMat matE = cvMat(3, 3, CV_64F, E ); - CvMat matF = cvMat(3, 3, CV_64F, F ); - - CvMat matQ = cvMat(4, 4, CV_64FC1, Q); - - char buf[1024]; - - if( displayCorners ) - cvNamedWindow( "corners", 1 ); -// READ IN THE LIST OF CHESSBOARDS: - if( !f ) + if( imagelist.size() % 2 != 0 ) { - fprintf(stderr, "can not open file %s\n", imageList ); + cout << "Error: the image list contains odd (non-even) number of elements\n"; return; } - if( !fgets(buf, sizeof(buf)-3, f) || sscanf(buf, "%d%d", &nx, &ny) != 2 ) - return; - n = nx*ny; - temp.resize(n); - temp_points[0].resize(n); - temp_points[1].resize(n); + bool displayCorners = true; + const int maxScale = 2; + const float squareSize = 1.f; // Set this to your actual square size + // ARRAY AND VECTOR STORAGE: - for(i=0;;i++) + vector > imagePoints[2]; + vector > objectPoints; + Size imageSize; + + int i, j, k, nimages = (int)imagelist.size()/2; + + imagePoints[0].resize(nimages); + imagePoints[1].resize(nimages); + vector goodImageList; + + for( i = j = 0; i < nimages; i++ ) { - int count = 0, result=0; - lr = i % 2; - vector& pts = temp_points[lr];//points[lr]; - if( !fgets( buf, sizeof(buf)-3, f )) - break; - size_t len = strlen(buf); - while( len > 0 && isspace(buf[len-1])) - buf[--len] = '\0'; - if( buf[0] == '#') - continue; - char fullpath[1024]; - sprintf(fullpath, "%s/%s", path, buf); - IplImage* img = cvLoadImage( fullpath, 0 ); - if( !img ) - { - printf("Cannot read file %s\n", fullpath); - return; - } - imageSize = cvGetSize(img); - imageNames[lr].push_back(buf); - //FIND CHESSBOARDS AND CORNERS THEREIN: - for( int s = 1; s <= maxScale; s++ ) + for( k = 0; k < 2; k++ ) { - IplImage* timg = img; - if( s > 1 ) + const string& filename = imagelist[i*2+k]; + Mat img = imread(filename, 0); + if(img.empty()) + break; + if( imageSize == Size() ) + imageSize = img.size(); + else if( img.size() != imageSize ) { - timg = cvCreateImage(cvSize(img->width*s,img->height*s), - img->depth, img->nChannels ); - cvResize( img, timg, CV_INTER_CUBIC ); + cout << "The image " << filename << " has the size different from the first image size. Skipping the pair\n"; + break; } - result = cvFindChessboardCorners( timg, cvSize(nx, ny), - &temp[0], &count, - CV_CALIB_CB_ADAPTIVE_THRESH | - CV_CALIB_CB_NORMALIZE_IMAGE); - if( timg != img ) - cvReleaseImage( &timg ); - if( result || s == maxScale ) - for( j = 0; j < count; j++ ) + bool found = false; + vector& corners = imagePoints[k][j]; + for( int scale = 1; scale <= maxScale; scale++ ) { - temp[j].x /= s; - temp[j].y /= s; + Mat timg; + if( scale == 1 ) + timg = img; + else + resize(img, timg, Size(), scale, scale); + found = findChessboardCorners(timg, boardSize, corners, + CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_NORMALIZE_IMAGE); + if( found ) + { + if( scale > 1 ) + { + Mat cornersMat(corners); + cornersMat *= 1./scale; + } + break; + } } - if( result ) + if( displayCorners ) + { + cout << filename << endl; + Mat cimg, cimg1; + cvtColor(img, cimg, CV_GRAY2BGR); + drawChessboardCorners(cimg, boardSize, corners, found); + double sf = 640./MAX(img.rows, img.cols); + resize(cimg, cimg1, Size(), sf, sf); + imshow("corners", cimg1); + char c = (char)waitKey(500); + if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit + exit(-1); + } + else + putchar('.'); + if( !found ) break; + cornerSubPix(img, corners, Size(11,11), Size(-1,-1), + TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, + 30, 0.01)); } - if( displayCorners ) + if( k == 2 ) { - printf("%s\n", buf); - IplImage* cimg = cvCreateImage( imageSize, 8, 3 ); - cvCvtColor( img, cimg, CV_GRAY2BGR ); - cvDrawChessboardCorners( cimg, cvSize(nx, ny), &temp[0], - count, result ); - IplImage* cimg1 = cvCreateImage(cvSize(640, 480), IPL_DEPTH_8U, 3); - cvResize(cimg, cimg1); - cvShowImage( "corners", cimg1 ); - cvReleaseImage( &cimg ); - cvReleaseImage( &cimg1 ); - int c = cvWaitKey(1000); - if( c == 27 || c == 'q' || c == 'Q' ) //Allow ESC to quit - exit(-1); - } - else - putchar('.'); - //N = pts.size(); - //pts.resize(N + n, cvPoint2D32f(0,0)); - //active[lr].push_back((uchar)result); - is_found[lr] = result > 0 ? 1 : 0; - //assert( result != 0 ); - if( result ) - { - //Calibration will suffer without subpixel interpolation - cvFindCornerSubPix( img, &temp[0], count, - cvSize(11, 11), cvSize(-1,-1), - cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, - 30, 0.01) ); - copy( temp.begin(), temp.end(), pts.begin() ); - } - cvReleaseImage( &img ); - - if(lr) - { - if(is_found[0] == 1 && is_found[1] == 1) - { - assert(temp_points[0].size() == temp_points[1].size()); - int current_size = points[0].size(); - - points[0].resize(current_size + temp_points[0].size(), cvPoint2D32f(0.0, 0.0)); - points[1].resize(current_size + temp_points[1].size(), cvPoint2D32f(0.0, 0.0)); - - copy(temp_points[0].begin(), temp_points[0].end(), points[0].begin() + current_size); - copy(temp_points[1].begin(), temp_points[1].end(), points[1].begin() + current_size); - - nframes++; - - printf("Pair successfully detected...\n"); - } - - is_found[0] = 0; - is_found[1] = 0; - + goodImageList.push_back(imagelist[i*2]); + goodImageList.push_back(imagelist[i*2+1]); + j++; } } - fclose(f); - printf("\n"); -// HARVEST CHESSBOARD 3D OBJECT POINT LIST: - objectPoints.resize(nframes*n); - for( i = 0; i < ny; i++ ) - for( j = 0; j < nx; j++ ) - objectPoints[i*nx + j] = - cvPoint3D32f(i*squareSize, j*squareSize, 0); - for( i = 1; i < nframes; i++ ) - copy( objectPoints.begin(), objectPoints.begin() + n, - objectPoints.begin() + i*n ); - npoints.resize(nframes,n); - N = nframes*n; - CvMat _objectPoints = cvMat(1, N, CV_32FC3, &objectPoints[0] ); - CvMat _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] ); - CvMat _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] ); - CvMat _npoints = cvMat(1, npoints.size(), CV_32S, &npoints[0] ); - cvSetIdentity(&_M1); - cvSetIdentity(&_M2); - cvZero(&_D1); - cvZero(&_D2); + cout << j << " pairs have been successfully detected.\n"; + nimages = j; + if( nimages < 2 ) + { + cout << "Error: too little pairs to run the calibration\n"; + return; + } + + imagePoints[0].resize(nimages); + imagePoints[1].resize(nimages); + objectPoints.resize(nimages); + + for( i = 0; i < nimages; i++ ) + { + for( j = 0; j < boardSize.height; j++ ) + for( k = 0; k < boardSize.width; k++ ) + objectPoints[i].push_back(Point3f(j*squareSize, k*squareSize, 0)); + } + + cout << "Running stereo calibration ...\n"; + + Mat cameraMatrix[2], distCoeffs[2]; + cameraMatrix[0] = Mat::eye(3, 3, CV_64F); + cameraMatrix[1] = Mat::eye(3, 3, CV_64F); + distCoeffs[0] = Mat::zeros(8, 1, CV_64F); + distCoeffs[1] = Mat::zeros(8, 1, CV_64F); + Mat R, T, E, F; -// CALIBRATE THE STEREO CAMERAS - printf("Running stereo calibration ..."); - fflush(stdout); - cvStereoCalibrate( &_objectPoints, &_imagePoints1, - &_imagePoints2, &_npoints, - &_M1, &_D1, &_M2, &_D2, - imageSize, &matR, &matT, &matE, &matF, - cvTermCriteria(CV_TERMCRIT_ITER+ - CV_TERMCRIT_EPS, 100, 1e-5), - CV_CALIB_FIX_ASPECT_RATIO + - CV_CALIB_ZERO_TANGENT_DIST + - CV_CALIB_SAME_FOCAL_LENGTH + - CV_CALIB_FIX_K3); - printf(" done\n"); + stereoCalibrate(objectPoints, imagePoints[0], imagePoints[1], + cameraMatrix[0], distCoeffs[0], + cameraMatrix[1], distCoeffs[1], + imageSize, R, T, E, F, + TermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS, 100, 1e-5), + CV_CALIB_FIX_ASPECT_RATIO + + CV_CALIB_ZERO_TANGENT_DIST + + CV_CALIB_SAME_FOCAL_LENGTH + + CV_CALIB_FIX_K3); + cout << "done\n"; // CALIBRATION QUALITY CHECK // because the output fundamental matrix implicitly // includes all the output information, // we can check the quality of calibration using the // epipolar geometry constraint: m2^t*F*m1=0 - vector lines[2]; - points[0].resize(N); - points[1].resize(N); - _imagePoints1 = cvMat(1, N, CV_32FC2, &points[0][0] ); - _imagePoints2 = cvMat(1, N, CV_32FC2, &points[1][0] ); - lines[0].resize(N); - lines[1].resize(N); - CvMat _L1 = cvMat(1, N, CV_32FC3, &lines[0][0]); - CvMat _L2 = cvMat(1, N, CV_32FC3, &lines[1][0]); -//Always work in undistorted space - cvUndistortPoints( &_imagePoints1, &_imagePoints1, - &_M1, &_D1, 0, &_M1 ); - cvUndistortPoints( &_imagePoints2, &_imagePoints2, - &_M2, &_D2, 0, &_M2 ); - cvComputeCorrespondEpilines( &_imagePoints1, 1, &matF, &_L1 ); - cvComputeCorrespondEpilines( &_imagePoints2, 2, &matF, &_L2 ); - double avgErr = 0; - for( i = 0; i < N; i++ ) + double err = 0; + int npoints = 0; + vector lines[2]; + for( i = 0; i < nimages; i++ ) { - double err = fabs(points[0][i].x*lines[1][i].x + - points[0][i].y*lines[1][i].y + lines[1][i].z) - + fabs(points[1][i].x*lines[0][i].x + - points[1][i].y*lines[0][i].y + lines[0][i].z); - avgErr += err; + int npt = (int)imagePoints[0][i].size(); + Mat imgpt[2]; + for( k = 0; k < 2; k++ ) + { + imgpt[k] = Mat(imagePoints[k][i]); + undistortPoints(imgpt[k], imgpt[k], cameraMatrix[k], distCoeffs[k], Mat(), cameraMatrix[k]); + computeCorrespondEpilines(imgpt[k], k+1, F, lines[k]); + } + for( j = 0; j < npt; j++ ) + { + double errij = fabs(imagePoints[0][i][j].x*lines[1][j][0] + + imagePoints[0][i][j].y*lines[1][j][1] + lines[1][j][2]) + + fabs(imagePoints[1][i][j].x*lines[0][j][0] + + imagePoints[1][i][j].y*lines[0][j][1] + lines[0][j][2]); + err += errij; + } + npoints += npt; } - printf( "avg err = %g\n", avgErr/(nframes*n) ); + cout << "average reprojection err = " << err/npoints << endl; // save intrinsic parameters - CvFileStorage* fstorage = cvOpenFileStorage("intrinsics.yml", NULL, CV_STORAGE_WRITE); - cvWrite(fstorage, "M1", &_M1); - cvWrite(fstorage, "D1", &_D1); - cvWrite(fstorage, "M2", &_M2); - cvWrite(fstorage, "D2", &_D2); - cvReleaseFileStorage(&fstorage); + FileStorage fs("intrinsics.yml", CV_STORAGE_WRITE); + if( fs.isOpened() ) + { + fs << "M1" << cameraMatrix[0] << "D1" << distCoeffs[0] << + "M2" << cameraMatrix[1] << "D2" << distCoeffs[1]; + fs.release(); + } + else + cout << "Error: can not save the intrinsic parameters\n"; -//COMPUTE AND DISPLAY RECTIFICATION - if( showUndistorted ) + Mat R1, R2, P1, P2, Q; + Rect roi1, roi2; + + stereoRectify(cameraMatrix[0], distCoeffs[0], + cameraMatrix[1], distCoeffs[1], + imageSize, R, T, R1, R2, P1, P2, Q, + 1, imageSize, &roi1, &roi2); + + fs.open("extrinsics.yml", CV_STORAGE_WRITE); + if( fs.isOpened() ) { - CvMat* mx1 = cvCreateMat( imageSize.height, - imageSize.width, CV_32F ); - CvMat* my1 = cvCreateMat( imageSize.height, - imageSize.width, CV_32F ); - CvMat* mx2 = cvCreateMat( imageSize.height, - imageSize.width, CV_32F ); - CvMat* my2 = cvCreateMat( imageSize.height, - imageSize.width, CV_32F ); - CvMat* img1r = cvCreateMat( imageSize.height, - imageSize.width, CV_8U ); - CvMat* img2r = cvCreateMat( imageSize.height, - imageSize.width, CV_8U ); - CvMat* disp = cvCreateMat( imageSize.height, - imageSize.width, CV_16S ); - double R1[3][3], R2[3][3], P1[3][4], P2[3][4]; - CvMat _R1 = cvMat(3, 3, CV_64F, R1); - CvMat _R2 = cvMat(3, 3, CV_64F, R2); + fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1" << P1 << "P2" << P2 << "Q" << Q; + fs.release(); + } + else + cout << "Error: can not save the intrinsic parameters\n"; + + // OpenCV can handle left-right + // or up-down camera arrangements + bool isVerticalStereo = fabs(P2.at(1, 3)) > fabs(P2.at(0, 3)); + +//COMPUTE AND DISPLAY RECTIFICATION + if( !showRectified ) + return; + + Mat rmap[2][2]; // IF BY CALIBRATED (BOUGUET'S METHOD) - if( useUncalibrated == 0 ) - { - CvMat _P1 = cvMat(3, 4, CV_64F, P1); - CvMat _P2 = cvMat(3, 4, CV_64F, P2); - - cvStereoRectify( &_M1, &_M2, &_D1, &_D2, imageSize, - &matR, &matT, - &_R1, &_R2, &_P1, &_P2, &matQ, - CV_CALIB_ZERO_DISPARITY, - 1, imageSize, &roi1, &roi2); - - CvFileStorage* file = cvOpenFileStorage("extrinsics.yml", NULL, CV_STORAGE_WRITE); - cvWrite(file, "R", &matR); - cvWrite(file, "T", &matT); - cvWrite(file, "R1", &_R1); - cvWrite(file, "R2", &_R2); - cvWrite(file, "P1", &_P1); - cvWrite(file, "P2", &_P2); - cvWrite(file, "Q", &matQ); - cvReleaseFileStorage(&file); - - isVerticalStereo = fabs(P2[1][3]) > fabs(P2[0][3]); - if(!isVerticalStereo) - roi2.x += imageSize.width; - else - roi2.y += imageSize.height; - //Precompute maps for cvRemap() - cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_P1,mx1,my1); - cvInitUndistortRectifyMap(&_M2,&_D2,&_R2,&_P2,mx2,my2); - } + if( !useCalibrated ) + { + // we already computed everything + } //OR ELSE HARTLEY'S METHOD - else if( useUncalibrated == 1 || useUncalibrated == 2 ) - // use intrinsic parameters of each camera, but - // compute the rectification transformation directly - // from the fundamental matrix + else + // use intrinsic parameters of each camera, but + // compute the rectification transformation directly + // from the fundamental matrix + { + vector allimgpt[2]; + for( k = 0; k < 2; k++ ) { - double H1[3][3], H2[3][3], iM[3][3]; - CvMat _H1 = cvMat(3, 3, CV_64F, H1); - CvMat _H2 = cvMat(3, 3, CV_64F, H2); - CvMat _iM = cvMat(3, 3, CV_64F, iM); - //Just to show you could have independently used F - if( useUncalibrated == 2 ) - cvFindFundamentalMat( &_imagePoints1, - &_imagePoints2, &matF); - cvStereoRectifyUncalibrated( &_imagePoints1, - &_imagePoints2, &matF, - imageSize, - &_H1, &_H2, 3); - cvInvert(&_M1, &_iM); - cvMatMul(&_H1, &_M1, &_R1); - cvMatMul(&_iM, &_R1, &_R1); - cvInvert(&_M2, &_iM); - cvMatMul(&_H2, &_M2, &_R2); - cvMatMul(&_iM, &_R2, &_R2); - //Precompute map for cvRemap() - cvInitUndistortRectifyMap(&_M1,&_D1,&_R1,&_M1,mx1,my1); - - cvInitUndistortRectifyMap(&_M2,&_D1,&_R2,&_M2,mx2,my2); + for( i = 0; i < nimages; i++ ) + std::copy(imagePoints[k][i].begin(), imagePoints[k][i].end(), back_inserter(allimgpt[k])); } - else - assert(0); - + F = findFundamentalMat(Mat(allimgpt[0]), Mat(allimgpt[1]), FM_8POINT, 0, 0); + Mat H1, H2; + stereoRectifyUncalibrated(Mat(allimgpt[0]), Mat(allimgpt[1]), F, imageSize, H1, H2, 3); - cvReleaseMat( &mx1 ); - cvReleaseMat( &my1 ); - cvReleaseMat( &mx2 ); - cvReleaseMat( &my2 ); - cvReleaseMat( &img1r ); - cvReleaseMat( &img2r ); - cvReleaseMat( &disp ); + R1 = cameraMatrix[0].inv()*H1*cameraMatrix[0]; + R2 = cameraMatrix[1].inv()*H2*cameraMatrix[1]; } + + //Precompute maps for cv::remap() + initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R1, P1, imageSize, CV_16SC2, rmap[0][0], rmap[0][1]); + initUndistortRectifyMap(cameraMatrix[1], distCoeffs[1], R2, P2, imageSize, CV_16SC2, rmap[1][0], rmap[1][1]); + + /*for( i = 0; i < nimages; i++ ) + { + Mat img = + + }*/ +} + + +static bool readStringList( const string& filename, vector& l ) +{ + l.resize(0); + FileStorage fs(filename, FileStorage::READ); + if( !fs.isOpened() ) + return false; + FileNode n = fs.getFirstTopLevelNode(); + if( n.type() != FileNode::SEQ ) + return false; + FileNodeIterator it = n.begin(), it_end = n.end(); + for( ; it != it_end; ++it ) + l.push_back((string)*it); + return true; +} + + +int print_help() +{ + cout << "Usage:\n ./stereo_calib -w board_width -h board_height \n"; + return 0; } + int main(int argc, char** argv) { - if(argc > 1 && !strcmp(argv[1], "--help")) + Size boardSize; + string imagelistfn; + + for( int i = 1; i < argc; i++ ) { - printf("Usage:\n ./stereo_calib \n"); - return 0; - } - - StereoCalib(argc > 1 ? argv[1] : ".", argc > 2 ? argv[2] : "stereo_calib.txt", 0); + if( string(argv[i]) == "-w" ) + sscanf(argv[++i], "%d", &boardSize.width); + else if( string(argv[i]) == "-h" ) + sscanf(argv[++i], "%d", &boardSize.height); + else if( string(argv[i]) == "--help" ) + return print_help(); + else if( argv[i][0] == '-' ) + { + cout << "invalid option " << argv[i] << endl; + return 0; + } + else + imagelistfn = argv[i]; + } + + if( imagelistfn == "" ) + { + imagelistfn = "stereo_calib.xml"; + boardSize = Size(9, 6); + } + + vector imagelist; + bool ok = readStringList(imagelistfn, imagelist); + + if( !ok || imagelist.empty() || boardSize.width <= 0 || boardSize.height <= 0 ) + return print_help(); + + StereoCalib(imagelist, boardSize); return 0; } -- 2.7.4