Blender  V3.3
resect.cc
Go to the documentation of this file.
1 // Copyright (c) 2011 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 
23 #include <cstdio>
24 
25 #include "libmv/base/vector.h"
26 #include "libmv/logging/logging.h"
31 #include "libmv/numeric/numeric.h"
34 
35 namespace libmv {
36 namespace {
37 
38 Mat2X PointMatrixFromMarkers(const vector<Marker>& markers) {
39  Mat2X points(2, markers.size());
40  for (int i = 0; i < markers.size(); ++i) {
41  points(0, i) = markers[i].x;
42  points(1, i) = markers[i].y;
43  }
44  return points;
45 }
46 
47 // Uses an incremental rotation:
48 //
49 // x = R' * R * X + t;
50 //
51 // to avoid issues with the rotation representation. R' is derived from a
52 // euler vector encoding the rotation in 3 parameters; the direction is the
53 // axis to rotate around and the magnitude is the amount of the rotation.
54 struct EuclideanResectCostFunction {
55  public:
56  typedef Vec FMatrixType;
57  typedef Vec6 XMatrixType;
58 
59  EuclideanResectCostFunction(const vector<Marker>& markers,
60  const EuclideanReconstruction& reconstruction,
61  const Mat3& initial_R)
62  : markers(markers),
65 
66  // dRt has dR (delta R) encoded as a euler vector in the first 3 parameters,
67  // followed by t in the next 3 parameters.
68  Vec operator()(const Vec6& dRt) const {
69  // Unpack R, t from dRt.
70  Mat3 R = RotationFromEulerVector(dRt.head<3>()) * initial_R;
71  Vec3 t = dRt.tail<3>();
72 
73  // Compute the reprojection error for each coordinate.
74  Vec residuals(2 * markers.size());
75  residuals.setZero();
76  for (int i = 0; i < markers.size(); ++i) {
77  const EuclideanPoint& point =
78  *reconstruction.PointForTrack(markers[i].track);
79  Vec3 projected = R * point.X + t;
80  projected /= projected(2);
81  residuals[2 * i + 0] = projected(0) - markers[i].x;
82  residuals[2 * i + 1] = projected(1) - markers[i].y;
83  }
84  return residuals;
85  }
86 
88  const EuclideanReconstruction& reconstruction;
89  const Mat3& initial_R;
90 };
91 
92 } // namespace
93 
96  bool final_pass) {
97  if (markers.size() < 5) {
98  return false;
99  }
100  Mat2X points_2d = PointMatrixFromMarkers(markers);
101  Mat3X points_3d(3, markers.size());
102  for (int i = 0; i < markers.size(); i++) {
103  points_3d.col(i) = reconstruction->PointForTrack(markers[i].track)->X;
104  }
105  LG << "Points for resect:\n" << points_2d;
106 
107  Mat3 R;
108  Vec3 t;
109 
110  if (0 ||
112  points_2d, points_3d, &R, &t, euclidean_resection::RESECTION_EPNP)) {
113  // printf("Resection for image %d failed\n", markers[0].image);
114  LG << "Resection for image " << markers[0].image << " failed;"
115  << " trying fallback projective resection.";
116 
117  LG << "No fallback; failing resection for " << markers[0].image;
118  return false;
119 
120  if (!final_pass)
121  return false;
122  // Euclidean resection failed. Fall back to projective resection, which is
123  // less reliable but better conditioned when there are many points.
124  Mat34 P;
125  Mat4X points_3d_homogeneous(4, markers.size());
126  for (int i = 0; i < markers.size(); i++) {
127  points_3d_homogeneous.col(i).head<3>() = points_3d.col(i);
128  points_3d_homogeneous(3, i) = 1.0;
129  }
130  resection::Resection(points_2d, points_3d_homogeneous, &P);
131  if ((P * points_3d_homogeneous.col(0))(2) < 0) {
132  LG << "Point behind camera; switch sign.";
133  P = -P;
134  }
135 
136  Mat3 ignored;
137  KRt_From_P(P, &ignored, &R, &t);
138 
139  // The R matrix should be a rotation, but don't rely on it.
140  Eigen::JacobiSVD<Mat3> svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV);
141 
142  LG << "Resection rotation is: " << svd.singularValues().transpose();
143  LG << "Determinant is: " << R.determinant();
144 
145  // Correct to make R a rotation.
146  R = svd.matrixU() * svd.matrixV().transpose();
147 
148  Vec3 xx = R * points_3d.col(0) + t;
149  if (xx(2) < 0.0) {
150  LG << "Final point is still behind camera...";
151  }
152  // XXX Need to check if error is horrible and fail here too in that case.
153  }
154 
155  // Refine the result.
157 
158  // Give the cost our initial guess for R.
159  EuclideanResectCostFunction resect_cost(markers, *reconstruction, R);
160 
161  // Encode the initial parameters: start with zero delta rotation, and the
162  // guess for t obtained from resection.
163  Vec6 dRt = Vec6::Zero();
164  dRt.tail<3>() = t;
165 
166  Solver solver(resect_cost);
167 
168  Solver::SolverParameters params;
169  /* Solver::Results results = */ solver.minimize(params, &dRt);
170  LG << "LM found incremental rotation: " << dRt.head<3>().transpose();
171  // TODO(keir): Check results to ensure clean termination.
172 
173  // Unpack the rotation and translation.
174  R = RotationFromEulerVector(dRt.head<3>()) * R;
175  t = dRt.tail<3>();
176 
177  LG << "Resection for image " << markers[0].image << " got:\n"
178  << "R:\n"
179  << R << "\nt:\n"
180  << t;
182  return true;
183 }
184 
185 namespace {
186 
187 // Directly parameterize the projection matrix, P, which is a 12 parameter
188 // homogeneous entry. In theory P should be parameterized with only 11
189 // parametetrs, but in practice it works fine to let the extra degree of
190 // freedom drift.
191 struct ProjectiveResectCostFunction {
192  public:
193  typedef Vec FMatrixType;
194  typedef Vec12 XMatrixType;
195 
196  ProjectiveResectCostFunction(const vector<Marker>& markers,
197  const ProjectiveReconstruction& reconstruction)
199 
200  Vec operator()(const Vec12& vector_P) const {
201  // Unpack P from vector_P.
202  Map<const Mat34> P(vector_P.data(), 3, 4);
203 
204  // Compute the reprojection error for each coordinate.
205  Vec residuals(2 * markers.size());
206  residuals.setZero();
207  for (int i = 0; i < markers.size(); ++i) {
208  const ProjectivePoint& point =
209  *reconstruction.PointForTrack(markers[i].track);
210  Vec3 projected = P * point.X;
211  projected /= projected(2);
212  residuals[2 * i + 0] = projected(0) - markers[i].x;
213  residuals[2 * i + 1] = projected(1) - markers[i].y;
214  }
215  return residuals;
216  }
217 
218  const vector<Marker>& markers;
219  const ProjectiveReconstruction& reconstruction;
220 };
221 
222 } // namespace
223 
226  if (markers.size() < 5) {
227  return false;
228  }
229 
230  // Stack the homogeneous 3D points as the columns of a matrix.
231  Mat2X points_2d = PointMatrixFromMarkers(markers);
232  Mat4X points_3d_homogeneous(4, markers.size());
233  for (int i = 0; i < markers.size(); i++) {
234  points_3d_homogeneous.col(i) =
236  }
237  LG << "Points for resect:\n" << points_2d;
238 
239  // Resection the point.
240  Mat34 P;
241  resection::Resection(points_2d, points_3d_homogeneous, &P);
242 
243  // Flip the sign of P if necessary to keep the point in front of the camera.
244  if ((P * points_3d_homogeneous.col(0))(2) < 0) {
245  LG << "Point behind camera; switch sign.";
246  P = -P;
247  }
248 
249  // TODO(keir): Check if error is horrible and fail in that case.
250 
251  // Refine the resulting projection matrix using geometric error.
253 
254  ProjectiveResectCostFunction resect_cost(markers, *reconstruction);
255 
256  // Pack the initial P matrix into a size-12 vector..
257  Vec12 vector_P = Map<Vec12>(P.data());
258 
259  Solver solver(resect_cost);
260 
261  Solver::SolverParameters params;
262  /* Solver::Results results = */ solver.minimize(params, &vector_P);
263  // TODO(keir): Check results to ensure clean termination.
264 
265  // Unpack the projection matrix.
266  P = Map<Mat34>(vector_P.data(), 3, 4);
267 
268  LG << "Resection for image " << markers[0].image << " got:\n"
269  << "P:\n"
270  << P;
272  return true;
273 }
274 } // namespace libmv
_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
in reality light always falls off quadratically Particle Retrieve the data of the particle that spawned the object for example to give variation to multiple instances of an object Point Retrieve information about points in a point cloud Retrieve the edges of an object as it appears to Cycles topology will always appear triangulated Convert a blackbody temperature to an RGB value Normal Generate a perturbed normal from an RGB normal map image Typically used for faking highly detailed surfaces Generate an OSL shader from a file or text data block Image Sample an image file as a texture Sky Generate a procedural sky texture Noise Generate fractal Perlin noise Wave Generate procedural bands or rings with noise Voronoi Generate Worley noise based on the distance to random points Typically used to generate textures such as or biological cells Brick Generate a procedural texture producing bricks Texture Retrieve multiple types of texture coordinates nTypically used as inputs for texture nodes Vector Convert a point
btMatrix3x3 transpose() const
Return the transpose of the matrix.
SIMD_FORCE_INLINE btVector3 operator()(const btVector3 &x) const
Return the transform of the vector.
Definition: btTransform.h:90
EuclideanPoint * PointForTrack(int track)
Returns a pointer to the point corresponding to track.
void InsertCamera(int image, const Mat3 &R, const Vec3 &t)
depth_tx normal_tx diffuse_light_tx specular_light_tx volume_light_tx environment_tx ambient_occlusion_tx aov_value_tx in_weight_img image(1, GPU_R32F, Qualifier::WRITE, ImageType::FLOAT_2D_ARRAY, "out_weight_img") .image(3
float[3][3] Mat3
Definition: gpu_matrix.cc:27
uiWidgetBaseParameters params[MAX_WIDGET_BASE_BATCH]
#define LG
static float P(float k)
Definition: math_interp.c:25
#define R
bool EuclideanResection(const Mat2X &x_camera, const Mat3X &X_world, Mat3 *R, Vec3 *t, ResectionMethod method)
void Resection(const Matrix< T, 2, Dynamic > &x, const Matrix< T, 4, Dynamic > &X, Matrix< T, 3, 4 > *P)
Definition: resection.h:37
Eigen::VectorXd Vec
Definition: numeric.h:61
Eigen::Matrix< double, 6, 1 > Vec6
Definition: numeric.h:109
bool ProjectiveResect(const vector< Marker > &markers, ProjectiveReconstruction *reconstruction)
Definition: resect.cc:224
Mat3 RotationFromEulerVector(Vec3 euler_vector)
Returns the rotaiton matrix built from given vector of euler angles.
Definition: numeric.h:482
Eigen::Matrix< double, 3, 3 > Mat3
Definition: numeric.h:72
bool EuclideanResect(const vector< Marker > &markers, EuclideanReconstruction *reconstruction, bool final_pass)
Definition: resect.cc:94
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
Eigen::Matrix< double, 12, 1 > Vec12
Definition: numeric.h:115
Eigen::Matrix< double, 2, Eigen::Dynamic > Mat2X
Definition: numeric.h:91
const Mat3 & initial_R
Definition: resect.cc:89
const EuclideanReconstruction & reconstruction
Definition: resect.cc:88
const vector< Marker > & markers
Definition: resect.cc:87