9512a41c00f33cd6d1a53fbfc96d73aca015be2e
[blender-staging.git] / extern / libmv / libmv / simple_pipeline / pipeline.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 #include <cstdio>
22
23 #include "libmv/logging/logging.h"
24 #include "libmv/simple_pipeline/bundle.h"
25 #include "libmv/simple_pipeline/intersect.h"
26 #include "libmv/simple_pipeline/resect.h"
27 #include "libmv/simple_pipeline/reconstruction.h"
28 #include "libmv/simple_pipeline/tracks.h"
29 #include "libmv/simple_pipeline/camera_intrinsics.h"
30
31 #ifdef _MSC_VER
32 #  define snprintf _snprintf
33 #endif
34
35 namespace libmv {
36 namespace {
37
38 // These are "strategy" classes which make it possible to use the same code for
39 // both projective and euclidean reconstruction.
40 // FIXME(MatthiasF): OOP would achieve the same goal while avoiding
41 // template bloat and making interface changes much easier.
42 struct EuclideanPipelineRoutines {
43   typedef EuclideanReconstruction Reconstruction;
44   typedef EuclideanCamera Camera;
45   typedef EuclideanPoint Point;
46
47   static void Bundle(const Tracks &tracks,
48                      EuclideanReconstruction *reconstruction) {
49     EuclideanBundle(tracks, reconstruction);
50   }
51
52   static bool Resect(const vector<Marker> &markers,
53                      EuclideanReconstruction *reconstruction, bool final_pass) {
54     return EuclideanResect(markers, reconstruction, final_pass);
55   }
56
57   static bool Intersect(const vector<Marker> &markers,
58                         EuclideanReconstruction *reconstruction) {
59     return EuclideanIntersect(markers, reconstruction);
60   }
61
62   static Marker ProjectMarker(const EuclideanPoint &point,
63                               const EuclideanCamera &camera,
64                               const CameraIntrinsics &intrinsics) {
65     Vec3 projected = camera.R * point.X + camera.t;
66     projected /= projected(2);
67
68     Marker reprojected_marker;
69     intrinsics.ApplyIntrinsics(projected(0),
70                                projected(1),
71                                &reprojected_marker.x,
72                                &reprojected_marker.y);
73
74     reprojected_marker.image = camera.image;
75     reprojected_marker.track = point.track;
76     return reprojected_marker;
77   }
78 };
79
80 struct ProjectivePipelineRoutines {
81   typedef ProjectiveReconstruction Reconstruction;
82   typedef ProjectiveCamera Camera;
83   typedef ProjectivePoint Point;
84
85   static void Bundle(const Tracks &tracks,
86                      ProjectiveReconstruction *reconstruction) {
87     ProjectiveBundle(tracks, reconstruction);
88   }
89
90   static bool Resect(const vector<Marker> &markers,
91                      ProjectiveReconstruction *reconstruction, bool final_pass) {
92     return ProjectiveResect(markers, reconstruction);
93   }
94
95   static bool Intersect(const vector<Marker> &markers,
96                         ProjectiveReconstruction *reconstruction) {
97     return ProjectiveIntersect(markers, reconstruction);
98   }
99
100   static Marker ProjectMarker(const ProjectivePoint &point,
101                               const ProjectiveCamera &camera,
102                               const CameraIntrinsics &intrinsics) {
103     Vec3 projected = camera.P * point.X;
104     projected /= projected(2);
105
106     Marker reprojected_marker;
107     intrinsics.ApplyIntrinsics(projected(0),
108                                projected(1),
109                                &reprojected_marker.x,
110                                &reprojected_marker.y);
111
112     reprojected_marker.image = camera.image;
113     reprojected_marker.track = point.track;
114     return reprojected_marker;
115   }
116 };
117
118 }  // namespace
119
120 template<typename PipelineRoutines>
121 void InternalCompleteReconstruction(
122     const Tracks &tracks,
123     typename PipelineRoutines::Reconstruction *reconstruction) {
124   int max_track = tracks.MaxTrack();
125   int max_image = tracks.MaxImage();
126   int num_resects = -1;
127   int num_intersects = -1;
128   LG << "Max track: " << max_track;
129   LG << "Max image: " << max_image;
130   LG << "Number of markers: " << tracks.NumMarkers();
131   while (num_resects != 0 || num_intersects != 0) {
132     // Do all possible intersections.
133     num_intersects = 0;
134     for (int track = 0; track <= max_track; ++track) {
135       if (reconstruction->PointForTrack(track)) {
136         LG << "Skipping point: " << track;
137         continue;
138       }
139       vector<Marker> all_markers = tracks.MarkersForTrack(track);
140       LG << "Got " << all_markers.size() << " markers for track " << track;
141
142       vector<Marker> reconstructed_markers;
143       for (int i = 0; i < all_markers.size(); ++i) {
144         if (reconstruction->CameraForImage(all_markers[i].image)) {
145           reconstructed_markers.push_back(all_markers[i]);
146         }
147       }
148       LG << "Got " << reconstructed_markers.size()
149          << " reconstructed markers for track " << track;
150       if (reconstructed_markers.size() >= 2) {
151         PipelineRoutines::Intersect(reconstructed_markers, reconstruction);
152         num_intersects++;
153         LG << "Ran Intersect() for track " << track;
154       }
155     }
156     if (num_intersects) {
157       PipelineRoutines::Bundle(tracks, reconstruction);
158       LG << "Ran Bundle() after intersections.";
159     }
160     LG << "Did " << num_intersects << " intersects.";
161
162     // Do all possible resections.
163     num_resects = 0;
164     for (int image = 0; image <= max_image; ++image) {
165       if (reconstruction->CameraForImage(image)) {
166         LG << "Skipping frame: " << image;
167         continue;
168       }
169       vector<Marker> all_markers = tracks.MarkersInImage(image);
170       LG << "Got " << all_markers.size() << " markers for image " << image;
171
172       vector<Marker> reconstructed_markers;
173       for (int i = 0; i < all_markers.size(); ++i) {
174         if (reconstruction->PointForTrack(all_markers[i].track)) {
175           reconstructed_markers.push_back(all_markers[i]);
176         }
177       }
178       LG << "Got " << reconstructed_markers.size()
179          << " reconstructed markers for image " << image;
180       if (reconstructed_markers.size() >= 5) {
181         if (PipelineRoutines::Resect(reconstructed_markers, reconstruction, false)) {
182           num_resects++;
183           LG << "Ran Resect() for image " << image;
184         } else {
185           LG << "Failed Resect() for image " << image;
186         }
187       }
188     }
189     if (num_resects) {
190       PipelineRoutines::Bundle(tracks, reconstruction);
191     }
192     LG << "Did " << num_resects << " resects.";
193   }
194
195   // One last pass...
196   num_resects = 0;
197   for (int image = 0; image <= max_image; ++image) {
198     if (reconstruction->CameraForImage(image)) {
199       LG << "Skipping frame: " << image;
200       continue;
201     }
202     vector<Marker> all_markers = tracks.MarkersInImage(image);
203
204     vector<Marker> reconstructed_markers;
205     for (int i = 0; i < all_markers.size(); ++i) {
206       if (reconstruction->PointForTrack(all_markers[i].track)) {
207         reconstructed_markers.push_back(all_markers[i]);
208       }
209     }
210     if (reconstructed_markers.size() >= 5) {
211       if (PipelineRoutines::Resect(reconstructed_markers, reconstruction, true)) {
212         num_resects++;
213         LG << "Ran Resect() for image " << image;
214       } else {
215         LG << "Failed Resect() for image " << image;
216       }
217     }
218   }
219   if (num_resects) {
220     PipelineRoutines::Bundle(tracks, reconstruction);
221   }
222 }
223
224 template<typename PipelineRoutines>
225 double InternalReprojectionError(const Tracks &image_tracks,
226                                  const typename PipelineRoutines::Reconstruction &reconstruction,
227                                  const CameraIntrinsics &intrinsics) {
228   int num_skipped = 0;
229   int num_reprojected = 0;
230   double total_error = 0.0;
231   vector<Marker> markers = image_tracks.AllMarkers();
232   for (int i = 0; i < markers.size(); ++i) {
233     const typename PipelineRoutines::Camera *camera =
234         reconstruction.CameraForImage(markers[i].image);
235     const typename PipelineRoutines::Point *point =
236         reconstruction.PointForTrack(markers[i].track);
237     if (!camera || !point) {
238       num_skipped++;
239       continue;
240     }
241     num_reprojected++;
242
243     Marker reprojected_marker =
244         PipelineRoutines::ProjectMarker(*point, *camera, intrinsics);
245     double ex = reprojected_marker.x - markers[i].x;
246     double ey = reprojected_marker.y - markers[i].y;
247
248     const int N = 100;
249     char line[N];
250     snprintf(line, N,
251            "image %-3d track %-3d "
252            "x %7.1f y %7.1f "
253            "rx %7.1f ry %7.1f "
254            "ex %7.1f ey %7.1f"
255            "    e %7.1f",
256            markers[i].image,
257            markers[i].track,
258            markers[i].x,
259            markers[i].y,
260            reprojected_marker.x,
261            reprojected_marker.y,
262            ex,
263            ey,
264            sqrt(ex*ex + ey*ey));
265     total_error += sqrt(ex*ex + ey*ey);
266   }
267   LG << "Skipped " << num_skipped << " markers.";
268   LG << "Reprojected " << num_reprojected << " markers.";
269   LG << "Total error: " << total_error;
270   LG << "Average error: " << (total_error / num_reprojected) << " [pixels].";
271   return total_error / num_reprojected;
272 }
273
274 double EuclideanReprojectionError(const Tracks &image_tracks,
275                                   const EuclideanReconstruction &reconstruction,
276                                   const CameraIntrinsics &intrinsics) {
277   return InternalReprojectionError<EuclideanPipelineRoutines>(image_tracks,
278                                                               reconstruction,
279                                                               intrinsics);
280 }
281
282 double ProjectiveReprojectionError(
283     const Tracks &image_tracks,
284     const ProjectiveReconstruction &reconstruction,
285     const CameraIntrinsics &intrinsics) {
286   return InternalReprojectionError<ProjectivePipelineRoutines>(image_tracks,
287                                                                reconstruction,
288                                                                intrinsics);
289 }
290
291 void EuclideanCompleteReconstruction(const Tracks &tracks,
292                                      EuclideanReconstruction *reconstruction) {
293   InternalCompleteReconstruction<EuclideanPipelineRoutines>(tracks,
294                                                             reconstruction);
295 }
296
297 void ProjectiveCompleteReconstruction(const Tracks &tracks,
298                                       ProjectiveReconstruction *reconstruction) {
299   InternalCompleteReconstruction<ProjectivePipelineRoutines>(tracks,
300                                                              reconstruction);
301 }
302
303 void InvertIntrinsicsForTracks(const Tracks &raw_tracks,
304                                const CameraIntrinsics &camera_intrinsics,
305                                Tracks *calibrated_tracks) {
306   vector<Marker> markers = raw_tracks.AllMarkers();
307   for (int i = 0; i < markers.size(); ++i) {
308     camera_intrinsics.InvertIntrinsics(markers[i].x,
309                                        markers[i].y,
310                                        &(markers[i].x),
311                                        &(markers[i].y));
312   }
313   *calibrated_tracks = Tracks(markers);
314 }
315
316 }  // namespace libmv