root / rgbdslam / gicp / transform.h @ 9240aaa3
History  View  Annotate  Download (2.33 KB)
1 
#ifndef DGC_TRANSFORM_H


2 
#define DGC_TRANSFORM_H

3  
4 
#ifdef __cplusplus

5 
//extern "C" {

6 
#endif

7  
8 
/** 3D transformation  can represent a combined translation and rotation */

9  
10 
typedef double dgc_transform_t[4][4]; 
11  
12 
/** Print a 3D transform as a 4x4 matrix

13 
@param t  transform to be printed

14 
¶m str  name of transform, which will be printed above matrix */

15  
16 
void dgc_transform_print(dgc_transform_t t, const char *str); 
17  
18 
/** Initializes a transform as the identity transform */

19  
20 
void dgc_transform_identity(dgc_transform_t t);

21  
22 
/** Left multiply transform t1 by transform t2 */

23  
24 
void dgc_transform_left_multiply(dgc_transform_t t1, dgc_transform_t t2);

25  
26 
/** Rotate tranform around global x axis */

27  
28 
void dgc_transform_rotate_x(dgc_transform_t t, double theta); 
29  
30 
/** Rotate transform around global y axis */

31  
32 
void dgc_transform_rotate_y(dgc_transform_t t, double theta); 
33  
34 
/** Rotate transform around global z axis */

35  
36 
void dgc_transform_rotate_z(dgc_transform_t t, double theta); 
37  
38 
/** Add translation to transform */

39  
40 
void dgc_transform_translate(dgc_transform_t t, double x, double y, double z); 
41  
42 
/** Apply transform to 3D point */

43  
44 
extern inline void dgc_transform_point(double *x, double *y, double *z, 
45 
dgc_transform_t t) 
46 
{ 
47 
double x2, y2, z2;

48  
49 
x2 = t[0][0] * *x + t[0][1] * *y + t[0][2] * *z + t[0][3]; 
50 
y2 = t[1][0] * *x + t[1][1] * *y + t[1][2] * *z + t[1][3]; 
51 
z2 = t[2][0] * *x + t[2][1] * *y + t[2][2] * *z + t[2][3]; 
52 
*x = x2; 
53 
*y = y2; 
54 
*z = z2; 
55 
} 
56  
57 
/** Copy transform src to dest */

58  
59 
void dgc_transform_copy(dgc_transform_t dest, dgc_transform_t src);

60  
61 
/** Read a transform from file */

62  
63 
int dgc_transform_read(dgc_transform_t t, const char *filename); 
64  
65 
/** Write a transform from file */

66  
67 
int dgc_transform_write(dgc_transform_t t, const char *filename); 
68  
69 
/** Read a transform from a string */

70  
71 
int dgc_transform_read_string(dgc_transform_t t, char *str); 
72  
73 
/** Get translation */

74  
75 
void dgc_transform_get_translation(dgc_transform_t t, double *x, double *y, 
76 
double *z);

77  
78 
/** Get rotation */

79  
80 
void dgc_transform_get_rotation(dgc_transform_t t, double *x, double *y, 
81 
double *z);

82  
83 
void dgc_transform_rpy(dgc_transform_t dest, dgc_transform_t src, double roll, 
84 
double pitch, double yaw); 
85  
86 
void dgc_transform_inverse(dgc_transform_t in, dgc_transform_t out);

87  
88 
#ifdef __cplusplus

89 
//}

90 
#endif

91  
92 
#endif
