Blender  V3.3
initialize_reconstruction.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 "libmv/base/vector.h"
24 #include "libmv/logging/logging.h"
28 #include "libmv/numeric/numeric.h"
31 
32 namespace libmv {
33 namespace {
34 
35 void GetImagesInMarkers(const vector<Marker>& markers,
36  int* image1,
37  int* image2) {
38  if (markers.size() < 2) {
39  return;
40  }
41  *image1 = markers[0].image;
42  for (int i = 1; i < markers.size(); ++i) {
43  if (markers[i].image != *image1) {
44  *image2 = markers[i].image;
45  return;
46  }
47  }
48  *image2 = -1;
49  LOG(FATAL) << "Only one image in the markers.";
50 }
51 
52 } // namespace
53 
56  if (markers.size() < 16) {
57  LG << "Not enough markers to initialize from two frames: "
58  << markers.size();
59  return false;
60  }
61 
62  int image1, image2;
63  GetImagesInMarkers(markers, &image1, &image2);
64 
65  Mat x1, x2;
68 
69  Mat3 F;
71 
72  // The F matrix should be an E matrix, but squash it just to be sure.
73  Mat3 E;
75 
76  // Recover motion between the two images. Since this function assumes a
77  // calibrated camera, use the identity for K.
78  Mat3 R;
79  Vec3 t;
80  Mat3 K = Mat3::Identity();
82  E, K, x1.col(0), K, x2.col(0), &R, &t)) {
83  LG << "Failed to compute R and t from E and K.";
84  return false;
85  }
86 
87  // Image 1 gets the reference frame, image 2 gets the relative motion.
88  reconstruction->InsertCamera(image1, Mat3::Identity(), Vec3::Zero());
89  reconstruction->InsertCamera(image2, R, t);
90 
91  LG << "From two frame reconstruction got:\nR:\n"
92  << R << "\nt:" << t.transpose();
93  return true;
94 }
95 
96 namespace {
97 
98 Mat3 DecodeF(const Vec9& encoded_F) {
99  // Decode F and force it to be rank 2.
100  Map<const Mat3> full_rank_F(encoded_F.data(), 3, 3);
101  Eigen::JacobiSVD<Mat3> svd(full_rank_F,
102  Eigen::ComputeFullU | Eigen::ComputeFullV);
103  Vec3 diagonal = svd.singularValues();
104  diagonal(2) = 0;
105  Mat3 F = svd.matrixU() * diagonal.asDiagonal() * svd.matrixV().transpose();
106  return F;
107 }
108 
109 // This is the stupidest way to refine F known to mankind, since it requires
110 // doing a full SVD of F at each iteration. This uses sampson error.
111 struct FundamentalSampsonCostFunction {
112  public:
113  typedef Vec FMatrixType;
114  typedef Vec9 XMatrixType;
115 
116  // Assumes markers are ordered by track.
117  explicit FundamentalSampsonCostFunction(const vector<Marker>& markers)
118  : markers(markers) {}
119 
120  Vec operator()(const Vec9& encoded_F) const {
121  // Decode F and force it to be rank 2.
122  Mat3 F = DecodeF(encoded_F);
123 
124  Vec residuals(markers.size() / 2);
125  residuals.setZero();
126  for (int i = 0; i < markers.size() / 2; ++i) {
127  const Marker& marker1 = markers[2 * i + 0];
128  const Marker& marker2 = markers[2 * i + 1];
129  CHECK_EQ(marker1.track, marker2.track);
130  Vec2 x1(marker1.x, marker1.y);
131  Vec2 x2(marker2.x, marker2.y);
132 
133  residuals[i] = SampsonDistance(F, x1, x2);
134  }
135  return residuals;
136  }
138 };
139 
140 } // namespace
141 
144  if (markers.size() < 16) {
145  return false;
146  }
147 
148  int image1, image2;
149  GetImagesInMarkers(markers, &image1, &image2);
150 
151  Mat x1, x2;
154 
155  Mat3 F;
157 
158  // XXX Verify sampson distance.
159 #if 0
160  // Refine the resulting projection fundamental matrix using Sampson's
161  // approximation of geometric error. This avoids having to do a full bundle
162  // at the cost of some accuracy.
163  //
164  // TODO(keir): After switching to a better bundling library, use a proper
165  // full bundle adjust here instead of this lame bundle adjustment.
167 
168  FundamentalSampsonCostFunction fundamental_cost(markers);
169 
170  // Pack the initial P matrix into a size-12 vector..
171  Vec9 encoded_F = Map<Vec9>(F.data(), 3, 3);
172 
173  Solver solver(fundamental_cost);
174 
175  Solver::SolverParameters params;
176  Solver::Results results = solver.minimize(params, &encoded_F);
177  // TODO(keir): Check results to ensure clean termination.
178 
179  // Recover F from the minimization.
180  F = DecodeF(encoded_F);
181 #endif
182 
183  // Image 1 gets P = [I|0], image 2 gets arbitrary P.
184  Mat34 P1 = Mat34::Zero();
185  P1.block<3, 3>(0, 0) = Mat3::Identity();
186  Mat34 P2;
187  ProjectionsFromFundamental(F, &P1, &P2);
188 
189  reconstruction->InsertCamera(image1, P1);
190  reconstruction->InsertCamera(image2, P2);
191 
192  LG << "From two frame reconstruction got P2:\n" << P2;
193  return true;
194 }
195 } // namespace libmv
#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 x2
_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
SIMD_FORCE_INLINE btVector3 operator()(const btVector3 &x) const
Return the transform of the vector.
Definition: btTransform.h:90
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
const vector< Marker > & markers
uiWidgetBaseParameters params[MAX_WIDGET_BASE_BATCH]
const ProjectiveReconstruction & reconstruction
Definition: intersect.cc:198
#define LG
#define CHECK_EQ(a, b)
Definition: log.h:50
#define LOG(severity)
Definition: log.h:36
#define F
#define R
Eigen::VectorXd Vec
Definition: numeric.h:61
double SampsonDistance(const Mat &F, const Vec2 &x1, const Vec2 &x2)
Definition: fundamental.cc:241
Eigen::Matrix< double, 3, 3 > Mat3
Definition: numeric.h:72
bool ProjectiveReconstructTwoFrames(const vector< Marker > &markers, ProjectiveReconstruction *reconstruction)
Eigen::Vector2d Vec2
Definition: numeric.h:105
Eigen::MatrixXd Mat
Definition: numeric.h:60
bool EuclideanReconstructTwoFrames(const vector< Marker > &markers, EuclideanReconstruction *reconstruction)
void ProjectionsFromFundamental(const Mat3 &F, Mat34 *P1, Mat34 *P2)
Definition: fundamental.cc:45
bool MotionFromEssentialAndCorrespondence(const Mat3 &E, const Mat3 &K1, const Vec2 &x1, const Mat3 &K2, const Vec2 &x2, Mat3 *R, Vec3 *t)
Definition: fundamental.cc:373
Eigen::Matrix< double, 3, 4 > Mat34
Definition: numeric.h:73
double NormalizedEightPointSolver(const Mat &x1, const Mat &x2, Mat3 *F)
Definition: fundamental.cc:109
Eigen::Vector3d Vec3
Definition: numeric.h:106
void CoordinatesForMarkersInImage(const vector< Marker > &markers, int image, Mat *coordinates)
void FundamentalToEssential(const Mat3 &F, Mat3 *E)
Definition: fundamental.cc:393
Eigen::Matrix< double, 9, 1 > Vec9
Definition: numeric.h:112