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