Camera tracking: moved camera solver into it's own job
[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/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(ProgressUpdateCallback *update_callback,
122     double progress,
123     const char *step = NULL)
124 {
125   if(update_callback) {
126     char message[256];
127
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
133     update_callback->invoke(progress, message);
134   }
135 }
136
137 template<typename PipelineRoutines>
138 void InternalCompleteReconstruction(
139     const Tracks &tracks,
140     typename PipelineRoutines::Reconstruction *reconstruction,
141     ProgressUpdateCallback *update_callback = NULL) {
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,
171                                          (double)tot_resects/(max_image));
172         PipelineRoutines::Intersect(reconstructed_markers, reconstruction);
173         num_intersects++;
174         LG << "Ran Intersect() for track " << track;
175       }
176     }
177     if (num_intersects) {
178       CompleteReconstructionLogProress(update_callback,
179                                        (double)tot_resects/(max_image),
180                                        "Bundling...");
181       PipelineRoutines::Bundle(tracks, reconstruction);
182       LG << "Ran Bundle() after intersections.";
183     }
184     LG << "Did " << num_intersects << " intersects.";
185
186     // Do all possible resections.
187     num_resects = 0;
188     for (int image = 0; image <= max_image; ++image) {
189       if (reconstruction->CameraForImage(image)) {
190         LG << "Skipping frame: " << image;
191         continue;
192       }
193       vector<Marker> all_markers = tracks.MarkersInImage(image);
194       LG << "Got " << all_markers.size() << " markers for image " << image;
195
196       vector<Marker> reconstructed_markers;
197       for (int i = 0; i < all_markers.size(); ++i) {
198         if (reconstruction->PointForTrack(all_markers[i].track)) {
199           reconstructed_markers.push_back(all_markers[i]);
200         }
201       }
202       LG << "Got " << reconstructed_markers.size()
203          << " reconstructed markers for image " << image;
204       if (reconstructed_markers.size() >= 5) {
205         CompleteReconstructionLogProress(update_callback,
206                                          (double)tot_resects/(max_image));
207         if (PipelineRoutines::Resect(reconstructed_markers, reconstruction, false)) {
208           num_resects++;
209           tot_resects++;
210           LG << "Ran Resect() for image " << image;
211         } else {
212           LG << "Failed Resect() for image " << image;
213         }
214       }
215     }
216     if (num_resects) {
217       CompleteReconstructionLogProress(update_callback,
218                                        (double)tot_resects/(max_image),
219                                        "Bundling...");
220       PipelineRoutines::Bundle(tracks, reconstruction);
221     }
222     LG << "Did " << num_resects << " resects.";
223   }
224
225   // One last pass...
226   num_resects = 0;
227   for (int image = 0; image <= max_image; ++image) {
228     if (reconstruction->CameraForImage(image)) {
229       LG << "Skipping frame: " << image;
230       continue;
231     }
232     vector<Marker> all_markers = tracks.MarkersInImage(image);
233
234     vector<Marker> reconstructed_markers;
235     for (int i = 0; i < all_markers.size(); ++i) {
236       if (reconstruction->PointForTrack(all_markers[i].track)) {
237         reconstructed_markers.push_back(all_markers[i]);
238       }
239     }
240     if (reconstructed_markers.size() >= 5) {
241       CompleteReconstructionLogProress(update_callback,
242                                        (double)tot_resects/(max_image));
243       if (PipelineRoutines::Resect(reconstructed_markers, reconstruction, true)) {
244         num_resects++;
245         LG << "Ran Resect() for image " << image;
246       } else {
247         LG << "Failed Resect() for image " << image;
248       }
249     }
250   }
251   if (num_resects) {
252     CompleteReconstructionLogProress(update_callback,
253                                      (double)tot_resects/(max_image),
254                                      "Bundling...");
255     PipelineRoutines::Bundle(tracks, reconstruction);
256   }
257 }
258
259 template<typename PipelineRoutines>
260 double InternalReprojectionError(const Tracks &image_tracks,
261                                  const typename PipelineRoutines::Reconstruction &reconstruction,
262                                  const CameraIntrinsics &intrinsics) {
263   int num_skipped = 0;
264   int num_reprojected = 0;
265   double total_error = 0.0;
266   vector<Marker> markers = image_tracks.AllMarkers();
267   for (int i = 0; i < markers.size(); ++i) {
268     const typename PipelineRoutines::Camera *camera =
269         reconstruction.CameraForImage(markers[i].image);
270     const typename PipelineRoutines::Point *point =
271         reconstruction.PointForTrack(markers[i].track);
272     if (!camera || !point) {
273       num_skipped++;
274       continue;
275     }
276     num_reprojected++;
277
278     Marker reprojected_marker =
279         PipelineRoutines::ProjectMarker(*point, *camera, intrinsics);
280     double ex = reprojected_marker.x - markers[i].x;
281     double ey = reprojected_marker.y - markers[i].y;
282 #if 0
283     const int N = 100;
284     char line[N];
285     snprintf(line, N,
286            "image %-3d track %-3d "
287            "x %7.1f y %7.1f "
288            "rx %7.1f ry %7.1f "
289            "ex %7.1f ey %7.1f"
290            "    e %7.1f",
291            markers[i].image,
292            markers[i].track,
293            markers[i].x,
294            markers[i].y,
295            reprojected_marker.x,
296            reprojected_marker.y,
297            ex,
298            ey,
299            sqrt(ex*ex + ey*ey));
300 #endif
301     total_error += sqrt(ex*ex + ey*ey);
302   }
303   LG << "Skipped " << num_skipped << " markers.";
304   LG << "Reprojected " << num_reprojected << " markers.";
305   LG << "Total error: " << total_error;
306   LG << "Average error: " << (total_error / num_reprojected) << " [pixels].";
307   return total_error / num_reprojected;
308 }
309
310 double EuclideanReprojectionError(const Tracks &image_tracks,
311                                   const EuclideanReconstruction &reconstruction,
312                                   const CameraIntrinsics &intrinsics) {
313   return InternalReprojectionError<EuclideanPipelineRoutines>(image_tracks,
314                                                               reconstruction,
315                                                               intrinsics);
316 }
317
318 double ProjectiveReprojectionError(
319     const Tracks &image_tracks,
320     const ProjectiveReconstruction &reconstruction,
321     const CameraIntrinsics &intrinsics) {
322   return InternalReprojectionError<ProjectivePipelineRoutines>(image_tracks,
323                                                                reconstruction,
324                                                                intrinsics);
325 }
326
327 void EuclideanCompleteReconstruction(const Tracks &tracks,
328                                      EuclideanReconstruction *reconstruction,
329                                      ProgressUpdateCallback *update_callback) {
330   InternalCompleteReconstruction<EuclideanPipelineRoutines>(tracks,
331                                                             reconstruction,
332                                                             update_callback);
333 }
334
335 void ProjectiveCompleteReconstruction(const Tracks &tracks,
336                                       ProjectiveReconstruction *reconstruction) {
337   InternalCompleteReconstruction<ProjectivePipelineRoutines>(tracks,
338                                                              reconstruction);
339 }
340
341 void InvertIntrinsicsForTracks(const Tracks &raw_tracks,
342                                const CameraIntrinsics &camera_intrinsics,
343                                Tracks *calibrated_tracks) {
344   vector<Marker> markers = raw_tracks.AllMarkers();
345   for (int i = 0; i < markers.size(); ++i) {
346     camera_intrinsics.InvertIntrinsics(markers[i].x,
347                                        markers[i].y,
348                                        &(markers[i].x),
349                                        &(markers[i].y));
350   }
351   *calibrated_tracks = Tracks(markers);
352 }
353
354 }  // namespace libmv