/
dq_homo.h
119 lines (111 loc) · 3.3 KB
/
dq_homo.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
#ifndef _DQ_HOMO_H
# define _DQ_HOMO_H
/**
* @file dq_homo.h
*
* @brief File containing functions related to 4 by 4 homogeneous matrix.
*/
/**
* @defgroup homo Auxiliary Homogeneous Matrix Functions
* @brief Set of auxiliary functions to manipulate homogeneous matrix.
*
* Due to the fact that the bottom row of each homogeneous matrix is always
* the same, we can simplify the amount of data needed to be stored.
*
* The full homogeneous matrix is:
*
* \f[
* H = \left( \begin{array}{cc}
* R & d \\
* 0 & 1
* \end{array} \right)
* \f]
*
* Where,
*
* \f{align}{
* R &\in \mathbb{R}^{3x3} \\
* d &\in \mathbb{R}^{3}
* \f}
*
* Where R is a 3x3 rotation matrix and d is a 3d translation vector. The
* bottom row is always \f$ ( 0, 0, 0, 1 ) \f$.
*
* We simplify this to a 3x4 matrix by eliminating the bottom row and
* thus we end up with:
*
* \f[
* H = \left( \begin{array}{cc}
* R & t
* \end{array} \right) \in \mathbb{R}^{3x4}
* \f]
*/
/** @{ */
/**
* @brief Creates a homogeneous matrix by joining a 3x3 rotation matrix and a 3d translation vector.
*
* @param[out] H Homogeneous matrix formed by R and d.
* @param[in] R 3x3 Rotation matrix.
* @param[in] d 3d translation vector.
* @sa homo_op_split
*/
void homo_cr_join( double H[3][4], double R[3][3], double d[3] );
/**
* @brief Multiplies two homogeneous matrix.
*
* \f[
* O = A * B
* \f]
*
* @param[out] O Resulting homogeneous matrix of the multiplication.
* @param[in] A First homogeneous matrix to operate on.
* @param[in] B Second homogeneous matrix to operate on.
* @sa homo_op_mul_vec
*/
void homo_op_mul( double O[3][4], double A[3][4], double B[3][4] );
/**
* @brief Splits a homogeneous matrix into a 3x3 rotation matrix and a 3d translation vector.
*
* @param[out] R 3x3 rotation matrix extracted from the homogeneous matrix.
* @param[out] d 3d translation vector extracted from the homogeneous matrix.
* @param[in] H Homogeneous matrix to split.
*/
void homo_op_split( double R[3][3], double d[3], double H[3][4] );
/**
* @brief Multiplies a homogeneous matrix by a 4d vector.
*
* To multiply a normal 3d vector, add 1 as it's 4th component.
*
* @param[out] o Resulting 4d vector of the multiplication.
* @param[in] H Homogeneous matrix to multiply against.
* @param[in] v 4d vector to multiply.
* @sa homo_op_mul
*/
void homo_op_mul_vec( double o[4], double H[3][4], const double v[4] );
/**
* @brief Compares two homogeneous matrix with variable precision.
*
* @param[in] A First homogeneous matrix to compare.
* @param[in] B Second homogeneous matrix to compare.
* @param[in] precision Precision to use when comparing.
* @return 0 if they are the same, 1 otherwise.
* @sa homo_ch_cmp
*/
int homo_ch_cmpV( double A[3][4], double B[3][4], double precision );
/**
* @brief Compares two homogeneous matrix.
*
* @param[in] A First homogeneous matrix to compare.
* @param[in] B Second homogeneous matrix to compare.
* @return 0 if they are the same, 1 otherwise.
* @sa homo_ch_cmpV
*/
int homo_ch_cmp( double A[3][4], double B[3][4] );
/**
* @brief Prints a homogeneous matrix on screen.
*
* @param[in] H Homogeneous matrix to print on screen.
*/
void homo_print( double H[3][4] );
/** @} */
#endif /* _DQ_HOMO_H */