IVT
ObjectPose.cpp
Go to the documentation of this file.
1 // ****************************************************************************
2 // This file is part of the Integrating Vision Toolkit (IVT).
3 //
4 // The IVT is maintained by the Karlsruhe Institute of Technology (KIT)
5 // (www.kit.edu) in cooperation with the company Keyetech (www.keyetech.de).
6 //
7 // Copyright (C) 2014 Karlsruhe Institute of Technology (KIT).
8 // All rights reserved.
9 //
10 // Redistribution and use in source and binary forms, with or without
11 // modification, are permitted provided that the following conditions are met:
12 //
13 // 1. Redistributions of source code must retain the above copyright
14 // notice, this list of conditions and the following disclaimer.
15 //
16 // 2. Redistributions in binary form must reproduce the above copyright
17 // notice, this list of conditions and the following disclaimer in the
18 // documentation and/or other materials provided with the distribution.
19 //
20 // 3. Neither the name of the KIT nor the names of its contributors may be
21 // used to endorse or promote products derived from this software
22 // without specific prior written permission.
23 //
24 // THIS SOFTWARE IS PROVIDED BY THE KIT AND CONTRIBUTORS “AS IS” AND ANY
25 // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
26 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
27 // DISCLAIMED. IN NO EVENT SHALL THE KIT OR CONTRIBUTORS BE LIABLE FOR ANY
28 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
29 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
31 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
32 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
33 // THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
34 // ****************************************************************************
35 // ****************************************************************************
36 // Filename: ObjectPose.h
37 // Author: Moritz Ritter
38 // Date: 08.04.2008
39 // ****************************************************************************
40 
41 // ****************************************************************************
42 // Implementation of the paper:
43 // "Fast and Globally Convergent Pose Estimation from Video Images"
44 // IEEE Transactions on Pattern Analysis and Machine Intelligence,
45 // vol. 22, no.6, June 2000
46 //
47 // Solves the 2D to 3D mapping problem for 3 or more non-colinear
48 // points (also for coplanar points).
49 // ****************************************************************************
50 
51 
52 // ****************************************************************************
53 // Includes
54 // ****************************************************************************
55 
56 #include <new> // for explicitly using correct new/delete operators on VC DSPs
57 
58 #include "ObjectPose.h"
59 
60 #include "Math/Math2d.h"
61 #include "Math/Math3d.h"
62 #include "Math/DoubleMatrix.h"
63 #include "Math/LinearAlgebra.h"
65 
66 #include <math.h>
67 #include <stdio.h>
68 
69 
70 // ****************************************************************************
71 // Defines
72 // ****************************************************************************
73 
74 #define TOL 1e-5
75 #define EPSILON 1e-8
76 
77 
78 
79 
80 // ****************************************************************************
81 // Constructor / Destructor
82 // ****************************************************************************
83 
84 CObjectPose::CObjectPose(const Vec3d *pObjectPoints, int nPoints)
85 {
86  int i;
87 
88  m_nPoints = nPoints;
89 
90  // normalized object points 3xn matrix
91  m_pP = new Vec3d[m_nPoints];
92 
93  m_tPbar = pObjectPoints[0];
94  for (i = 1; i < m_nPoints; i++)
95  Math3d::AddToVec(m_tPbar, pObjectPoints[i]);
96 
97  Math3d::MulVecScalar(m_tPbar, 1.0f / m_nPoints, m_tPbar);
98 
99  for (i = 0; i < m_nPoints; i++)
100  Math3d::SubtractVecVec(pObjectPoints[i], m_tPbar, m_pP[i]);
101 
102  // homogeneous normalized image points 3xn matrix
103  m_pQ = new Vec3d[m_nPoints];
104 
105  // 3x3xn matrix
106  m_pF = new Mat3d[m_nPoints];
107 }
108 
110 {
111  delete [] m_pP;
112  delete [] m_pQ;
113  delete [] m_pF;
114 }
115 
116 
117 // ****************************************************************************
118 // Methods
119 // ****************************************************************************
120 
121 bool CObjectPose::EstimatePose(const Vec2d *pImagePoints,
122  Mat3d &rotationMatrix, Vec3d &translationVector,
123  const CCalibration *pCalibration, int nMaxIterations)
124 {
125  if (m_nPoints < 3)
126  {
127  printf("error: too few points for object pose estimation.\n");
128  return false;
129  }
130 
131  int i;
132 
133  // normalize image points
134  const Vec2d &focalLength = pCalibration->GetCameraParameters().focalLength;
135  const Vec2d &principalPoint = pCalibration->GetCameraParameters().principalPoint;
136 
137  for (i = 0; i < m_nPoints; i++)
138  {
139  m_pQ[i].x = (pImagePoints[i].x - principalPoint.x) / focalLength.x;
140  m_pQ[i].y = (pImagePoints[i].y - principalPoint.y) / focalLength.y;
141  m_pQ[i].z = 1.0;
142  }
143 
144  // compute projection matrices
145  for (i = 0; i < m_nPoints; i++)
146  {
147  const float scalar = 1.0f / Math3d::SquaredLength(m_pQ[i]);
148  Math3d::MulVecTransposedVec(m_pQ[i], m_pQ[i], m_pF[i]);
149  Math3d::MulMatScalar(m_pF[i], scalar, m_pF[i]);
150  }
151 
152  // compute the matrix factor required for computing t ( inv( eye(3) - sum(F,3) / n ) / n )
153  Mat3d sum;
154  Mat3d temp;
155  // sum(F, 3)
156  sum = m_pF[0];
157  for (i = 1; i < m_nPoints; i++)
158  Math3d::AddToMat(sum, m_pF[i]);
159  // sum / n
160  Math3d::MulMatScalar(sum, 1.0f / m_nPoints, sum);
161  // eye(3) - sum
163  // inv(temp)
164  Math3d::Invert(temp, temp);
165  // m_tFactor = temp / n
166  Math3d::MulMatScalar(temp, 1.0f / m_nPoints, m_tFactor);
167 
168  float old_error, new_error;
169  int it = 0;
170 
171  AbsKernel(rotationMatrix, translationVector, old_error);
172  it++;
173  AbsKernel(rotationMatrix, translationVector, new_error);
174  it++;
175 
176  while (fabsf((old_error - new_error) / old_error) > TOL && new_error > EPSILON && it < nMaxIterations)
177  {
178  old_error = new_error;
179  AbsKernel(rotationMatrix, translationVector, new_error);
180  it++;
181  }
182 
183  // get back to original reference frame
184  Vec3d vec;
185  Math3d::MulMatVec(rotationMatrix, m_tPbar, vec);
186  Math3d::SubtractVecVec(translationVector, vec, translationVector);
187 
188  // take into account extrinsic calibration
189  const Mat3d &Rc = pCalibration->m_rotation_inverse;
190  const Vec3d &tc = pCalibration->m_translation_inverse;
191  Math3d::MulMatMat(Rc, rotationMatrix, rotationMatrix);
192  Math3d::MulMatVec(Rc, translationVector, tc, translationVector);
193 
194  return true;
195 }
196 
197 void CObjectPose::AbsKernel(Mat3d &R, Vec3d &t, float &error)
198 {
199  int i;
200 
201  // compute Q
202  for (i = 0; i < m_nPoints; i++)
203  Math3d::MulMatVec(m_pF[i], m_pQ[i], m_pQ[i]);
204 
205  // compute Q relative to qbar
206  Vec3d qbar;
207  qbar = m_pQ[0];
208  for (i = 1; i < m_nPoints; i++)
209  Math3d::AddToVec(qbar, m_pQ[i]);
210  Math3d::MulVecScalar(qbar, 1.0f / m_nPoints, qbar);
211  for (i = 0; i < m_nPoints; i++)
212  Math3d::SubtractVecVec(m_pQ[i], qbar, m_pQ[i]);
213 
214  // compute M matrix ( M = M + P(:, i) * Q(:, i)' )
215  CDoubleMatrix fM(3, 3);
216  Mat3d temp;
217  for (i = 0; i < 9; i++)
218  fM.data[i] = 0;
219  for (i = 0; i < m_nPoints; i++)
220  {
221  Math3d::MulVecTransposedVec(m_pP[i], m_pQ[i], temp);
222  fM.data[0] = fM.data[0] + temp.r1; fM.data[1] = fM.data[1] + temp.r2; fM.data[2] = fM.data[2] + temp.r3;
223  fM.data[3] = fM.data[3] + temp.r4; fM.data[4] = fM.data[4] + temp.r5; fM.data[5] = fM.data[5] + temp.r6;
224  fM.data[6] = fM.data[6] + temp.r7; fM.data[7] = fM.data[7] + temp.r8; fM.data[8] = fM.data[8] + temp.r9;
225  }
226 
227  // calculate SVD of M
228  CDoubleMatrix fU(3, 3);
229  CDoubleMatrix fS(3, 3);
230  CDoubleMatrix fV(3, 3);
231  LinearAlgebra::SVD(&fM, &fS, &fU, &fV);
232 
233  // compute transposed U
234  Mat3d transU;
235  LinearAlgebra::Transpose(&fU, &fU);
236  transU.r1 = float(fU.data[0]); transU.r2 = float(fU.data[1]); transU.r3 = float(fU.data[2]);
237  transU.r4 = float(fU.data[3]); transU.r5 = float(fU.data[4]); transU.r6 = float(fU.data[5]);
238  transU.r7 = float(fU.data[6]); transU.r8 = float(fU.data[7]); transU.r9 = float(fU.data[8]);
239 
240  // R = V * U'
241  temp.r1 = float(fV.data[0]); temp.r2 = float(fV.data[1]); temp.r3 = float(fV.data[2]);
242  temp.r4 = float(fV.data[3]); temp.r5 = float(fV.data[4]); temp.r6 = float(fV.data[5]);
243  temp.r7 = float(fV.data[6]); temp.r8 = float(fV.data[7]); temp.r9 = float(fV.data[8]);
244  Math3d::MulMatMat(temp, transU, R);
245 
246  if (Math3d::Det(R) > 0)
247  {
248  // no need to change R
249  // estimate current translation
250  t = tEstimate(R);
251 
252  if (t.z < 0) // we need to invert t (object should be in front of the camera)
253  {
254  // R = -[V(:, 1:2) -V(:, 3)] * U'
255  temp.r1 = float(-fV.data[0]); temp.r2 = float(-fV.data[1]); temp.r3 = float(fV.data[2]);
256  temp.r4 = float(-fV.data[3]); temp.r5 = float(-fV.data[4]); temp.r6 = float(fV.data[5]);
257  temp.r7 = float(-fV.data[6]); temp.r8 = float(-fV.data[7]); temp.r9 = float(fV.data[8]);
258  Math3d::MulMatMat(temp, transU, R);
259  // estimate current translation
260  t = tEstimate(R);
261  }
262  }
263  else
264  {
265  // R = [V(:, 1:2) -V(:, 3)] * U'
266  temp.r1 = float(fV.data[0]); temp.r2 = float(fV.data[1]); temp.r3 = float(-fV.data[2]);
267  temp.r4 = float(fV.data[3]); temp.r5 = float(fV.data[4]); temp.r6 = float(-fV.data[5]);
268  temp.r7 = float(fV.data[6]); temp.r8 = float(fV.data[7]); temp.r9 = float(-fV.data[8]);
269  Math3d::MulMatMat(temp, transU, R);
270  // estimate current translation
271  t = tEstimate(R);
272 
273  if (t.z < 0) // we need to invert t (object should be in front of the camera)
274  {
275  // R = -V * U'
276  temp.r1 = float(-fV.data[0]); temp.r2 = float(-fV.data[1]); temp.r3 = float(-fV.data[2]);
277  temp.r4 = float(-fV.data[3]); temp.r5 = float(-fV.data[4]); temp.r6 = float(-fV.data[5]);
278  temp.r7 = float(-fV.data[6]); temp.r8 = float(-fV.data[7]); temp.r9 = float(-fV.data[8]);
279  Math3d::MulMatMat(temp, transU, R);
280  // estimate current translation
281  t = tEstimate(R);
282  }
283  }
284 
285  // calculate current Q
286  for (i = 0; i < m_nPoints; i++)
287  {
288  // Q(:, i) = R * P(: , i) + t
289  Math3d::MulMatVec(R, m_pP[i], m_pQ[i]);
290  Math3d::AddVecVec(m_pQ[i], t, m_pQ[i]);
291  }
292 
293  // calculate error
294  error = 0.0f;
295  Vec3d vec;
296 
297  for (i = 0; i < m_nPoints; i++)
298  {
299  // (eye(3) - F(:, :, i)) * Q(:, i)
301  Math3d::MulMatVec(temp, m_pQ[i], vec);
302 
303  error += Math3d::SquaredLength(vec);
304  }
305 }
306 
307 Vec3d CObjectPose::tEstimate(Mat3d &R)
308 {
309  Vec3d sum = Math3d::zero_vec;
310  Vec3d temp;
311 
312  // sum = sum + F(:, :, i) * R * P(:, i)
313  for (int i = 0; i < m_nPoints; i++)
314  {
315  Math3d::MulMatVec(R, m_pP[i], temp);
316  Math3d::MulMatVec(m_pF[i], temp, temp);
317  Math3d::AddToVec(sum, temp);
318  }
319 
320  Math3d::MulMatVec(m_tFactor, sum, sum);
321 
322  return sum;
323 }
void SVD(const CFloatMatrix *A, CFloatMatrix *W, CFloatMatrix *U=0, CFloatMatrix *V=0, bool bAllowModifyA=false, bool bReturnUTransposed=false, bool bReturnVTransposed=false)
Definition: SVD.cpp:1660
void Invert(const Mat3d &matrix, Mat3d &result)
Definition: Math3d.cpp:657
#define TOL
Definition: ObjectPose.cpp:74
Data structure for the representation of a 2D vector.
Definition: Math2d.h:82
float y
Definition: Math3d.h:75
float r4
Definition: Math3d.h:95
float r3
Definition: Math3d.h:95
Data structure for the representation of a matrix of values of the data type double.
Definition: DoubleMatrix.h:54
Vec3d m_translation_inverse
Translation vector of the inverted extrinsic transformation.
Definition: Calibration.h:454
void AddToMat(Mat3d &matrix, const Mat3d &matrixToAdd)
Definition: Math3d.cpp:695
float x
Definition: Math2d.h:84
float r1
Definition: Math3d.h:95
float r7
Definition: Math3d.h:95
float x
Definition: Math3d.h:75
float r2
Definition: Math3d.h:95
const Mat3d unit_mat
Definition: Math3d.cpp:60
float Det(const Mat3d &matrix)
Definition: Math3d.cpp:721
const CCameraParameters & GetCameraParameters() const
Gives access to the camera parameters.
Definition: Calibration.h:268
void MulVecTransposedVec(const Vec3d &vector1, const Vec3d &vector2, Mat3d &result)
Definition: Math3d.cpp:467
float r8
Definition: Math3d.h:95
CObjectPose(const Vec3d *pObjectPoints, int nPoints)
Definition: ObjectPose.cpp:84
void SubtractVecVec(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
Definition: Math3d.cpp:522
float y
Definition: Math2d.h:84
void MulMatMat(const Mat3d &matrix1, const Mat3d &matrix2, Mat3d &result)
Definition: Math3d.cpp:444
const Vec3d zero_vec
Definition: Math3d.cpp:59
#define EPSILON
Definition: ObjectPose.cpp:75
void Transpose(const CFloatMatrix *pMatrix, CFloatMatrix *pResultMatrix)
Data structure for the representation of a 3D vector.
Definition: Math3d.h:73
void MulMatVec(const Mat3d &matrix, const Vec3d &vec, Vec3d &result)
Definition: Math3d.cpp:422
Camera model parameters and functions for a single camera.
Definition: Calibration.h:125
float z
Definition: Math3d.h:75
float SquaredLength(const Vec3d &vec)
Definition: Math3d.cpp:590
bool EstimatePose(const Vec2d *pImagePoints, Mat3d &rotationMatrix, Vec3d &translationVector, const CCalibration *pCalibration, int nMaxIterations=350)
Definition: ObjectPose.cpp:121
float r6
Definition: Math3d.h:95
void AddVecVec(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
Definition: Math3d.cpp:495
Mat3d m_rotation_inverse
Rotation matrix of the inverted extrinsic transformation.
Definition: Calibration.h:443
void AddToVec(Vec3d &vec, const Vec3d &vectorToAdd)
Definition: Math3d.cpp:481
void SubtractMatMat(const Mat3d &matrix1, const Mat3d &matrix2, Mat3d &result)
Definition: Math3d.cpp:708
Data structure for the representation of a 3x3 matrix.
Definition: Math3d.h:93
void MulMatScalar(const Mat3d &matrix, float scalar, Mat3d &result)
Definition: Math3d.cpp:509
float r9
Definition: Math3d.h:95
void MulVecScalar(const Vec3d &vec, float scalar, Vec3d &result)
Definition: Math3d.cpp:502
float r5
Definition: Math3d.h:95