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