ViSP
Main Page
Related Pages
Modules
Classes
Examples
All
Classes
Functions
Variables
Enumerations
Enumerator
Friends
Groups
Pages
vpFeaturePoint3D.cpp
1
/****************************************************************************
2
*
3
* $Id: vpFeaturePoint3D.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
* 3D point visual feature.
36
*
37
* Authors:
38
* Eric Marchand
39
* Fabien Spindler
40
*
41
*****************************************************************************/
42
43
44
#include <visp/vpBasicFeature.h>
45
#include <visp/vpFeaturePoint3D.h>
46
47
// Exception
48
#include <visp/vpException.h>
49
#include <visp/vpMatrixException.h>
50
#include <visp/vpFeatureException.h>
51
52
// Debug trace
53
#include <visp/vpDebug.h>
54
55
56
57
58
59
/*
60
61
attributes and members directly related to the vpBasicFeature needs
62
other functionalities are useful but not mandatory
63
64
*/
65
73
void
74
vpFeaturePoint3D::init
()
75
{
76
//feature dimension
77
dim_s
= 3 ;
78
nbParameters
= 3;
79
80
// memory allocation
81
s
.
resize
(
dim_s
) ;
82
if
(
flags
== NULL)
83
flags
=
new
bool
[
nbParameters
];
84
for
(
unsigned
int
i = 0; i <
nbParameters
; i++)
flags
[i] =
false
;
85
86
//default value XYZ
87
s
[0] = 0;
88
s
[1] = 0;
89
s
[2] = 1;
90
91
}
92
99
vpFeaturePoint3D::vpFeaturePoint3D
() :
vpBasicFeature
()
100
{
101
init
() ;
102
}
103
104
114
void
115
vpFeaturePoint3D::set_X
(
const
double
X)
116
{
117
s
[0] = X ;
118
flags
[0] =
true
;
119
}
120
130
void
131
vpFeaturePoint3D::set_Y
(
const
double
Y)
132
{
133
s
[1] = Y ;
134
flags
[1] =
true
;
135
}
136
146
void
147
vpFeaturePoint3D::set_Z
(
const
double
Z)
148
{
149
s
[2] = Z ;
150
flags
[2] =
true
;
151
}
152
161
void
162
vpFeaturePoint3D::set_XYZ
(
const
double
X,
163
const
double
Y,
164
const
double
Z)
165
{
166
set_X
(X) ;
167
set_Y
(Y) ;
168
set_Z
(Z) ;
169
170
for
(
unsigned
int
i = 0; i <
nbParameters
; i++)
flags
[i] =
true
;
171
}
172
174
double
175
vpFeaturePoint3D::get_X
()
const
176
{
177
return
s
[0] ;
178
}
179
181
double
182
vpFeaturePoint3D::get_Y
()
const
183
{
184
return
s
[1] ;
185
}
186
188
double
189
vpFeaturePoint3D::get_Z
()
const
190
{
191
return
s
[2] ;
192
}
193
261
vpMatrix
262
vpFeaturePoint3D::interaction
(
const
unsigned
int
select)
263
{
264
vpMatrix
L ;
265
266
L.
resize
(0,6) ;
267
268
if
(
deallocate
==
vpBasicFeature::user
)
269
{
270
for
(
unsigned
int
i = 0; i <
nbParameters
; i++)
271
{
272
if
(
flags
[i] ==
false
)
273
{
274
switch
(i){
275
case
0:
276
vpTRACE
(
"Warning !!! The interaction matrix is computed but X was not set yet"
);
277
break
;
278
case
1:
279
vpTRACE
(
"Warning !!! The interaction matrix is computed but Y was not set yet"
);
280
break
;
281
case
2:
282
vpTRACE
(
"Warning !!! The interaction matrix is computed but Z was not set yet"
);
283
break
;
284
default
:
285
vpTRACE
(
"Problem during the reading of the variable flags"
);
286
}
287
}
288
}
289
resetFlags
();
290
}
291
292
double
X =
get_X
() ;
293
double
Y =
get_Y
() ;
294
double
Z =
get_Z
() ;
295
296
if
(
vpFeaturePoint3D::selectX
() & select )
297
{
298
vpMatrix
Lx(1,6) ; Lx = 0;
299
300
Lx[0][0] = -1 ;
301
Lx[0][1] = 0 ;
302
Lx[0][2] = 0 ;
303
Lx[0][3] = 0 ;
304
Lx[0][4] = -Z ;
305
Lx[0][5] = Y ;
306
307
L =
vpMatrix::stackMatrices
(L,Lx) ;
308
}
309
310
if
(
vpFeaturePoint3D::selectY
() & select )
311
{
312
vpMatrix
Ly(1,6) ; Ly = 0;
313
314
Ly[0][0] = 0 ;
315
Ly[0][1] = -1 ;
316
Ly[0][2] = 0 ;
317
Ly[0][3] = Z ;
318
Ly[0][4] = 0 ;
319
Ly[0][5] = -X ;
320
321
L =
vpMatrix::stackMatrices
(L,Ly) ;
322
}
323
if
(
vpFeaturePoint3D::selectZ
() & select )
324
{
325
vpMatrix
Lz(1,6) ; Lz = 0;
326
327
Lz[0][0] = 0 ;
328
Lz[0][1] = 0 ;
329
Lz[0][2] = -1 ;
330
Lz[0][3] = -Y ;
331
Lz[0][4] = X ;
332
Lz[0][5] = 0 ;
333
334
L =
vpMatrix::stackMatrices
(L,Lz) ;
335
}
336
return
L ;
337
}
338
388
vpColVector
389
vpFeaturePoint3D::error
(
const
vpBasicFeature
&s_star,
390
const
unsigned
int
select)
391
{
392
vpColVector
e(0) ;
393
394
try
{
395
if
(
vpFeaturePoint3D::selectX
() & select )
396
{
397
vpColVector
ex(1) ;
398
ex[0] =
s
[0] - s_star[0] ;
399
400
e =
vpMatrix::stackMatrices
(e,ex) ;
401
}
402
403
if
(
vpFeaturePoint3D::selectY
() & select )
404
{
405
vpColVector
ey(1) ;
406
ey[0] =
s
[1] - s_star[1] ;
407
e =
vpMatrix::stackMatrices
(e,ey) ;
408
}
409
410
if
(
vpFeaturePoint3D::selectZ
() & select )
411
{
412
vpColVector
ez(1) ;
413
ez[0] =
s
[2] - s_star[2] ;
414
e =
vpMatrix::stackMatrices
(e,ez) ;
415
}
416
}
417
catch
(
vpMatrixException
me)
418
{
419
vpERROR_TRACE
(
"caught a Matrix related error"
) ;
420
std::cout <<std::endl << me << std::endl ;
421
throw
(me) ;
422
}
423
catch
(
vpException
me)
424
{
425
vpERROR_TRACE
(
"caught another error"
) ;
426
std::cout <<std::endl << me << std::endl ;
427
throw
(me) ;
428
}
429
430
return
e ;
431
432
}
433
450
void
451
vpFeaturePoint3D::buildFrom
(
const
vpPoint
&p)
452
{
453
454
// cP is expressed in homogeneous coordinates
455
// we devide by the fourth coordinate
456
s
[0] = p.
cP
[0]/p.
cP
[3] ;
457
s
[1] = p.
cP
[1]/p.
cP
[3] ;
458
s
[2] = p.
cP
[2]/p.
cP
[3] ;
459
460
double
Z =
s
[2] ;
461
if
(Z < 0)
462
{
463
vpERROR_TRACE
(
"Point is behind the camera "
) ;
464
std::cout <<
"Z = "
<< Z << std::endl ;
465
466
throw
(
vpFeatureException
(
vpFeatureException::badInitializationError
,
467
"Point is behind the camera "
)) ;
468
}
469
470
if
(fabs(Z) < 1e-6)
471
{
472
vpERROR_TRACE
(
"Point Z coordinates is null "
) ;
473
std::cout <<
"Z = "
<< Z << std::endl ;
474
475
throw
(
vpFeatureException
(
vpFeatureException::badInitializationError
,
476
"Point Z coordinates is null"
)) ;
477
}
478
479
for
(
unsigned
int
i = 0; i <
nbParameters
; i++)
flags
[i] =
true
;
480
481
}
482
499
void
500
vpFeaturePoint3D::buildFrom
(
const
double
X,
const
double
Y,
const
double
Z)
501
{
502
503
s
[0] = X ;
504
s
[1] = Y ;
505
s
[2] = Z ;
506
507
if
(Z < 0)
508
{
509
vpERROR_TRACE
(
"Point is behind the camera "
) ;
510
std::cout <<
"Z = "
<< Z << std::endl ;
511
512
throw
(
vpFeatureException
(
vpFeatureException::badInitializationError
,
513
"Point is behind the camera "
)) ;
514
}
515
516
if
(fabs(Z) < 1e-6)
517
{
518
vpERROR_TRACE
(
"Point Z coordinates is null "
) ;
519
std::cout <<
"Z = "
<< Z << std::endl ;
520
521
throw
(
vpFeatureException
(
vpFeatureException::badInitializationError
,
522
"Point Z coordinates is null"
)) ;
523
}
524
525
for
(
unsigned
int
i = 0; i <
nbParameters
; i++)
flags
[i] =
true
;
526
527
}
528
529
553
void
554
vpFeaturePoint3D::print
(
const
unsigned
int
select )
const
555
{
556
557
std::cout <<
"Point3D: "
;
558
if
(
vpFeaturePoint3D::selectX
() & select )
559
std::cout <<
" X="
<<
get_X
() ;
560
if
(
vpFeaturePoint3D::selectY
() & select )
561
std::cout <<
" Y="
<<
get_Y
() ;
562
if
(
vpFeaturePoint3D::selectZ
() & select )
563
std::cout <<
" Z="
<<
get_Z
() ;
564
std::cout <<std::endl ;
565
}
566
567
579
vpFeaturePoint3D
*
vpFeaturePoint3D::duplicate
()
const
580
{
581
vpFeaturePoint3D
*feature =
new
vpFeaturePoint3D
;
582
return
feature ;
583
}
584
589
void
590
vpFeaturePoint3D::display
(
const
vpCameraParameters
&
/*cam*/
,
591
const
vpImage<unsigned char>
&
/* I */
,
592
const
vpColor
&
/* color */
,
593
unsigned
int
/* thickness */
)
const
594
{
595
static
int
firsttime =0 ;
596
597
if
(firsttime==0)
598
{
599
firsttime=1 ;
600
vpERROR_TRACE
(
"not implemented"
) ;
601
// Do not throw and error since it is not subject
602
// to produce a failure
603
}
604
}
605
610
void
611
vpFeaturePoint3D::display
(
const
vpCameraParameters
&
/*cam*/
,
612
const
vpImage<vpRGBa>
&
/* I */
,
613
const
vpColor
&
/* color */
,
614
unsigned
int
/* thickness */
)
const
615
{
616
static
int
firsttime =0 ;
617
618
if
(firsttime==0)
619
{
620
firsttime=1 ;
621
vpERROR_TRACE
(
"not implemented"
) ;
622
// Do not throw and error since it is not subject
623
// to produce a failure
624
}
625
}
626
627
/*
628
* Local variables:
629
* c-basic-offset: 2
630
* End:
631
*/
src
visual-feature
vpFeaturePoint3D.cpp
Generated on Thu Nov 7 2013 03:14:09 for ViSP by
1.8.4