← 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/modules/calib3d/src/solvepnp.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/modules/calib3d/src/solvepnp.cpp
 
1
/*M///////////////////////////////////////////////////////////////////////////////////////
 
2
 //
 
3
 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
 
4
 //
 
5
 //  By downloading, copying, installing or using the software you agree to this license.
 
6
 //  If you do not agree to this license, do not download, install,
 
7
 //  copy or use the software.
 
8
 //
 
9
 //
 
10
 //                           License Agreement
 
11
 //                For Open Source Computer Vision Library
 
12
 //
 
13
 // Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
 
14
 // Copyright (C) 2009, Willow Garage Inc., all rights reserved.
 
15
 // Third party copyrights are property of their respective owners.
 
16
 //
 
17
 // Redistribution and use in source and binary forms, with or without modification,
 
18
 // are permitted provided that the following conditions are met:
 
19
 //
 
20
 //   * Redistribution's of source code must retain the above copyright notice,
 
21
 //     this list of conditions and the following disclaimer.
 
22
 //
 
23
 //   * Redistribution's in binary form must reproduce the above copyright notice,
 
24
 //     this list of conditions and the following disclaimer in the documentation
 
25
 //     and/or other materials provided with the distribution.
 
26
 //
 
27
 //   * The name of the copyright holders may not be used to endorse or promote products
 
28
 //     derived from this software without specific prior written permission.
 
29
 //
 
30
 // This software is provided by the copyright holders and contributors "as is" and
 
31
 // any express or implied warranties, including, but not limited to, the implied
 
32
 // warranties of merchantability and fitness for a particular purpose are disclaimed.
 
33
 // In no event shall the Intel Corporation or contributors be liable for any direct,
 
34
 // indirect, incidental, special, exemplary, or consequential damages
 
35
 // (including, but not limited to, procurement of substitute goods or services;
 
36
 // loss of use, data, or profits; or business interruption) however caused
 
37
 // and on any theory of liability, whether in contract, strict liability,
 
38
 // or tort (including negligence or otherwise) arising in any way out of
 
39
 // the use of this software, even if advised of the possibility of such damage.
 
40
 //
 
41
 //M*/
 
42
 
 
43
#include "precomp.hpp"
 
44
#include "upnp.h"
 
45
#include "dls.h"
 
46
#include "epnp.h"
 
47
#include "p3p.h"
 
48
#include "opencv2/calib3d/calib3d_c.h"
 
49
 
 
50
#include <iostream>
 
51
 
 
52
namespace cv
 
53
{
 
54
 
 
55
bool solvePnP( InputArray _opoints, InputArray _ipoints,
 
56
               InputArray _cameraMatrix, InputArray _distCoeffs,
 
57
               OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess, int flags )
 
58
{
 
59
    Mat opoints = _opoints.getMat(), ipoints = _ipoints.getMat();
 
60
    int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
 
61
    CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
 
62
 
 
63
    Mat rvec, tvec;
 
64
    if( flags != SOLVEPNP_ITERATIVE )
 
65
        useExtrinsicGuess = false;
 
66
 
 
67
    if( useExtrinsicGuess )
 
68
    {
 
69
        int rtype = _rvec.type(), ttype = _tvec.type();
 
70
        Size rsize = _rvec.size(), tsize = _tvec.size();
 
71
        CV_Assert( (rtype == CV_32F || rtype == CV_64F) &&
 
72
                   (ttype == CV_32F || ttype == CV_64F) );
 
73
        CV_Assert( (rsize == Size(1, 3) || rsize == Size(3, 1)) &&
 
74
                   (tsize == Size(1, 3) || tsize == Size(3, 1)) );
 
75
    }
 
76
    else
 
77
    {
 
78
        _rvec.create(3, 1, CV_64F);
 
79
        _tvec.create(3, 1, CV_64F);
 
80
    }
 
81
    rvec = _rvec.getMat();
 
82
    tvec = _tvec.getMat();
 
83
 
 
84
    Mat cameraMatrix0 = _cameraMatrix.getMat();
 
85
    Mat distCoeffs0 = _distCoeffs.getMat();
 
86
    Mat cameraMatrix = Mat_<double>(cameraMatrix0);
 
87
    Mat distCoeffs = Mat_<double>(distCoeffs0);
 
88
    bool result = false;
 
89
 
 
90
    if (flags == SOLVEPNP_EPNP || flags == SOLVEPNP_DLS || flags == SOLVEPNP_UPNP)
 
91
    {
 
92
        Mat undistortedPoints;
 
93
        undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
 
94
        epnp PnP(cameraMatrix, opoints, undistortedPoints);
 
95
 
 
96
        Mat R;
 
97
        PnP.compute_pose(R, tvec);
 
98
        Rodrigues(R, rvec);
 
99
        result = true;
 
100
    }
 
101
    else if (flags == SOLVEPNP_P3P)
 
102
    {
 
103
        CV_Assert( npoints == 4);
 
104
        Mat undistortedPoints;
 
105
        undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
 
106
        p3p P3Psolver(cameraMatrix);
 
107
 
 
108
        Mat R;
 
109
        result = P3Psolver.solve(R, tvec, opoints, undistortedPoints);
 
110
        if (result)
 
111
            Rodrigues(R, rvec);
 
112
    }
 
113
    else if (flags == SOLVEPNP_ITERATIVE)
 
114
    {
 
115
        CvMat c_objectPoints = opoints, c_imagePoints = ipoints;
 
116
        CvMat c_cameraMatrix = cameraMatrix, c_distCoeffs = distCoeffs;
 
117
        CvMat c_rvec = rvec, c_tvec = tvec;
 
118
        cvFindExtrinsicCameraParams2(&c_objectPoints, &c_imagePoints, &c_cameraMatrix,
 
119
                                     c_distCoeffs.rows*c_distCoeffs.cols ? &c_distCoeffs : 0,
 
120
                                     &c_rvec, &c_tvec, useExtrinsicGuess );
 
121
        result = true;
 
122
    }
 
123
    /*else if (flags == SOLVEPNP_DLS)
 
124
    {
 
125
        Mat undistortedPoints;
 
126
        undistortPoints(ipoints, undistortedPoints, cameraMatrix, distCoeffs);
 
127
 
 
128
        dls PnP(opoints, undistortedPoints);
 
129
 
 
130
        Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
 
131
        bool result = PnP.compute_pose(R, tvec);
 
132
        if (result)
 
133
            Rodrigues(R, rvec);
 
134
        return result;
 
135
    }
 
136
    else if (flags == SOLVEPNP_UPNP)
 
137
    {
 
138
        upnp PnP(cameraMatrix, opoints, ipoints);
 
139
 
 
140
        Mat R, rvec = _rvec.getMat(), tvec = _tvec.getMat();
 
141
        PnP.compute_pose(R, tvec);
 
142
        Rodrigues(R, rvec);
 
143
        return true;
 
144
    }*/
 
145
    else
 
146
        CV_Error(CV_StsBadArg, "The flags argument must be one of SOLVEPNP_ITERATIVE, SOLVEPNP_P3P, SOLVEPNP_EPNP or SOLVEPNP_DLS");
 
147
    return result;
 
148
}
 
149
 
 
150
class PnPRansacCallback : public PointSetRegistrator::Callback
 
151
{
 
152
 
 
153
public:
 
154
 
 
155
    PnPRansacCallback(Mat _cameraMatrix=Mat(3,3,CV_64F), Mat _distCoeffs=Mat(4,1,CV_64F), int _flags=SOLVEPNP_ITERATIVE,
 
156
            bool _useExtrinsicGuess=false, Mat _rvec=Mat(), Mat _tvec=Mat() )
 
157
        : cameraMatrix(_cameraMatrix), distCoeffs(_distCoeffs), flags(_flags), useExtrinsicGuess(_useExtrinsicGuess),
 
158
          rvec(_rvec), tvec(_tvec) {}
 
159
 
 
160
    /* Pre: True */
 
161
    /* Post: compute _model with given points an return number of found models */
 
162
    int runKernel( InputArray _m1, InputArray _m2, OutputArray _model ) const
 
163
    {
 
164
        Mat opoints = _m1.getMat(), ipoints = _m2.getMat();
 
165
 
 
166
        bool correspondence = solvePnP( _m1, _m2, cameraMatrix, distCoeffs,
 
167
                                            rvec, tvec, useExtrinsicGuess, flags );
 
168
 
 
169
        Mat _local_model;
 
170
        hconcat(rvec, tvec, _local_model);
 
171
        _local_model.copyTo(_model);
 
172
 
 
173
        return correspondence;
 
174
    }
 
175
 
 
176
    /* Pre: True */
 
177
    /* Post: fill _err with projection errors */
 
178
    void computeError( InputArray _m1, InputArray _m2, InputArray _model, OutputArray _err ) const
 
179
    {
 
180
 
 
181
        Mat opoints = _m1.getMat(), ipoints = _m2.getMat(), model = _model.getMat();
 
182
 
 
183
        int i, count = opoints.checkVector(3);
 
184
        Mat _rvec = model.col(0);
 
185
        Mat _tvec = model.col(1);
 
186
 
 
187
 
 
188
        Mat projpoints(count, 2, CV_32FC1);
 
189
        projectPoints(opoints, _rvec, _tvec, cameraMatrix, distCoeffs, projpoints);
 
190
 
 
191
        const Point2f* ipoints_ptr = ipoints.ptr<Point2f>();
 
192
        const Point2f* projpoints_ptr = projpoints.ptr<Point2f>();
 
193
 
 
194
        _err.create(count, 1, CV_32FC1);
 
195
        float* err = _err.getMat().ptr<float>();
 
196
 
 
197
        for ( i = 0; i < count; ++i)
 
198
            err[i] = (float)norm( ipoints_ptr[i] - projpoints_ptr[i] );
 
199
 
 
200
    }
 
201
 
 
202
 
 
203
    Mat cameraMatrix;
 
204
    Mat distCoeffs;
 
205
    int flags;
 
206
    bool useExtrinsicGuess;
 
207
    Mat rvec;
 
208
    Mat tvec;
 
209
};
 
210
 
 
211
bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
 
212
                        InputArray _cameraMatrix, InputArray _distCoeffs,
 
213
                        OutputArray _rvec, OutputArray _tvec, bool useExtrinsicGuess,
 
214
                        int iterationsCount, float reprojectionError, double confidence,
 
215
                        OutputArray _inliers, int flags)
 
216
{
 
217
 
 
218
    Mat opoints0 = _opoints.getMat(), ipoints0 = _ipoints.getMat();
 
219
    Mat opoints, ipoints;
 
220
    if( opoints0.depth() == CV_64F || !opoints0.isContinuous() )
 
221
        opoints0.convertTo(opoints, CV_32F);
 
222
    else
 
223
        opoints = opoints0;
 
224
    if( ipoints0.depth() == CV_64F || !ipoints0.isContinuous() )
 
225
        ipoints0.convertTo(ipoints, CV_32F);
 
226
    else
 
227
        ipoints = ipoints0;
 
228
 
 
229
    int npoints = std::max(opoints.checkVector(3, CV_32F), opoints.checkVector(3, CV_64F));
 
230
    CV_Assert( npoints >= 0 && npoints == std::max(ipoints.checkVector(2, CV_32F), ipoints.checkVector(2, CV_64F)) );
 
231
 
 
232
    CV_Assert(opoints.isContinuous());
 
233
    CV_Assert(opoints.depth() == CV_32F || opoints.depth() == CV_64F);
 
234
    CV_Assert((opoints.rows == 1 && opoints.channels() == 3) || opoints.cols*opoints.channels() == 3);
 
235
    CV_Assert(ipoints.isContinuous());
 
236
    CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
 
237
    CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
 
238
 
 
239
    _rvec.create(3, 1, CV_64FC1);
 
240
    _tvec.create(3, 1, CV_64FC1);
 
241
 
 
242
    Mat rvec = useExtrinsicGuess ? _rvec.getMat() : Mat(3, 1, CV_64FC1);
 
243
    Mat tvec = useExtrinsicGuess ? _tvec.getMat() : Mat(3, 1, CV_64FC1);
 
244
    Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
 
245
 
 
246
    int model_points = 5;
 
247
    int ransac_kernel_method = SOLVEPNP_EPNP;
 
248
 
 
249
    if( npoints == 4 )
 
250
    {
 
251
        model_points = 4;
 
252
        ransac_kernel_method = SOLVEPNP_P3P;
 
253
    }
 
254
 
 
255
    Ptr<PointSetRegistrator::Callback> cb; // pointer to callback
 
256
    cb = makePtr<PnPRansacCallback>( cameraMatrix, distCoeffs, ransac_kernel_method, useExtrinsicGuess, rvec, tvec);
 
257
 
 
258
    double param1 = reprojectionError;                // reprojection error
 
259
    double param2 = confidence;                       // confidence
 
260
    int param3 = iterationsCount;                     // number maximum iterations
 
261
 
 
262
    Mat _local_model(3, 2, CV_64FC1);
 
263
    Mat _mask_local_inliers(1, opoints.rows, CV_8UC1);
 
264
 
 
265
    // call Ransac
 
266
    int result = createRANSACPointSetRegistrator(cb, model_points,
 
267
        param1, param2, param3)->run(opoints, ipoints, _local_model, _mask_local_inliers);
 
268
 
 
269
    if( result > 0 )
 
270
    {
 
271
        vector<Point3d> opoints_inliers;
 
272
        vector<Point2d> ipoints_inliers;
 
273
        opoints.convertTo(opoints_inliers, CV_64F);
 
274
        ipoints.convertTo(ipoints_inliers, CV_64F);
 
275
 
 
276
        const uchar* mask = _mask_local_inliers.ptr<uchar>();
 
277
        int npoints1 = compressElems(&opoints_inliers[0], mask, 1, npoints);
 
278
        compressElems(&ipoints_inliers[0], mask, 1, npoints);
 
279
 
 
280
        opoints_inliers.resize(npoints1);
 
281
        ipoints_inliers.resize(npoints1);
 
282
        result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix,
 
283
                          distCoeffs, rvec, tvec, false, flags == SOLVEPNP_P3P ? SOLVEPNP_EPNP : flags) ? 1 : -1;
 
284
    }
 
285
 
 
286
    if( result <= 0 || _local_model.rows <= 0)
 
287
    {
 
288
        _rvec.assign(rvec);    // output rotation vector
 
289
        _tvec.assign(tvec);    // output translation vector
 
290
 
 
291
        if( _inliers.needed() )
 
292
            _inliers.release();
 
293
 
 
294
        return false;
 
295
    }
 
296
    else
 
297
    {
 
298
        _rvec.assign(_local_model.col(0));    // output rotation vector
 
299
        _tvec.assign(_local_model.col(1));    // output translation vector
 
300
    }
 
301
 
 
302
    if(_inliers.needed())
 
303
    {
 
304
        Mat _local_inliers;
 
305
        for (int i = 0; i < npoints; ++i)
 
306
        {
 
307
            if((int)_mask_local_inliers.at<uchar>(i) != 0) // inliers mask
 
308
                _local_inliers.push_back(i);    // output inliers vector
 
309
        }
 
310
        _local_inliers.copyTo(_inliers);
 
311
    }
 
312
    return true;
 
313
}
 
314
 
 
315
}