← Back to branch summary

~paparazzi-uav/paparazzi/v5.0-manual

« back to all changes in this revision

Viewing changes to sw/ext/opencv_bebop/opencv/samples/cpp/select3dobj.cpp

  • Committer: Paparazzi buildbot
  • Date: 2016-05-18 15:00:29 UTC
  • Revision ID: felix.ruess+docbot@gmail.com-20160518150029-e8lgzi5kvb4p7un9
Manual import commit 4b8bbb730080dac23cf816b98908dacfabe2a8ec from v5.0 branch.

Show diffs side-by-side

added added

removed removed

Lines of Context:
sw/ext/opencv_bebop/opencv/samples/cpp/select3dobj.cpp
 
1
/*
 
2
 *
 
3
 * select3obj.cpp With a calibration chessboard on a table, mark an object in a 3D box and
 
4
 *                track that object in all subseqent frames as long as the camera can see
 
5
 *                the chessboard. Also segments the object using the box projection. This
 
6
 *                program is useful for collecting large datasets of many views of an object
 
7
 *                on a table.
 
8
 *
 
9
 */
 
10
 
 
11
#include "opencv2/core.hpp"
 
12
#include <opencv2/core/utility.hpp>
 
13
#include "opencv2/imgproc.hpp"
 
14
#include "opencv2/calib3d.hpp"
 
15
#include "opencv2/imgcodecs.hpp"
 
16
#include "opencv2/videoio.hpp"
 
17
#include "opencv2/highgui.hpp"
 
18
 
 
19
#include <ctype.h>
 
20
#include <stdio.h>
 
21
#include <stdlib.h>
 
22
 
 
23
using namespace std;
 
24
using namespace cv;
 
25
 
 
26
const char* helphelp =
 
27
"\nThis program's purpose is to collect data sets of an object and its segmentation mask.\n"
 
28
"\n"
 
29
"It shows how to use a calibrated camera together with a calibration pattern to\n"
 
30
"compute the homography of the plane the calibration pattern is on. It also shows grabCut\n"
 
31
"segmentation etc.\n"
 
32
"\n"
 
33
"select3dobj -w=<board_width> -h=<board_height> [-s=<square_size>]\n"
 
34
"           -i=<camera_intrinsics_filename> -o=<output_prefix>\n"
 
35
"\n"
 
36
" -w=<board_width>          Number of chessboard corners wide\n"
 
37
" -h=<board_height>         Number of chessboard corners width\n"
 
38
" [-s=<square_size>]            Optional measure of chessboard squares in meters\n"
 
39
" -i=<camera_intrinsics_filename> Camera matrix .yml file from calibration.cpp\n"
 
40
" -o=<output_prefix>        Prefix the output segmentation images with this\n"
 
41
" [video_filename/cameraId]  If present, read from that video file or that ID\n"
 
42
"\n"
 
43
"Using a camera's intrinsics (from calibrating a camera -- see calibration.cpp) and an\n"
 
44
"image of the object sitting on a planar surface with a calibration pattern of\n"
 
45
"(board_width x board_height) on the surface, we draw a 3D box aroung the object. From\n"
 
46
"then on, we can move a camera and as long as it sees the chessboard calibration pattern,\n"
 
47
"it will store a mask of where the object is. We get succesive images using <output_prefix>\n"
 
48
"of the segmentation mask containing the object. This makes creating training sets easy.\n"
 
49
"It is best of the chessboard is odd x even in dimensions to avoid amiguous poses.\n"
 
50
"\n"
 
51
"The actions one can use while the program is running are:\n"
 
52
"\n"
 
53
"  Select object as 3D box with the mouse.\n"
 
54
"   First draw one line on the plane to outline the projection of that object on the plane\n"
 
55
"    Then extend that line into a box to encompass the projection of that object onto the plane\n"
 
56
"    The use the mouse again to extend the box upwards from the plane to encase the object.\n"
 
57
"  Then use the following commands\n"
 
58
"    ESC   - Reset the selection\n"
 
59
"    SPACE - Skip the frame; move to the next frame (not in video mode)\n"
 
60
"    ENTER - Confirm the selection. Grab next object in video mode.\n"
 
61
"    q     - Exit the program\n"
 
62
"\n\n";
 
63
 
 
64
// static void help()
 
65
// {
 
66
//     puts(helphelp);
 
67
// }
 
68
 
 
69
 
 
70
struct MouseEvent
 
71
{
 
72
    MouseEvent() { event = -1; buttonState = 0; }
 
73
    Point pt;
 
74
    int event;
 
75
    int buttonState;
 
76
};
 
77
 
 
78
static void onMouse(int event, int x, int y, int flags, void* userdata)
 
79
{
 
80
    MouseEvent* data = (MouseEvent*)userdata;
 
81
    data->event = event;
 
82
    data->pt = Point(x,y);
 
83
    data->buttonState = flags;
 
84
}
 
85
 
 
86
static bool readCameraMatrix(const string& filename,
 
87
                             Mat& cameraMatrix, Mat& distCoeffs,
 
88
                             Size& calibratedImageSize )
 
89
{
 
90
    FileStorage fs(filename, FileStorage::READ);
 
91
    fs["image_width"] >> calibratedImageSize.width;
 
92
    fs["image_height"] >> calibratedImageSize.height;
 
93
    fs["distortion_coefficients"] >> distCoeffs;
 
94
    fs["camera_matrix"] >> cameraMatrix;
 
95
 
 
96
    if( distCoeffs.type() != CV_64F )
 
97
        distCoeffs = Mat_<double>(distCoeffs);
 
98
    if( cameraMatrix.type() != CV_64F )
 
99
        cameraMatrix = Mat_<double>(cameraMatrix);
 
100
 
 
101
    return true;
 
102
}
 
103
 
 
104
static void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners)
 
105
{
 
106
    corners.resize(0);
 
107
 
 
108
    for( int i = 0; i < boardSize.height; i++ )
 
109
        for( int j = 0; j < boardSize.width; j++ )
 
110
            corners.push_back(Point3f(float(j*squareSize),
 
111
                                      float(i*squareSize), 0));
 
112
}
 
113
 
 
114
 
 
115
static Point3f image2plane(Point2f imgpt, const Mat& R, const Mat& tvec,
 
116
                           const Mat& cameraMatrix, double Z)
 
117
{
 
118
    Mat R1 = R.clone();
 
119
    R1.col(2) = R1.col(2)*Z + tvec;
 
120
    Mat_<double> v = (cameraMatrix*R1).inv()*(Mat_<double>(3,1) << imgpt.x, imgpt.y, 1);
 
121
    double iw = fabs(v(2,0)) > DBL_EPSILON ? 1./v(2,0) : 0;
 
122
    return Point3f((float)(v(0,0)*iw), (float)(v(1,0)*iw), (float)Z);
 
123
}
 
124
 
 
125
 
 
126
static Rect extract3DBox(const Mat& frame, Mat& shownFrame, Mat& selectedObjFrame,
 
127
                         const Mat& cameraMatrix, const Mat& rvec, const Mat& tvec,
 
128
                         const vector<Point3f>& box, int nobjpt, bool runExtraSegmentation)
 
129
{
 
130
    selectedObjFrame = Mat::zeros(frame.size(), frame.type());
 
131
    if( nobjpt == 0 )
 
132
        return Rect();
 
133
    vector<Point3f> objpt;
 
134
    vector<Point2f> imgpt;
 
135
 
 
136
    objpt.push_back(box[0]);
 
137
    if( nobjpt > 1 )
 
138
        objpt.push_back(box[1]);
 
139
    if( nobjpt > 2 )
 
140
    {
 
141
        objpt.push_back(box[2]);
 
142
        objpt.push_back(objpt[2] - objpt[1] + objpt[0]);
 
143
    }
 
144
    if( nobjpt > 3 )
 
145
        for( int i = 0; i < 4; i++ )
 
146
            objpt.push_back(Point3f(objpt[i].x, objpt[i].y, box[3].z));
 
147
 
 
148
    projectPoints(Mat(objpt), rvec, tvec, cameraMatrix, Mat(), imgpt);
 
149
 
 
150
    if( !shownFrame.empty() )
 
151
    {
 
152
        if( nobjpt == 1 )
 
153
            circle(shownFrame, imgpt[0], 3, Scalar(0,255,0), -1, LINE_AA);
 
154
        else if( nobjpt == 2 )
 
155
        {
 
156
            circle(shownFrame, imgpt[0], 3, Scalar(0,255,0), -1, LINE_AA);
 
157
            circle(shownFrame, imgpt[1], 3, Scalar(0,255,0), -1, LINE_AA);
 
158
            line(shownFrame, imgpt[0], imgpt[1], Scalar(0,255,0), 3, LINE_AA);
 
159
        }
 
160
        else if( nobjpt == 3 )
 
161
            for( int i = 0; i < 4; i++ )
 
162
            {
 
163
                circle(shownFrame, imgpt[i], 3, Scalar(0,255,0), -1, LINE_AA);
 
164
                line(shownFrame, imgpt[i], imgpt[(i+1)%4], Scalar(0,255,0), 3, LINE_AA);
 
165
            }
 
166
        else
 
167
            for( int i = 0; i < 8; i++ )
 
168
            {
 
169
                circle(shownFrame, imgpt[i], 3, Scalar(0,255,0), -1, LINE_AA);
 
170
                line(shownFrame, imgpt[i], imgpt[(i+1)%4 + (i/4)*4], Scalar(0,255,0), 3, LINE_AA);
 
171
                line(shownFrame, imgpt[i], imgpt[i%4], Scalar(0,255,0), 3, LINE_AA);
 
172
            }
 
173
    }
 
174
 
 
175
    if( nobjpt <= 2 )
 
176
        return Rect();
 
177
    vector<Point> hull;
 
178
    convexHull(Mat_<Point>(Mat(imgpt)), hull);
 
179
    Mat selectedObjMask = Mat::zeros(frame.size(), CV_8U);
 
180
    fillConvexPoly(selectedObjMask, &hull[0], (int)hull.size(), Scalar::all(255), 8, 0);
 
181
    Rect roi = boundingRect(Mat(hull)) & Rect(Point(), frame.size());
 
182
 
 
183
    if( runExtraSegmentation )
 
184
    {
 
185
        selectedObjMask = Scalar::all(GC_BGD);
 
186
        fillConvexPoly(selectedObjMask, &hull[0], (int)hull.size(), Scalar::all(GC_PR_FGD), 8, 0);
 
187
        Mat bgdModel, fgdModel;
 
188
        grabCut(frame, selectedObjMask, roi, bgdModel, fgdModel,
 
189
                3, GC_INIT_WITH_RECT + GC_INIT_WITH_MASK);
 
190
        bitwise_and(selectedObjMask, Scalar::all(1), selectedObjMask);
 
191
    }
 
192
 
 
193
    frame.copyTo(selectedObjFrame, selectedObjMask);
 
194
    return roi;
 
195
}
 
196
 
 
197
 
 
198
static int select3DBox(const string& windowname, const string& selWinName, const Mat& frame,
 
199
                       const Mat& cameraMatrix, const Mat& rvec, const Mat& tvec,
 
200
                       vector<Point3f>& box)
 
201
{
 
202
    const float eps = 1e-3f;
 
203
    MouseEvent mouse;
 
204
 
 
205
    setMouseCallback(windowname, onMouse, &mouse);
 
206
    vector<Point3f> tempobj(8);
 
207
    vector<Point2f> imgpt(4), tempimg(8);
 
208
    vector<Point> temphull;
 
209
    int nobjpt = 0;
 
210
    Mat R, selectedObjMask, selectedObjFrame, shownFrame;
 
211
    Rodrigues(rvec, R);
 
212
    box.resize(4);
 
213
 
 
214
    for(;;)
 
215
    {
 
216
        float Z = 0.f;
 
217
        bool dragging = (mouse.buttonState & EVENT_FLAG_LBUTTON) != 0;
 
218
        int npt = nobjpt;
 
219
 
 
220
        if( (mouse.event == EVENT_LBUTTONDOWN ||
 
221
             mouse.event == EVENT_LBUTTONUP ||
 
222
             dragging) && nobjpt < 4 )
 
223
        {
 
224
            Point2f m = mouse.pt;
 
225
 
 
226
            if( nobjpt < 2 )
 
227
                imgpt[npt] = m;
 
228
            else
 
229
            {
 
230
                tempobj.resize(1);
 
231
                int nearestIdx = npt-1;
 
232
                if( nobjpt == 3 )
 
233
                {
 
234
                    nearestIdx = 0;
 
235
                    for( int i = 1; i < npt; i++ )
 
236
                        if( norm(m - imgpt[i]) < norm(m - imgpt[nearestIdx]) )
 
237
                            nearestIdx = i;
 
238
                }
 
239
 
 
240
                if( npt == 2 )
 
241
                {
 
242
                    float dx = box[1].x - box[0].x, dy = box[1].y - box[0].y;
 
243
                    float len = 1.f/std::sqrt(dx*dx+dy*dy);
 
244
                    tempobj[0] = Point3f(dy*len + box[nearestIdx].x,
 
245
                                         -dx*len + box[nearestIdx].y, 0.f);
 
246
                }
 
247
                else
 
248
                    tempobj[0] = Point3f(box[nearestIdx].x, box[nearestIdx].y, 1.f);
 
249
 
 
250
                projectPoints(Mat(tempobj), rvec, tvec, cameraMatrix, Mat(), tempimg);
 
251
 
 
252
                Point2f a = imgpt[nearestIdx], b = tempimg[0], d1 = b - a, d2 = m - a;
 
253
                float n1 = (float)norm(d1), n2 = (float)norm(d2);
 
254
                if( n1*n2 < eps )
 
255
                    imgpt[npt] = a;
 
256
                else
 
257
                {
 
258
                    Z = d1.dot(d2)/(n1*n1);
 
259
                    imgpt[npt] = d1*Z + a;
 
260
                }
 
261
            }
 
262
            box[npt] = image2plane(imgpt[npt], R, tvec, cameraMatrix, npt<3 ? 0 : Z);
 
263
 
 
264
            if( (npt == 0 && mouse.event == EVENT_LBUTTONDOWN) ||
 
265
               (npt > 0 && norm(box[npt] - box[npt-1]) > eps &&
 
266
                mouse.event == EVENT_LBUTTONUP) )
 
267
            {
 
268
                nobjpt++;
 
269
                if( nobjpt < 4 )
 
270
                {
 
271
                    imgpt[nobjpt] = imgpt[nobjpt-1];
 
272
                    box[nobjpt] = box[nobjpt-1];
 
273
                }
 
274
            }
 
275
 
 
276
            // reset the event
 
277
            mouse.event = -1;
 
278
            //mouse.buttonState = 0;
 
279
            npt++;
 
280
        }
 
281
 
 
282
        frame.copyTo(shownFrame);
 
283
        extract3DBox(frame, shownFrame, selectedObjFrame,
 
284
                     cameraMatrix, rvec, tvec, box, npt, false);
 
285
        imshow(windowname, shownFrame);
 
286
        imshow(selWinName, selectedObjFrame);
 
287
 
 
288
        int c = waitKey(30);
 
289
        if( (c & 255) == 27 )
 
290
        {
 
291
            nobjpt = 0;
 
292
        }
 
293
        if( c == 'q' || c == 'Q' || c == ' ' )
 
294
        {
 
295
            box.clear();
 
296
            return c == ' ' ? -1 : -100;
 
297
        }
 
298
        if( (c == '\r' || c == '\n') && nobjpt == 4 && box[3].z != 0 )
 
299
            return 1;
 
300
    }
 
301
}
 
302
 
 
303
 
 
304
static bool readModelViews( const string& filename, vector<Point3f>& box,
 
305
                            vector<string>& imagelist,
 
306
                            vector<Rect>& roiList, vector<Vec6f>& poseList )
 
307
{
 
308
    imagelist.resize(0);
 
309
    roiList.resize(0);
 
310
    poseList.resize(0);
 
311
    box.resize(0);
 
312
 
 
313
    FileStorage fs(filename, FileStorage::READ);
 
314
    if( !fs.isOpened() )
 
315
        return false;
 
316
    fs["box"] >> box;
 
317
 
 
318
    FileNode all = fs["views"];
 
319
    if( all.type() != FileNode::SEQ )
 
320
        return false;
 
321
    FileNodeIterator it = all.begin(), it_end = all.end();
 
322
 
 
323
    for(; it != it_end; ++it)
 
324
    {
 
325
        FileNode n = *it;
 
326
        imagelist.push_back((string)n["image"]);
 
327
        FileNode nr = n["rect"];
 
328
        roiList.push_back(Rect((int)nr[0], (int)nr[1], (int)nr[2], (int)nr[3]));
 
329
        FileNode np = n["pose"];
 
330
        poseList.push_back(Vec6f((float)np[0], (float)np[1], (float)np[2],
 
331
                                 (float)np[3], (float)np[4], (float)np[5]));
 
332
    }
 
333
 
 
334
    return true;
 
335
}
 
336
 
 
337
 
 
338
static bool writeModelViews(const string& filename, const vector<Point3f>& box,
 
339
                            const vector<string>& imagelist,
 
340
                            const vector<Rect>& roiList,
 
341
                            const vector<Vec6f>& poseList)
 
342
{
 
343
    FileStorage fs(filename, FileStorage::WRITE);
 
344
    if( !fs.isOpened() )
 
345
        return false;
 
346
 
 
347
    fs << "box" << "[:";
 
348
    fs << box << "]" << "views" << "[";
 
349
 
 
350
    size_t i, nviews = imagelist.size();
 
351
 
 
352
    CV_Assert( nviews == roiList.size() && nviews == poseList.size() );
 
353
 
 
354
    for( i = 0; i < nviews; i++ )
 
355
    {
 
356
        Rect r = roiList[i];
 
357
        Vec6f p = poseList[i];
 
358
 
 
359
        fs << "{" << "image" << imagelist[i] <<
 
360
            "roi" << "[:" << r.x << r.y << r.width << r.height << "]" <<
 
361
            "pose" << "[:" << p[0] << p[1] << p[2] << p[3] << p[4] << p[5] << "]" << "}";
 
362
    }
 
363
    fs << "]";
 
364
 
 
365
    return true;
 
366
}
 
367
 
 
368
 
 
369
static bool readStringList( const string& filename, vector<string>& l )
 
370
{
 
371
    l.resize(0);
 
372
    FileStorage fs(filename, FileStorage::READ);
 
373
    if( !fs.isOpened() )
 
374
        return false;
 
375
    FileNode n = fs.getFirstTopLevelNode();
 
376
    if( n.type() != FileNode::SEQ )
 
377
        return false;
 
378
    FileNodeIterator it = n.begin(), it_end = n.end();
 
379
    for( ; it != it_end; ++it )
 
380
        l.push_back((string)*it);
 
381
    return true;
 
382
}
 
383
 
 
384
 
 
385
int main(int argc, char** argv)
 
386
{
 
387
    const char* help = "Usage: select3dobj -w=<board_width> -h=<board_height> [-s=<square_size>]\n"
 
388
           "\t-i=<intrinsics_filename> -o=<output_prefix> [video_filename/cameraId]\n";
 
389
    const char* screen_help =
 
390
    "Actions: \n"
 
391
    "\tSelect object as 3D box with the mouse. That's it\n"
 
392
    "\tESC - Reset the selection\n"
 
393
    "\tSPACE - Skip the frame; move to the next frame (not in video mode)\n"
 
394
    "\tENTER - Confirm the selection. Grab next object in video mode.\n"
 
395
    "\tq - Exit the program\n";
 
396
 
 
397
    cv::CommandLineParser parser(argc, argv, "{help h||}{w||}{h||}{s|1|}{i||}{o||}{@input|0|}");
 
398
    if (parser.has("help"))
 
399
    {
 
400
        puts(helphelp);
 
401
        puts(help);
 
402
        return 0;
 
403
    }
 
404
    string intrinsicsFilename;
 
405
    string outprefix = "";
 
406
    string inputName = "";
 
407
    int cameraId = 0;
 
408
    Size boardSize;
 
409
    double squareSize;
 
410
    vector<string> imageList;
 
411
    intrinsicsFilename = parser.get<string>("i");
 
412
    outprefix = parser.get<string>("o");
 
413
    boardSize.width = parser.get<int>("w");
 
414
    boardSize.height = parser.get<int>("h");
 
415
    squareSize = parser.get<double>("s");
 
416
    if ( parser.get<string>("@input").size() == 1 && isdigit(parser.get<string>("@input")[0]) )
 
417
        cameraId = parser.get<int>("@input");
 
418
    else
 
419
        inputName = parser.get<string>("@input");
 
420
    if (!parser.check())
 
421
    {
 
422
        puts(help);
 
423
        parser.printErrors();
 
424
        return 0;
 
425
    }
 
426
    if ( boardSize.width <= 0 )
 
427
    {
 
428
        printf("Incorrect -w parameter (must be a positive integer)\n");
 
429
        puts(help);
 
430
        return 0;
 
431
    }
 
432
    if ( boardSize.height <= 0 )
 
433
    {
 
434
        printf("Incorrect -h parameter (must be a positive integer)\n");
 
435
        puts(help);
 
436
        return 0;
 
437
    }
 
438
    if ( squareSize <= 0 )
 
439
    {
 
440
        printf("Incorrect -s parameter (must be a positive real number)\n");
 
441
        puts(help);
 
442
        return 0;
 
443
    }
 
444
    Mat cameraMatrix, distCoeffs;
 
445
    Size calibratedImageSize;
 
446
    readCameraMatrix(intrinsicsFilename, cameraMatrix, distCoeffs, calibratedImageSize );
 
447
 
 
448
    VideoCapture capture;
 
449
    if( !inputName.empty() )
 
450
    {
 
451
        if( !readStringList(inputName, imageList) &&
 
452
            !capture.open(inputName))
 
453
        {
 
454
            fprintf( stderr, "The input file could not be opened\n" );
 
455
            return -1;
 
456
        }
 
457
    }
 
458
    else
 
459
        capture.open(cameraId);
 
460
 
 
461
    if( !capture.isOpened() && imageList.empty() )
 
462
        return fprintf( stderr, "Could not initialize video capture\n" ), -2;
 
463
 
 
464
    const char* outbarename = 0;
 
465
    {
 
466
        outbarename = strrchr(outprefix.c_str(), '/');
 
467
        const char* tmp = strrchr(outprefix.c_str(), '\\');
 
468
        char cmd[1000];
 
469
        sprintf(cmd, "mkdir %s", outprefix.c_str());
 
470
        if( tmp && tmp > outbarename )
 
471
            outbarename = tmp;
 
472
        if( outbarename )
 
473
        {
 
474
            cmd[6 + outbarename - outprefix.c_str()] = '\0';
 
475
            int result = system(cmd);
 
476
            CV_Assert(result == 0);
 
477
            outbarename++;
 
478
        }
 
479
        else
 
480
            outbarename = outprefix.c_str();
 
481
    }
 
482
 
 
483
    Mat frame, shownFrame, selectedObjFrame, mapxy;
 
484
 
 
485
    namedWindow("View", 1);
 
486
    namedWindow("Selected Object", 1);
 
487
    setMouseCallback("View", onMouse, 0);
 
488
    bool boardFound = false;
 
489
 
 
490
    string indexFilename = format("%s_index.yml", outprefix.c_str());
 
491
 
 
492
    vector<string> capturedImgList;
 
493
    vector<Rect> roiList;
 
494
    vector<Vec6f> poseList;
 
495
    vector<Point3f> box, boardPoints;
 
496
 
 
497
    readModelViews(indexFilename, box, capturedImgList, roiList, poseList);
 
498
    calcChessboardCorners(boardSize, (float)squareSize, boardPoints);
 
499
    int frameIdx = 0;
 
500
    bool grabNext = !imageList.empty();
 
501
 
 
502
    puts(screen_help);
 
503
 
 
504
    for(int i = 0;;i++)
 
505
    {
 
506
        Mat frame0;
 
507
        if( !imageList.empty() )
 
508
        {
 
509
            if( i < (int)imageList.size() )
 
510
                frame0 = imread(string(imageList[i]), 1);
 
511
        }
 
512
        else
 
513
            capture >> frame0;
 
514
        if( frame0.empty() )
 
515
            break;
 
516
        if( frame.empty() )
 
517
        {
 
518
            if( frame0.size() != calibratedImageSize )
 
519
            {
 
520
                double sx = (double)frame0.cols/calibratedImageSize.width;
 
521
                double sy = (double)frame0.rows/calibratedImageSize.height;
 
522
 
 
523
                // adjust the camera matrix for the new resolution
 
524
                cameraMatrix.at<double>(0,0) *= sx;
 
525
                cameraMatrix.at<double>(0,2) *= sx;
 
526
                cameraMatrix.at<double>(1,1) *= sy;
 
527
                cameraMatrix.at<double>(1,2) *= sy;
 
528
            }
 
529
            Mat dummy;
 
530
            initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
 
531
                                    cameraMatrix, frame0.size(),
 
532
                                    CV_32FC2, mapxy, dummy );
 
533
            distCoeffs = Mat::zeros(5, 1, CV_64F);
 
534
        }
 
535
        remap(frame0, frame, mapxy, Mat(), INTER_LINEAR);
 
536
        vector<Point2f> foundBoardCorners;
 
537
        boardFound = findChessboardCorners(frame, boardSize, foundBoardCorners);
 
538
 
 
539
        Mat rvec, tvec;
 
540
        if( boardFound )
 
541
            solvePnP(Mat(boardPoints), Mat(foundBoardCorners), cameraMatrix,
 
542
                     distCoeffs, rvec, tvec, false);
 
543
 
 
544
        frame.copyTo(shownFrame);
 
545
        drawChessboardCorners(shownFrame, boardSize, Mat(foundBoardCorners), boardFound);
 
546
        selectedObjFrame = Mat::zeros(frame.size(), frame.type());
 
547
 
 
548
        if( boardFound && grabNext )
 
549
        {
 
550
            if( box.empty() )
 
551
            {
 
552
                int code = select3DBox("View", "Selected Object", frame,
 
553
                                        cameraMatrix, rvec, tvec, box);
 
554
                if( code == -100 )
 
555
                    break;
 
556
            }
 
557
 
 
558
            if( !box.empty() )
 
559
            {
 
560
                Rect r = extract3DBox(frame, shownFrame, selectedObjFrame,
 
561
                                      cameraMatrix, rvec, tvec, box, 4, true);
 
562
                if( r.area() )
 
563
                {
 
564
                    const int maxFrameIdx = 10000;
 
565
                    char path[1000];
 
566
                    for(;frameIdx < maxFrameIdx;frameIdx++)
 
567
                    {
 
568
                        sprintf(path, "%s%04d.jpg", outprefix.c_str(), frameIdx);
 
569
                        FILE* f = fopen(path, "rb");
 
570
                        if( !f )
 
571
                            break;
 
572
                        fclose(f);
 
573
                    }
 
574
                    if( frameIdx == maxFrameIdx )
 
575
                    {
 
576
                        printf("Can not save the image as %s<...>.jpg", outprefix.c_str());
 
577
                        break;
 
578
                    }
 
579
                    imwrite(path, selectedObjFrame(r));
 
580
 
 
581
                    capturedImgList.push_back(string(path));
 
582
                    roiList.push_back(r);
 
583
 
 
584
                    float p[6];
 
585
                    Mat RV(3, 1, CV_32F, p), TV(3, 1, CV_32F, p+3);
 
586
                    rvec.convertTo(RV, RV.type());
 
587
                    tvec.convertTo(TV, TV.type());
 
588
                    poseList.push_back(Vec6f(p[0], p[1], p[2], p[3], p[4], p[5]));
 
589
                }
 
590
            }
 
591
            grabNext = !imageList.empty();
 
592
        }
 
593
 
 
594
        imshow("View", shownFrame);
 
595
        imshow("Selected Object", selectedObjFrame);
 
596
        int c = waitKey(imageList.empty() && !box.empty() ? 30 : 300);
 
597
        if( c == 'q' || c == 'Q' )
 
598
            break;
 
599
        if( c == '\r' || c == '\n' )
 
600
            grabNext = true;
 
601
    }
 
602
 
 
603
    writeModelViews(indexFilename, box, capturedImgList, roiList, poseList);
 
604
    return 0;
 
605
}