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