90dfde156604f0f115e9f78d0690f0ca5d7484a7
[blender.git] / extern / libmv / libmv / simple_pipeline / modal_solver.cc
1 // Copyright (c) 2012 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
21 #include "libmv/simple_pipeline/modal_solver.h"
22
23 #include <cstdio>
24
25 #include "ceres/ceres.h"
26 #include "ceres/rotation.h"
27 #include "libmv/logging/logging.h"
28 #include "libmv/multiview/panography.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 ModalSolverLogProress(ProgressUpdateCallback *update_callback,
46                            double progress) {
47   if (update_callback) {
48     char message[256];
49
50     snprintf(message, sizeof(message), "Solving progress %d%%",
51              (int)(progress * 100));
52
53     update_callback->invoke(progress, message);
54   }
55 }
56
57 struct ModalReprojectionError {
58   ModalReprojectionError(double observed_x,
59                          double observed_y,
60                          const Vec3 &bundle)
61     : observed_x(observed_x), observed_y(observed_y), bundle(bundle) { }
62
63   template <typename T>
64   bool operator()(const T* quaternion,   // Rotation quaternion
65                   T* residuals) const {
66     T R[9];
67     ceres::QuaternionToRotation(quaternion, R);
68
69     // Convert bundle position from double to T.
70     T X[3];
71     X[0] = T(bundle(0));
72     X[1] = T(bundle(1));
73     X[2] = T(bundle(2));
74
75     // Compute projective coordinates: x = RX.
76     T x[3];
77     x[0] = R[0]*X[0] + R[3]*X[1] + R[6]*X[2];
78     x[1] = R[1]*X[0] + R[4]*X[1] + R[7]*X[2];
79     x[2] = R[2]*X[0] + R[5]*X[1] + R[8]*X[2];
80
81     // Compute normalized coordinates: x /= x[2].
82     T xn = x[0] / x[2];
83     T yn = x[1] / x[2];
84
85     // The error is the difference between reprojected
86     // and observed marker position.
87     residuals[0] = xn - T(observed_x);
88     residuals[1] = yn - T(observed_y);
89
90     return true;
91   }
92
93   double observed_x;
94   double observed_y;
95   Vec3 bundle;
96 };
97 }  // namespace
98
99 void ModalSolver(const Tracks &tracks,
100                  EuclideanReconstruction *reconstruction,
101                  ProgressUpdateCallback *update_callback) {
102   int max_image = tracks.MaxImage();
103   int max_track = tracks.MaxTrack();
104
105   LG << "Max image: " << max_image;
106   LG << "Max track: " << max_track;
107
108   // For minimization we're using quaternions.
109   Vec3 zero_rotation = Vec3::Zero();
110   Vec4 quaternion;
111   ceres::AngleAxisToQuaternion(&zero_rotation(0), &quaternion(0));
112
113   for (int image = 0; image <= max_image; ++image) {
114     vector<Marker> all_markers = tracks.MarkersInImage(image);
115
116     ModalSolverLogProress(update_callback, (float) image / max_image);
117
118     // Skip empty images without doing anything.
119     if (all_markers.size() == 0) {
120       LG << "Skipping image: " << image;
121       continue;
122     }
123
124     // STEP 1: Estimate rotation analytically.
125     Mat3 current_R;
126     ceres::QuaternionToRotation(&quaternion(0), &current_R(0, 0));
127
128     // Construct point cloud for current and previous images,
129     // using markers appear at current image for which we know
130     // 3D positions.
131     Mat x1, x2;
132     for (int i = 0; i < all_markers.size(); ++i) {
133       Marker &marker = all_markers[i];
134       EuclideanPoint *point = reconstruction->PointForTrack(marker.track);
135       if (point) {
136         Vec3 X;
137         ProjectMarkerOnSphere(marker, X);
138
139         int last_column = x1.cols();
140         x1.conservativeResize(3, last_column + 1);
141         x2.conservativeResize(3, last_column + 1);
142
143         x1.col(last_column) = current_R * point->X;
144         x2.col(last_column) = X;
145       }
146     }
147
148     if (x1.cols() >= 2) {
149       Mat3 delta_R;
150
151       // Compute delta rotation matrix for two point clouds.
152       // Could be a bit confusing at first glance, but order
153       // of clouds is indeed so.
154       GetR_FixedCameraCenter(x2, x1, 1.0, &delta_R);
155
156       // Convert delta rotation form matrix to final image
157       // rotation stored in a quaternion
158       Vec3 delta_angle_axis;
159       ceres::RotationMatrixToAngleAxis(&delta_R(0, 0), &delta_angle_axis(0));
160
161       Vec3 current_angle_axis;
162       ceres::QuaternionToAngleAxis(&quaternion(0), &current_angle_axis(0));
163
164       Vec3 angle_axis = current_angle_axis + delta_angle_axis;
165
166       ceres::AngleAxisToQuaternion(&angle_axis(0), &quaternion(0));
167
168       LG << "Analytically computed quaternion "
169          << quaternion.transpose();
170     }
171
172     // STEP 2: Refine rotation with Ceres.
173     ceres::Problem problem;
174
175     ceres::LocalParameterization* quaternion_parameterization =
176         new ceres::QuaternionParameterization;
177
178     int num_residuals = 0;
179     for (int i = 0; i < all_markers.size(); ++i) {
180       Marker &marker = all_markers[i];
181       EuclideanPoint *point = reconstruction->PointForTrack(marker.track);
182
183       if (point) {
184         problem.AddResidualBlock(new ceres::AutoDiffCostFunction<
185             ModalReprojectionError,
186             2, /* num_residuals */
187             4>(new ModalReprojectionError(marker.x, marker.y,
188                                           point->X)),
189             NULL,
190             &quaternion(0));
191         num_residuals++;
192
193         problem.SetParameterization(&quaternion(0),
194                                     quaternion_parameterization);
195       }
196     }
197
198     LG << "Number of residuals: " << num_residuals;
199
200     if (num_residuals) {
201       // Configure the solve.
202       ceres::Solver::Options solver_options;
203       solver_options.linear_solver_type = ceres::DENSE_QR;
204       solver_options.max_num_iterations = 50;
205       solver_options.update_state_every_iteration = true;
206       solver_options.gradient_tolerance = 1e-36;
207       solver_options.parameter_tolerance = 1e-36;
208       solver_options.function_tolerance = 1e-36;
209
210       // Run the solve.
211       ceres::Solver::Summary summary;
212       ceres::Solve(solver_options, &problem, &summary);
213
214       LG << "Summary:\n" << summary.FullReport();
215       LG << "Refined quaternion " << quaternion.transpose();
216     }
217
218     // Convert quaternion to rotation matrix.
219     Mat3 R;
220     ceres::QuaternionToRotation(&quaternion(0), &R(0, 0));
221     reconstruction->InsertCamera(image, R, Vec3::Zero());
222
223     // STEP 3: reproject all new markers appeared at image
224
225     // Check if there're new markers appeared on current image
226     // and reproject them on sphere to obtain 3D position/
227     for (int track = 0; track <= max_track; ++track) {
228       if (!reconstruction->PointForTrack(track)) {
229         Marker marker = tracks.MarkerInImageForTrack(image, track);
230
231         if (marker.image == image) {
232           // New track appeared on this image,
233           // project it's position onto sphere.
234
235           LG << "Projecting track " << track << " at image " << image;
236
237           Vec3 X;
238           ProjectMarkerOnSphere(marker, X);
239           reconstruction->InsertPoint(track, R.inverse() * X);
240         }
241       }
242     }
243   }
244 }
245
246 }  // namespace libmv