IVT
POSIT.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: Posit.cpp
37 // Author: Pedram Azad
38 // Date: 23.01.2008
39 // ****************************************************************************
40 
41 
42 // ****************************************************************************
43 // Includes
44 // ****************************************************************************
45 
46 #include <new> // for explicitly using correct new/delete operators on VC DSPs
47 
48 #include "POSIT.h"
49 
50 #include "Math/Math2d.h"
51 #include "Math/Math3d.h"
52 #include "Math/DoubleMatrix.h"
53 #include "Math/DoubleVector.h"
54 #include "Math/LinearAlgebra.h"
56 
57 #include <stdio.h>
58 
59 
60 // ****************************************************************************
61 // This is an implementation of the algorithm described in:
62 // D. F. DeMenthon and L. S. Davis, "Model-based Object Pose in 25 Lines of Code",
63 // in Proc. of European Conference on Computer Vision (ECCV), pages 335-343, 1992.
64 // respectively
65 // in International Journal of Computer Vision (IJCV), vol. 15, 1995.
66 // ****************************************************************************
67 
68 
69 
70 // ****************************************************************************
71 // Functions
72 // ****************************************************************************
73 
74 bool POSIT::POSIT(const Vec3d *pPoints3D, const Vec2d *pPoints2D, int nPoints, Mat3d &R, Vec3d &t, const CCalibration *pCalibration, int nIterations)
75 {
76  const int N = nPoints;
77  const Vec3d *M = pPoints3D;
78  const Vec2d *m = pPoints2D;
79 
80  if (N < 4)
81  {
82  printf("error: too few points for POSIT\n");
83  return false;
84  }
85 
86  const float f = 0.5f * (pCalibration->GetCameraParameters().focalLength.x + pCalibration->GetCameraParameters().focalLength.y);
87  const float cx = pCalibration->GetCameraParameters().principalPoint.x;
88  const float cy = pCalibration->GetCameraParameters().principalPoint.y;
89 
90  const int N_ = N - 1;
91  int l;
92 
93  // build matrix A and B (pseudoinverse of A)
94  CDoubleMatrix A(3, N_);
95  for (l = 0; l < N_; l++)
96  {
97  A(0, l) = M[l + 1].x - M[0].x;
98  A(1, l) = M[l + 1].y - M[0].y;
99  A(2, l) = M[l + 1].z - M[0].z;
100  }
101 
102  // calculate pseudoinverse
103  CDoubleMatrix B(N_, 3);
105 
106  CDoubleVector x_(N_);
107  CDoubleVector y_(N_);
108 
109  // Step 1: initialize epsilons
110  float *e = new float[N_];
111  for (l = 0; l < N_; l++)
112  e[l] = 0.0f;
113 
114  Vec3d i, j, k;
115  float s;
116 
117  // loop
118  for (int n = 0; n < nIterations; n++)
119  {
120  // Step 2
121  for (l = 0; l < N_; l++)
122  {
123  x_.data[l] = (m[l + 1].x - cx) * (1.0f + e[l]) - (m[0].x - cx);
124  y_.data[l] = (m[l + 1].y - cy) * (1.0f + e[l]) - (m[0].y - cy);
125  }
126 
127  CDoubleVector I(3), J(3);
128  LinearAlgebra::MulMatVec(&B, &x_, &I);
129  LinearAlgebra::MulMatVec(&B, &y_, &J);
130 
131  Math3d::SetVec(i, float(I.data[0]), float(I.data[1]), float(I.data[2]));
132  Math3d::SetVec(j, float(J.data[0]), float(J.data[1]), float(J.data[2]));
133 
134  const float s1 = Math3d::Length(i);
135  const float s2 = Math3d::Length(j);
136  s = 0.5f * (s1 + s2);
137 
140 
141  // Step 3
142  Math3d::CrossProduct(i, j, k);
144 
145  const float Z0 = f / s;
146 
147  for (l = 0; l < N_; l++)
148  {
149  Vec3d M0Mi = { float(A(0, l)), float(A(1, l)), float(A(2, l)) };
150  e[l] = Math3d::ScalarProduct(M0Mi, k) / Z0;
151  }
152  }
153 
154  delete [] e;
155 
156  // Step 4 (checking differences between epsilons as termination condition)
157  // is skipped and a fixed number of iterations is used
158 
159  // Step 5
160  Math3d::SetVec(t, m[0].x - cx, m[0].y - cy, f);
161  Math3d::MulVecScalar(t, 1.0f / s, t);
162 
163  Vec3d j_;
164  Math3d::CrossProduct(k, i, j_);
166 
167  Math3d::SetMat(R, i.x, i.y, i.z, j_.x, j_.y, j_.z, k.x, k.y, k.z);
168 
169  // handle cases in which M[0] is not the zero vector
170  Vec3d temp;
171  Math3d::MulMatVec(R, M[0], temp);
172  Math3d::SubtractFromVec(t, temp);
173 
174  // take into account extrinsic calibration
175  const Mat3d &Rc = pCalibration->m_rotation_inverse;
176  const Vec3d &tc = pCalibration->m_translation_inverse;
177 
178  Math3d::MulMatMat(Rc, R, R);
179  Math3d::MulMatVec(Rc, t, tc, t);
180 
181  return true;
182 }
Data structure for the representation of a 2D vector.
Definition: Math2d.h:82
float y
Definition: Math3d.h:75
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
double * data
Definition: DoubleVector.h:79
float x
Definition: Math2d.h:84
Data structure for the representation of a vector of values of the data type double.
Definition: DoubleVector.h:54
float ScalarProduct(const Vec3d &vector1, const Vec3d &vector2)
Definition: Math3d.cpp:559
float x
Definition: Math3d.h:75
float Length(const Vec3d &vec)
Definition: Math3d.cpp:585
const CCameraParameters & GetCameraParameters() const
Gives access to the camera parameters.
Definition: Calibration.h:268
void NormalizeVec(Vec3d &vec)
Definition: Math3d.cpp:573
float y
Definition: Math2d.h:84
void MulMatMat(const Mat3d &matrix1, const Mat3d &matrix2, Mat3d &result)
Definition: Math3d.cpp:444
bool POSIT(const Vec3d *pPoints3D, const Vec2d *pPoints2D, int nPoints, Mat3d &R, Vec3d &t, const CCalibration *pCalibration, int nIterations=20)
Definition: POSIT.cpp:74
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
void SetMat(Mat3d &matrix, float r1, float r2, float r3, float r4, float r5, float r6, float r7, float r8, float r9)
Definition: Math3d.cpp:257
Camera model parameters and functions for a single camera.
Definition: Calibration.h:125
float z
Definition: Math3d.h:75
void CalculatePseudoInverseSVD(const CFloatMatrix *pInputMatrix, CFloatMatrix *pOutputMatrix)
void SubtractFromVec(Vec3d &vec, const Vec3d &vectorToSubtract)
Definition: Math3d.cpp:488
Mat3d m_rotation_inverse
Rotation matrix of the inverted extrinsic transformation.
Definition: Calibration.h:443
Data structure for the representation of a 3x3 matrix.
Definition: Math3d.h:93
void MulMatVec(const CFloatMatrix *pMatrix, const CFloatVector *pVector, CFloatVector *pResultVector)
void MulVecScalar(const Vec3d &vec, float scalar, Vec3d &result)
Definition: Math3d.cpp:502
void SetVec(Vec3d &vec, float x, float y, float z)
Definition: Math3d.cpp:243
void CrossProduct(const Vec3d &vector1, const Vec3d &vector2, Vec3d &result)
Definition: Math3d.cpp:564