TrifocalTensor.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 "TrifocalTensor.h"
00025 
00026 namespace alvar {
00027 
00028 TrifocalTensor::TrifocalTensor() {
00029 }
00030 
00031 TrifocalTensor::TrifocalTensor(const Pose &p0, const Pose &p1) {
00032   computeTensor(p0, p1);
00033 }
00034 
00035 TrifocalTensor::TrifocalTensor(const Pose &p0, const Pose &p1, const Pose &p2) {
00036   computeTensor(p0, p1, p2);
00037 }
00038 
00039 TrifocalTensor::~TrifocalTensor() {
00040 }
00041 
00042 void TrifocalTensor::computeTensor(const Pose &p0, const Pose &p1) {
00043   double data_p12[4][4], data_p13[4][4];
00044   CvMat p12 = cvMat( 4, 4, CV_64F, data_p12 );
00045   CvMat p13 = cvMat( 4, 4, CV_64F, data_p13 );
00046   p0.GetMatrix(&p12);
00047   p1.GetMatrix(&p13);
00048 
00049   for (int i = 0; i < 3; i++)
00050     for (int j = 0; j < 3; j++)
00051       for (int k = 0; k < 3; k++) {
00052 
00053               T[i][j][k] = 
00054                 (data_p12[j][i] * data_p13[k][3])
00055                 - 
00056                 (data_p12[j][3] * data_p13[k][i]);
00057   }
00058 }
00059 
00060 double *getRow(double* m, int row) {
00061   return &m[4*row];
00062 }
00063 
00064 double det(double *r0, double *r1, double *r2, double *r3) {
00065   double m[16];
00066   memcpy(&m[0], r0, 4*sizeof(double));
00067   memcpy(&m[4], r1, 4*sizeof(double));
00068   memcpy(&m[8], r2, 4*sizeof(double));
00069   memcpy(&m[12], r3, 4*sizeof(double));
00070   CvMat M = cvMat(4, 4, CV_64F, m);
00071   return cvDet(&M);
00072 }
00073 
00074 void TrifocalTensor::computeTensor(const Pose &p0, const Pose &p1, const Pose &p2) {
00075   double data_p0[16], data_p1[16], data_p2[16];
00076   CvMat P0 = cvMat( 4, 4, CV_64F, data_p0 );
00077   CvMat P1 = cvMat( 4, 4, CV_64F, data_p1 );
00078   CvMat P2 = cvMat( 4, 4, CV_64F, data_p2 );
00079   p0.GetMatrix(&P0);
00080   p1.GetMatrix(&P1);
00081   p2.GetMatrix(&P2);
00082 
00083   for (int i = 0; i < 3; i++)
00084     for (int j = 0; j < 3; j++)
00085       for (int k = 0; k < 3; k++) {
00086         double sign = i==1 ? -1 : 1;
00087         T[i][j][k] = sign * det(getRow(data_p0, (i+1)%3),
00088                                 getRow(data_p0, (i+2)%3),
00089                                 getRow(data_p1, j),
00090                                 getRow(data_p2, k));
00091       }  
00092 }
00093 
00094 double TrifocalTensor::projectAxis(const CvPoint2D64f &p0, const CvPoint2D64f &p1, int l) {
00095   double v00 =
00096     p1.x * (  p0.x * T[0][0][l]
00097               + p0.y * T[1][0][l]
00098               + T[2][0][l])
00099     - 
00100     p1.x * (  p0.x * T[0][0][l]
00101               + p0.y * T[1][0][l]
00102               + T[2][0][l]);
00103   double v01 =
00104     p1.x * (  p0.x * T[0][1][l]
00105               + p0.y * T[1][1][l]
00106               + T[2][1][l])
00107     - 
00108     p1.y * (  p0.x * T[0][0][l]
00109               + p0.y * T[1][0][l]
00110               + T[2][0][l]);
00111   double v02 =
00112     p1.x * (  p0.x * T[0][2][l]
00113               + p0.y * T[1][2][l]
00114               + T[2][2][l])
00115     - ( p0.x * T[0][0][l]
00116         + p0.y * T[1][0][l]
00117         + T[2][0][l]);
00118   
00119   double v10 =
00120     p1.y * (  p0.x * T[0][0][l]
00121               + p0.y * T[1][0][l]
00122               + T[2][0][l])
00123     - 
00124     p1.x * (  p0.x * T[0][1][l]
00125               + p0.y * T[1][1][l]
00126               + T[2][1][l]);
00127   double v11 =
00128     p1.y * (  p0.x * T[0][1][l]
00129               + p0.y * T[1][1][l]
00130               + T[2][1][l])
00131     - 
00132     p1.y * (  p0.x * T[0][1][l]
00133               + p0.y * T[1][1][l]
00134               + T[2][1][l]);
00135   double v12 =
00136     p1.y * (  p0.x * T[0][2][l]
00137               + p0.y * T[1][2][l]
00138               + T[2][2][l])
00139     - ( p0.x * T[0][1][l]
00140         + p0.y * T[1][1][l]
00141         + T[2][1][l]);
00142   
00143   double v20 =
00144     (  p0.x * T[0][0][l]
00145        + p0.y * T[1][0][l]
00146        + T[2][0][l])
00147     - 
00148     p1.x * (  p0.x * T[0][2][l]
00149               + p0.y * T[1][2][l]
00150               + T[2][2][l]);
00151   double v21 =
00152     (  p0.x * T[0][1][l]
00153        + p0.y * T[1][1][l]
00154        + T[2][1][l])
00155     - 
00156     p1.y * (  p0.x * T[0][2][l]
00157               + p0.y * T[1][2][l]
00158               + T[2][2][l]);
00159   
00160   double v22 =
00161     (  p0.x * T[0][2][l]
00162        + p0.y * T[1][2][l]
00163        + T[2][2][l])
00164     - 
00165     (  p0.x * T[0][2][l]
00166        + p0.y * T[1][2][l]
00167        + T[2][2][l]);
00168   
00169   double v = 0;
00170   if (fabs(v00) > fabs(v)) v = v00;
00171   if (fabs(v01) > fabs(v)) v = v01;
00172   if (fabs(v02) > fabs(v)) v = v02;
00173   if (fabs(v10) > fabs(v)) v = v10;
00174   if (fabs(v11) > fabs(v)) v = v11;
00175   if (fabs(v12) > fabs(v)) v = v12;
00176   if (fabs(v20) > fabs(v)) v = v20;
00177   if (fabs(v21) > fabs(v)) v = v21;
00178   if (fabs(v22) > fabs(v)) v = v22;
00179 
00180   return v;
00181 }
00182 
00183 void TrifocalTensor::project(const CvPoint2D64f &p0, 
00184                              const CvPoint2D64f &p1, 
00185                              CvPoint2D64f &p2) {
00186   double z = projectAxis(p0, p1, 2);
00187   p2.x = projectAxis(p0, p1, 0) / z;
00188   p2.y = projectAxis(p0, p1, 1) / z;
00189 }
00190 
00191 double TrifocalTensor::projectError(const CvPoint2D64f &p0, 
00192                                     const CvPoint2D64f &p1, 
00193                                     const CvPoint2D64f &p2) {
00194   double v0 = projectAxis(p0, p1, 0);
00195   double v1 = projectAxis(p0, p1, 1);
00196   double v2 = projectAxis(p0, p1, 2);
00197 
00198   double e0 = v0/v2 - p2.x;
00199   double e1 = v1/v2 - p2.y;
00200   return e0*e0+e1*e1;
00201 }
00202 
00203 } // namespace alvar


ar_track_alvar
Author(s): Scott Niekum
autogenerated on Sat Dec 28 2013 16:46:16