Blender  V3.3
projection.cc
Go to the documentation of this file.
1 // Copyright (c) 2007, 2008 libmv authors.
2 //
3 // Permission is hereby granted, free of charge, to any person obtaining a copy
4 // of this software and associated documentation files (the "Software"), to
5 // deal in the Software without restriction, including without limitation the
6 // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
7 // sell copies of the Software, and to permit persons to whom the Software is
8 // furnished to do so, subject to the following conditions:
9 //
10 // The above copyright notice and this permission notice shall be included in
11 // all copies or substantial portions of the Software.
12 //
13 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
18 // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
19 // IN THE SOFTWARE.
20 
22 #include "libmv/numeric/numeric.h"
23 
24 namespace libmv {
25 
26 void P_From_KRt(const Mat3& K, const Mat3& R, const Vec3& t, Mat34* P) {
27  P->block<3, 3>(0, 0) = R;
28  P->col(3) = t;
29  (*P) = K * (*P);
30 }
31 
32 void KRt_From_P(const Mat34& P, Mat3* Kp, Mat3* Rp, Vec3* tp) {
33  // Decompose using the RQ decomposition HZ A4.1.1 pag.579.
34  Mat3 K = P.block(0, 0, 3, 3);
35 
36  Mat3 Q;
37  Q.setIdentity();
38 
39  // Set K(2,1) to zero.
40  if (K(2, 1) != 0) {
41  double c = -K(2, 2);
42  double s = K(2, 1);
43  double l = sqrt(c * c + s * s);
44  c /= l;
45  s /= l;
46  Mat3 Qx;
47  // clang-format off
48  Qx << 1, 0, 0,
49  0, c, -s,
50  0, s, c;
51  // clang-format on
52  K = K * Qx;
53  Q = Qx.transpose() * Q;
54  }
55  // Set K(2,0) to zero.
56  if (K(2, 0) != 0) {
57  double c = K(2, 2);
58  double s = K(2, 0);
59  double l = sqrt(c * c + s * s);
60  c /= l;
61  s /= l;
62  Mat3 Qy;
63  // clang-format off
64  Qy << c, 0, s,
65  0, 1, 0,
66  -s, 0, c;
67  // clang-format on
68  K = K * Qy;
69  Q = Qy.transpose() * Q;
70  }
71  // Set K(1,0) to zero.
72  if (K(1, 0) != 0) {
73  double c = -K(1, 1);
74  double s = K(1, 0);
75  double l = sqrt(c * c + s * s);
76  c /= l;
77  s /= l;
78  Mat3 Qz;
79  // clang-format off
80  Qz << c, -s, 0,
81  s, c, 0,
82  0, 0, 1;
83  // clang-format on
84  K = K * Qz;
85  Q = Qz.transpose() * Q;
86  }
87 
88  Mat3 R = Q;
89 
90  // Ensure that the diagonal is positive.
91  // TODO(pau) Change this to ensure that:
92  // - K(0,0) > 0
93  // - K(2,2) = 1
94  // - det(R) = 1
95  if (K(2, 2) < 0) {
96  K = -K;
97  R = -R;
98  }
99  if (K(1, 1) < 0) {
100  Mat3 S;
101  // clang-format off
102  S << 1, 0, 0,
103  0, -1, 0,
104  0, 0, 1;
105  // clang-format on
106  K = K * S;
107  R = S * R;
108  }
109  if (K(0, 0) < 0) {
110  Mat3 S;
111  // clang-format off
112  S << -1, 0, 0,
113  0, 1, 0,
114  0, 0, 1;
115  // clang-format on
116  K = K * S;
117  R = S * R;
118  }
119 
120  // Compute translation.
121  Vec p(3);
122  p << P(0, 3), P(1, 3), P(2, 3);
123  // TODO(pau) This sould be done by a SolveLinearSystem(A, b, &x) call.
124  // TODO(keir) use the eigen LU solver syntax...
125  Vec3 t = K.inverse() * p;
126 
127  // scale K so that K(2,2) = 1
128  K = K / K(2, 2);
129 
130  *Kp = K;
131  *Rp = R;
132  *tp = t;
133 }
134 
136  const Vec2& principal_point,
137  const Vec2& principal_point_new,
138  Mat34* P_new) {
139  Mat3 T;
140  // clang-format off
141  T << 1, 0, principal_point_new(0) - principal_point(0),
142  0, 1, principal_point_new(1) - principal_point(1),
143  0, 0, 1;
144  // clang-format on
145  *P_new = T * P;
146 }
147 
149  const Vec2& principal_point,
150  double aspect_ratio,
151  double aspect_ratio_new,
152  Mat34* P_new) {
153  Mat3 T;
154  // clang-format off
155  T << 1, 0, 0,
156  0, aspect_ratio_new / aspect_ratio, 0,
157  0, 0, 1;
158  // clang-format on
159  Mat34 P_temp;
160 
161  ProjectionShiftPrincipalPoint(P, principal_point, Vec2(0, 0), &P_temp);
162  P_temp = T * P_temp;
163  ProjectionShiftPrincipalPoint(P_temp, Vec2(0, 0), principal_point, P_new);
164 }
165 
166 void HomogeneousToEuclidean(const Mat& H, Mat* X) {
167  int d = H.rows() - 1;
168  int n = H.cols();
169  X->resize(d, n);
170  for (size_t i = 0; i < n; ++i) {
171  double h = H(d, i);
172  for (int j = 0; j < d; ++j) {
173  (*X)(j, i) = H(j, i) / h;
174  }
175  }
176 }
177 
179  e->resize(2, h.cols());
180  e->row(0) = h.row(0).array() / h.row(2).array();
181  e->row(1) = h.row(1).array() / h.row(2).array();
182 }
184  e->resize(3, h.cols());
185  e->row(0) = h.row(0).array() / h.row(3).array();
186  e->row(1) = h.row(1).array() / h.row(3).array();
187  e->row(2) = h.row(2).array() / h.row(3).array();
188 }
189 
190 void HomogeneousToEuclidean(const Vec3& H, Vec2* X) {
191  double w = H(2);
192  *X << H(0) / w, H(1) / w;
193 }
194 
195 void HomogeneousToEuclidean(const Vec4& H, Vec3* X) {
196  double w = H(3);
197  *X << H(0) / w, H(1) / w, H(2) / w;
198 }
199 
200 void EuclideanToHomogeneous(const Mat& X, Mat* H) {
201  int d = X.rows();
202  int n = X.cols();
203  H->resize(d + 1, n);
204  H->block(0, 0, d, n) = X;
205  H->row(d).setOnes();
206 }
207 
208 void EuclideanToHomogeneous(const Vec2& X, Vec3* H) {
209  *H << X(0), X(1), 1;
210 }
211 
212 void EuclideanToHomogeneous(const Vec3& X, Vec4* H) {
213  *H << X(0), X(1), X(2), 1;
214 }
215 
216 // TODO(julien) Call conditioning.h/ApplyTransformationToPoints ?
217 void EuclideanToNormalizedCamera(const Mat2X& x, const Mat3& K, Mat2X* n) {
218  Mat3X x_image_h;
219  EuclideanToHomogeneous(x, &x_image_h);
220  Mat3X x_camera_h = K.inverse() * x_image_h;
221  HomogeneousToEuclidean(x_camera_h, n);
222 }
223 
224 void HomogeneousToNormalizedCamera(const Mat3X& x, const Mat3& K, Mat2X* n) {
225  Mat3X x_camera_h = K.inverse() * x;
226  HomogeneousToEuclidean(x_camera_h, n);
227 }
228 
229 double Depth(const Mat3& R, const Vec3& t, const Vec3& X) {
230  return (R * X)(2) + t(2);
231 }
232 
233 double Depth(const Mat3& R, const Vec3& t, const Vec4& X) {
234  Vec3 Xe = X.head<3>() / X(3);
235  return Depth(R, t, Xe);
236 }
237 
238 } // namespace libmv
sqrt(x)+1/max(0
#define K(key)
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble t
#define X
Definition: GeomUtils.cpp:199
ATTR_WARN_UNUSED_RESULT const BMLoop * l
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:119
static float P(float k)
Definition: math_interp.c:25
#define T
#define R
#define H(x, y, z)
static unsigned c
Definition: RandGen.cpp:83
Eigen::VectorXd Vec
Definition: numeric.h:61
Eigen::Vector4d Vec4
Definition: numeric.h:107
void EuclideanToHomogeneous(const Mat &X, Mat *H)
Definition: projection.cc:200
Eigen::Matrix< double, 3, 3 > Mat3
Definition: numeric.h:72
Eigen::Vector2d Vec2
Definition: numeric.h:105
void HomogeneousToEuclidean(const Mat &H, Mat *X)
Definition: projection.cc:166
void HomogeneousToNormalizedCamera(const Mat3X &x, const Mat3 &K, Mat2X *n)
Definition: projection.cc:224
Eigen::MatrixXd Mat
Definition: numeric.h:60
void ProjectionShiftPrincipalPoint(const Mat34 &P, const Vec2 &principal_point, const Vec2 &principal_point_new, Mat34 *P_new)
Definition: projection.cc:135
double Depth(const Mat3 &R, const Vec3 &t, const Vec3 &X)
Definition: projection.cc:229
void KRt_From_P(const Mat34 &P, Mat3 *Kp, Mat3 *Rp, Vec3 *tp)
Definition: projection.cc:32
Eigen::Matrix< double, 3, 4 > Mat34
Definition: numeric.h:73
Eigen::Vector3d Vec3
Definition: numeric.h:106
Eigen::Matrix< double, 4, Eigen::Dynamic > Mat4X
Definition: numeric.h:93
Eigen::Matrix< double, 3, Eigen::Dynamic > Mat3X
Definition: numeric.h:92
void ProjectionChangeAspectRatio(const Mat34 &P, const Vec2 &principal_point, double aspect_ratio, double aspect_ratio_new, Mat34 *P_new)
Definition: projection.cc:148
Eigen::Matrix< double, 2, Eigen::Dynamic > Mat2X
Definition: numeric.h:91
void P_From_KRt(const Mat3 &K, const Mat3 &R, const Vec3 &t, Mat34 *P)
Definition: projection.cc:26
void EuclideanToNormalizedCamera(const Mat2X &x, const Mat3 &K, Mat2X *n)
Definition: projection.cc:217