cb8822dcf44db82ba5014046be59cefa28a6d8f8
[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/reconstruction.h"
31 #include "libmv/simple_pipeline/tracks.h"
32 #include "third_party/ssba/Geometry/v3d_cameramatrix.h"
33 #include "third_party/ssba/Geometry/v3d_metricbundle.h"
34 #include "third_party/ssba/Math/v3d_linear.h"
35 #include "third_party/ssba/Math/v3d_linear_utils.h"
36
37 namespace libmv {
38
39 void EuclideanBundle(const Tracks &tracks,
40                      EuclideanReconstruction *reconstruction) {
41   vector<Marker> markers = tracks.AllMarkers();
42
43   // "index" in this context is the index that V3D's optimizer will see. The
44   // V3D index must be dense in that the cameras are numbered 0...n-1, which is
45   // not the case for the "image" numbering that arises from the tracks
46   // structure. The complicated mapping is necessary to convert between the two
47   // representations.
48   std::map<EuclideanCamera *, int> camera_to_index;
49   std::map<EuclideanPoint *, int> point_to_index;
50   vector<EuclideanCamera *> index_to_camera;
51   vector<EuclideanPoint *> index_to_point;
52   int num_cameras = 0;
53   int num_points = 0;
54   for (int i = 0; i < markers.size(); ++i) {
55     const Marker &marker = markers[i];
56     EuclideanCamera *camera = reconstruction->CameraForImage(marker.image);
57     EuclideanPoint *point = reconstruction->PointForTrack(marker.track);
58     if (camera && point) {
59       if (camera_to_index.find(camera) == camera_to_index.end()) {
60         camera_to_index[camera] = num_cameras;
61         index_to_camera.push_back(camera);
62         num_cameras++;
63       }
64       if (point_to_index.find(point) == point_to_index.end()) {
65         point_to_index[point] = num_points;
66         index_to_point.push_back(point);
67         num_points++;
68       }
69     }
70   }
71
72   // Make a V3D identity matrix, needed in a few places for K, since this
73   // assumes a calibrated setup.
74   V3D::Matrix3x3d identity3x3;
75   identity3x3[0][0] = 1.0;
76   identity3x3[0][1] = 0.0;
77   identity3x3[0][2] = 0.0;
78   identity3x3[1][0] = 0.0;
79   identity3x3[1][1] = 1.0;
80   identity3x3[1][2] = 0.0;
81   identity3x3[2][0] = 0.0;
82   identity3x3[2][1] = 0.0;
83   identity3x3[2][2] = 1.0;
84
85   // Convert libmv's cameras to V3D's cameras.
86   std::vector<V3D::CameraMatrix> v3d_cameras(index_to_camera.size());
87   for (int k = 0; k < index_to_camera.size(); ++k) {
88     V3D::Matrix3x3d R;
89     V3D::Vector3d t;
90
91     // Libmv's rotation matrix type.
92     const Mat3 &R_libmv = index_to_camera[k]->R;
93     const Vec3 &t_libmv = index_to_camera[k]->t;
94
95     for (int i = 0; i < 3; ++i) {
96       for (int j = 0; j < 3; ++j) {
97         R[i][j] = R_libmv(i, j);
98       }
99       t[i] = t_libmv(i);
100     }
101     v3d_cameras[k].setIntrinsic(identity3x3);
102     v3d_cameras[k].setRotation(R);
103     v3d_cameras[k].setTranslation(t);
104   }
105   LG << "Number of cameras: " << index_to_camera.size();
106
107   // Convert libmv's points to V3D's points.
108   std::vector<V3D::Vector3d> v3d_points(index_to_point.size());
109   for (int i = 0; i < index_to_point.size(); i++) {
110     v3d_points[i][0] = index_to_point[i]->X(0);
111     v3d_points[i][1] = index_to_point[i]->X(1);
112     v3d_points[i][2] = index_to_point[i]->X(2);
113   }
114   LG << "Number of points: " << index_to_point.size();
115
116   // Convert libmv's measurements to v3d measurements.
117   int num_residuals = 0;
118   std::vector<V3D::Vector2d> v3d_measurements;
119   std::vector<int> v3d_camera_for_measurement;
120   std::vector<int> v3d_point_for_measurement;
121   for (int i = 0; i < markers.size(); ++i) {
122     EuclideanCamera *camera = reconstruction->CameraForImage(markers[i].image);
123     EuclideanPoint *point = reconstruction->PointForTrack(markers[i].track);
124     if (!camera || !point) {
125       continue;
126     }
127     V3D::Vector2d v3d_point;
128     v3d_point[0] = markers[i].x;
129     v3d_point[1] = markers[i].y;
130     v3d_measurements.push_back(v3d_point);
131     v3d_camera_for_measurement.push_back(camera_to_index[camera]);
132     v3d_point_for_measurement.push_back(point_to_index[point]);
133     num_residuals++;
134   }
135   LG << "Number of residuals: " << num_residuals;
136   
137   // This is calibrated reconstruction, so use zero distortion.
138   V3D::StdDistortionFunction v3d_distortion;
139   v3d_distortion.k1 = 0;
140   v3d_distortion.k2 = 0;
141   v3d_distortion.p1 = 0;
142   v3d_distortion.p2 = 0;
143
144   // Finally, run the bundle adjustment.
145   double const inlierThreshold = 500000.0;
146   V3D::CommonInternalsMetricBundleOptimizer opt(V3D::FULL_BUNDLE_METRIC,
147                                                 inlierThreshold,
148                                                 identity3x3,
149                                                 v3d_distortion,
150                                                 v3d_cameras,
151                                                 v3d_points,
152                                                 v3d_measurements,
153                                                 v3d_camera_for_measurement,
154                                                 v3d_point_for_measurement);
155   opt.maxIterations = 50;
156   opt.minimize();
157   LG << "Bundle status: " << opt.status;
158
159   // Convert V3D's cameras back to libmv's cameras.
160   for (int k = 0; k < num_cameras; k++) {
161     V3D::Matrix3x4d const Rt = v3d_cameras[k].getOrientation();
162     for (int i = 0; i < 3; ++i) {
163       for (int j = 0; j < 3; ++j) {
164         index_to_camera[k]->R(i, j) = Rt[i][j];
165       }
166       index_to_camera[k]->t(i) = Rt[i][3];
167     }
168   }
169
170   // Convert V3D's points back to libmv's points.
171   for (int k = 0; k < num_points; k++) {
172     for (int i = 0; i < 3; ++i) {
173       index_to_point[k]->X(i) = v3d_points[k][i];
174     }
175   }
176 }
177
178 void ProjectiveBundle(const Tracks & /*tracks*/,
179                       ProjectiveReconstruction * /*reconstruction*/) {
180   // TODO(keir): Implement this! This can't work until we have a better bundler
181   // than SSBA, since SSBA has no support for projective bundling.
182 }
183
184 }  // namespace libmv