Pose.cpp
Go to the documentation of this file.
00001 /*
00002  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
00003  *
00004  * Copyright 2007-2012 VTT Technical Research Centre of Finland
00005  *
00006  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
00007  *          <http://www.vtt.fi/multimedia/alvar.html>
00008  *
00009  * ALVAR is free software; you can redistribute it and/or modify it under the
00010  * terms of the GNU Lesser General Public License as published by the Free
00011  * Software Foundation; either version 2.1 of the License, or (at your option)
00012  * any later version.
00013  *
00014  * This library is distributed in the hope that it will be useful, but WITHOUT
00015  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00016  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
00017  * for more details.
00018  *
00019  * You should have received a copy of the GNU Lesser General Public License
00020  * along with ALVAR; if not, see
00021  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
00022  */
00023 
00024 #include "ar_track_alvar/Pose.h"
00025 
00026 using namespace std;
00027 
00028 namespace alvar {
00029 using namespace std;
00030 
00031 void Pose::Output() const {
00032         cout<<quaternion[0]<<","<<quaternion[1]<<","<<quaternion[2]<<","<<quaternion[3]<<"|";
00033         cout<<translation[0]<<","<<translation[1]<<","<<translation[2]<<endl;
00034 }
00035 
00036 Pose::Pose() : Rotation() {
00037         cvInitMatHeader(&translation_mat, 4, 1, CV_64F, translation);
00038         cvZero(&translation_mat);
00039         cvmSet(&translation_mat, 3, 0, 1);
00040 }
00041 
00042 Pose::Pose(CvMat *tra, CvMat *rot, RotationType t) : Rotation(rot, t) {
00043         cvInitMatHeader(&translation_mat, 4, 1, CV_64F, translation);
00044         cvZero(&translation_mat);
00045         cvmSet(&translation_mat, 3, 0, 1);
00046         // Fill in translation part
00047         cvmSet(&translation_mat, 0, 0, cvmGet(tra, 0, 0));
00048         cvmSet(&translation_mat, 1, 0, cvmGet(tra, 1, 0));
00049         cvmSet(&translation_mat, 2, 0, cvmGet(tra, 2, 0));
00050 }
00051 
00052 Pose::Pose(CvMat *mat) : Rotation(mat, MAT) {
00053         cvInitMatHeader(&translation_mat, 4, 1, CV_64F, translation);
00054         cvZero(&translation_mat);
00055         cvmSet(&translation_mat, 3, 0, 1);
00056         // Fill in translation part
00057         if (mat->cols == 4) {
00058                 cvmSet(&translation_mat, 0, 0, cvmGet(mat, 0, 3));
00059                 cvmSet(&translation_mat, 1, 0, cvmGet(mat, 1, 3));
00060                 cvmSet(&translation_mat, 2, 0, cvmGet(mat, 2, 3));
00061         }
00062 }
00063 
00064 Pose::Pose(const Pose& p) :Rotation(p) {
00065         cvInitMatHeader(&translation_mat, 4, 1, CV_64F, translation);
00066         cvCopy(&p.translation_mat, &translation_mat);
00067 }
00068 
00069 void Pose::Reset()
00070 {
00071         cvZero(&quaternion_mat); cvmSet(&quaternion_mat, 0, 0, 1);
00072         cvZero(&translation_mat);
00073 }
00074 
00075 void Pose::SetMatrix(const CvMat *mat)
00076 {
00077         double tmp[9];
00078         for(int i = 0; i < 3; ++i)
00079                 for(int j = 0; j < 3; ++j)
00080                         tmp[i*3+j] = cvmGet(mat, i, j);
00081                         
00082         Mat9ToQuat(tmp, quaternion);
00083         if (mat->cols == 4) {
00084                 cvmSet(&translation_mat, 0, 0, cvmGet(mat, 0, 3));
00085                 cvmSet(&translation_mat, 1, 0, cvmGet(mat, 1, 3));
00086                 cvmSet(&translation_mat, 2, 0, cvmGet(mat, 2, 3));
00087                 cvmSet(&translation_mat, 3, 0, 1);
00088         }
00089 }
00090 
00091 void Pose::GetMatrix(CvMat *mat) const
00092 {
00093         if (mat->width == 3) {
00094                 QuatToMat9(quaternion, mat->data.db);
00095         } else if (mat->width == 4) {
00096                 cvSetIdentity(mat);
00097                 QuatToMat16(quaternion, mat->data.db);
00098                 cvmSet(mat, 0, 3, cvmGet(&translation_mat, 0, 0));
00099                 cvmSet(mat, 1, 3, cvmGet(&translation_mat, 1, 0));
00100                 cvmSet(mat, 2, 3, cvmGet(&translation_mat, 2, 0));
00101         }
00102 }
00103 
00104 void Pose::GetMatrixGL(double gl[16], bool mirror)
00105 {
00106         if (mirror) Mirror(false, true, true);
00107         CvMat gl_mat = cvMat(4, 4, CV_64F, gl);
00108         GetMatrix(&gl_mat);
00109         cvTranspose(&gl_mat, &gl_mat);
00110         if (mirror) Mirror(false, true, true);
00111 }
00112 
00113 void Pose::SetMatrixGL(double gl[16], bool mirror)
00114 {
00115         double gll[16];
00116         memcpy(gll, gl, sizeof(double)*16);
00117         CvMat gl_mat = cvMat(4, 4, CV_64F, gll);
00118         cvTranspose(&gl_mat, &gl_mat);
00119         SetMatrix(&gl_mat);
00120         if (mirror) Mirror(false, true, true);
00121 }
00122 
00123 void Pose::Transpose()
00124 {
00125         double tmp[16];
00126         CvMat tmp_mat = cvMat(4, 4, CV_64F, tmp);
00127         GetMatrix(&tmp_mat);
00128         cvTranspose(&tmp_mat, &tmp_mat);
00129         SetMatrix(&tmp_mat);
00130 }
00131 
00132 void Pose::Invert()
00133 {
00134         double tmp[16];
00135         CvMat tmp_mat = cvMat(4, 4, CV_64F, tmp);
00136         GetMatrix(&tmp_mat);
00137         cvInvert(&tmp_mat, &tmp_mat);
00138         SetMatrix(&tmp_mat);
00139 }
00140 
00141 void Pose::Mirror(bool x, bool y, bool z)
00142 {
00143         double tmp[16];
00144         CvMat tmp_mat = cvMat(4, 4, CV_64F, tmp);
00145         GetMatrix(&tmp_mat);
00146         MirrorMat(&tmp_mat, x, y, z);
00147         SetMatrix(&tmp_mat);
00148 }
00149 
00150 void Pose::SetTranslation(const CvMat *tra) {
00151         cvmSet(&translation_mat, 0, 0, cvmGet(tra, 0, 0));
00152         cvmSet(&translation_mat, 1, 0, cvmGet(tra, 1, 0));
00153         cvmSet(&translation_mat, 2, 0, cvmGet(tra, 2, 0));
00154         cvmSet(&translation_mat, 3, 0, 1);
00155 }
00156 void Pose::SetTranslation(const double *tra) {
00157         translation[0] = tra[0];
00158         translation[1] = tra[1];
00159         translation[2] = tra[2];
00160         translation[3] = 1;
00161 }
00162 void Pose::SetTranslation(const double x, const double y, const double z) {
00163         translation[0] = x;
00164         translation[1] = y;
00165         translation[2] = z;
00166         translation[3] = 1;
00167 }
00168 void Pose::GetTranslation( CvMat *tra) const{
00169         cvmSet(tra, 0, 0, cvmGet(&translation_mat, 0, 0));
00170         cvmSet(tra, 1, 0, cvmGet(&translation_mat, 1, 0));
00171         cvmSet(tra, 2, 0, cvmGet(&translation_mat, 2, 0));
00172         if (tra->rows == 4)     cvmSet(tra, 3, 0, 1);
00173 }
00174 
00175 Pose& Pose::operator = (const Pose& p)
00176 {
00177         memcpy(quaternion, p.quaternion, 4*sizeof(double));
00178         memcpy(translation, p.translation, 4*sizeof(double));
00179         return *this;
00180 }
00181 
00182 } // namespace alvar


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Thu Feb 16 2017 03:23:02