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