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