improved the calibration and select3dobj samples

samples/cpp/calibration.cpp

@@ -9,20 +9,27 @@
 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 [<list_of_views.txt>]
+ 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.:
+ 
+<?xml version="1.0"?>
+<opencv_storage>
+<images>
+"view000.png"
+"view001.png"
+<!-- view002.png -->
+"view003.png"
+"view010.png"
+"one_extra_view.jpg"
+</images>
+</opencv_storage>
+
+ you can also use a video file or live camera input to calibrate the camera
  
-/* 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)
  */
 
 enum { DETECTION = 0, CAPTURING = 1, CALIBRATED = 2 };
@@ -170,24 +177,18 @@ void saveCameraParams( const string& filename,
     }
 }
 
-bool readStringList( const string& filename, vector<string>& l )
+static bool readStringList( const string& filename, vector<string>& l )
 {
     l.resize(0);
-    FILE* f = fopen(filename.c_str(), "rt");
+    FileStorage fs(filename, FileStorage::READ);
-    if(!f)
+    if( !fs.isOpened() )
         return false;
-    
+    FileNode n = fs.getFirstTopLevelNode();
-    for(;;)
+    if( n.type() != FileNode::SEQ )
-    {
+        return false;
-        char buf[1000];
+    FileNodeIterator it = n.begin(), it_end = n.end();
-        if( !fgets( buf, sizeof(buf)-2, f ))
+    for( ; it != it_end; ++it )
-            break;
+        l.push_back((string)*it);
-        char* ptr = strchr(buf, '\n');
-        if( ptr ) *ptr = '\0';
-        if( buf[0] != '\0' && buf[0] != '#' )
-            l.push_back(string(buf));
-    }
-    fclose(f);
     return true;
 }
 
@@ -347,13 +348,10 @@ int main( int argc, char** argv )
 
     if( inputFilename )
     {
-        capture.open(inputFilename);
+        if( readStringList(inputFilename, imageList) )
-        if( !capture.isOpened() && !readStringList(inputFilename, imageList) )
-        {
-            fprintf( stderr, "The input file could not be opened\n" );
-            return -1;
-        }
             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;

samples/cpp/select3dobj.cpp

@@ -9,60 +9,53 @@
 
 using namespace cv;
 
-static void print_help()
+struct MouseEvent
 {
-    printf("Usage: select3dobj -w <board_width -h <board_height> [-s <square_size>]\n"
+    MouseEvent() { event = -1; buttonState = 0; }
-           "\t-i <intrinsics_filename> -o <output_prefix> [video_filename/cameraId]\n");
+    Point pt;
-}
+    int event;
-
+    int buttonState;
-struct CameraData
-{
-	Size imageSize;
-	Size boardSize;
-	double squareSize;
-	Mat distCoeffs;
-	Mat cameraMatrix;
-    vector<Point3f> objPoints;
 };
 
-Point mouseLoc;
+static void onMouse(int event, int x, int y, int flags, void* userdata)
-int mouseEvent = -1;
-int mouseButtonState = 0;
-
-static void onMouse(int event, int x, int y, int flags, void*)
 {
-    mouseEvent = event;
+    MouseEvent* data = (MouseEvent*)userdata;
-    mouseLoc = Point(x,y);
+    data->event = event;
-    mouseButtonState = flags;
+    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_width"] >> calibratedImageSize.width;
-    fs["image_height"] >> calibrated.imageSize.height;
+    fs["image_height"] >> calibratedImageSize.height;
-    fs["board_width"] >> calibrated.boardSize.width;
+    fs["distortion_coefficients"] >> distCoeffs;
-    fs["board_height"] >> calibrated.boardSize.height;
+    fs["camera_matrix"] >> cameraMatrix;
-    fs["square_size"] >> calibrated.squareSize;
-    fs["distortion_coefficients"] >> calibrated.distCoeffs;
-    if( calibrated.distCoeffs.type() != CV_64F )
-        calibrated.distCoeffs = Mat_<double>(calibrated.distCoeffs);
-    if( calibrated.cameraMatrix.type() != CV_64F )
-        calibrated.cameraMatrix = Mat_<double>(calibrated.cameraMatrix);
     
-    fs["camera_matrix"] >> calibrated.cameraMatrix;
+    if( distCoeffs.type() != CV_64F )
+        distCoeffs = Mat_<double>(distCoeffs);
+    if( cameraMatrix.type() != CV_64F )
+        cameraMatrix = Mat_<double>(cameraMatrix);
     
-    calibrated.objPoints.resize(0);
-    
-    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<Point3f>& 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);
 }
 
+
+static Rect extract3DBox(const Mat& frame, Mat& shownFrame, Mat& selectedObjFrame, 
+                         const Mat& cameraMatrix, const Mat& rvec, const Mat& tvec,
+                         const vector<Point3f>& box, int nobjpt)
+{
+    selectedObjFrame = Mat::zeros(frame.size(), frame.type());
+    if( nobjpt == 0 )
+        return Rect();
+    vector<Point3f> objpt;
+    vector<Point2f> 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<Point> hull;
+    convexHull(Mat_<Point>(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<Point3f>& box)
+{
+    const float eps = 1e-3f;
+    MouseEvent mouse;
+    
+    setMouseCallback(windowname, onMouse, &mouse);
+    vector<Point3f> tempobj(8);
+    vector<Point2f> imgpt(4), tempimg(8);
+    vector<Point> 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<Point3f>& box,
+                            vector<string>& imagelist,
+                            vector<Rect>& roiList, vector<Vec6f>& 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<Point3f>& box,
+                            const vector<string>& imagelist,
+                            const vector<Rect>& roiList,
+                            const vector<Vec6f>& 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<string>& 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* imgFilename = 0;//"frame.jpg";
+    const char* help = "Usage: select3dobj -w <board_width -h <board_height> [-s <square_size>]\n"
-    const float eps = 1e-3f;
+           "\t-i <intrinsics_filename> -o <output_prefix> [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;
+	double squareSize = 1;
-	bool paused = false;
+    vector<string> imageList;
-    vector<Point3f> objpts(4);
-    vector<Point2f> imgpts(4);
-    vector<Point2f> mousepts(4);
-    int nobjpt = 0;
     
     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 ||
+		boardSize.width <= 0 || boardSize.height <= 0 )
-		squareSize <= 0 )
 	{
-		printf("One of required parameters are missing\n");
+		printf("Some of the required parameters are missing\n");
-		print_help();
+		puts(help);
 		return 0;
 	}
 	
-	CameraData calibrated;
+    Mat cameraMatrix, distCoeffs;
-    readCameraMatrix(intrinsicsFilename, calibrated);
+    Size calibratedImageSize;
-	calibrated.boardSize = boardSize;
+    readCameraMatrix(intrinsicsFilename, cameraMatrix, distCoeffs, calibratedImageSize );
-	calibrated.squareSize = squareSize;
     
-	VideoCapture cap;
+	VideoCapture capture;
-    if( !imgFilename )
+    if( inputName )
     {
-        if( videoFilename )
+        if( !readStringList(inputName, imageList) &&
-            cap.open(videoFilename);
+            !capture.open(inputName))
+        {
+            fprintf( stderr, "The input file could not be opened\n" );
+            return -1;
+        }
+    }
     else
-            cap.open(0);
+        capture.open(cameraId);
     
-        if( !cap.isOpened() )
+    if( !capture.isOpened() && imageList.empty() )
-        {
+        return fprintf( stderr, "Could not initialize video capture\n" ), -2;
-            printf("Can not initialize video capture\n");
-            print_help();
-            return 0;
-        }
-    }
     
     const char* outbarename = 0;
     {
@@ -189,211 +424,88 @@ int main(int argc, char** argv)
             outbarename = outprefix;
     }
 	
-	Mat frame0, frame, shownFrame, selectedObjMask, selectedObjFrame, mapxy, R, rvec, tvec;
+	Mat frame, shownFrame, selectedObjFrame, mapxy;
-    vector<Point2f> boardCorners;
-    vector<Point3f> tempobj(8);
-    vector<Point2f> tempimg(8);
-    vector<Point> temphull(8);
     
-	namedWindow("Video", 1);
+	namedWindow("View", 1);
     namedWindow("Selected Object", 1);
-    setMouseCallback("Video", onMouse, 0);
+    setMouseCallback("View", onMouse, 0);
     bool boardFound = false;
     
-    char path[1000];
+    string indexFilename = format("%s_index.yml", outprefix);
-    sprintf(path, "%s_index.txt", outprefix);
+    
-    FILE* fframes = fopen(path, "a+t");
+    vector<string> capturedImgList;
-    if(!fframes)
+    vector<Rect> roiList;
-    {
+    vector<Vec6f> poseList;
-        printf("Can not open path for writing. Permission denied?\n");
+    vector<Point3f> box, boardPoints;
-        return 0;
+    
-    }
+    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;
+        Mat frame0;
-        int nOutlinePt = 0;
+        if( !imageList.empty() )
-        
-		if( !paused )
         {
-			if( imgFilename )
+            if( i < (int)imageList.size() )
-            {
+                frame0 = imread(string(imageList[i]), 1);
-                frame0 = imread(string(imgFilename), 1);
-                paused = true;
         }
         else
-                cap >> frame0;
+            capture >> frame0;
         if( !frame0.data )
             break;
         if( !frame.data )
         {
-                if( frame0.size() != calibrated.imageSize )
+            if( frame0.size() != calibratedImageSize )
             {
+                double sx = (double)frame0.cols/calibratedImageSize.width;
+                double sy = (double)frame0.rows/calibratedImageSize.height;
+                
                 // adjust the camera matrix for the new resolution
-                    calibrated.cameraMatrix.at<double>(0,0) *= frame.cols/calibrated.imageSize.width;
+                cameraMatrix.at<double>(0,0) *= sx;
-                    calibrated.cameraMatrix.at<double>(0,2) *= frame.cols/calibrated.imageSize.width;
+                cameraMatrix.at<double>(0,2) *= sx;
-                    calibrated.cameraMatrix.at<double>(1,1) *= frame.rows/calibrated.imageSize.height;
+                cameraMatrix.at<double>(1,1) *= sy;
-                    calibrated.cameraMatrix.at<double>(1,2) *= frame.rows/calibrated.imageSize.height;
+                cameraMatrix.at<double>(1,2) *= sy;
-                    calibrated.imageSize = frame0.size();
             }
             Mat dummy;
-                // initialize undistortion maps
+            initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
-                initUndistortRectifyMap(calibrated.cameraMatrix, calibrated.distCoeffs, Mat(),
+                                    cameraMatrix, frame0.size(),
-                                        calibrated.cameraMatrix, calibrated.imageSize,
                                     CV_32FC2, mapxy, dummy );
-                calibrated.distCoeffs = Mat::zeros(5, 1, CV_64F);
+            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);
+        vector<Point2f> foundBoardCorners;
+        boardFound = findChessboardCorners(frame, boardSize, foundBoardCorners);
         
+        Mat rvec, tvec;
         if( boardFound )
-            {
+            solvePnP(Mat(boardPoints), Mat(foundBoardCorners), cameraMatrix,
-                solvePnP(Mat(calibrated.objPoints), Mat(boardCorners), calibrated.cameraMatrix,
+                     distCoeffs, rvec, tvec, false);
-                         calibrated.distCoeffs, rvec, tvec, false);
+        
-                Rodrigues(rvec, R);
-            }
-		}
         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;
+            if( box.empty() )
-            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 )
             {
-                // update object box
+                int code = select3DBox("View", "Selected Object", frame,
-                mousepts[npt] = mouseLoc;
+                                        cameraMatrix, rvec, tvec, box);
-
+                if( code == -100 )
-                /*if(!paused)
-                    imwrite("frame.jpg", frame0);*/
-                paused = true;
-                if( nobjpt < 2 )
-                    imgpts[npt] = mousepts[npt];
-                else
-                {
-                    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
-                    {
-                        Z = d1.dot(d2)/(n1*n1);
-                        imgpts[npt] = d1*Z + a;
-                    }
-                }
-                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 )
-                    {
-                        imgpts[nobjpt] = imgpts[nobjpt-1];
-                        objpts[nobjpt] = objpts[nobjpt-1];
-                        mousepts[nobjpt] = mousepts[nobjpt-1];
-                    }
-                }
-                
-                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_<Point>(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;
-            }
-        }
-
-        imshow("Video", shownFrame);
-        imshow("Selected Object", selectedObjFrame);
-		int c = waitKey(30);
-		if( (c & 255) == 27 )
-            nobjpt = 0;
-        if( c == ' ' )
-            paused = !paused;
-        if( c == 'q' || c == 'Q' )
                     break;
-        if( (c == '\r' || c == '\n') && objselected && nOutlinePt == 8 )
+            }
-        {
-            Rect r = boundingRect(Mat(temphull));
         
-            for(;;frameIdx++)
+            if( !box.empty() )
+            {
+                Rect r = extract3DBox(frame, shownFrame, selectedObjFrame,
+                                      cameraMatrix, rvec, tvec, box, 4);
+                if( r.area() )
+                {
+                    const int maxFrameIdx = 10000;
+                    char path[1000];
+                    for(;frameIdx < maxFrameIdx;frameIdx++)
                     {
                         sprintf(path, "%s%04d.jpg", outprefix, frameIdx);
                         FILE* f = fopen(path, "rb");
@@ -401,18 +513,35 @@ int main(int argc, char** argv)
                             break;
                         fclose(f);
                     }
+                    if( frameIdx == maxFrameIdx )
+                    {
+                        printf("Can not save the image as %s<...>.jpg", outprefix);
+                        break;
+                    }
+                    imwrite(path, selectedObjFrame(r));
                     
-            imwrite(path, selectedObjFrame(r&Rect(0,0,selectedObjFrame.cols,selectedObjFrame.rows)));
+                    capturedImgList.push_back(string(path));
-            fprintf(fframes, "%s%04d.jpg (%.4f %.4f %.4f) (%.4f %.4f %.4f)", outbarename, frameIdx,
+                    roiList.push_back(r);
-                    rvec.at<double>(0,0), rvec.at<double>(1,0), rvec.at<double>(2,0),
+                    
-                    tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0));
+                    float p[6];
-            for( int i = 0; i < 8; i++ )
+                    Mat RV(3, 1, CV_32F, p), TV(3, 1, CV_32F, p+3);
-                fprintf(fframes, " (%.2f %.2f %.2f)", objpts[i].x, objpts[i].y, objpts[i].z);
+                    rvec.convertTo(RV, RV.type());
-            fprintf(fframes, "\n");
+                    tvec.convertTo(TV, TV.type());
-            frameIdx++;
+                    poseList.push_back(Vec6f(p[0], p[1], p[2], p[3], p[4], p[5]));
                 }
             }
+            grabNext = !imageList.empty();
+        }
 
-    fclose(fframes);
+        imshow("View", shownFrame);
+        imshow("Selected Object", selectedObjFrame);
+		int c = waitKey(imageList.empty() ? 30 : 300);
+        if( c == 'q' || c == 'Q' )
+            break;
+        if( c == '\r' || c == '\n' )
+            grabNext = true;
+	}
+
+    writeModelViews(indexFilename, box, capturedImgList, roiList, poseList);
     return 0;
 }