Blender  V3.3
modal_solver.cc
Go to the documentation of this file.
1 // Copyright (c) 2015 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 "ceres/ceres.h"
26 #include "ceres/rotation.h"
27 #include "libmv/logging/logging.h"
29 
30 #ifdef _MSC_VER
31 # define snprintf _snprintf
32 #endif
33 
34 namespace libmv {
35 
36 namespace {
37 void ProjectMarkerOnSphere(const Marker& marker, Vec3& X) {
38  X(0) = marker.x;
39  X(1) = marker.y;
40  X(2) = 1.0;
41 
42  X *= 5.0 / X.norm();
43 }
44 
45 void ModalSolverLogProgress(ProgressUpdateCallback* update_callback,
46  double progress) {
47  if (update_callback) {
48  char message[256];
49 
50  snprintf(message,
51  sizeof(message),
52  "Solving progress %d%%",
53  (int)(progress * 100));
54 
55  update_callback->invoke(progress, message);
56  }
57 }
58 
59 struct ModalReprojectionError {
60  ModalReprojectionError(double observed_x,
61  double observed_y,
62  const double weight,
63  const Vec3& bundle)
64  : observed_x_(observed_x),
65  observed_y_(observed_y),
66  weight_(weight),
67  bundle_(bundle) {}
68 
69  // TODO(keir): This should support bundling focal length as well.
70  template <typename T>
71  bool operator()(const T* quaternion, T* residuals) const {
72  // Convert bundle position from double to T.
73  T X[3] = {T(bundle_(0)), T(bundle_(1)), T(bundle_(2))};
74 
75  // Compute the point position in camera coordinates: x = RX.
76  T x[3];
77 
78  // This flips the sense of the quaternion, to adhere to Blender conventions.
79  T quaternion_inverse[4] = {
80  quaternion[0],
81  -quaternion[1],
82  -quaternion[2],
83  -quaternion[3],
84  };
85  ceres::QuaternionRotatePoint(quaternion_inverse, X, x);
86 
87  // Compute normalized coordinates by dividing out the depth.
88  T xn = x[0] / x[2];
89  T yn = x[1] / x[2];
90 
91  // The error is the difference between reprojected and observed marker
92  // positions, weighted by the passed in weight.
93  residuals[0] = T(weight_) * (xn - T(observed_x_));
94  residuals[1] = T(weight_) * (yn - T(observed_y_));
95 
96  return true;
97  }
98 
99  double observed_x_;
100  double observed_y_;
101  double weight_;
103 };
104 } // namespace
105 
108  ProgressUpdateCallback* update_callback) {
109  int max_image = tracks.MaxImage();
110  int max_track = tracks.MaxTrack();
111 
112  LG << "Max image: " << max_image;
113  LG << "Max track: " << max_track;
114 
115  // For minimization we're using quaternions.
116  Vec3 zero_rotation = Vec3::Zero();
117  Vec4 quaternion;
118  ceres::AngleAxisToQuaternion(&zero_rotation(0), &quaternion(0));
119 
120  for (int image = 0; image <= max_image; ++image) {
121  vector<Marker> all_markers = tracks.MarkersInImage(image);
122 
123  ModalSolverLogProgress(update_callback, (float)image / max_image);
124 
125  // Skip empty images without doing anything.
126  if (all_markers.size() == 0) {
127  LG << "Skipping image: " << image;
128  continue;
129  }
130 
131  // STEP 1: Estimate rotation analytically.
132  Mat3 current_R;
133  ceres::QuaternionToRotation(&quaternion(0), &current_R(0, 0));
134 
135  // Construct point cloud for current and previous images,
136  // using markers appear at current image for which we know
137  // 3D positions.
138  Mat x1, x2;
139  for (int i = 0; i < all_markers.size(); ++i) {
140  Marker& marker = all_markers[i];
142  if (point) {
143  Vec3 X;
144  ProjectMarkerOnSphere(marker, X);
145 
146  int last_column = x1.cols();
147  x1.conservativeResize(3, last_column + 1);
148  x2.conservativeResize(3, last_column + 1);
149 
150  x1.col(last_column) = current_R * point->X;
151  x2.col(last_column) = X;
152  }
153  }
154 
155  if (x1.cols() >= 2) {
156  Mat3 delta_R;
157 
158  // Compute delta rotation matrix for two point clouds.
159  // Could be a bit confusing at first glance, but order
160  // of clouds is indeed so.
161  GetR_FixedCameraCenter(x2, x1, 1.0, &delta_R);
162 
163  // Convert delta rotation form matrix to final image
164  // rotation stored in a quaternion
165  Vec3 delta_angle_axis;
166  ceres::RotationMatrixToAngleAxis(&delta_R(0, 0), &delta_angle_axis(0));
167 
168  Vec3 current_angle_axis;
169  ceres::QuaternionToAngleAxis(&quaternion(0), &current_angle_axis(0));
170 
171  Vec3 angle_axis = current_angle_axis + delta_angle_axis;
172 
173  ceres::AngleAxisToQuaternion(&angle_axis(0), &quaternion(0));
174 
175  LG << "Analytically computed quaternion " << quaternion.transpose();
176  }
177 
178  // STEP 2: Refine rotation with Ceres.
179  ceres::Problem problem;
180 
181  // NOTE: Parameterization is lazily initialized when it is really needed,
182  // and is re-used by all parameters block.
183  ceres::Manifold* quaternion_manifold = NULL;
184 
185  int num_residuals = 0;
186  for (int i = 0; i < all_markers.size(); ++i) {
187  Marker& marker = all_markers[i];
189 
190  if (point && marker.weight != 0.0) {
191  problem.AddResidualBlock(
192  new ceres::AutoDiffCostFunction<ModalReprojectionError,
193  2, /* num_residuals */
194  4>(new ModalReprojectionError(
195  marker.x, marker.y, marker.weight, point->X)),
196  NULL,
197  &quaternion(0));
198  num_residuals++;
199 
200  if (quaternion_manifold == NULL) {
201  quaternion_manifold = new ceres::QuaternionManifold();
202  }
203 
204  problem.SetManifold(&quaternion(0), quaternion_manifold);
205  }
206  }
207 
208  LG << "Number of residuals: " << num_residuals;
209 
210  if (num_residuals) {
211  // Configure the solve.
212  ceres::Solver::Options solver_options;
213  solver_options.linear_solver_type = ceres::DENSE_QR;
214  solver_options.max_num_iterations = 50;
215  solver_options.update_state_every_iteration = true;
216  solver_options.gradient_tolerance = 1e-36;
217  solver_options.parameter_tolerance = 1e-36;
218  solver_options.function_tolerance = 1e-36;
219 
220  // Run the solve.
221  ceres::Solver::Summary summary;
222  ceres::Solve(solver_options, &problem, &summary);
223 
224  LG << "Summary:\n" << summary.FullReport();
225  LG << "Refined quaternion " << quaternion.transpose();
226  }
227 
228  // Convert quaternion to rotation matrix.
229  Mat3 R;
230  ceres::QuaternionToRotation(&quaternion(0), &R(0, 0));
231  reconstruction->InsertCamera(image, R, Vec3::Zero());
232 
233  // STEP 3: reproject all new markers appeared at image
234 
235  // Check if there're new markers appeared on current image
236  // and reproject them on sphere to obtain 3D position/
237  for (int track = 0; track <= max_track; ++track) {
238  if (!reconstruction->PointForTrack(track)) {
239  Marker marker = tracks.MarkerInImageForTrack(image, track);
240 
241  if (marker.image == image) {
242  // New track appeared on this image,
243  // project its position onto sphere.
244 
245  LG << "Projecting track " << track << " at image " << image;
246 
247  Vec3 X;
248  ProjectMarkerOnSphere(marker, X);
249  reconstruction->InsertPoint(track, R.inverse() * X);
250  }
251  }
252  }
253  }
254 }
255 
256 } // namespace libmv
#define snprintf
Definition: BLI_winstuff.h:53
_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
#define X
Definition: GeomUtils.cpp:199
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
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
SIMD_FORCE_INLINE btVector3 operator()(const btVector3 &x) const
Return the transform of the vector.
Definition: btTransform.h:90
ProjectivePoint * PointForTrack(int track)
Returns a pointer to the point corresponding to track.
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
const ProjectiveReconstruction & reconstruction
Definition: intersect.cc:198
#define LG
#define T
#define R
double observed_y_
double weight_
double observed_x_
Definition: modal_solver.cc:99
Vec3 bundle_
Eigen::Vector4d Vec4
Definition: numeric.h:107
void ModalSolver(const Tracks &tracks, EuclideanReconstruction *reconstruction, ProgressUpdateCallback *update_callback)
Eigen::Matrix< double, 3, 3 > Mat3
Definition: numeric.h:72
Eigen::MatrixXd Mat
Definition: numeric.h:60
void GetR_FixedCameraCenter(const Mat &x1, const Mat &x2, const double focal, Mat3 *R)
Definition: panography.cc:99
Eigen::Vector3d Vec3
Definition: numeric.h:106
ListBase tracks
Definition: tracking.c:60