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