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