From: Vadim Pisarevsky Date: Sun, 13 Jun 2010 19:08:24 +0000 (+0000) Subject: improved the calibration and select3dobj samples X-Git-Tag: accepted/tizen/6.0/unified/20201030.111113~9069 X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=b2244f87acc1d3ab4d74746c22cce6500dd34548;p=platform%2Fupstream%2Fopencv.git improved the calibration and select3dobj samples --- diff --git a/samples/cpp/calibration.cpp b/samples/cpp/calibration.cpp index b929fbf..f8d71e7 100644 --- a/samples/cpp/calibration.cpp +++ b/samples/cpp/calibration.cpp @@ -9,21 +9,28 @@ using namespace cv; using namespace std; -// example command line (for copy-n-paste): -// calibration -w 6 -h 8 -s 2 -n 10 -o camera.yml -op -oe [] - -/* The list of views may look as following (discard the starting and ending ------ separators): -------------------- -view000.png -view001.png -#view002.png -view003.png -view010.png -one_extra_view.jpg -------------------- -that is, the file will contain 6 lines, view002.png will not be used for calibration, -other ones will be (those, in which the chessboard pattern will be found) -*/ +/* + example command line (for copy-n-paste): + calibration -w 6 -h 8 -s 2 -o camera.yml -op -oe image_list.xml + + where image_list.xml is the standard OpenCV XML/YAML + file consisting of the list of strings, e.g.: + + + + +"view000.png" +"view001.png" + +"view003.png" +"view010.png" +"one_extra_view.jpg" + + + + you can also use a video file or live camera input to calibrate the camera + + */ enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 }; @@ -170,24 +177,18 @@ void saveCameraParams( const string& filename, } } -bool readStringList( const string& filename, vector& l ) +static bool readStringList( const string& filename, vector& l ) { l.resize(0); - FILE* f = fopen(filename.c_str(), "rt"); - if(!f) + FileStorage fs(filename, FileStorage::READ); + if( !fs.isOpened() ) return false; - - for(;;) - { - char buf[1000]; - if( !fgets( buf, sizeof(buf)-2, f )) - break; - char* ptr = strchr(buf, '\n'); - if( ptr ) *ptr = '\0'; - if( buf[0] != '\0' && buf[0] != '#' ) - l.push_back(string(buf)); - } - fclose(f); + 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; } @@ -347,13 +348,10 @@ int main( int argc, char** argv ) if( inputFilename ) { - capture.open(inputFilename); - if( !capture.isOpened() && !readStringList(inputFilename, imageList) ) - { - fprintf( stderr, "The input file could not be opened\n" ); - return -1; - } - mode = CAPTURING; + if( readStringList(inputFilename, imageList) ) + mode = CAPTURING; + else + capture.open(inputFilename); } else capture.open(cameraId); @@ -461,6 +459,8 @@ int main( int argc, char** argv ) mode = CALIBRATED; else mode = DETECTION; + if( !capture.isOpened() ) + break; } } return 0; diff --git a/samples/cpp/select3dobj.cpp b/samples/cpp/select3dobj.cpp index ab5c5ee..38fb1d0 100644 --- a/samples/cpp/select3dobj.cpp +++ b/samples/cpp/select3dobj.cpp @@ -9,60 +9,53 @@ using namespace cv; -static void print_help() +struct MouseEvent { - printf("Usage: select3dobj -w [-s ]\n" - "\t-i -o [video_filename/cameraId]\n"); -} - -struct CameraData -{ - Size imageSize; - Size boardSize; - double squareSize; - Mat distCoeffs; - Mat cameraMatrix; - vector objPoints; + MouseEvent() { event = -1; buttonState = 0; } + Point pt; + int event; + int buttonState; }; -Point mouseLoc; -int mouseEvent = -1; -int mouseButtonState = 0; - -static void onMouse(int event, int x, int y, int flags, void*) +static void onMouse(int event, int x, int y, int flags, void* userdata) { - mouseEvent = event; - mouseLoc = Point(x,y); - mouseButtonState = flags; + MouseEvent* data = (MouseEvent*)userdata; + data->event = event; + data->pt = Point(x,y); + data->buttonState = flags; } -static bool readCameraMatrix(const string& filename, CameraData& calibrated) +static bool readCameraMatrix(const string& filename, + Mat& cameraMatrix, Mat& distCoeffs, + Size& calibratedImageSize ) { FileStorage fs(filename, FileStorage::READ); - fs["image_width"] >> calibrated.imageSize.width; - fs["image_height"] >> calibrated.imageSize.height; - fs["board_width"] >> calibrated.boardSize.width; - fs["board_height"] >> calibrated.boardSize.height; - fs["square_size"] >> calibrated.squareSize; - fs["distortion_coefficients"] >> calibrated.distCoeffs; - if( calibrated.distCoeffs.type() != CV_64F ) - calibrated.distCoeffs = Mat_(calibrated.distCoeffs); - if( calibrated.cameraMatrix.type() != CV_64F ) - calibrated.cameraMatrix = Mat_(calibrated.cameraMatrix); - - fs["camera_matrix"] >> calibrated.cameraMatrix; + fs["image_width"] >> calibratedImageSize.width; + fs["image_height"] >> calibratedImageSize.height; + fs["distortion_coefficients"] >> distCoeffs; + fs["camera_matrix"] >> cameraMatrix; - calibrated.objPoints.resize(0); + if( distCoeffs.type() != CV_64F ) + distCoeffs = Mat_(distCoeffs); + if( cameraMatrix.type() != CV_64F ) + cameraMatrix = Mat_(cameraMatrix); - for( int i = 0; i < calibrated.boardSize.height; i++ ) - for( int j = 0; j < calibrated.boardSize.width; j++ ) - calibrated.objPoints.push_back( - Point3f(float(j*calibrated.squareSize), - float(i*calibrated.squareSize), 0)); return true; } -static Point3f image2plane(Point2f imgpt, const Mat& R, const Mat& tvec, const Mat& cameraMatrix, double Z) +static void calcChessboardCorners(Size boardSize, float squareSize, vector& corners) +{ + corners.resize(0); + + for( int i = 0; i < boardSize.height; i++ ) + for( int j = 0; j < boardSize.width; j++ ) + corners.push_back(Point3f(float(j*squareSize), + float(i*squareSize), 0)); +} + + +static Point3f image2plane(Point2f imgpt, const Mat& R, const Mat& tvec, + const Mat& cameraMatrix, double Z) { Mat R1 = R.clone(); R1.col(2) = R1.col(2)*Z + tvec; @@ -71,27 +64,271 @@ static Point3f image2plane(Point2f imgpt, const Mat& R, const Mat& tvec, const M return Point3f(v(0,0)*iw, v(1,0)*iw, Z); } -int main(int argc, char** argv) + +static Rect extract3DBox(const Mat& frame, Mat& shownFrame, Mat& selectedObjFrame, + const Mat& cameraMatrix, const Mat& rvec, const Mat& tvec, + const vector& box, int nobjpt) +{ + selectedObjFrame = Mat::zeros(frame.size(), frame.type()); + if( nobjpt == 0 ) + return Rect(); + vector objpt; + vector imgpt; + + objpt.push_back(box[0]); + if( nobjpt > 1 ) + objpt.push_back(box[1]); + if( nobjpt > 2 ) + { + objpt.push_back(box[2]); + objpt.push_back(objpt[2] - objpt[1] + objpt[0]); + } + if( nobjpt > 3 ) + for( int i = 0; i < 4; i++ ) + objpt.push_back(Point3f(objpt[i].x, objpt[i].y, box[3].z)); + + projectPoints(Mat(objpt), rvec, tvec, cameraMatrix, Mat(), imgpt); + + if( shownFrame.data ) + { + if( nobjpt == 1 ) + circle(shownFrame, imgpt[0], 3, Scalar(0,255,0), -1, CV_AA); + else if( nobjpt == 2 ) + { + circle(shownFrame, imgpt[0], 3, Scalar(0,255,0), -1, CV_AA); + circle(shownFrame, imgpt[1], 3, Scalar(0,255,0), -1, CV_AA); + line(shownFrame, imgpt[0], imgpt[1], Scalar(0,255,0), 3, CV_AA); + } + else if( nobjpt == 3 ) + for( int i = 0; i < 4; i++ ) + { + circle(shownFrame, imgpt[i], 3, Scalar(0,255,0), -1, CV_AA); + line(shownFrame, imgpt[i], imgpt[(i+1)%4], Scalar(0,255,0), 3, CV_AA); + } + else + for( int i = 0; i < 8; i++ ) + { + circle(shownFrame, imgpt[i], 3, Scalar(0,255,0), -1, CV_AA); + line(shownFrame, imgpt[i], imgpt[(i+1)%4 + (i/4)*4], Scalar(0,255,0), 3, CV_AA); + line(shownFrame, imgpt[i], imgpt[i%4], Scalar(0,255,0), 3, CV_AA); + } + } + + if( nobjpt <= 2 ) + return Rect(); + vector hull; + convexHull(Mat_(Mat(imgpt)), hull); + Mat selectedObjMask = Mat::zeros(frame.size(), CV_8U); + fillConvexPoly(selectedObjMask, &hull[0], hull.size(), Scalar::all(255), 8, 0); + frame.copyTo(selectedObjFrame, selectedObjMask); + return boundingRect(Mat(hull)) & Rect(Point(), frame.size()); +} + + +static int select3DBox(const string& windowname, const string& selWinName, const Mat& frame, + const Mat& cameraMatrix, const Mat& rvec, const Mat& tvec, + vector& box) { - const char* imgFilename = 0;//"frame.jpg"; const float eps = 1e-3f; + MouseEvent mouse; + + setMouseCallback(windowname, onMouse, &mouse); + vector tempobj(8); + vector imgpt(4), tempimg(8); + vector temphull; + int nobjpt = 0; + Mat R, selectedObjMask, selectedObjFrame, shownFrame; + Rodrigues(rvec, R); + box.resize(4); + + for(;;) + { + float Z = 0.f; + bool dragging = (mouse.buttonState & CV_EVENT_FLAG_LBUTTON) != 0; + int npt = nobjpt; + + if( (mouse.event == CV_EVENT_LBUTTONDOWN || + mouse.event == CV_EVENT_LBUTTONUP || + dragging) && nobjpt < 4 ) + { + Point2f m = mouse.pt; + + if( nobjpt < 2 ) + imgpt[npt] = m; + else + { + tempobj.resize(1); + int nearestIdx = npt-1; + if( nobjpt == 3 ) + { + nearestIdx = 0; + for( int i = 1; i < npt; i++ ) + if( norm(m - imgpt[i]) < norm(m - imgpt[nearestIdx]) ) + nearestIdx = i; + } + + if( npt == 2 ) + { + float dx = box[1].x - box[0].x, dy = box[1].y - box[0].y; + float len = 1.f/std::sqrt(dx*dx+dy*dy); + tempobj[0] = Point3f(dy*len + box[nearestIdx].x, + -dx*len + box[nearestIdx].y, 0.f); + } + else + tempobj[0] = Point3f(box[nearestIdx].x, box[nearestIdx].y, 1.f); + + projectPoints(Mat(tempobj), rvec, tvec, cameraMatrix, Mat(), tempimg); + + Point2f a = imgpt[nearestIdx], b = tempimg[0], d1 = b - a, d2 = m - a; + float n1 = norm(d1), n2 = norm(d2); + if( n1*n2 < eps ) + imgpt[npt] = a; + else + { + Z = d1.dot(d2)/(n1*n1); + imgpt[npt] = d1*Z + a; + } + } + box[npt] = image2plane(imgpt[npt], R, tvec, cameraMatrix, npt<3 ? 0 : Z); + + if( (npt == 0 && mouse.event == CV_EVENT_LBUTTONDOWN) || + (npt > 0 && norm(box[npt] - box[npt-1]) > eps && + mouse.event == CV_EVENT_LBUTTONUP) ) + { + nobjpt++; + if( nobjpt < 4 ) + { + imgpt[nobjpt] = imgpt[nobjpt-1]; + box[nobjpt] = box[nobjpt-1]; + } + } + + // reset the event + mouse.event = -1; + //mouse.buttonState = 0; + npt++; + } + + frame.copyTo(shownFrame); + extract3DBox(frame, shownFrame, selectedObjFrame, + cameraMatrix, rvec, tvec, box, npt); + imshow(windowname, shownFrame); + imshow(selWinName, selectedObjFrame); + + int c = waitKey(30); + if( (c & 255) == 27 ) + { + nobjpt = 0; + } + if( c == 'q' || c == 'Q' || c == ' ' ) + { + box.clear(); + return c == ' ' ? -1 : -100; + } + if( c == '\r' || c == '\n' && nobjpt == 4 && box[3].z > 0 ) + return 1; + } +} + + +static bool readModelViews( const string& filename, vector& box, + vector& imagelist, + vector& roiList, vector& poseList ) +{ + imagelist.resize(0); + roiList.resize(0); + poseList.resize(0); + box.resize(0); + + FileStorage fs(filename, FileStorage::READ); + if( !fs.isOpened() ) + return false; + fs["box"] >> box; + + FileNode all = fs["views"]; + if( all.type() != FileNode::SEQ ) + return false; + FileNodeIterator it = all.begin(), it_end = all.end(); + + for(; it != it_end; ++it) + { + FileNode n = *it; + imagelist.push_back((string)n["image"]); + FileNode nr = n["rect"]; + roiList.push_back(Rect((int)nr[0], (int)nr[1], (int)nr[2], (int)nr[3])); + FileNode np = n["pose"]; + poseList.push_back(Vec6f((float)np[0], (float)np[1], (float)np[2], + (float)np[3], (float)np[4], (float)np[5])); + } + + return true; +} + + +static bool writeModelViews(const string& filename, const vector& box, + const vector& imagelist, + const vector& roiList, + const vector& poseList) +{ + FileStorage fs(filename, FileStorage::WRITE); + if( !fs.isOpened() ) + return false; + + fs << "box" << "[:"; + fs << box << "]" << "views" << "["; + + size_t i, nviews = imagelist.size(); + + CV_Assert( nviews == roiList.size() && nviews == poseList.size() ); + + for( i = 0; i < nviews; i++ ) + { + Rect r = roiList[i]; + Vec6f p = poseList[i]; + + fs << "{" << "image" << imagelist[i] << + "roi" << "[:" << r.x << r.y << r.width << r.height << "]" << + "pose" << "[:" << p[0] << p[1] << p[2] << p[3] << p[4] << p[5] << "]" << "}"; + } + fs << "]"; + + return true; +} + + +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 main(int argc, char** argv) +{ + const char* help = "Usage: select3dobj -w [-s ]\n" + "\t-i -o [video_filename/cameraId]\n"; if(argc < 5) { - print_help(); + puts(help); return 0; } const char* intrinsicsFilename = 0; const char* outprefix = 0; - const char* videoFilename = 0; + const char* inputName = 0; int cameraId = 0; Size boardSize; - double squareSize = 0; - bool paused = false; - vector objpts(4); - vector imgpts(4); - vector mousepts(4); - int nobjpt = 0; + double squareSize = 1; + vector imageList; for( int i = 1; i < argc; i++ ) { @@ -104,7 +341,7 @@ int main(int argc, char** argv) if(sscanf(argv[++i], "%d", &boardSize.width) != 1 || boardSize.width <= 0) { printf("Incorrect -w parameter (must be a positive integer)\n"); - print_help(); + puts(help); return 0; } } @@ -113,7 +350,7 @@ int main(int argc, char** argv) if(sscanf(argv[++i], "%d", &boardSize.height) != 1 || boardSize.height <= 0) { printf("Incorrect -h parameter (must be a positive integer)\n"); - print_help(); + puts(help); return 0; } } @@ -122,7 +359,7 @@ int main(int argc, char** argv) if(sscanf(argv[++i], "%lf", &squareSize) != 1 || squareSize <= 0) { printf("Incorrect -w parameter (must be a positive real number)\n"); - print_help(); + puts(help); return 0; } } @@ -131,45 +368,43 @@ int main(int argc, char** argv) if( isdigit(argv[i][0])) sscanf(argv[i], "%d", &cameraId); else - videoFilename = argv[i]; + inputName = argv[i]; } else { printf("Incorrect option\n"); - print_help(); + puts(help); return 0; } } if( !intrinsicsFilename || !outprefix || - boardSize.width <= 0 || boardSize.height <= 0 || - squareSize <= 0 ) + boardSize.width <= 0 || boardSize.height <= 0 ) { - printf("One of required parameters are missing\n"); - print_help(); + printf("Some of the required parameters are missing\n"); + puts(help); return 0; } - CameraData calibrated; - readCameraMatrix(intrinsicsFilename, calibrated); - calibrated.boardSize = boardSize; - calibrated.squareSize = squareSize; + Mat cameraMatrix, distCoeffs; + Size calibratedImageSize; + readCameraMatrix(intrinsicsFilename, cameraMatrix, distCoeffs, calibratedImageSize ); - VideoCapture cap; - if( !imgFilename ) + VideoCapture capture; + if( inputName ) { - if( videoFilename ) - cap.open(videoFilename); - else - cap.open(0); - - if( !cap.isOpened() ) + if( !readStringList(inputName, imageList) && + !capture.open(inputName)) { - printf("Can not initialize video capture\n"); - print_help(); - return 0; + fprintf( stderr, "The input file could not be opened\n" ); + return -1; } } + else + capture.open(cameraId); + + if( !capture.isOpened() && imageList.empty() ) + return fprintf( stderr, "Could not initialize video capture\n" ), -2; const char* outbarename = 0; { @@ -189,230 +424,124 @@ int main(int argc, char** argv) outbarename = outprefix; } - Mat frame0, frame, shownFrame, selectedObjMask, selectedObjFrame, mapxy, R, rvec, tvec; - vector boardCorners; - vector tempobj(8); - vector tempimg(8); - vector temphull(8); + Mat frame, shownFrame, selectedObjFrame, mapxy; - namedWindow("Video", 1); + namedWindow("View", 1); namedWindow("Selected Object", 1); - setMouseCallback("Video", onMouse, 0); + setMouseCallback("View", onMouse, 0); bool boardFound = false; - char path[1000]; - sprintf(path, "%s_index.txt", outprefix); - FILE* fframes = fopen(path, "a+t"); - if(!fframes) - { - printf("Can not open path for writing. Permission denied?\n"); - return 0; - } + string indexFilename = format("%s_index.yml", outprefix); + + vector capturedImgList; + vector roiList; + vector poseList; + vector box, boardPoints; + + readModelViews(indexFilename, box, capturedImgList, roiList, poseList); + calcChessboardCorners(boardSize, squareSize, boardPoints); int frameIdx = 0; + bool grabNext = !imageList.empty(); - for(;;) + for(int i = 0;;i++) { - bool objselected = false; - int nOutlinePt = 0; - - if( !paused ) - { - if( imgFilename ) - { - frame0 = imread(string(imgFilename), 1); - paused = true; - } - else - cap >> frame0; - if( !frame0.data ) - break; - if( !frame.data ) - { - if( frame0.size() != calibrated.imageSize ) - { - // adjust the camera matrix for the new resolution - calibrated.cameraMatrix.at(0,0) *= frame.cols/calibrated.imageSize.width; - calibrated.cameraMatrix.at(0,2) *= frame.cols/calibrated.imageSize.width; - calibrated.cameraMatrix.at(1,1) *= frame.rows/calibrated.imageSize.height; - calibrated.cameraMatrix.at(1,2) *= frame.rows/calibrated.imageSize.height; - calibrated.imageSize = frame0.size(); - } - Mat dummy; - // initialize undistortion maps - initUndistortRectifyMap(calibrated.cameraMatrix, calibrated.distCoeffs, Mat(), - calibrated.cameraMatrix, calibrated.imageSize, - CV_32FC2, mapxy, dummy ); - calibrated.distCoeffs = Mat::zeros(5, 1, CV_64F); - selectedObjMask = Mat::zeros(frame0.size(), CV_8U); - selectedObjFrame = frame0.clone(); - } - remap(frame0, frame, mapxy, Mat(), INTER_LINEAR); - boardFound = findChessboardCorners(frame, calibrated.boardSize, boardCorners); - - if( boardFound ) + Mat frame0; + if( !imageList.empty() ) + { + if( i < (int)imageList.size() ) + frame0 = imread(string(imageList[i]), 1); + } + else + capture >> frame0; + if( !frame0.data ) + break; + if( !frame.data ) + { + if( frame0.size() != calibratedImageSize ) { - solvePnP(Mat(calibrated.objPoints), Mat(boardCorners), calibrated.cameraMatrix, - calibrated.distCoeffs, rvec, tvec, false); - Rodrigues(rvec, R); + double sx = (double)frame0.cols/calibratedImageSize.width; + double sy = (double)frame0.rows/calibratedImageSize.height; + + // adjust the camera matrix for the new resolution + cameraMatrix.at(0,0) *= sx; + cameraMatrix.at(0,2) *= sx; + cameraMatrix.at(1,1) *= sy; + cameraMatrix.at(1,2) *= sy; } - } + Mat dummy; + initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(), + cameraMatrix, frame0.size(), + CV_32FC2, mapxy, dummy ); + distCoeffs = Mat::zeros(5, 1, CV_64F); + } + remap(frame0, frame, mapxy, Mat(), INTER_LINEAR); + vector foundBoardCorners; + boardFound = findChessboardCorners(frame, boardSize, foundBoardCorners); + + Mat rvec, tvec; + if( boardFound ) + solvePnP(Mat(boardPoints), Mat(foundBoardCorners), cameraMatrix, + distCoeffs, rvec, tvec, false); + frame.copyTo(shownFrame); - selectedObjFrame = Scalar::all(0); + drawChessboardCorners(shownFrame, boardSize, Mat(foundBoardCorners), boardFound); + selectedObjFrame = Mat::zeros(frame.size(), frame.type()); - if( boardFound ) + if( boardFound && grabNext ) { - float Z = 0.f; - bool dragging = (mouseButtonState & CV_EVENT_FLAG_LBUTTON) != 0; - int npt = nobjpt; - - drawChessboardCorners(shownFrame, calibrated.boardSize, Mat(boardCorners), true); - - if( (mouseEvent == CV_EVENT_LBUTTONDOWN || - mouseEvent == CV_EVENT_LBUTTONUP || - dragging) && nobjpt < 4 ) + if( box.empty() ) { - // update object box - mousepts[npt] = mouseLoc; - - /*if(!paused) - imwrite("frame.jpg", frame0);*/ - paused = true; - if( nobjpt < 2 ) - imgpts[npt] = mousepts[npt]; - else + int code = select3DBox("View", "Selected Object", frame, + cameraMatrix, rvec, tvec, box); + if( code == -100 ) + break; + } + + if( !box.empty() ) + { + Rect r = extract3DBox(frame, shownFrame, selectedObjFrame, + cameraMatrix, rvec, tvec, box, 4); + if( r.area() ) { - tempobj.resize(1); - int nearestIdx = npt-1; - /*for( int i = 1; i < npt; i++ ) - if( norm(mousepts[npt] - mousepts[i]) < norm(mousepts[npt] - imgpts[nearestIdx]) ) - nearestIdx = i;*/ - - if( npt == 2 ) - { - float dx = objpts[1].x - objpts[0].x, dy = objpts[1].y - objpts[0].y; - float len = 1.f/std::sqrt(dx*dx+dy*dy); - tempobj[0] = Point3f(dy*len + objpts[nearestIdx].x, -dx*len + objpts[nearestIdx].y, 0.f); - } - else - tempobj[0] = Point3f(objpts[nearestIdx].x, objpts[nearestIdx].y, 1.f); - - projectPoints(Mat(tempobj), rvec, tvec, calibrated.cameraMatrix, - calibrated.distCoeffs, tempimg); - - Point2f a = mousepts[nearestIdx], b = tempimg[0], - m = mousepts[npt], d1 = b - a, d2 = m - a; - float n1 = norm(d1), n2 = norm(d2); - if( n1*n2 < eps ) - imgpts[npt] = a; - else + const int maxFrameIdx = 10000; + char path[1000]; + for(;frameIdx < maxFrameIdx;frameIdx++) { - Z = d1.dot(d2)/(n1*n1); - imgpts[npt] = d1*Z + a; + sprintf(path, "%s%04d.jpg", outprefix, frameIdx); + FILE* f = fopen(path, "rb"); + if( !f ) + break; + fclose(f); } - } - objpts[npt] = image2plane(imgpts[npt], R, tvec, - calibrated.cameraMatrix, npt<3 ? 0 : Z); - - if( (npt == 0 && mouseEvent == CV_EVENT_LBUTTONDOWN) || - (npt > 0 && norm(objpts[npt] - objpts[npt-1]) > eps && - mouseEvent == CV_EVENT_LBUTTONUP) ) - { - nobjpt++; - if( nobjpt < 4 ) + if( frameIdx == maxFrameIdx ) { - imgpts[nobjpt] = imgpts[nobjpt-1]; - objpts[nobjpt] = objpts[nobjpt-1]; - mousepts[nobjpt] = mousepts[nobjpt-1]; + printf("Can not save the image as %s<...>.jpg", outprefix); + break; } + imwrite(path, selectedObjFrame(r)); + + capturedImgList.push_back(string(path)); + roiList.push_back(r); + + float p[6]; + Mat RV(3, 1, CV_32F, p), TV(3, 1, CV_32F, p+3); + rvec.convertTo(RV, RV.type()); + tvec.convertTo(TV, TV.type()); + poseList.push_back(Vec6f(p[0], p[1], p[2], p[3], p[4], p[5])); } - - mouseEvent = -1; // reset the event - } - - // draw object box (or a part of it) - tempobj.resize(8); - tempobj[0] = objpts[0]; - tempobj[1] = objpts[1]; - tempobj[2] = objpts[2]; - tempobj[3] = (objpts[2] - objpts[1]) + objpts[0]; - Z = objpts[3].z; - tempobj[4] = tempobj[0] + Point3f(0,0,Z); - tempobj[5] = tempobj[1] + Point3f(0,0,Z); - tempobj[6] = tempobj[2] + Point3f(0,0,Z); - tempobj[7] = tempobj[3] + Point3f(0,0,Z); - - projectPoints(Mat(tempobj), rvec, tvec, calibrated.cameraMatrix, - calibrated.distCoeffs, tempimg); - - if( npt == 0 && nobjpt == 0 ) - nOutlinePt = 0; - else if( npt == 0 ) - { - nOutlinePt = 1; - circle(shownFrame, tempimg[0], 3, Scalar(0,255,0), -1, CV_AA); - } - else if( npt == 1 ) - { - nOutlinePt = 2; - line(shownFrame, tempimg[0], tempimg[1], Scalar(0,255,0), 3, CV_AA); - circle(shownFrame, tempimg[0], 3, Scalar(0,255,0), -1, CV_AA); - circle(shownFrame, tempimg[1], 3, Scalar(0,255,0), -1, CV_AA); - } - else - { - nOutlinePt = npt == 2 ? 4 : 8; - for( int i = 0; i < nOutlinePt; i++ ) - { - circle(shownFrame, tempimg[i], 3, Scalar(0,255,0), -1, CV_AA); - line(shownFrame, tempimg[i], tempimg[(i+1)%4 + (i/4)*4], Scalar(0,255,0), 3, CV_AA); - line(shownFrame, tempimg[i], tempimg[i%4], Scalar(0,255,0), 3, CV_AA); - } - } - - if( nOutlinePt > 2 ) - { - convexHull(Mat_(Mat(tempimg).rowRange(0,nOutlinePt)), temphull); - selectedObjMask = Scalar::all(0); - fillConvexPoly(selectedObjMask, &temphull[0], temphull.size(), - Scalar::all(255), 8, 0); - frame.copyTo(selectedObjFrame, selectedObjMask); - objselected = true; } + grabNext = !imageList.empty(); } - imshow("Video", shownFrame); + imshow("View", shownFrame); imshow("Selected Object", selectedObjFrame); - int c = waitKey(30); - if( (c & 255) == 27 ) - nobjpt = 0; - if( c == ' ' ) - paused = !paused; + int c = waitKey(imageList.empty() ? 30 : 300); if( c == 'q' || c == 'Q' ) break; - if( (c == '\r' || c == '\n') && objselected && nOutlinePt == 8 ) - { - Rect r = boundingRect(Mat(temphull)); - - for(;;frameIdx++) - { - sprintf(path, "%s%04d.jpg", outprefix, frameIdx); - FILE* f = fopen(path, "rb"); - if( !f ) - break; - fclose(f); - } - - imwrite(path, selectedObjFrame(r&Rect(0,0,selectedObjFrame.cols,selectedObjFrame.rows))); - fprintf(fframes, "%s%04d.jpg (%.4f %.4f %.4f) (%.4f %.4f %.4f)", outbarename, frameIdx, - rvec.at(0,0), rvec.at(1,0), rvec.at(2,0), - tvec.at(0,0), tvec.at(1,0), tvec.at(2,0)); - for( int i = 0; i < 8; i++ ) - fprintf(fframes, " (%.2f %.2f %.2f)", objpts[i].x, objpts[i].y, objpts[i].z); - fprintf(fframes, "\n"); - frameIdx++; - } + if( c == '\r' || c == '\n' ) + grabNext = true; } - fclose(fframes); + writeModelViews(indexFilename, box, capturedImgList, roiList, poseList); return 0; }