ViSP
Main Page
Related Pages
Modules
Classes
Examples
All
Classes
Functions
Variables
Enumerations
Enumerator
Friends
Groups
Pages
vpPoseVector.cpp
1
/****************************************************************************
2
*
3
* $Id: vpPoseVector.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
* Pose object. A pose is a size 6 vector [t, tu]^T where tu is
36
* a rotation vector (theta u representation) and t is a translation vector.
37
*
38
* Authors:
39
* Eric Marchand
40
* Fabien Spindler
41
*
42
*****************************************************************************/
43
44
51
#include <visp/vpPoseVector.h>
52
#include <visp/vpMath.h>
53
#include <visp/vpDebug.h>
54
#include <visp/vpMatrixException.h>
55
#include <visp/vpException.h>
56
60
void
61
vpPoseVector::init()
62
{
63
resize
(6) ;
64
}
65
78
vpPoseVector::vpPoseVector
()
79
{
80
init() ;
81
}
82
98
vpPoseVector::vpPoseVector
(
const
double
tx,
99
const
double
ty,
100
const
double
tz,
101
const
double
tux,
102
const
double
tuy,
103
const
double
tuz)
104
{
105
init() ;
106
107
(*this)[0] = tx ;
108
(*this)[1] = ty ;
109
(*this)[2] = tz ;
110
111
(*this)[3] = tux ;
112
(*this)[4] = tuy ;
113
(*this)[5] = tuz ;
114
}
115
126
vpPoseVector::vpPoseVector
(
const
vpTranslationVector
& t,
127
const
vpThetaUVector
& tu)
128
{
129
init() ;
130
buildFrom
(t,tu) ;
131
}
132
145
vpPoseVector::vpPoseVector
(
const
vpTranslationVector
& t,
146
const
vpRotationMatrix
& R)
147
{
148
init() ;
149
buildFrom
(t,R) ;
150
}
151
162
vpPoseVector::vpPoseVector
(
const
vpHomogeneousMatrix
& M)
163
{
164
init() ;
165
buildFrom
(M) ;
166
}
167
179
vpPoseVector
180
vpPoseVector::buildFrom
(
const
vpHomogeneousMatrix
& M)
181
{
182
vpRotationMatrix
R ; M.
extract
(R) ;
183
vpTranslationVector
t
; M.
extract
(t) ;
184
buildFrom
(t,R) ;
185
return
*this ;
186
}
187
199
vpPoseVector
200
vpPoseVector::buildFrom
(
const
vpTranslationVector
& t,
201
const
vpThetaUVector
& tu)
202
{
203
for
(
unsigned
int
i =0 ; i < 3 ; i++)
204
{
205
(*this)[i] = t[i] ;
206
(*this)[i+3] = tu[i] ;
207
}
208
return
*this ;
209
}
210
224
vpPoseVector
225
vpPoseVector::buildFrom
(
const
vpTranslationVector
& t,
226
const
vpRotationMatrix
& R)
227
{
228
vpThetaUVector
tu ;
229
tu.
buildFrom
(R) ;
230
231
buildFrom
(t,tu) ;
232
return
*this ;
233
}
234
235
236
256
void
257
vpPoseVector::print
()
258
{
259
for
(
unsigned
int
i =0 ; i < 6 ; i++)
260
if
(i<3) std::cout << (*this)[i] <<
" "
;
261
else
std::cout <<
vpMath::deg
((*
this
)[i]) <<
" "
;
262
std::cout <<std::endl ;
263
264
}
265
276
void
277
vpPoseVector::save
(std::ofstream &f)
const
278
{
279
if
(f != NULL)
280
{
281
f << *this ;
282
}
283
else
284
{
285
vpERROR_TRACE
(
"\t\t file not open "
);
286
throw
(
vpException
(
vpException::ioError
,
"\t\t file not open"
)) ;
287
}
288
}
289
290
301
void
302
vpPoseVector::load
(std::ifstream &f)
303
{
304
if
(f != NULL)
305
{
306
for
(
unsigned
int
i=0 ; i < 6 ; i++)
307
{
308
f>> (*this)[i] ;
309
}
310
}
311
else
312
{
313
vpERROR_TRACE
(
"\t\t file not open "
);
314
throw
(
vpException
(
vpException::ioError
,
"\t\t file not open"
)) ;
315
}
316
}
317
318
319
/*
320
* Local variables:
321
* c-basic-offset: 2
322
* End:
323
*/
src
math
transformation
vpPoseVector.cpp
Generated on Fri Sep 27 2013 21:09:15 for ViSP by
1.8.4