ViSP
Main Page
Related Pages
Modules
Classes
Examples
All
Classes
Functions
Variables
Enumerations
Enumerator
Friends
Groups
Pages
vpHomography.cpp
1
/****************************************************************************
2
*
3
* $Id: vpHomography.cpp 4056 2013-01-05 13:04:42Z fspindle $
4
*
5
* This file is part of the ViSP software.
6
* Copyright (C) 2005 - 2013 by INRIA. All rights reserved.
7
*
8
* This software is free software; you can redistribute it and/or
9
* modify it under the terms of the GNU General Public License
10
* ("GPL") version 2 as published by the Free Software Foundation.
11
* See the file LICENSE.txt at the root directory of this source
12
* distribution for additional information about the GNU GPL.
13
*
14
* For using ViSP with software that can not be combined with the GNU
15
* GPL, please contact INRIA about acquiring a ViSP Professional
16
* Edition License.
17
*
18
* See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19
*
20
* This software was developed at:
21
* INRIA Rennes - Bretagne Atlantique
22
* Campus Universitaire de Beaulieu
23
* 35042 Rennes Cedex
24
* France
25
* http://www.irisa.fr/lagadic
26
*
27
* If you have questions regarding the use of this file, please contact
28
* INRIA at visp@inria.fr
29
*
30
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32
*
33
*
34
* Description:
35
* Homography transformation.
36
*
37
* Authors:
38
* Muriel Pressigout
39
* Fabien Spindler
40
*
41
*****************************************************************************/
42
43
50
#include <visp/vpHomography.h>
51
#include <visp/vpDebug.h>
52
#include <visp/vpMatrix.h>
53
54
// Exception
55
#include <visp/vpException.h>
56
#include <visp/vpMatrixException.h>
57
58
// Debug trace
59
#include <visp/vpDebug.h>
60
61
#include <stdio.h>
62
63
64
69
void
70
vpHomography::init()
71
{
72
unsigned
int
i,j ;
73
74
try
{
75
vpMatrix::resize
(3,3) ;
76
}
77
catch
(
vpException
me)
78
{
79
vpERROR_TRACE
(
"Error caught"
) ;
80
throw ;
81
}
82
83
84
for
(i=0 ; i < 3 ; i++)
85
for
(j=0 ; j < 3; j++)
86
if
(i==j)
87
(*this)[i][j] = 1.0 ;
88
else
89
(*
this
)[i][j] = 0.0;
90
91
}
92
96
vpHomography::vpHomography
() :
vpMatrix
()
97
{
98
init() ;
99
}
100
101
106
vpHomography::vpHomography
(
const
vpHomography
&aHb) :
vpMatrix
()
107
{
108
init() ;
109
*
this
= aHb ;
110
}
111
116
vpHomography::vpHomography
(
const
vpHomogeneousMatrix
&aMb,
117
const
vpPlane
&_bP) :
vpMatrix
()
118
{
119
120
121
init() ;
122
123
buildFrom
(aMb,_bP) ;
124
125
126
}
127
128
vpHomography::vpHomography
(
const
vpThetaUVector
&tu,
129
const
vpTranslationVector
&atb,
130
const
vpPlane
&_bP) :
vpMatrix
()
131
{
132
init() ;
133
buildFrom
(tu,atb,_bP) ;
134
}
135
136
vpHomography::vpHomography
(
const
vpRotationMatrix
&aRb,
137
const
vpTranslationVector
&atb,
138
const
vpPlane
&_bP) :
vpMatrix
()
139
{
140
init() ;
141
buildFrom
(aRb,atb,_bP) ;
142
}
143
144
vpHomography::vpHomography
(
const
vpPoseVector
&arb,
145
const
vpPlane
&_bP) :
vpMatrix
()
146
{
147
148
init() ;
149
buildFrom
(arb,_bP) ;
150
}
151
152
153
154
void
155
vpHomography::buildFrom
(
const
vpHomogeneousMatrix
&aMb,
156
const
vpPlane
&_bP)
157
{
158
159
160
insert(aMb) ;
161
insert(_bP) ;
162
build
() ;
163
164
165
}
166
167
void
168
vpHomography::buildFrom
(
const
vpThetaUVector
&tu,
169
const
vpTranslationVector
&atb,
170
const
vpPlane
&_bP)
171
{
172
173
insert(tu) ;
174
insert(atb) ;
175
insert(_bP) ;
176
build
() ;
177
}
178
179
void
180
vpHomography::buildFrom
(
const
vpRotationMatrix
&aRb,
181
const
vpTranslationVector
&atb,
182
const
vpPlane
&_bP)
183
{
184
init() ;
185
insert(aRb) ;
186
insert(atb) ;
187
insert(_bP) ;
188
build
() ;
189
}
190
191
void
192
vpHomography::buildFrom
(
const
vpPoseVector
&arb,
193
const
vpPlane
&_bP)
194
{
195
196
aMb.
buildFrom
(arb[0],arb[1],arb[2],arb[3],arb[4],arb[5]) ;
197
insert(_bP) ;
198
build
() ;
199
}
200
201
202
203
/*********************************************************************/
204
205
210
void
211
vpHomography::insert(
const
vpRotationMatrix
&aRb)
212
{
213
aMb.
insert
(aRb) ;
214
//build() ;
215
}
220
void
221
vpHomography::insert(
const
vpHomogeneousMatrix
&_aMb)
222
{
223
224
aMb = _aMb ;
225
//build() ;
226
}
227
228
234
void
235
vpHomography::insert(
const
vpThetaUVector
&tu)
236
{
237
vpRotationMatrix
aRb(tu) ;
238
aMb.
insert
(aRb) ;
239
//build() ;
240
}
241
242
247
void
248
vpHomography::insert(
const
vpTranslationVector
&atb)
249
{
250
aMb.
insert
(atb) ;
251
//build() ;
252
}
253
258
void
259
vpHomography::insert(
const
vpPlane
&_bP)
260
{
261
262
bP= _bP ;
263
//build() ;
264
}
265
266
273
vpHomography
274
vpHomography::inverse
()
const
275
{
276
vpHomography
bHa ;
277
278
279
vpMatrix::pseudoInverse
(bHa,1e-16) ;
280
281
return
bHa;
282
}
283
290
void
291
vpHomography::inverse
(
vpHomography
&bHa)
const
292
{
293
bHa =
inverse
() ;
294
}
295
296
297
void
298
vpHomography::save
(std::ofstream &f)
const
299
{
300
if
(f != NULL)
301
{
302
f << *this ;
303
}
304
else
305
{
306
vpERROR_TRACE
(
"\t\t file not open "
);
307
throw
(
vpException
(
vpException::ioError
,
"\t\t file not open"
)) ;
308
}
309
}
310
324
vpHomography
vpHomography::operator*
(
const
vpHomography
&H)
const
325
{
326
vpHomography
Hp;
327
for
(
unsigned
int
i = 0; i < 3; i++) {
328
for
(
unsigned
int
j = 0; j < 3; j++) {
329
double
s = 0.;
330
for
(
unsigned
int
k = 0; k < 3; k ++) {
331
s += (*this)[i][k] * H[k][j];
332
}
333
Hp[i][j] = s;
334
}
335
}
336
return
Hp;
337
}
338
353
vpHomography
vpHomography::operator*
(
const
double
&v)
const
354
{
355
vpHomography
H;
356
357
for
(
unsigned
int
i=0; i < 9; i ++) {
358
H.
data
[i] = this->
data
[i] * v;
359
}
360
361
return
H;
362
}
363
377
vpHomography
vpHomography::operator/
(
const
double
&v)
const
378
{
379
vpHomography
H;
380
double
one_over_v = 1. / v;
381
382
for
(
unsigned
int
i=0; i < 9; i ++) {
383
H.
data
[i] = this->
data
[i] * one_over_v;
384
}
385
386
return
H;
387
}
394
void
395
vpHomography::load
(std::ifstream &f)
396
{
397
if
(f != NULL)
398
{
399
for
(
unsigned
int
i=0 ; i < 3 ; i++)
400
for
(
unsigned
int
j=0 ; j < 3 ; j++)
401
{
402
f>> (*this)[i][j] ;
403
}
404
}
405
else
406
{
407
vpERROR_TRACE
(
"\t\t file not open "
);
408
throw
(
vpException
(
vpException::ioError
,
"\t\t file not open"
)) ;
409
}
410
}
411
412
413
415
void
416
vpHomography::print
()
417
{
418
std::cout <<*
this
<< std::endl ;
419
}
420
428
void
429
vpHomography::build
()
430
{
431
unsigned
int
i,j ;
432
433
vpColVector
n(3) ;
434
vpColVector
atb(3) ;
435
for
(i=0 ; i < 3 ; i++)
436
{
437
atb[i] = aMb[i][3] ;
438
for
(j=0 ; j < 3 ; j++) (*
this
)[i][j] = aMb[i][j];
439
}
440
441
bP.
getNormal
(n) ;
442
443
double
d = bP.
getD
() ;
444
*
this
-= atb*n.
t
()/d ;
// the d used in the equation is such as nX=d is the
445
// plane equation. So if the plane is described by
446
// Ax+By+Cz+D=0, d=-D
447
448
}
449
458
void
459
vpHomography::build
(
vpHomography
&aHb,
460
const
vpHomogeneousMatrix
&aMb,
461
const
vpPlane
&bP)
462
{
463
unsigned
int
i,j ;
464
465
vpColVector
n(3) ;
466
vpColVector
atb(3) ;
467
for
(i=0 ; i < 3 ; i++)
468
{
469
atb[i] = aMb[i][3] ;
470
for
(j=0 ; j < 3 ; j++) aHb[i][j] = aMb[i][j];
471
}
472
473
bP.
getNormal
(n) ;
474
475
double
d = bP.
getD
() ;
476
aHb -= atb*n.
t
()/d ;
// the d used in the equation is such as nX=d is the
477
// plane equation. So if the plane is described by
478
// Ax+By+Cz+D=0, d=-D
479
480
}
481
482
/*
483
* Local variables:
484
* c-basic-offset: 2
485
* End:
486
*/
src
computer-vision
homography-estimation
vpHomography.cpp
Generated on Tue Sep 17 2013 00:21:35 for ViSP by
1.8.4