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