← 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/shape/src/tps_trans.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/shape/src/tps_trans.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
 
 
45
namespace cv
 
46
{
 
47
 
 
48
class ThinPlateSplineShapeTransformerImpl : public ThinPlateSplineShapeTransformer
 
49
{
 
50
public:
 
51
    /* Constructors */
 
52
    ThinPlateSplineShapeTransformerImpl()
 
53
    {
 
54
        regularizationParameter=0;
 
55
        name_ = "ShapeTransformer.TPS";
 
56
        tpsComputed=false;
 
57
    }
 
58
 
 
59
    ThinPlateSplineShapeTransformerImpl(double _regularizationParameter)
 
60
    {
 
61
        regularizationParameter=_regularizationParameter;
 
62
        name_ = "ShapeTransformer.TPS";
 
63
        tpsComputed=false;
 
64
    }
 
65
 
 
66
    /* Destructor */
 
67
    ~ThinPlateSplineShapeTransformerImpl()
 
68
    {
 
69
    }
 
70
 
 
71
    //! the main operators
 
72
    virtual void estimateTransformation(InputArray transformingShape, InputArray targetShape, std::vector<DMatch> &matches);
 
73
    virtual float applyTransformation(InputArray inPts, OutputArray output=noArray());
 
74
    virtual void warpImage(InputArray transformingImage, OutputArray output,
 
75
                           int flags, int borderMode, const Scalar& borderValue) const;
 
76
 
 
77
    //! Setters/Getters
 
78
    virtual void setRegularizationParameter(double _regularizationParameter) {regularizationParameter=_regularizationParameter;}
 
79
    virtual double getRegularizationParameter() const {return regularizationParameter;}
 
80
 
 
81
    //! write/read
 
82
    virtual void write(FileStorage& fs) const
 
83
    {
 
84
        fs << "name" << name_
 
85
           << "regularization" << regularizationParameter;
 
86
    }
 
87
 
 
88
    virtual void read(const FileNode& fn)
 
89
    {
 
90
        CV_Assert( (String)fn["name"] == name_ );
 
91
        regularizationParameter = (int)fn["regularization"];
 
92
    }
 
93
 
 
94
private:
 
95
    bool tpsComputed;
 
96
    double regularizationParameter;
 
97
    float transformCost;
 
98
    Mat tpsParameters;
 
99
    Mat shapeReference;
 
100
 
 
101
protected:
 
102
    String name_;
 
103
};
 
104
 
 
105
static float distance(Point2f p, Point2f q)
 
106
{
 
107
    Point2f diff = p - q;
 
108
    float norma = diff.x*diff.x + diff.y*diff.y;// - 2*diff.x*diff.y;
 
109
    if (norma<0) norma=0;
 
110
    //else norma = std::sqrt(norma);
 
111
    norma = norma*std::log(norma+FLT_EPSILON);
 
112
    return norma;
 
113
}
 
114
 
 
115
static Point2f _applyTransformation(const Mat &shapeRef, const Point2f point, const Mat &tpsParameters)
 
116
{
 
117
    Point2f out;
 
118
    for (int i=0; i<2; i++)
 
119
    {
 
120
        float a1=tpsParameters.at<float>(tpsParameters.rows-3,i);
 
121
        float ax=tpsParameters.at<float>(tpsParameters.rows-2,i);
 
122
        float ay=tpsParameters.at<float>(tpsParameters.rows-1,i);
 
123
 
 
124
        float affine=a1+ax*point.x+ay*point.y;
 
125
        float nonrigid=0;
 
126
        for (int j=0; j<shapeRef.rows; j++)
 
127
        {
 
128
            nonrigid+=tpsParameters.at<float>(j,i)*
 
129
                    distance(Point2f(shapeRef.at<float>(j,0),shapeRef.at<float>(j,1)),
 
130
                            point);
 
131
        }
 
132
        if (i==0)
 
133
        {
 
134
            out.x=affine+nonrigid;
 
135
        }
 
136
        if (i==1)
 
137
        {
 
138
            out.y=affine+nonrigid;
 
139
        }
 
140
    }
 
141
    return out;
 
142
}
 
143
 
 
144
/* public methods */
 
145
void ThinPlateSplineShapeTransformerImpl::warpImage(InputArray transformingImage, OutputArray output,
 
146
                                      int flags, int borderMode, const Scalar& borderValue) const
 
147
{
 
148
    CV_Assert(tpsComputed==true);
 
149
 
 
150
    Mat theinput = transformingImage.getMat();
 
151
    Mat mapX(theinput.rows, theinput.cols, CV_32FC1);
 
152
    Mat mapY(theinput.rows, theinput.cols, CV_32FC1);
 
153
 
 
154
    for (int row = 0; row < theinput.rows; row++)
 
155
    {
 
156
        for (int col = 0; col < theinput.cols; col++)
 
157
        {
 
158
            Point2f pt = _applyTransformation(shapeReference, Point2f(float(col), float(row)), tpsParameters);
 
159
            mapX.at<float>(row, col) = pt.x;
 
160
            mapY.at<float>(row, col) = pt.y;
 
161
        }
 
162
    }
 
163
    remap(transformingImage, output, mapX, mapY, flags, borderMode, borderValue);
 
164
}
 
165
 
 
166
float ThinPlateSplineShapeTransformerImpl::applyTransformation(InputArray inPts, OutputArray outPts)
 
167
{
 
168
    CV_Assert(tpsComputed);
 
169
    Mat pts1 = inPts.getMat();
 
170
    CV_Assert((pts1.channels()==2) && (pts1.cols>0));
 
171
 
 
172
    //Apply transformation in the complete set of points
 
173
    // Ensambling output //
 
174
    if (outPts.needed())
 
175
    {
 
176
        outPts.create(1,pts1.cols, CV_32FC2);
 
177
        Mat outMat = outPts.getMat();
 
178
        for (int i=0; i<pts1.cols; i++)
 
179
        {
 
180
            Point2f pt=pts1.at<Point2f>(0,i);
 
181
            outMat.at<Point2f>(0,i)=_applyTransformation(shapeReference, pt, tpsParameters);
 
182
        }
 
183
    }
 
184
 
 
185
    return transformCost;
 
186
}
 
187
 
 
188
void ThinPlateSplineShapeTransformerImpl::estimateTransformation(InputArray _pts1, InputArray _pts2,
 
189
                                                               std::vector<DMatch>& _matches )
 
190
{
 
191
    Mat pts1 = _pts1.getMat();
 
192
    Mat pts2 = _pts2.getMat();
 
193
    CV_Assert((pts1.channels()==2) && (pts1.cols>0) && (pts2.channels()==2) && (pts2.cols>0));
 
194
    CV_Assert(_matches.size()>1);
 
195
 
 
196
    if (pts1.type() != CV_32F)
 
197
        pts1.convertTo(pts1, CV_32F);
 
198
    if (pts2.type() != CV_32F)
 
199
        pts2.convertTo(pts2, CV_32F);
 
200
 
 
201
    // Use only valid matchings //
 
202
    std::vector<DMatch> matches;
 
203
    for (size_t i=0; i<_matches.size(); i++)
 
204
    {
 
205
        if (_matches[i].queryIdx<pts1.cols &&
 
206
            _matches[i].trainIdx<pts2.cols)
 
207
        {
 
208
            matches.push_back(_matches[i]);
 
209
        }
 
210
    }
 
211
 
 
212
    // Organizing the correspondent points in matrix style //
 
213
    Mat shape1((int)matches.size(),2,CV_32F); // transforming shape
 
214
    Mat shape2((int)matches.size(),2,CV_32F); // target shape
 
215
    for (int i=0, end = (int)matches.size(); i<end; i++)
 
216
    {
 
217
        Point2f pt1=pts1.at<Point2f>(0,matches[i].queryIdx);
 
218
        shape1.at<float>(i,0) = pt1.x;
 
219
        shape1.at<float>(i,1) = pt1.y;
 
220
 
 
221
        Point2f pt2=pts2.at<Point2f>(0,matches[i].trainIdx);
 
222
        shape2.at<float>(i,0) = pt2.x;
 
223
        shape2.at<float>(i,1) = pt2.y;
 
224
    }
 
225
    shape1.copyTo(shapeReference);
 
226
 
 
227
    // Building the matrices for solving the L*(w|a)=(v|0) problem with L={[K|P];[P'|0]}
 
228
 
 
229
    //Building K and P (Neede to buil L)
 
230
    Mat matK((int)matches.size(),(int)matches.size(),CV_32F);
 
231
    Mat matP((int)matches.size(),3,CV_32F);
 
232
    for (int i=0, end=(int)matches.size(); i<end; i++)
 
233
    {
 
234
        for (int j=0; j<end; j++)
 
235
        {
 
236
            if (i==j)
 
237
            {
 
238
                matK.at<float>(i,j)=float(regularizationParameter);
 
239
            }
 
240
            else
 
241
            {
 
242
                matK.at<float>(i,j) = distance(Point2f(shape1.at<float>(i,0),shape1.at<float>(i,1)),
 
243
                                               Point2f(shape1.at<float>(j,0),shape1.at<float>(j,1)));
 
244
            }
 
245
        }
 
246
        matP.at<float>(i,0) = 1;
 
247
        matP.at<float>(i,1) = shape1.at<float>(i,0);
 
248
        matP.at<float>(i,2) = shape1.at<float>(i,1);
 
249
    }
 
250
 
 
251
    //Building L
 
252
    Mat matL=Mat::zeros((int)matches.size()+3,(int)matches.size()+3,CV_32F);
 
253
    Mat matLroi(matL, Rect(0,0,(int)matches.size(),(int)matches.size())); //roi for K
 
254
    matK.copyTo(matLroi);
 
255
    matLroi = Mat(matL,Rect((int)matches.size(),0,3,(int)matches.size())); //roi for P
 
256
    matP.copyTo(matLroi);
 
257
    Mat matPt;
 
258
    transpose(matP,matPt);
 
259
    matLroi = Mat(matL,Rect(0,(int)matches.size(),(int)matches.size(),3)); //roi for P'
 
260
    matPt.copyTo(matLroi);
 
261
 
 
262
    //Building B (v|0)
 
263
    Mat matB = Mat::zeros((int)matches.size()+3,2,CV_32F);
 
264
    for (int i=0, end = (int)matches.size(); i<end; i++)
 
265
    {
 
266
        matB.at<float>(i,0) = shape2.at<float>(i,0); //x's
 
267
        matB.at<float>(i,1) = shape2.at<float>(i,1); //y's
 
268
    }
 
269
 
 
270
    //Obtaining transformation params (w|a)
 
271
    solve(matL, matB, tpsParameters, DECOMP_LU);
 
272
    //tpsParameters = matL.inv()*matB;
 
273
 
 
274
    //Setting transform Cost and Shape reference
 
275
    Mat w(tpsParameters, Rect(0,0,2,tpsParameters.rows-3));
 
276
    Mat Q=w.t()*matK*w;
 
277
    transformCost=fabs(Q.at<float>(0,0)*Q.at<float>(1,1));//fabs(mean(Q.diag(0))[0]);//std::max(Q.at<float>(0,0),Q.at<float>(1,1));
 
278
    tpsComputed=true;
 
279
}
 
280
 
 
281
Ptr <ThinPlateSplineShapeTransformer> createThinPlateSplineShapeTransformer(double regularizationParameter)
 
282
{
 
283
    return Ptr<ThinPlateSplineShapeTransformer>( new ThinPlateSplineShapeTransformerImpl(regularizationParameter) );
 
284
}
 
285
 
 
286
} // cv