Pose.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
3  *
4  * Copyright 2007-2012 VTT Technical Research Centre of Finland
5  *
6  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
7  * <http://www.vtt.fi/multimedia/alvar.html>
8  *
9  * ALVAR is free software; you can redistribute it and/or modify it under the
10  * terms of the GNU Lesser General Public License as published by the Free
11  * Software Foundation; either version 2.1 of the License, or (at your option)
12  * any later version.
13  *
14  * This library is distributed in the hope that it will be useful, but WITHOUT
15  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
16  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
17  * for more details.
18  *
19  * You should have received a copy of the GNU Lesser General Public License
20  * along with ALVAR; if not, see
21  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
22  */
23 
24 #include "ar_track_alvar/Pose.h"
25 
26 using namespace std;
27 
28 namespace alvar {
29 using namespace std;
30 
31 void Pose::Output() const {
32  cout<<quaternion[0]<<","<<quaternion[1]<<","<<quaternion[2]<<","<<quaternion[3]<<"|";
33  cout<<translation[0]<<","<<translation[1]<<","<<translation[2]<<endl;
34 }
35 
36 Pose::Pose() : Rotation() {
37  cvInitMatHeader(&translation_mat, 4, 1, CV_64F, translation);
38  cvZero(&translation_mat);
39  cvmSet(&translation_mat, 3, 0, 1);
40 }
41 
42 Pose::Pose(CvMat *tra, CvMat *rot, RotationType t) : Rotation(rot, t) {
43  cvInitMatHeader(&translation_mat, 4, 1, CV_64F, translation);
44  cvZero(&translation_mat);
45  cvmSet(&translation_mat, 3, 0, 1);
46  // Fill in translation part
47  cvmSet(&translation_mat, 0, 0, cvmGet(tra, 0, 0));
48  cvmSet(&translation_mat, 1, 0, cvmGet(tra, 1, 0));
49  cvmSet(&translation_mat, 2, 0, cvmGet(tra, 2, 0));
50 }
51 
52 Pose::Pose(CvMat *mat) : Rotation(mat, MAT) {
53  cvInitMatHeader(&translation_mat, 4, 1, CV_64F, translation);
54  cvZero(&translation_mat);
55  cvmSet(&translation_mat, 3, 0, 1);
56  // Fill in translation part
57  if (mat->cols == 4) {
58  cvmSet(&translation_mat, 0, 0, cvmGet(mat, 0, 3));
59  cvmSet(&translation_mat, 1, 0, cvmGet(mat, 1, 3));
60  cvmSet(&translation_mat, 2, 0, cvmGet(mat, 2, 3));
61  }
62 }
63 
64 Pose::Pose(const Pose& p) :Rotation(p) {
65  cvInitMatHeader(&translation_mat, 4, 1, CV_64F, translation);
66  cvCopy(&p.translation_mat, &translation_mat);
67 }
68 
70 {
71  cvZero(&quaternion_mat); cvmSet(&quaternion_mat, 0, 0, 1);
72  cvZero(&translation_mat);
73 }
74 
75 void Pose::SetMatrix(const CvMat *mat)
76 {
77  double tmp[9];
78  for(int i = 0; i < 3; ++i)
79  for(int j = 0; j < 3; ++j)
80  tmp[i*3+j] = cvmGet(mat, i, j);
81 
82  Mat9ToQuat(tmp, quaternion);
83  if (mat->cols == 4) {
84  cvmSet(&translation_mat, 0, 0, cvmGet(mat, 0, 3));
85  cvmSet(&translation_mat, 1, 0, cvmGet(mat, 1, 3));
86  cvmSet(&translation_mat, 2, 0, cvmGet(mat, 2, 3));
87  cvmSet(&translation_mat, 3, 0, 1);
88  }
89 }
90 
91 void Pose::GetMatrix(CvMat *mat) const
92 {
93  if (mat->width == 3) {
94  QuatToMat9(quaternion, mat->data.db);
95  } else if (mat->width == 4) {
96  cvSetIdentity(mat);
97  QuatToMat16(quaternion, mat->data.db);
98  cvmSet(mat, 0, 3, cvmGet(&translation_mat, 0, 0));
99  cvmSet(mat, 1, 3, cvmGet(&translation_mat, 1, 0));
100  cvmSet(mat, 2, 3, cvmGet(&translation_mat, 2, 0));
101  }
102 }
103 
104 void Pose::GetMatrixGL(double gl[16], bool mirror)
105 {
106  if (mirror) Mirror(false, true, true);
107  CvMat gl_mat = cvMat(4, 4, CV_64F, gl);
108  GetMatrix(&gl_mat);
109  cvTranspose(&gl_mat, &gl_mat);
110  if (mirror) Mirror(false, true, true);
111 }
112 
113 void Pose::SetMatrixGL(double gl[16], bool mirror)
114 {
115  double gll[16];
116  memcpy(gll, gl, sizeof(double)*16);
117  CvMat gl_mat = cvMat(4, 4, CV_64F, gll);
118  cvTranspose(&gl_mat, &gl_mat);
119  SetMatrix(&gl_mat);
120  if (mirror) Mirror(false, true, true);
121 }
122 
124 {
125  double tmp[16];
126  CvMat tmp_mat = cvMat(4, 4, CV_64F, tmp);
127  GetMatrix(&tmp_mat);
128  cvTranspose(&tmp_mat, &tmp_mat);
129  SetMatrix(&tmp_mat);
130 }
131 
133 {
134  double tmp[16];
135  CvMat tmp_mat = cvMat(4, 4, CV_64F, tmp);
136  GetMatrix(&tmp_mat);
137  cvInvert(&tmp_mat, &tmp_mat);
138  SetMatrix(&tmp_mat);
139 }
140 
141 void Pose::Mirror(bool x, bool y, bool z)
142 {
143  double tmp[16];
144  CvMat tmp_mat = cvMat(4, 4, CV_64F, tmp);
145  GetMatrix(&tmp_mat);
146  MirrorMat(&tmp_mat, x, y, z);
147  SetMatrix(&tmp_mat);
148 }
149 
150 void Pose::SetTranslation(const CvMat *tra) {
151  cvmSet(&translation_mat, 0, 0, cvmGet(tra, 0, 0));
152  cvmSet(&translation_mat, 1, 0, cvmGet(tra, 1, 0));
153  cvmSet(&translation_mat, 2, 0, cvmGet(tra, 2, 0));
154  cvmSet(&translation_mat, 3, 0, 1);
155 }
156 void Pose::SetTranslation(const double *tra) {
157  translation[0] = tra[0];
158  translation[1] = tra[1];
159  translation[2] = tra[2];
160  translation[3] = 1;
161 }
162 void Pose::SetTranslation(const double x, const double y, const double z) {
163  translation[0] = x;
164  translation[1] = y;
165  translation[2] = z;
166  translation[3] = 1;
167 }
168 void Pose::GetTranslation( CvMat *tra) const{
169  cvmSet(tra, 0, 0, cvmGet(&translation_mat, 0, 0));
170  cvmSet(tra, 1, 0, cvmGet(&translation_mat, 1, 0));
171  cvmSet(tra, 2, 0, cvmGet(&translation_mat, 2, 0));
172  if (tra->rows == 4) cvmSet(tra, 3, 0, 1);
173 }
174 
176 {
177  memcpy(quaternion, p.quaternion, 4*sizeof(double));
178  memcpy(translation, p.translation, 4*sizeof(double));
179  return *this;
180 }
181 
182 } // namespace alvar
Main ALVAR namespace.
Definition: Alvar.h:174
void SetMatrix(const CvMat *mat)
Definition: Pose.cpp:75
CvMat translation_mat
Definition: Pose.h:55
void GetMatrixGL(double gl[16], bool mirror=true)
Get the transformation matrix representation of the Pose using OpenGL&#39;s transposed format...
Definition: Pose.cpp:104
static void QuatToMat9(const double *quat, double *mat)
Converts a rotation described by a quaternion into 3x3 rotation matrix.
Definition: Rotation.cpp:152
void GetTranslation(CvMat *tra) const
Definition: Pose.cpp:168
CvMat quaternion_mat
Definition: Rotation.h:47
RotationType
Rotation can be represented in four ways: quaternion (QUAT), matrix (MAT), euler angles (EUL) and exp...
Definition: Rotation.h:53
Pose & operator=(const Pose &p)
Definition: Pose.cpp:175
static void MirrorMat(CvMat *mat, bool x, bool y, bool z)
Simple function to mirror a rotation matrix in different directions.
Definition: Rotation.cpp:75
static void Mat9ToQuat(const double *mat, double *quat)
Converts a 3x3 rotation martix into quaternion form.
Definition: Rotation.cpp:257
void Reset()
Definition: Pose.cpp:69
void SetMatrixGL(double gl[16], bool mirror=true)
Set the Pose using OpenGL&#39;s transposed format. Note, that by default this also mirrors both the y- an...
Definition: Pose.cpp:113
Pose representation derived from the Rotation class
Definition: Pose.h:50
void Mirror(bool x, bool y, bool z)
Definition: Pose.cpp:141
double quaternion[4]
Definition: Rotation.h:48
void Invert()
Definition: Pose.cpp:132
void GetMatrix(CvMat *mat) const
Definition: Pose.cpp:91
void Transpose()
Definition: Pose.cpp:123
This file implements a pose.
void SetTranslation(const CvMat *tra)
Definition: Pose.cpp:150
Rotation structure and transformations between different parameterizations.
Definition: Rotation.h:43
double translation[4]
Definition: Pose.h:54
static void QuatToMat16(const double *quat, double *mat)
Converts a rotation described by a quaternion into 4x4 OpenGL-like transformation matrix...
Definition: Rotation.cpp:185


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Jun 6 2019 19:27:24