Weighted tracks
[blender.git] / extern / libmv / libmv-capi.cc
1 /*
2  * ***** BEGIN GPL LICENSE BLOCK *****
3  *
4  * This program is free software; you can redistribute it and/or
5  * modify it under the terms of the GNU General Public License
6  * as published by the Free Software Foundation; either version 2
7  * of the License, or (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  * GNU General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with this program; if not, write to the Free Software Foundation,
16  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
17  *
18  * The Original Code is Copyright (C) 2011 Blender Foundation.
19  * All rights reserved.
20  *
21  * Contributor(s): Blender Foundation,
22  *                 Sergey Sharybin
23  *
24  * ***** END GPL LICENSE BLOCK *****
25  */
26
27 #ifdef WITH_LIBMV
28
29 /* define this to generate PNG images with content of search areas
30    tracking between which failed */
31 #undef DUMP_FAILURE
32
33 /* define this to generate PNG images with content of search areas
34    on every itteration of tracking */
35 #undef DUMP_ALWAYS
36
37 #include "libmv-capi.h"
38
39 #include <cstdlib>
40 #include <cassert>
41
42 #if defined(DUMP_FAILURE) || defined (DUMP_ALWAYS)
43 #  include <png.h>
44 #endif
45
46 #include "libmv-capi_intern.h"
47 #include "libmv/logging/logging.h"
48 #include "libmv/multiview/homography.h"
49 #include "libmv/tracking/track_region.h"
50 #include "libmv/simple_pipeline/callbacks.h"
51 #include "libmv/simple_pipeline/tracks.h"
52 #include "libmv/simple_pipeline/initialize_reconstruction.h"
53 #include "libmv/simple_pipeline/bundle.h"
54 #include "libmv/simple_pipeline/detect.h"
55 #include "libmv/simple_pipeline/pipeline.h"
56 #include "libmv/simple_pipeline/camera_intrinsics.h"
57 #include "libmv/simple_pipeline/modal_solver.h"
58 #include "libmv/simple_pipeline/reconstruction_scale.h"
59 #include "libmv/simple_pipeline/keyframe_selection.h"
60
61 struct libmv_Reconstruction {
62         libmv::EuclideanReconstruction reconstruction;
63
64         /* used for per-track average error calculation after reconstruction */
65         libmv::Tracks tracks;
66         libmv::CameraIntrinsics intrinsics;
67
68         double error;
69 };
70
71 struct libmv_Features {
72         int count, margin;
73         libmv::Feature *features;
74 };
75
76 /* ************ Logging ************ */
77
78 void libmv_initLogging(const char *argv0)
79 {
80         /* Make it so FATAL messages are always print into console */
81         char severity_fatal[32];
82         snprintf(severity_fatal, sizeof(severity_fatal), "%d",
83                  google::GLOG_FATAL);
84
85         google::InitGoogleLogging(argv0);
86         google::SetCommandLineOption("logtostderr", "1");
87         google::SetCommandLineOption("v", "0");
88         google::SetCommandLineOption("stderrthreshold", severity_fatal);
89         google::SetCommandLineOption("minloglevel", severity_fatal);
90 }
91
92 void libmv_startDebugLogging(void)
93 {
94         google::SetCommandLineOption("logtostderr", "1");
95         google::SetCommandLineOption("v", "2");
96         google::SetCommandLineOption("stderrthreshold", "1");
97         google::SetCommandLineOption("minloglevel", "0");
98 }
99
100 void libmv_setLoggingVerbosity(int verbosity)
101 {
102         char val[10];
103         snprintf(val, sizeof(val), "%d", verbosity);
104
105         google::SetCommandLineOption("v", val);
106 }
107
108 /* ************ Utility ************ */
109
110 static void floatBufToImage(const float *buf, int width, int height, int channels, libmv::FloatImage *image)
111 {
112         int x, y, k, a = 0;
113
114         image->Resize(height, width, channels);
115
116         for (y = 0; y < height; y++) {
117                 for (x = 0; x < width; x++) {
118                         for (k = 0; k < channels; k++) {
119                                 (*image)(y, x, k) = buf[a++];
120                         }
121                 }
122         }
123 }
124
125 static void imageToFloatBuf(const libmv::FloatImage *image, int channels, float *buf)
126 {
127         int x, y, k, a = 0;
128
129         for (y = 0; y < image->Height(); y++) {
130                 for (x = 0; x < image->Width(); x++) {
131                         for (k = 0; k < channels; k++) {
132                                 buf[a++] = (*image)(y, x, k);
133                         }
134                 }
135         }
136 }
137
138 #if defined(DUMP_FAILURE) || defined (DUMP_ALWAYS)
139 static void savePNGImage(png_bytep *row_pointers, int width, int height, int depth, int color_type,
140                          const char *file_name)
141 {
142         png_infop info_ptr;
143         png_structp png_ptr;
144         FILE *fp = fopen(file_name, "wb");
145
146         if (!fp)
147                 return;
148
149         /* Initialize stuff */
150         png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
151         info_ptr = png_create_info_struct(png_ptr);
152
153         if (setjmp(png_jmpbuf(png_ptr))) {
154                 fclose(fp);
155                 return;
156         }
157
158         png_init_io(png_ptr, fp);
159
160         /* write header */
161         if (setjmp(png_jmpbuf(png_ptr))) {
162                 fclose(fp);
163                 return;
164         }
165
166         png_set_IHDR(png_ptr, info_ptr, width, height,
167                 depth, color_type, PNG_INTERLACE_NONE,
168                 PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);
169
170         png_write_info(png_ptr, info_ptr);
171
172         /* write bytes */
173         if (setjmp(png_jmpbuf(png_ptr))) {
174                 fclose(fp);
175                 return;
176         }
177
178         png_write_image(png_ptr, row_pointers);
179
180         /* end write */
181         if (setjmp(png_jmpbuf(png_ptr))) {
182                 fclose(fp);
183                 return;
184         }
185
186         png_write_end(png_ptr, NULL);
187
188         fclose(fp);
189 }
190
191 static void saveImage(const char *prefix, libmv::FloatImage image, int x0, int y0)
192 {
193         int x, y;
194         png_bytep *row_pointers;
195
196         row_pointers = (png_bytep *) malloc(sizeof(png_bytep) * image.Height());
197
198         for (y = 0; y < image.Height(); y++) {
199                 row_pointers[y] = (png_bytep) malloc(sizeof(png_byte) * 4 * image.Width());
200
201                 for (x = 0; x < image.Width(); x++) {
202                         if (x0 == x && image.Height() - y0 - 1 == y) {
203                                 row_pointers[y][x * 4 + 0] = 255;
204                                 row_pointers[y][x * 4 + 1] = 0;
205                                 row_pointers[y][x * 4 + 2] = 0;
206                                 row_pointers[y][x * 4 + 3] = 255;
207                         }
208                         else {
209                                 float pixel = image(image.Height() - y - 1, x, 0);
210                                 row_pointers[y][x * 4 + 0] = pixel * 255;
211                                 row_pointers[y][x * 4 + 1] = pixel * 255;
212                                 row_pointers[y][x * 4 + 2] = pixel * 255;
213                                 row_pointers[y][x * 4 + 3] = 255;
214                         }
215                 }
216         }
217
218         {
219                 static int a = 0;
220                 char buf[128];
221                 snprintf(buf, sizeof(buf), "%s_%02d.png", prefix, ++a);
222                 savePNGImage(row_pointers, image.Width(), image.Height(), 8, PNG_COLOR_TYPE_RGBA, buf);
223         }
224
225         for (y = 0; y < image.Height(); y++) {
226                 free(row_pointers[y]);
227         }
228         free(row_pointers);
229 }
230
231 static void saveBytesImage(const char *prefix, unsigned char *data, int width, int height)
232 {
233         int x, y;
234         png_bytep *row_pointers;
235
236         row_pointers = (png_bytep *) malloc(sizeof(png_bytep) * height);
237
238         for (y = 0; y < height; y++) {
239                 row_pointers[y] = (png_bytep) malloc(sizeof(png_byte) * 4 * width);
240
241                 for (x = 0; x < width; x++) {
242                         char pixel = data[width * y + x];
243                         row_pointers[y][x * 4 + 0] = pixel;
244                         row_pointers[y][x * 4 + 1] = pixel;
245                         row_pointers[y][x * 4 + 2] = pixel;
246                         row_pointers[y][x * 4 + 3] = 255;
247                 }
248         }
249
250         {
251                 static int a = 0;
252                 char buf[128];
253                 snprintf(buf, sizeof(buf), "%s_%02d.png", prefix, ++a);
254                 savePNGImage(row_pointers, width, height, 8, PNG_COLOR_TYPE_RGBA, buf);
255         }
256
257         for (y = 0; y < height; y++) {
258                 free(row_pointers[y]);
259         }
260         free(row_pointers);
261 }
262 #endif
263
264 /* ************ Planar tracker ************ */
265
266 /* TrackRegion */
267 int libmv_trackRegion(const libmv_TrackRegionOptions *options,
268                       const float *image1, int image1_width, int image1_height,
269                       const float *image2, int image2_width, int image2_height,
270                       const double *x1, const double *y1,
271                       libmv_TrackRegionResult *result,
272                       double *x2, double *y2)
273 {
274         double xx1[5], yy1[5];
275         double xx2[5], yy2[5];
276         bool tracking_result = false;
277
278         /* Convert to doubles for the libmv api. The four corners and the center. */
279         for (int i = 0; i < 5; ++i) {
280                 xx1[i] = x1[i];
281                 yy1[i] = y1[i];
282                 xx2[i] = x2[i];
283                 yy2[i] = y2[i];
284         }
285
286         libmv::TrackRegionOptions track_region_options;
287         libmv::FloatImage image1_mask;
288
289         switch (options->motion_model) {
290 #define LIBMV_CONVERT(the_model) \
291         case libmv::TrackRegionOptions::the_model: \
292                 track_region_options.mode = libmv::TrackRegionOptions::the_model; \
293                 break;
294                 LIBMV_CONVERT(TRANSLATION)
295                 LIBMV_CONVERT(TRANSLATION_ROTATION)
296                 LIBMV_CONVERT(TRANSLATION_SCALE)
297                 LIBMV_CONVERT(TRANSLATION_ROTATION_SCALE)
298                 LIBMV_CONVERT(AFFINE)
299                 LIBMV_CONVERT(HOMOGRAPHY)
300 #undef LIBMV_CONVERT
301         }
302
303         track_region_options.minimum_correlation = options->minimum_correlation;
304         track_region_options.max_iterations = options->num_iterations;
305         track_region_options.sigma = options->sigma;
306         track_region_options.num_extra_points = 1;
307         track_region_options.image1_mask = NULL;
308         track_region_options.use_brute_initialization = options->use_brute;
309         track_region_options.use_normalized_intensities = options->use_normalization;
310
311         if (options->image1_mask) {
312                 floatBufToImage(options->image1_mask, image1_width, image1_height, 1, &image1_mask);
313
314                 track_region_options.image1_mask = &image1_mask;
315         }
316
317         /* Convert from raw float buffers to libmv's FloatImage. */
318         libmv::FloatImage old_patch, new_patch;
319         floatBufToImage(image1, image1_width, image1_height, 1, &old_patch);
320         floatBufToImage(image2, image2_width, image2_height, 1, &new_patch);
321
322         libmv::TrackRegionResult track_region_result;
323         libmv::TrackRegion(old_patch, new_patch, xx1, yy1, track_region_options, xx2, yy2, &track_region_result);
324
325         /* Convert to floats for the blender api. */
326         for (int i = 0; i < 5; ++i) {
327                 x2[i] = xx2[i];
328                 y2[i] = yy2[i];
329         }
330
331         /* TODO(keir): Update the termination string with failure details. */
332         if (track_region_result.termination == libmv::TrackRegionResult::PARAMETER_TOLERANCE ||
333             track_region_result.termination == libmv::TrackRegionResult::FUNCTION_TOLERANCE  ||
334             track_region_result.termination == libmv::TrackRegionResult::GRADIENT_TOLERANCE  ||
335             track_region_result.termination == libmv::TrackRegionResult::NO_CONVERGENCE)
336         {
337                 tracking_result = true;
338         }
339
340 #if defined(DUMP_FAILURE) || defined(DUMP_ALWAYS)
341 #if defined(DUMP_ALWAYS)
342         {
343 #else
344         if (!tracking_result) {
345 #endif
346                 saveImage("old_patch", old_patch, x1[4], y1[4]);
347                 saveImage("new_patch", new_patch, x2[4], y2[4]);
348
349                 if (options->image1_mask)
350                         saveImage("mask", image1_mask, x2[4], y2[4]);
351         }
352 #endif
353
354         return tracking_result;
355 }
356
357 void libmv_samplePlanarPatch(const float *image, int width, int height,
358                              int channels, const double *xs, const double *ys,
359                              int num_samples_x, int num_samples_y,
360                              const float *mask, float *patch,
361                              double *warped_position_x, double *warped_position_y)
362 {
363         libmv::FloatImage libmv_image, libmv_patch, libmv_mask;
364         libmv::FloatImage *libmv_mask_for_sample = NULL;
365
366         floatBufToImage(image, width, height, channels, &libmv_image);
367
368         if (mask) {
369                 floatBufToImage(mask, width, height, 1, &libmv_mask);
370
371                 libmv_mask_for_sample = &libmv_mask;
372         }
373
374         libmv::SamplePlanarPatch(libmv_image, xs, ys, num_samples_x, num_samples_y,
375                                  libmv_mask_for_sample, &libmv_patch,
376                                  warped_position_x, warped_position_y);
377
378         imageToFloatBuf(&libmv_patch, channels, patch);
379 }
380
381 /* ************ Tracks ************ */
382
383 struct libmv_Tracks *libmv_tracksNew(void)
384 {
385         libmv::Tracks *libmv_tracks = LIBMV_OBJECT_NEW(libmv::Tracks);
386
387         return (struct libmv_Tracks *)libmv_tracks;
388 }
389
390 void libmv_tracksDestroy(struct libmv_Tracks *libmv_tracks)
391 {
392         using libmv::Tracks;
393         LIBMV_OBJECT_DELETE(libmv_tracks, Tracks);
394 }
395
396 void libmv_tracksInsert(struct libmv_Tracks *libmv_tracks, int image, int track, double x, double y, double weight)
397 {
398         ((libmv::Tracks*) libmv_tracks)->Insert(image, track, x, y, weight);
399 }
400
401 /* ************ Reconstruction ************ */
402
403 class ReconstructUpdateCallback : public libmv::ProgressUpdateCallback {
404 public:
405         ReconstructUpdateCallback(reconstruct_progress_update_cb progress_update_callback, void *callback_customdata)
406         {
407                 progress_update_callback_ = progress_update_callback;
408                 callback_customdata_ = callback_customdata;
409         }
410
411         void invoke(double progress, const char *message)
412         {
413                 if (progress_update_callback_) {
414                         progress_update_callback_(callback_customdata_, progress, message);
415                 }
416         }
417 protected:
418         reconstruct_progress_update_cb progress_update_callback_;
419         void *callback_customdata_;
420 };
421
422 static void libmv_solveRefineIntrinsics(const libmv::Tracks &tracks,
423                                         const int refine_intrinsics,
424                                         const int bundle_constraints,
425                                         reconstruct_progress_update_cb progress_update_callback,
426                                         void *callback_customdata,
427                                         libmv::EuclideanReconstruction *reconstruction,
428                                         libmv::CameraIntrinsics *intrinsics)
429 {
430         /* only a few combinations are supported but trust the caller */
431         int bundle_intrinsics = 0;
432
433         if (refine_intrinsics & LIBMV_REFINE_FOCAL_LENGTH) {
434                 bundle_intrinsics |= libmv::BUNDLE_FOCAL_LENGTH;
435         }
436         if (refine_intrinsics & LIBMV_REFINE_PRINCIPAL_POINT) {
437                 bundle_intrinsics |= libmv::BUNDLE_PRINCIPAL_POINT;
438         }
439         if (refine_intrinsics & LIBMV_REFINE_RADIAL_DISTORTION_K1) {
440                 bundle_intrinsics |= libmv::BUNDLE_RADIAL_K1;
441         }
442         if (refine_intrinsics & LIBMV_REFINE_RADIAL_DISTORTION_K2) {
443                 bundle_intrinsics |= libmv::BUNDLE_RADIAL_K2;
444         }
445
446         progress_update_callback(callback_customdata, 1.0, "Refining solution");
447
448         libmv::EuclideanBundleCommonIntrinsics(tracks,
449                                                bundle_intrinsics,
450                                                bundle_constraints,
451                                                reconstruction,
452                                                intrinsics);
453 }
454
455 static void cameraIntrinsicsFromOptions(const libmv_CameraIntrinsicsOptions *camera_intrinsics_options,
456                                         libmv::CameraIntrinsics *camera_intrinsics)
457 {
458         camera_intrinsics->SetFocalLength(camera_intrinsics_options->focal_length,
459                                           camera_intrinsics_options->focal_length);
460
461         camera_intrinsics->SetPrincipalPoint(camera_intrinsics_options->principal_point_x,
462                                              camera_intrinsics_options->principal_point_y);
463
464         camera_intrinsics->SetRadialDistortion(camera_intrinsics_options->k1,
465                                                camera_intrinsics_options->k2,
466                                                camera_intrinsics_options->k3);
467
468         camera_intrinsics->SetImageSize(camera_intrinsics_options->image_width,
469                                         camera_intrinsics_options->image_height);
470 }
471
472 static libmv::Tracks getNormalizedTracks(const libmv::Tracks &tracks, const libmv::CameraIntrinsics &camera_intrinsics)
473 {
474         libmv::vector<libmv::Marker> markers = tracks.AllMarkers();
475
476         for (int i = 0; i < markers.size(); ++i) {
477                 camera_intrinsics.InvertIntrinsics(markers[i].x, markers[i].y,
478                         &(markers[i].x), &(markers[i].y));
479         }
480
481         return libmv::Tracks(markers);
482 }
483
484 static void finishReconstruction(const libmv::Tracks &tracks, const libmv::CameraIntrinsics &camera_intrinsics,
485                                  struct libmv_Reconstruction *libmv_reconstruction,
486                                  reconstruct_progress_update_cb progress_update_callback,
487                                  void *callback_customdata)
488 {
489         libmv::EuclideanReconstruction &reconstruction = libmv_reconstruction->reconstruction;
490
491         /* reprojection error calculation */
492         progress_update_callback(callback_customdata, 1.0, "Finishing solution");
493         libmv_reconstruction->tracks = tracks;
494         libmv_reconstruction->error = libmv::EuclideanReprojectionError(tracks, reconstruction, camera_intrinsics);
495 }
496
497 static bool selectTwoKeyframesBasedOnGRICAndVariance(
498                           libmv::Tracks &tracks,
499                           libmv::Tracks &normalized_tracks,
500                           libmv::CameraIntrinsics &camera_intrinsics,
501                           int &keyframe1,
502                           int &keyframe2)
503 {
504         libmv::vector<int> keyframes;
505
506         /* Get list of all keyframe candidates first. */
507         SelectKeyframesBasedOnGRICAndVariance(normalized_tracks,
508                                               camera_intrinsics,
509                                               keyframes);
510
511         if (keyframes.size() < 2) {
512                 LG << "Not enough keyframes detected by GRIC";
513                 return false;
514         }
515         else if (keyframes.size() == 2) {
516                 keyframe1 = keyframes[0];
517                 keyframe2 = keyframes[1];
518                 return true;
519         }
520
521         /* Now choose two keyframes with minimal reprojection error after initial
522          * reconstruction choose keyframes with the least reprojection error after
523          * solving from two candidate keyframes.
524          *
525          * In fact, currently libmv returns single pair only, so this code will
526          * not actually run. But in the future this could change, so let's stay
527          * prepared.
528          */
529         int previous_keyframe = keyframes[0];
530         double best_error = std::numeric_limits<double>::max();
531         for (int i = 1; i < keyframes.size(); i++) {
532                 libmv::EuclideanReconstruction reconstruction;
533                 int current_keyframe = keyframes[i];
534
535                 libmv::vector<libmv::Marker> keyframe_markers =
536                         normalized_tracks.MarkersForTracksInBothImages(previous_keyframe,
537                                                                        current_keyframe);
538
539                 libmv::Tracks keyframe_tracks(keyframe_markers);
540
541                 /* get a solution from two keyframes only */
542                 libmv::EuclideanReconstructTwoFrames(keyframe_markers, &reconstruction);
543                 libmv::EuclideanBundle(keyframe_tracks, &reconstruction);
544                 libmv::EuclideanCompleteReconstruction(keyframe_tracks,
545                                                        &reconstruction, NULL);
546
547                 double current_error =
548                         libmv::EuclideanReprojectionError(tracks,
549                                                           reconstruction,
550                                                           camera_intrinsics);
551
552                 LG << "Error between " << previous_keyframe
553                    << " and " << current_keyframe
554                    << ": " << current_error;
555
556                 if (current_error < best_error) {
557                         best_error = current_error;
558                         keyframe1 = previous_keyframe;
559                         keyframe2 = current_keyframe;
560                 }
561
562                 previous_keyframe = current_keyframe;
563         }
564
565         return true;
566 }
567
568 struct libmv_Reconstruction *libmv_solveReconstruction(const struct libmv_Tracks *libmv_tracks,
569                 const libmv_CameraIntrinsicsOptions *libmv_camera_intrinsics_options,
570                 libmv_ReconstructionOptions *libmv_reconstruction_options,
571                 reconstruct_progress_update_cb progress_update_callback,
572                 void *callback_customdata)
573 {
574         struct libmv_Reconstruction *libmv_reconstruction = LIBMV_OBJECT_NEW(libmv_Reconstruction);
575
576         libmv::Tracks &tracks = *((libmv::Tracks *) libmv_tracks);
577         libmv::EuclideanReconstruction &reconstruction = libmv_reconstruction->reconstruction;
578         libmv::CameraIntrinsics &camera_intrinsics = libmv_reconstruction->intrinsics;
579
580         ReconstructUpdateCallback update_callback =
581                 ReconstructUpdateCallback(progress_update_callback, callback_customdata);
582
583         /* Retrieve reconstruction options from C-API to libmv API */
584         cameraIntrinsicsFromOptions(libmv_camera_intrinsics_options, &camera_intrinsics);
585
586         /* Invert the camera intrinsics */
587         libmv::Tracks normalized_tracks = getNormalizedTracks(tracks, camera_intrinsics);
588
589         /* keyframe selection */
590         int keyframe1 = libmv_reconstruction_options->keyframe1,
591             keyframe2 = libmv_reconstruction_options->keyframe2;
592
593         if (libmv_reconstruction_options->select_keyframes) {
594                 LG << "Using automatic keyframe selection";
595
596                 update_callback.invoke(0, "Selecting keyframes");
597
598                 selectTwoKeyframesBasedOnGRICAndVariance(tracks,
599                                                          normalized_tracks,
600                                                          camera_intrinsics,
601                                                          keyframe1,
602                                                          keyframe2);
603
604                 /* so keyframes in the interface would be updated */
605                 libmv_reconstruction_options->keyframe1 = keyframe1;
606                 libmv_reconstruction_options->keyframe2 = keyframe2;
607         }
608
609         /* actual reconstruction */
610         LG << "frames to init from: " << keyframe1 << " " << keyframe2;
611
612         libmv::vector<libmv::Marker> keyframe_markers =
613                 normalized_tracks.MarkersForTracksInBothImages(keyframe1, keyframe2);
614
615         LG << "number of markers for init: " << keyframe_markers.size();
616
617         update_callback.invoke(0, "Initial reconstruction");
618
619         libmv::EuclideanReconstructTwoFrames(keyframe_markers, &reconstruction);
620         libmv::EuclideanBundle(normalized_tracks, &reconstruction);
621         libmv::EuclideanCompleteReconstruction(normalized_tracks,
622                                                &reconstruction, &update_callback);
623
624         /* refinement */
625         if (libmv_reconstruction_options->refine_intrinsics) {
626                 libmv_solveRefineIntrinsics(tracks,
627                                             libmv_reconstruction_options->refine_intrinsics,
628                                             libmv::BUNDLE_NO_CONSTRAINTS,
629                                             progress_update_callback,
630                                             callback_customdata,
631                                             &reconstruction,
632                                             &camera_intrinsics);
633         }
634
635         /* set reconstruction scale to unity */
636         libmv::EuclideanScaleToUnity(&reconstruction);
637
638         /* finish reconstruction */
639         finishReconstruction(tracks, camera_intrinsics, libmv_reconstruction,
640                              progress_update_callback, callback_customdata);
641
642         return (struct libmv_Reconstruction *)libmv_reconstruction;
643 }
644
645 struct libmv_Reconstruction *libmv_solveModal(const struct libmv_Tracks *libmv_tracks,
646                 const libmv_CameraIntrinsicsOptions *libmv_camera_intrinsics_options,
647                 const libmv_ReconstructionOptions *libmv_reconstruction_options,
648                 reconstruct_progress_update_cb progress_update_callback,
649                 void *callback_customdata)
650 {
651         struct libmv_Reconstruction *libmv_reconstruction = LIBMV_OBJECT_NEW(libmv_Reconstruction);
652
653         libmv::Tracks &tracks = *((libmv::Tracks *) libmv_tracks);
654         libmv::EuclideanReconstruction &reconstruction = libmv_reconstruction->reconstruction;
655         libmv::CameraIntrinsics &camera_intrinsics = libmv_reconstruction->intrinsics;
656
657         ReconstructUpdateCallback update_callback =
658                 ReconstructUpdateCallback(progress_update_callback, callback_customdata);
659
660         cameraIntrinsicsFromOptions(libmv_camera_intrinsics_options, &camera_intrinsics);
661
662         /* Invert the camera intrinsics. */
663         libmv::Tracks normalized_tracks = getNormalizedTracks(tracks, camera_intrinsics);
664
665         /* Actual reconstruction. */
666         libmv::ModalSolver(normalized_tracks, &reconstruction, &update_callback);
667
668         libmv::CameraIntrinsics empty_intrinsics;
669         libmv::EuclideanBundleCommonIntrinsics(normalized_tracks,
670                                                libmv::BUNDLE_NO_INTRINSICS,
671                                                libmv::BUNDLE_NO_TRANSLATION,
672                                                &reconstruction,
673                                                &empty_intrinsics);
674
675         /* Refinement. */
676         if (libmv_reconstruction_options->refine_intrinsics) {
677                 libmv_solveRefineIntrinsics(tracks,
678                                             libmv_reconstruction_options->refine_intrinsics,
679                                             libmv::BUNDLE_NO_TRANSLATION,
680                                             progress_update_callback, callback_customdata,
681                                             &reconstruction,
682                                             &camera_intrinsics);
683         }
684
685         /* Finish reconstruction. */
686         finishReconstruction(tracks, camera_intrinsics, libmv_reconstruction,
687                              progress_update_callback, callback_customdata);
688
689         return (struct libmv_Reconstruction *)libmv_reconstruction;
690 }
691
692 void libmv_reconstructionDestroy(struct libmv_Reconstruction *libmv_reconstruction)
693 {
694         LIBMV_OBJECT_DELETE(libmv_reconstruction, libmv_Reconstruction);
695 }
696
697 int libmv_reprojectionPointForTrack(const struct libmv_Reconstruction *libmv_reconstruction, int track, double pos[3])
698 {
699         const libmv::EuclideanReconstruction *reconstruction = &libmv_reconstruction->reconstruction;
700         const libmv::EuclideanPoint *point = reconstruction->PointForTrack(track);
701
702         if (point) {
703                 pos[0] = point->X[0];
704                 pos[1] = point->X[2];
705                 pos[2] = point->X[1];
706
707                 return 1;
708         }
709
710         return 0;
711 }
712
713 static libmv::Marker ProjectMarker(const libmv::EuclideanPoint &point,
714                                    const libmv::EuclideanCamera &camera,
715                                    const libmv::CameraIntrinsics &intrinsics)
716 {
717         libmv::Vec3 projected = camera.R * point.X + camera.t;
718         projected /= projected(2);
719
720         libmv::Marker reprojected_marker;
721         intrinsics.ApplyIntrinsics(projected(0), projected(1), &reprojected_marker.x, &reprojected_marker.y);
722
723         reprojected_marker.image = camera.image;
724         reprojected_marker.track = point.track;
725
726         return reprojected_marker;
727 }
728
729 double libmv_reprojectionErrorForTrack(const struct libmv_Reconstruction *libmv_reconstruction, int track)
730 {
731         const libmv::EuclideanReconstruction *reconstruction = &libmv_reconstruction->reconstruction;
732         const libmv::CameraIntrinsics *intrinsics = &libmv_reconstruction->intrinsics;
733         libmv::vector<libmv::Marker> markers = libmv_reconstruction->tracks.MarkersForTrack(track);
734
735         int num_reprojected = 0;
736         double total_error = 0.0;
737
738         for (int i = 0; i < markers.size(); ++i) {
739                 const libmv::EuclideanCamera *camera = reconstruction->CameraForImage(markers[i].image);
740                 const libmv::EuclideanPoint *point = reconstruction->PointForTrack(markers[i].track);
741
742                 if (!camera || !point) {
743                         continue;
744                 }
745
746                 num_reprojected++;
747
748                 libmv::Marker reprojected_marker = ProjectMarker(*point, *camera, *intrinsics);
749                 double ex = reprojected_marker.x - markers[i].x;
750                 double ey = reprojected_marker.y - markers[i].y;
751
752                 total_error += sqrt(ex * ex + ey * ey);
753         }
754
755         return total_error / num_reprojected;
756 }
757
758 double libmv_reprojectionErrorForImage(const struct libmv_Reconstruction *libmv_reconstruction, int image)
759 {
760         const libmv::EuclideanReconstruction *reconstruction = &libmv_reconstruction->reconstruction;
761         const libmv::CameraIntrinsics *intrinsics = &libmv_reconstruction->intrinsics;
762         libmv::vector<libmv::Marker> markers = libmv_reconstruction->tracks.MarkersInImage(image);
763         const libmv::EuclideanCamera *camera = reconstruction->CameraForImage(image);
764         int num_reprojected = 0;
765         double total_error = 0.0;
766
767         if (!camera)
768                 return 0;
769
770         for (int i = 0; i < markers.size(); ++i) {
771                 const libmv::EuclideanPoint *point = reconstruction->PointForTrack(markers[i].track);
772
773                 if (!point) {
774                         continue;
775                 }
776
777                 num_reprojected++;
778
779                 libmv::Marker reprojected_marker = ProjectMarker(*point, *camera, *intrinsics);
780                 double ex = reprojected_marker.x - markers[i].x;
781                 double ey = reprojected_marker.y - markers[i].y;
782
783                 total_error += sqrt(ex * ex + ey * ey);
784         }
785
786         return total_error / num_reprojected;
787 }
788
789 int libmv_reprojectionCameraForImage(const struct libmv_Reconstruction *libmv_reconstruction,
790                                       int image, double mat[4][4])
791 {
792         const libmv::EuclideanReconstruction *reconstruction = &libmv_reconstruction->reconstruction;
793         const libmv::EuclideanCamera *camera = reconstruction->CameraForImage(image);
794
795         if (camera) {
796                 for (int j = 0; j < 3; ++j) {
797                         for (int k = 0; k < 3; ++k) {
798                                 int l = k;
799
800                                 if (k == 1) l = 2;
801                                 else if (k == 2) l = 1;
802
803                                 if (j == 2) mat[j][l] = -camera->R(j,k);
804                                 else mat[j][l] = camera->R(j,k);
805                         }
806                         mat[j][3] = 0.0;
807                 }
808
809                 libmv::Vec3 optical_center = -camera->R.transpose() * camera->t;
810
811                 mat[3][0] = optical_center(0);
812                 mat[3][1] = optical_center(2);
813                 mat[3][2] = optical_center(1);
814
815                 mat[3][3] = 1.0;
816
817                 return 1;
818         }
819
820         return 0;
821 }
822
823 double libmv_reprojectionError(const struct libmv_Reconstruction *libmv_reconstruction)
824 {
825         return libmv_reconstruction->error;
826 }
827
828 struct libmv_CameraIntrinsics *libmv_reconstructionExtractIntrinsics(struct libmv_Reconstruction *libmv_reconstruction)
829 {
830         return (struct libmv_CameraIntrinsics *)&libmv_reconstruction->intrinsics;
831 }
832
833 /* ************ Feature detector ************ */
834
835 struct libmv_Features *libmv_detectFeaturesFAST(const unsigned char *data,
836                                                 int width, int height, int stride,
837                                                 int margin, int min_trackness, int min_distance)
838 {
839         libmv::Feature *features = NULL;
840         std::vector<libmv::Feature> v;
841         struct libmv_Features *libmv_features = LIBMV_STRUCT_NEW(libmv_Features, 1);
842         int i = 0, count;
843
844         if (margin) {
845                 data += margin * stride+margin;
846                 width -= 2 * margin;
847                 height -= 2 * margin;
848         }
849
850         v = libmv::DetectFAST(data, width, height, stride, min_trackness, min_distance);
851
852         count = v.size();
853
854         if (count) {
855                 features = LIBMV_STRUCT_NEW(libmv::Feature, count);
856
857                 for(std::vector<libmv::Feature>::iterator it = v.begin(); it != v.end(); it++) {
858                         features[i++] = *it;
859                 }
860         }
861
862         libmv_features->features = features;
863         libmv_features->count = count;
864         libmv_features->margin = margin;
865
866         return (struct libmv_Features *)libmv_features;
867 }
868
869 struct libmv_Features *libmv_detectFeaturesMORAVEC(const unsigned char *data,
870                                                    int width, int height, int stride,
871                                                    int margin, int count, int min_distance)
872 {
873         libmv::Feature *features = NULL;
874         struct libmv_Features *libmv_features = LIBMV_STRUCT_NEW(libmv_Features, 1);
875
876         if (count) {
877                 if (margin) {
878                         data += margin * stride+margin;
879                         width -= 2 * margin;
880                         height -= 2 * margin;
881                 }
882
883                 features = LIBMV_STRUCT_NEW(libmv::Feature, count);
884                 libmv::DetectMORAVEC(data, stride, width, height, features, &count, min_distance, NULL);
885         }
886
887         libmv_features->count = count;
888         libmv_features->margin = margin;
889         libmv_features->features = features;
890
891         return libmv_features;
892 }
893
894 void libmv_featuresDestroy(struct libmv_Features *libmv_features)
895 {
896         if (libmv_features->features) {
897                 LIBMV_STRUCT_DELETE(libmv_features->features);
898         }
899
900         LIBMV_STRUCT_DELETE(libmv_features);
901 }
902
903 int libmv_countFeatures(const struct libmv_Features *libmv_features)
904 {
905         return libmv_features->count;
906 }
907
908 void libmv_getFeature(const struct libmv_Features *libmv_features, int number, double *x, double *y, double *score, double *size)
909 {
910         libmv::Feature feature = libmv_features->features[number];
911
912         *x = feature.x + libmv_features->margin;
913         *y = feature.y + libmv_features->margin;
914         *score = feature.score;
915         *size = feature.size;
916 }
917
918 /* ************ Camera intrinsics ************ */
919
920 struct libmv_CameraIntrinsics *libmv_cameraIntrinsicsNewEmpty(void)
921 {
922         libmv::CameraIntrinsics *camera_intrinsics = LIBMV_OBJECT_NEW(libmv::CameraIntrinsics);
923
924         return (struct libmv_CameraIntrinsics *) camera_intrinsics;
925 }
926
927 struct libmv_CameraIntrinsics *libmv_cameraIntrinsicsNew(const libmv_CameraIntrinsicsOptions *libmv_camera_intrinsics_options)
928 {
929         libmv::CameraIntrinsics *camera_intrinsics = LIBMV_OBJECT_NEW(libmv::CameraIntrinsics);
930
931         cameraIntrinsicsFromOptions(libmv_camera_intrinsics_options, camera_intrinsics);
932
933         return (struct libmv_CameraIntrinsics *) camera_intrinsics;
934 }
935
936 struct libmv_CameraIntrinsics *libmv_cameraIntrinsicsCopy(const libmv_CameraIntrinsics *libmvIntrinsics)
937 {
938         libmv::CameraIntrinsics *orig_intrinsics = (libmv::CameraIntrinsics *) libmvIntrinsics;
939         libmv::CameraIntrinsics *new_intrinsics = LIBMV_OBJECT_NEW(libmv::CameraIntrinsics, *orig_intrinsics);
940
941         return (struct libmv_CameraIntrinsics *) new_intrinsics;
942 }
943
944 void libmv_cameraIntrinsicsDestroy(struct libmv_CameraIntrinsics *libmvIntrinsics)
945 {
946         using libmv::CameraIntrinsics;
947         LIBMV_OBJECT_DELETE(libmvIntrinsics, CameraIntrinsics);
948 }
949
950 void libmv_cameraIntrinsicsUpdate(const libmv_CameraIntrinsicsOptions *libmv_camera_intrinsics_options,
951                                   struct libmv_CameraIntrinsics *libmv_intrinsics)
952 {
953         libmv::CameraIntrinsics *camera_intrinsics = (libmv::CameraIntrinsics *) libmv_intrinsics;
954
955         double focal_length = libmv_camera_intrinsics_options->focal_length;
956         double principal_x = libmv_camera_intrinsics_options->principal_point_x;
957         double principal_y = libmv_camera_intrinsics_options->principal_point_y;
958         double k1 = libmv_camera_intrinsics_options->k1;
959         double k2 = libmv_camera_intrinsics_options->k2;
960         double k3 = libmv_camera_intrinsics_options->k3;
961         int image_width = libmv_camera_intrinsics_options->image_width;
962         int image_height = libmv_camera_intrinsics_options->image_height;
963
964         /* try avoid unnecessary updates so pre-computed distortion grids are not freed */
965
966         if (camera_intrinsics->focal_length() != focal_length)
967                 camera_intrinsics->SetFocalLength(focal_length, focal_length);
968
969         if (camera_intrinsics->principal_point_x() != principal_x ||
970             camera_intrinsics->principal_point_y() != principal_y)
971         {
972                 camera_intrinsics->SetPrincipalPoint(principal_x, principal_y);
973         }
974
975         if (camera_intrinsics->k1() != k1 ||
976             camera_intrinsics->k2() != k2 ||
977             camera_intrinsics->k3() != k3)
978         {
979                 camera_intrinsics->SetRadialDistortion(k1, k2, k3);
980         }
981
982         if (camera_intrinsics->image_width() != image_width ||
983             camera_intrinsics->image_height() != image_height)
984         {
985                 camera_intrinsics->SetImageSize(image_width, image_height);
986         }
987 }
988
989 void libmv_cameraIntrinsicsSetThreads(struct libmv_CameraIntrinsics *libmv_intrinsics, int threads)
990 {
991         libmv::CameraIntrinsics *camera_intrinsics = (libmv::CameraIntrinsics *) libmv_intrinsics;
992
993         camera_intrinsics->SetThreads(threads);
994 }
995
996 void libmv_cameraIntrinsicsExtract(const struct libmv_CameraIntrinsics *libmv_intrinsics, double *focal_length,
997                                    double *principal_x, double *principal_y, double *k1, double *k2, double *k3,
998                                    int *width, int *height)
999 {
1000         libmv::CameraIntrinsics *camera_intrinsics = (libmv::CameraIntrinsics *) libmv_intrinsics;
1001
1002         *focal_length = camera_intrinsics->focal_length();
1003         *principal_x = camera_intrinsics->principal_point_x();
1004         *principal_y = camera_intrinsics->principal_point_y();
1005         *k1 = camera_intrinsics->k1();
1006         *k2 = camera_intrinsics->k2();
1007         *k3 = camera_intrinsics->k3();
1008 }
1009
1010 void libmv_cameraIntrinsicsUndistortByte(const struct libmv_CameraIntrinsics *libmv_intrinsics,
1011                                          unsigned char *src, unsigned char *dst, int width, int height,
1012                                          float overscan, int channels)
1013 {
1014         libmv::CameraIntrinsics *camera_intrinsics = (libmv::CameraIntrinsics *) libmv_intrinsics;
1015
1016         camera_intrinsics->Undistort(src, dst, width, height, overscan, channels);
1017 }
1018
1019 void libmv_cameraIntrinsicsUndistortFloat(const struct libmv_CameraIntrinsics *libmvIntrinsics,
1020                                           float *src, float *dst, int width, int height,
1021                                           float overscan, int channels)
1022 {
1023         libmv::CameraIntrinsics *intrinsics = (libmv::CameraIntrinsics *) libmvIntrinsics;
1024
1025         intrinsics->Undistort(src, dst, width, height, overscan, channels);
1026 }
1027
1028 void libmv_cameraIntrinsicsDistortByte(const struct libmv_CameraIntrinsics *libmvIntrinsics,
1029                                        unsigned char *src, unsigned char *dst, int width, int height,
1030                                        float overscan, int channels)
1031 {
1032         libmv::CameraIntrinsics *intrinsics = (libmv::CameraIntrinsics *) libmvIntrinsics;
1033         intrinsics->Distort(src, dst, width, height, overscan, channels);
1034 }
1035
1036 void libmv_cameraIntrinsicsDistortFloat(const struct libmv_CameraIntrinsics *libmvIntrinsics,
1037                                         float *src, float *dst, int width, int height,
1038                                         float overscan, int channels)
1039 {
1040         libmv::CameraIntrinsics *intrinsics = (libmv::CameraIntrinsics *) libmvIntrinsics;
1041
1042         intrinsics->Distort(src, dst, width, height, overscan, channels);
1043 }
1044
1045 void libmv_cameraIntrinsicsApply(const libmv_CameraIntrinsicsOptions *libmv_camera_intrinsics_options,
1046                                  double x, double y, double *x1, double *y1)
1047 {
1048         libmv::CameraIntrinsics camera_intrinsics;
1049
1050         cameraIntrinsicsFromOptions(libmv_camera_intrinsics_options, &camera_intrinsics);
1051
1052         if (libmv_camera_intrinsics_options->focal_length) {
1053                 /* do a lens undistortion if focal length is non-zero only */
1054
1055                 camera_intrinsics.ApplyIntrinsics(x, y, x1, y1);
1056         }
1057 }
1058
1059 void libmv_cameraIntrinsicsInvert(const libmv_CameraIntrinsicsOptions *libmv_camera_intrinsics_options,
1060                                   double x, double y, double *x1, double *y1)
1061 {
1062         libmv::CameraIntrinsics camera_intrinsics;
1063
1064         cameraIntrinsicsFromOptions(libmv_camera_intrinsics_options, &camera_intrinsics);
1065
1066         if (libmv_camera_intrinsics_options->focal_length) {
1067                 /* do a lens distortion if focal length is non-zero only */
1068
1069                 camera_intrinsics.InvertIntrinsics(x, y, x1, y1);
1070         }
1071 }
1072
1073 void libmv_homography2DFromCorrespondencesEuc(double (*x1)[2], double (*x2)[2], int num_points, double H[3][3])
1074 {
1075         libmv::Mat x1_mat, x2_mat;
1076         libmv::Mat3 H_mat;
1077
1078         x1_mat.resize(2, num_points);
1079         x2_mat.resize(2, num_points);
1080
1081         for (int i = 0; i < num_points; i++) {
1082                 x1_mat.col(i) = libmv::Vec2(x1[i][0], x1[i][1]);
1083                 x2_mat.col(i) = libmv::Vec2(x2[i][0], x2[i][1]);
1084         }
1085
1086         LG << "x1: " << x1_mat;
1087         LG << "x2: " << x2_mat;
1088
1089         libmv::HomographyEstimationOptions options;
1090         libmv::Homography2DFromCorrespondencesEuc(x1_mat, x2_mat, options, &H_mat);
1091
1092         LG << "H: " << H_mat;
1093
1094         memcpy(H, H_mat.data(), 9 * sizeof(double));
1095 }
1096
1097 #endif