Assorted camera tracker improvements
[blender.git] / extern / libmv / libmv / simple_pipeline / bundle.cc
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
21 #define V3DLIB_ENABLE_SUITESPARSE 1
22
23 #include <map>
24
25 #include "libmv/base/vector.h"
26 #include "libmv/logging/logging.h"
27 #include "libmv/multiview/fundamental.h"
28 #include "libmv/multiview/projection.h"
29 #include "libmv/numeric/numeric.h"
30 #include "libmv/simple_pipeline/camera_intrinsics.h"
31 #include "libmv/simple_pipeline/bundle.h"
32 #include "libmv/simple_pipeline/reconstruction.h"
33 #include "libmv/simple_pipeline/tracks.h"
34 #include "third_party/ssba/Geometry/v3d_cameramatrix.h"
35 #include "third_party/ssba/Geometry/v3d_metricbundle.h"
36 #include "third_party/ssba/Math/v3d_linear.h"
37 #include "third_party/ssba/Math/v3d_linear_utils.h"
38
39 namespace libmv {
40
41 void EuclideanBundle(const Tracks &tracks,
42                      EuclideanReconstruction *reconstruction) {
43   CameraIntrinsics intrinsics;
44   EuclideanBundleCommonIntrinsics(tracks,
45                                   BUNDLE_NO_INTRINSICS,
46                                   reconstruction,
47                                   &intrinsics);
48 }
49
50 void EuclideanBundleCommonIntrinsics(const Tracks &tracks,
51                                      int bundle_intrinsics,
52                                      EuclideanReconstruction *reconstruction,
53                                      CameraIntrinsics *intrinsics) {
54   LG << "Original intrinsics: " << *intrinsics;
55   vector<Marker> markers = tracks.AllMarkers();
56
57   // "index" in this context is the index that V3D's optimizer will see. The
58   // V3D index must be dense in that the cameras are numbered 0...n-1, which is
59   // not the case for the "image" numbering that arises from the tracks
60   // structure. The complicated mapping is necessary to convert between the two
61   // representations.
62   std::map<EuclideanCamera *, int> camera_to_index;
63   std::map<EuclideanPoint *, int> point_to_index;
64   vector<EuclideanCamera *> index_to_camera;
65   vector<EuclideanPoint *> index_to_point;
66   int num_cameras = 0;
67   int num_points = 0;
68   for (int i = 0; i < markers.size(); ++i) {
69     const Marker &marker = markers[i];
70     EuclideanCamera *camera = reconstruction->CameraForImage(marker.image);
71     EuclideanPoint *point = reconstruction->PointForTrack(marker.track);
72     if (camera && point) {
73       if (camera_to_index.find(camera) == camera_to_index.end()) {
74         camera_to_index[camera] = num_cameras;
75         index_to_camera.push_back(camera);
76         num_cameras++;
77       }
78       if (point_to_index.find(point) == point_to_index.end()) {
79         point_to_index[point] = num_points;
80         index_to_point.push_back(point);
81         num_points++;
82       }
83     }
84   }
85
86   // Convert libmv's K matrix to V3d's K matrix.
87   V3D::Matrix3x3d v3d_K;
88   for (int i = 0; i < 3; ++i) {
89     for (int j = 0; j < 3; ++j) {
90       v3d_K[i][j] = intrinsics->K()(i, j);
91     }
92   }
93
94   // Convert libmv's distortion to v3d distortion.
95   V3D::StdDistortionFunction v3d_distortion;
96   v3d_distortion.k1 = intrinsics->k1();
97   v3d_distortion.k2 = intrinsics->k2();
98   v3d_distortion.p1 = intrinsics->p1();
99   v3d_distortion.p2 = intrinsics->p2();
100
101   // Convert libmv's cameras to V3D's cameras.
102   std::vector<V3D::CameraMatrix> v3d_cameras(index_to_camera.size());
103   for (int k = 0; k < index_to_camera.size(); ++k) {
104     V3D::Matrix3x3d R;
105     V3D::Vector3d t;
106
107     // Libmv's rotation matrix type.
108     const Mat3 &R_libmv = index_to_camera[k]->R;
109     const Vec3 &t_libmv = index_to_camera[k]->t;
110
111     for (int i = 0; i < 3; ++i) {
112       for (int j = 0; j < 3; ++j) {
113         R[i][j] = R_libmv(i, j);
114       }
115       t[i] = t_libmv(i);
116     }
117     v3d_cameras[k].setIntrinsic(v3d_K);
118     v3d_cameras[k].setRotation(R);
119     v3d_cameras[k].setTranslation(t);
120   }
121   LG << "Number of cameras: " << index_to_camera.size();
122
123   // Convert libmv's points to V3D's points.
124   std::vector<V3D::Vector3d> v3d_points(index_to_point.size());
125   for (int i = 0; i < index_to_point.size(); i++) {
126     v3d_points[i][0] = index_to_point[i]->X(0);
127     v3d_points[i][1] = index_to_point[i]->X(1);
128     v3d_points[i][2] = index_to_point[i]->X(2);
129   }
130   LG << "Number of points: " << index_to_point.size();
131
132   // Convert libmv's measurements to v3d measurements.
133   int num_residuals = 0;
134   std::vector<V3D::Vector2d> v3d_measurements;
135   std::vector<int> v3d_camera_for_measurement;
136   std::vector<int> v3d_point_for_measurement;
137   for (int i = 0; i < markers.size(); ++i) {
138     EuclideanCamera *camera = reconstruction->CameraForImage(markers[i].image);
139     EuclideanPoint *point = reconstruction->PointForTrack(markers[i].track);
140     if (!camera || !point) {
141       continue;
142     }
143     V3D::Vector2d v3d_point;
144     v3d_point[0] = markers[i].x;
145     v3d_point[1] = markers[i].y;
146     v3d_measurements.push_back(v3d_point);
147     v3d_camera_for_measurement.push_back(camera_to_index[camera]);
148     v3d_point_for_measurement.push_back(point_to_index[point]);
149     num_residuals++;
150   }
151   LG << "Number of residuals: " << num_residuals;
152   
153   // Convert from libmv's specification for which intrinsics to bundle to V3D's.
154   int v3d_bundle_intrinsics;
155   if (bundle_intrinsics == BUNDLE_NO_INTRINSICS) {
156     LG << "Bundling only camera positions.";
157     v3d_bundle_intrinsics = V3D::FULL_BUNDLE_METRIC;
158   } else if (bundle_intrinsics == BUNDLE_FOCAL_LENGTH) {
159     LG << "Bundling f.";
160     v3d_bundle_intrinsics = V3D::FULL_BUNDLE_FOCAL_LENGTH;
161   } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
162                                    BUNDLE_PRINCIPAL_POINT)) {
163     LG << "Bundling f, px, py.";
164     v3d_bundle_intrinsics = V3D::FULL_BUNDLE_FOCAL_LENGTH_PP;
165   } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
166                                    BUNDLE_PRINCIPAL_POINT |
167                                    BUNDLE_RADIAL)) {
168     LG << "Bundling f, px, py, k1, k2.";
169     v3d_bundle_intrinsics = V3D::FULL_BUNDLE_RADIAL;
170   } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
171                                    BUNDLE_PRINCIPAL_POINT |
172                                    BUNDLE_RADIAL |
173                                    BUNDLE_TANGENTIAL)) {
174     LG << "Bundling f, px, py, k1, k2, p1, p2.";
175     v3d_bundle_intrinsics = V3D::FULL_BUNDLE_RADIAL_TANGENTIAL;
176   } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
177                                    BUNDLE_RADIAL |
178                                    BUNDLE_TANGENTIAL)) {
179     LG << "Bundling f, px, py, k1, k2, p1, p2.";
180     v3d_bundle_intrinsics = V3D::FULL_BUNDLE_RADIAL_TANGENTIAL;
181   } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
182                                    BUNDLE_RADIAL)) {
183     LG << "Bundling f, k1, k2.";
184     v3d_bundle_intrinsics = V3D::FULL_BUNDLE_FOCAL_AND_RADIAL;
185   } else if (bundle_intrinsics == (BUNDLE_FOCAL_LENGTH |
186                                    BUNDLE_RADIAL_K1)) {
187     LG << "Bundling f, k1.";
188     v3d_bundle_intrinsics = V3D::FULL_BUNDLE_FOCAL_AND_RADIAL_K1;
189   } else {
190     LOG(FATAL) << "Unsupported bundle combination.";
191   }
192
193   // Ignore any outliers; assume supervised tracking.
194   double v3d_inlier_threshold = 500000.0;
195
196   // Finally, run the bundle adjustment.
197   V3D::CommonInternalsMetricBundleOptimizer opt(v3d_bundle_intrinsics,
198                                                 v3d_inlier_threshold,
199                                                 v3d_K,
200                                                 v3d_distortion,
201                                                 v3d_cameras,
202                                                 v3d_points,
203                                                 v3d_measurements,
204                                                 v3d_camera_for_measurement,
205                                                 v3d_point_for_measurement);
206   opt.maxIterations = 500;
207   opt.minimize();
208   if (opt.status == V3D::LEVENBERG_OPTIMIZER_TIMEOUT) {
209     LG << "Bundle status: Timed out.";
210   } else if (opt.status == V3D::LEVENBERG_OPTIMIZER_SMALL_UPDATE) {
211     LG << "Bundle status: Small update.";
212   } else if (opt.status == V3D::LEVENBERG_OPTIMIZER_CONVERGED) {
213     LG << "Bundle status: Converged.";
214   }
215
216   // Convert V3D's K matrix back to libmv's K matrix.
217   Mat3 K;
218   for (int i = 0; i < 3; ++i) {
219     for (int j = 0; j < 3; ++j) {
220       K(i, j) = v3d_K[i][j];
221     }
222   }
223   intrinsics->SetK(K);
224
225   // Convert V3D's distortion back to libmv's distortion.
226   intrinsics->SetRadialDistortion(v3d_distortion.k1, v3d_distortion.k2, 0.0);
227   intrinsics->SetTangentialDistortion(v3d_distortion.p1, v3d_distortion.p2);
228
229   // Convert V3D's cameras back to libmv's cameras.
230   for (int k = 0; k < num_cameras; k++) {
231     V3D::Matrix3x4d const Rt = v3d_cameras[k].getOrientation();
232     for (int i = 0; i < 3; ++i) {
233       for (int j = 0; j < 3; ++j) {
234         index_to_camera[k]->R(i, j) = Rt[i][j];
235       }
236       index_to_camera[k]->t(i) = Rt[i][3];
237     }
238   }
239
240   // Convert V3D's points back to libmv's points.
241   for (int k = 0; k < num_points; k++) {
242     for (int i = 0; i < 3; ++i) {
243       index_to_point[k]->X(i) = v3d_points[k][i];
244     }
245   }
246   LG << "Final intrinsics: " << *intrinsics;
247 }
248
249 void ProjectiveBundle(const Tracks & /*tracks*/,
250                       ProjectiveReconstruction * /*reconstruction*/) {
251   // TODO(keir): Implement this! This can't work until we have a better bundler
252   // than SSBA, since SSBA has no support for projective bundling.
253 }
254
255 }  // namespace libmv