TrifocalTensor.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 "TrifocalTensor.h"
25 
26 namespace alvar {
27 
29 }
30 
31 TrifocalTensor::TrifocalTensor(const Pose &p0, const Pose &p1) {
32  computeTensor(p0, p1);
33 }
34 
35 TrifocalTensor::TrifocalTensor(const Pose &p0, const Pose &p1, const Pose &p2) {
36  computeTensor(p0, p1, p2);
37 }
38 
40 }
41 
42 void TrifocalTensor::computeTensor(const Pose &p0, const Pose &p1) {
43  double data_p12[4][4], data_p13[4][4];
44  CvMat p12 = cvMat( 4, 4, CV_64F, data_p12 );
45  CvMat p13 = cvMat( 4, 4, CV_64F, data_p13 );
46  p0.GetMatrix(&p12);
47  p1.GetMatrix(&p13);
48 
49  for (int i = 0; i < 3; i++)
50  for (int j = 0; j < 3; j++)
51  for (int k = 0; k < 3; k++) {
52 
53  T[i][j][k] =
54  (data_p12[j][i] * data_p13[k][3])
55  -
56  (data_p12[j][3] * data_p13[k][i]);
57  }
58 }
59 
60 double *getRow(double* m, int row) {
61  return &m[4*row];
62 }
63 
64 double det(double *r0, double *r1, double *r2, double *r3) {
65  double m[16];
66  memcpy(&m[0], r0, 4*sizeof(double));
67  memcpy(&m[4], r1, 4*sizeof(double));
68  memcpy(&m[8], r2, 4*sizeof(double));
69  memcpy(&m[12], r3, 4*sizeof(double));
70  CvMat M = cvMat(4, 4, CV_64F, m);
71  return cvDet(&M);
72 }
73 
74 void TrifocalTensor::computeTensor(const Pose &p0, const Pose &p1, const Pose &p2) {
75  double data_p0[16], data_p1[16], data_p2[16];
76  CvMat P0 = cvMat( 4, 4, CV_64F, data_p0 );
77  CvMat P1 = cvMat( 4, 4, CV_64F, data_p1 );
78  CvMat P2 = cvMat( 4, 4, CV_64F, data_p2 );
79  p0.GetMatrix(&P0);
80  p1.GetMatrix(&P1);
81  p2.GetMatrix(&P2);
82 
83  for (int i = 0; i < 3; i++)
84  for (int j = 0; j < 3; j++)
85  for (int k = 0; k < 3; k++) {
86  double sign = i==1 ? -1 : 1;
87  T[i][j][k] = sign * det(getRow(data_p0, (i+1)%3),
88  getRow(data_p0, (i+2)%3),
89  getRow(data_p1, j),
90  getRow(data_p2, k));
91  }
92 }
93 
94 double TrifocalTensor::projectAxis(const CvPoint2D64f &p0, const CvPoint2D64f &p1, int l) {
95  double v00 =
96  p1.x * ( p0.x * T[0][0][l]
97  + p0.y * T[1][0][l]
98  + T[2][0][l])
99  -
100  p1.x * ( p0.x * T[0][0][l]
101  + p0.y * T[1][0][l]
102  + T[2][0][l]);
103  double v01 =
104  p1.x * ( p0.x * T[0][1][l]
105  + p0.y * T[1][1][l]
106  + T[2][1][l])
107  -
108  p1.y * ( p0.x * T[0][0][l]
109  + p0.y * T[1][0][l]
110  + T[2][0][l]);
111  double v02 =
112  p1.x * ( p0.x * T[0][2][l]
113  + p0.y * T[1][2][l]
114  + T[2][2][l])
115  - ( p0.x * T[0][0][l]
116  + p0.y * T[1][0][l]
117  + T[2][0][l]);
118 
119  double v10 =
120  p1.y * ( p0.x * T[0][0][l]
121  + p0.y * T[1][0][l]
122  + T[2][0][l])
123  -
124  p1.x * ( p0.x * T[0][1][l]
125  + p0.y * T[1][1][l]
126  + T[2][1][l]);
127  double v11 =
128  p1.y * ( p0.x * T[0][1][l]
129  + p0.y * T[1][1][l]
130  + T[2][1][l])
131  -
132  p1.y * ( p0.x * T[0][1][l]
133  + p0.y * T[1][1][l]
134  + T[2][1][l]);
135  double v12 =
136  p1.y * ( p0.x * T[0][2][l]
137  + p0.y * T[1][2][l]
138  + T[2][2][l])
139  - ( p0.x * T[0][1][l]
140  + p0.y * T[1][1][l]
141  + T[2][1][l]);
142 
143  double v20 =
144  ( p0.x * T[0][0][l]
145  + p0.y * T[1][0][l]
146  + T[2][0][l])
147  -
148  p1.x * ( p0.x * T[0][2][l]
149  + p0.y * T[1][2][l]
150  + T[2][2][l]);
151  double v21 =
152  ( p0.x * T[0][1][l]
153  + p0.y * T[1][1][l]
154  + T[2][1][l])
155  -
156  p1.y * ( p0.x * T[0][2][l]
157  + p0.y * T[1][2][l]
158  + T[2][2][l]);
159 
160  double v22 =
161  ( p0.x * T[0][2][l]
162  + p0.y * T[1][2][l]
163  + T[2][2][l])
164  -
165  ( p0.x * T[0][2][l]
166  + p0.y * T[1][2][l]
167  + T[2][2][l]);
168 
169  double v = 0;
170  if (fabs(v00) > fabs(v)) v = v00;
171  if (fabs(v01) > fabs(v)) v = v01;
172  if (fabs(v02) > fabs(v)) v = v02;
173  if (fabs(v10) > fabs(v)) v = v10;
174  if (fabs(v11) > fabs(v)) v = v11;
175  if (fabs(v12) > fabs(v)) v = v12;
176  if (fabs(v20) > fabs(v)) v = v20;
177  if (fabs(v21) > fabs(v)) v = v21;
178  if (fabs(v22) > fabs(v)) v = v22;
179 
180  return v;
181 }
182 
183 void TrifocalTensor::project(const CvPoint2D64f &p0,
184  const CvPoint2D64f &p1,
185  CvPoint2D64f &p2) {
186  double z = projectAxis(p0, p1, 2);
187  p2.x = projectAxis(p0, p1, 0) / z;
188  p2.y = projectAxis(p0, p1, 1) / z;
189 }
190 
191 double TrifocalTensor::projectError(const CvPoint2D64f &p0,
192  const CvPoint2D64f &p1,
193  const CvPoint2D64f &p2) {
194  double v0 = projectAxis(p0, p1, 0);
195  double v1 = projectAxis(p0, p1, 1);
196  double v2 = projectAxis(p0, p1, 2);
197 
198  double e0 = v0/v2 - p2.x;
199  double e1 = v1/v2 - p2.y;
200  return e0*e0+e1*e1;
201 }
202 
203 } // namespace alvar
Main ALVAR namespace.
Definition: Alvar.h:174
double projectAxis(const CvPoint2D64f &p0, const CvPoint2D64f &p1, int l)
void computeTensor(const Pose &P1, const Pose &P2)
Initializes the tensor from identity pose and two other known poses.
double projectError(const CvPoint2D64f &p0, const CvPoint2D64f &p1, const CvPoint2D64f &p2)
Computes how much three points differ from the tensor.
double det(double *r0, double *r1, double *r2, double *r3)
This file implements a trifocal tensor.
double T[3][3][3]
Pose representation derived from the Rotation class
Definition: Pose.h:50
double * getRow(double *m, int row)
TFSIMD_FORCE_INLINE const tfScalar & z() const
void project(const CvPoint2D64f &p0, const CvPoint2D64f &p1, CvPoint2D64f &p2)
Computes the projection of a point in the third pose.
void GetMatrix(CvMat *mat) const
Definition: Pose.cpp:91


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