8894930f3b927249e67c006b80bd8aaf396fb0b5
[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
384         /* Convert from raw float buffers to libmv's FloatImage. */
385         libmv::FloatImage old_patch, new_patch;
386         floatBufToImage(image1, width, height, &old_patch);
387         floatBufToImage(image2, width, height, &new_patch);
388
389         libmv::TrackRegionResult track_region_result;
390         libmv::TrackRegion(old_patch, new_patch, xx1, yy1, track_region_options, xx2, yy2, &track_region_result);
391
392         /* Convert to floats for the blender api. */
393         for (int i = 0; i < 5; ++i) {
394                 x2[i] = xx2[i];
395                 y2[i] = yy2[i];
396         }
397
398         /* TODO(keir): Update the termination string with failure details. */
399         if (track_region_result.termination == libmv::TrackRegionResult::PARAMETER_TOLERANCE ||
400             track_region_result.termination == libmv::TrackRegionResult::FUNCTION_TOLERANCE  ||
401             track_region_result.termination == libmv::TrackRegionResult::GRADIENT_TOLERANCE  ||
402             track_region_result.termination == libmv::TrackRegionResult::NO_CONVERGENCE      ||
403             track_region_result.termination == libmv::TrackRegionResult::INSUFFICIENT_CORRELATION)
404         {
405                 tracking_result = true;
406         }
407
408 #if defined(DUMP_FAILURE) || defined(DUMP_ALWAYS)
409 #if defined(DUMP_ALWAYS)
410         {
411 #else
412         if (!tracking_result) {
413 #endif
414                 saveImage("old_patch", old_patch, x1[4], y1[4]);
415                 saveImage("new_patch", new_patch, x2[4], y2[4]);
416         }
417 #endif
418
419         return tracking_result;
420 }
421
422 /* ************ Tracks ************ */
423
424 libmv_Tracks *libmv_tracksNew(void)
425 {
426         libmv::Tracks *libmv_tracks = new libmv::Tracks();
427
428         return (libmv_Tracks *)libmv_tracks;
429 }
430
431 void libmv_tracksInsert(struct libmv_Tracks *libmv_tracks, int image, int track, double x, double y)
432 {
433         ((libmv::Tracks*)libmv_tracks)->Insert(image, track, x, y);
434 }
435
436 void libmv_tracksDestroy(libmv_Tracks *libmv_tracks)
437 {
438         delete (libmv::Tracks*)libmv_tracks;
439 }
440
441 /* ************ Reconstruction solver ************ */
442
443 class ReconstructUpdateCallback : public libmv::ProgressUpdateCallback {
444 public:
445         ReconstructUpdateCallback(reconstruct_progress_update_cb progress_update_callback,
446                         void *callback_customdata)
447         {
448                 progress_update_callback_ = progress_update_callback;
449                 callback_customdata_ = callback_customdata;
450         }
451
452         void invoke(double progress, const char *message)
453         {
454                 if(progress_update_callback_) {
455                         progress_update_callback_(callback_customdata_, progress, message);
456                 }
457         }
458 protected:
459         reconstruct_progress_update_cb progress_update_callback_;
460         void *callback_customdata_;
461 };
462
463 int libmv_refineParametersAreValid(int parameters) {
464         return (parameters == (LIBMV_REFINE_FOCAL_LENGTH))         ||
465                (parameters == (LIBMV_REFINE_FOCAL_LENGTH           |
466                                LIBMV_REFINE_PRINCIPAL_POINT))      ||
467                (parameters == (LIBMV_REFINE_FOCAL_LENGTH           |
468                                LIBMV_REFINE_PRINCIPAL_POINT        |
469                                LIBMV_REFINE_RADIAL_DISTORTION_K1   |
470                                LIBMV_REFINE_RADIAL_DISTORTION_K2)) ||
471                (parameters == (LIBMV_REFINE_FOCAL_LENGTH           |
472                                LIBMV_REFINE_RADIAL_DISTORTION_K1   |
473                                LIBMV_REFINE_RADIAL_DISTORTION_K2)) ||
474                (parameters == (LIBMV_REFINE_FOCAL_LENGTH           |
475                                LIBMV_REFINE_RADIAL_DISTORTION_K1));
476 }
477
478 void libmv_solveRefineIntrinsics(libmv::Tracks *tracks, libmv::CameraIntrinsics *intrinsics,
479                         libmv::EuclideanReconstruction *reconstruction, int refine_intrinsics,
480                         reconstruct_progress_update_cb progress_update_callback, void *callback_customdata)
481 {
482         /* only a few combinations are supported but trust the caller */
483         int libmv_refine_flags = 0;
484
485         if (refine_intrinsics & LIBMV_REFINE_FOCAL_LENGTH) {
486                 libmv_refine_flags |= libmv::BUNDLE_FOCAL_LENGTH;
487         }
488         if (refine_intrinsics & LIBMV_REFINE_PRINCIPAL_POINT) {
489                 libmv_refine_flags |= libmv::BUNDLE_PRINCIPAL_POINT;
490         }
491         if (refine_intrinsics & LIBMV_REFINE_RADIAL_DISTORTION_K1) {
492                 libmv_refine_flags |= libmv::BUNDLE_RADIAL_K1;
493         }
494         if (refine_intrinsics & LIBMV_REFINE_RADIAL_DISTORTION_K2) {
495                 libmv_refine_flags |= libmv::BUNDLE_RADIAL_K2;
496         }
497
498         progress_update_callback(callback_customdata, 1.0, "Refining solution");
499
500         libmv::EuclideanBundleCommonIntrinsics(*(libmv::Tracks *)tracks, libmv_refine_flags,
501                 reconstruction, intrinsics);
502 }
503
504 libmv_Reconstruction *libmv_solveReconstruction(libmv_Tracks *tracks, int keyframe1, int keyframe2,
505                         int refine_intrinsics, double focal_length, double principal_x, double principal_y, double k1, double k2, double k3,
506                         reconstruct_progress_update_cb progress_update_callback, void *callback_customdata)
507 {
508         /* Invert the camera intrinsics. */
509         libmv::vector<libmv::Marker> markers = ((libmv::Tracks*)tracks)->AllMarkers();
510         libmv_Reconstruction *libmv_reconstruction = new libmv_Reconstruction();
511         libmv::EuclideanReconstruction *reconstruction = &libmv_reconstruction->reconstruction;
512         libmv::CameraIntrinsics *intrinsics = &libmv_reconstruction->intrinsics;
513
514         ReconstructUpdateCallback update_callback =
515                 ReconstructUpdateCallback(progress_update_callback, callback_customdata);
516
517         intrinsics->SetFocalLength(focal_length, focal_length);
518         intrinsics->SetPrincipalPoint(principal_x, principal_y);
519         intrinsics->SetRadialDistortion(k1, k2, k3);
520
521         for (int i = 0; i < markers.size(); ++i) {
522                 intrinsics->InvertIntrinsics(markers[i].x,
523                         markers[i].y,
524                         &(markers[i].x),
525                         &(markers[i].y));
526         }
527
528         libmv::Tracks normalized_tracks(markers);
529
530         LG << "frames to init from: " << keyframe1 << " " << keyframe2;
531         libmv::vector<libmv::Marker> keyframe_markers =
532                 normalized_tracks.MarkersForTracksInBothImages(keyframe1, keyframe2);
533         LG << "number of markers for init: " << keyframe_markers.size();
534
535         update_callback.invoke(0, "Initial reconstruction");
536
537         libmv::EuclideanReconstructTwoFrames(keyframe_markers, reconstruction);
538         libmv::EuclideanBundle(normalized_tracks, reconstruction);
539         libmv::EuclideanCompleteReconstruction(normalized_tracks, reconstruction, &update_callback);
540
541         if (refine_intrinsics) {
542                 libmv_solveRefineIntrinsics((libmv::Tracks *)tracks, intrinsics, reconstruction,
543                         refine_intrinsics, progress_update_callback, callback_customdata);
544         }
545
546         progress_update_callback(callback_customdata, 1.0, "Finishing solution");
547         libmv_reconstruction->tracks = *(libmv::Tracks *)tracks;
548         libmv_reconstruction->error = libmv::EuclideanReprojectionError(*(libmv::Tracks *)tracks, *reconstruction, *intrinsics);
549
550         return (libmv_Reconstruction *)libmv_reconstruction;
551 }
552
553 struct libmv_Reconstruction *libmv_solveModal(struct libmv_Tracks *tracks, double focal_length,
554                         double principal_x, double principal_y, double k1, double k2, double k3,
555                         reconstruct_progress_update_cb progress_update_callback, void *callback_customdata)
556 {
557         /* Invert the camera intrinsics. */
558         libmv::vector<libmv::Marker> markers = ((libmv::Tracks*)tracks)->AllMarkers();
559         libmv_Reconstruction *libmv_reconstruction = new libmv_Reconstruction();
560         libmv::EuclideanReconstruction *reconstruction = &libmv_reconstruction->reconstruction;
561         libmv::CameraIntrinsics *intrinsics = &libmv_reconstruction->intrinsics;
562
563         ReconstructUpdateCallback update_callback =
564                 ReconstructUpdateCallback(progress_update_callback, callback_customdata);
565
566         intrinsics->SetFocalLength(focal_length, focal_length);
567         intrinsics->SetPrincipalPoint(principal_x, principal_y);
568         intrinsics->SetRadialDistortion(k1, k2, k3);
569
570         for (int i = 0; i < markers.size(); ++i) {
571                 intrinsics->InvertIntrinsics(markers[i].x,
572                         markers[i].y,
573                         &(markers[i].x),
574                         &(markers[i].y));
575         }
576
577         libmv::Tracks normalized_tracks(markers);
578
579         libmv::ModalSolver(normalized_tracks, reconstruction, &update_callback);
580
581         progress_update_callback(callback_customdata, 1.0, "Finishing solution");
582         libmv_reconstruction->tracks = *(libmv::Tracks *)tracks;
583         libmv_reconstruction->error = libmv::EuclideanReprojectionError(*(libmv::Tracks *)tracks, *reconstruction, *intrinsics);
584
585         return (libmv_Reconstruction *)libmv_reconstruction;
586 }
587
588 int libmv_reporojectionPointForTrack(libmv_Reconstruction *libmv_reconstruction, int track, double pos[3])
589 {
590         libmv::EuclideanReconstruction *reconstruction = &libmv_reconstruction->reconstruction;
591         libmv::EuclideanPoint *point = reconstruction->PointForTrack(track);
592
593         if(point) {
594                 pos[0] = point->X[0];
595                 pos[1] = point->X[2];
596                 pos[2] = point->X[1];
597
598                 return 1;
599         }
600
601         return 0;
602 }
603
604 static libmv::Marker ProjectMarker(const libmv::EuclideanPoint &point, const libmv::EuclideanCamera &camera,
605                         const libmv::CameraIntrinsics &intrinsics) {
606         libmv::Vec3 projected = camera.R * point.X + camera.t;
607         projected /= projected(2);
608
609         libmv::Marker reprojected_marker;
610         intrinsics.ApplyIntrinsics(projected(0), projected(1), &reprojected_marker.x, &reprojected_marker.y);
611
612         reprojected_marker.image = camera.image;
613         reprojected_marker.track = point.track;
614
615         return reprojected_marker;
616 }
617
618 double libmv_reporojectionErrorForTrack(libmv_Reconstruction *libmv_reconstruction, int track)
619 {
620         libmv::EuclideanReconstruction *reconstruction = &libmv_reconstruction->reconstruction;
621         libmv::CameraIntrinsics *intrinsics = &libmv_reconstruction->intrinsics;
622         libmv::vector<libmv::Marker> markers = libmv_reconstruction->tracks.MarkersForTrack(track);
623
624         int num_reprojected = 0;
625         double total_error = 0.0;
626
627         for (int i = 0; i < markers.size(); ++i) {
628                 const libmv::EuclideanCamera *camera = reconstruction->CameraForImage(markers[i].image);
629                 const libmv::EuclideanPoint *point = reconstruction->PointForTrack(markers[i].track);
630
631                 if (!camera || !point) {
632                         continue;
633                 }
634
635                 num_reprojected++;
636
637                 libmv::Marker reprojected_marker = ProjectMarker(*point, *camera, *intrinsics);
638                 double ex = reprojected_marker.x - markers[i].x;
639                 double ey = reprojected_marker.y - markers[i].y;
640
641                 total_error += sqrt(ex*ex + ey*ey);
642         }
643
644         return total_error / num_reprojected;
645 }
646
647 double libmv_reporojectionErrorForImage(libmv_Reconstruction *libmv_reconstruction, int image)
648 {
649         libmv::EuclideanReconstruction *reconstruction = &libmv_reconstruction->reconstruction;
650         libmv::CameraIntrinsics *intrinsics = &libmv_reconstruction->intrinsics;
651         libmv::vector<libmv::Marker> markers = libmv_reconstruction->tracks.MarkersInImage(image);
652         const libmv::EuclideanCamera *camera = reconstruction->CameraForImage(image);
653         int num_reprojected = 0;
654         double total_error = 0.0;
655
656         if (!camera)
657                 return 0;
658
659         for (int i = 0; i < markers.size(); ++i) {
660                 const libmv::EuclideanPoint *point = reconstruction->PointForTrack(markers[i].track);
661
662                 if (!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 int libmv_reporojectionCameraForImage(libmv_Reconstruction *libmv_reconstruction, int image, double mat[4][4])
679 {
680         libmv::EuclideanReconstruction *reconstruction = &libmv_reconstruction->reconstruction;
681         libmv::EuclideanCamera *camera = reconstruction->CameraForImage(image);
682
683         if(camera) {
684                 for (int j = 0; j < 3; ++j) {
685                         for (int k = 0; k < 3; ++k) {
686                                 int l = k;
687
688                                 if (k == 1) l = 2;
689                                 else if (k == 2) l = 1;
690
691                                 if (j == 2) mat[j][l] = -camera->R(j,k);
692                                 else mat[j][l] = camera->R(j,k);
693                         }
694                         mat[j][3]= 0.0;
695                 }
696
697                 libmv::Vec3 optical_center = -camera->R.transpose() * camera->t;
698
699                 mat[3][0] = optical_center(0);
700                 mat[3][1] = optical_center(2);
701                 mat[3][2] = optical_center(1);
702
703                 mat[3][3]= 1.0;
704
705                 return 1;
706         }
707
708         return 0;
709 }
710
711 double libmv_reprojectionError(libmv_Reconstruction *libmv_reconstruction)
712 {
713         return libmv_reconstruction->error;
714 }
715
716 void libmv_destroyReconstruction(libmv_Reconstruction *libmv_reconstruction)
717 {
718         delete libmv_reconstruction;
719 }
720
721 /* ************ feature detector ************ */
722
723 struct libmv_Features *libmv_detectFeaturesFAST(unsigned char *data, int width, int height, int stride,
724                         int margin, int min_trackness, int min_distance)
725 {
726         libmv::Feature *features = NULL;
727         std::vector<libmv::Feature> v;
728         libmv_Features *libmv_features = new libmv_Features();
729         int i= 0, count;
730
731         if(margin) {
732                 data += margin*stride+margin;
733                 width -= 2*margin;
734                 height -= 2*margin;
735         }
736
737         v = libmv::DetectFAST(data, width, height, stride, min_trackness, min_distance);
738
739         count = v.size();
740
741         if(count) {
742                 features= new libmv::Feature[count];
743
744                 for(std::vector<libmv::Feature>::iterator it = v.begin(); it != v.end(); it++) {
745                         features[i++]= *it;
746                 }
747         }
748
749         libmv_features->features = features;
750         libmv_features->count = count;
751         libmv_features->margin = margin;
752
753         return (libmv_Features *)libmv_features;
754 }
755
756 struct libmv_Features *libmv_detectFeaturesMORAVEC(unsigned char *data, int width, int height, int stride,
757                         int margin, int count, int min_distance)
758 {
759         libmv::Feature *features = NULL;
760         libmv_Features *libmv_features = new libmv_Features;
761
762         if(count) {
763                 if(margin) {
764                         data += margin*stride+margin;
765                         width -= 2*margin;
766                         height -= 2*margin;
767                 }
768
769                 features = new libmv::Feature[count];
770                 libmv::DetectMORAVEC(data, stride, width, height, features, &count, min_distance, NULL);
771         }
772
773         libmv_features->count = count;
774         libmv_features->margin = margin;
775         libmv_features->features = features;
776
777         return libmv_features;
778 }
779
780 int libmv_countFeatures(struct libmv_Features *libmv_features)
781 {
782         return libmv_features->count;
783 }
784
785 void libmv_getFeature(struct libmv_Features *libmv_features, int number, double *x, double *y, double *score, double *size)
786 {
787         libmv::Feature feature= libmv_features->features[number];
788
789         *x = feature.x + libmv_features->margin;
790         *y = feature.y + libmv_features->margin;
791         *score = feature.score;
792         *size = feature.size;
793 }
794
795 void libmv_destroyFeatures(struct libmv_Features *libmv_features)
796 {
797         if(libmv_features->features)
798                 delete [] libmv_features->features;
799
800         delete libmv_features;
801 }
802
803 /* ************ camera intrinsics ************ */
804
805 struct libmv_CameraIntrinsics *libmv_ReconstructionExtractIntrinsics(struct libmv_Reconstruction *libmv_Reconstruction) {
806         return (struct libmv_CameraIntrinsics *)&libmv_Reconstruction->intrinsics;
807 }
808
809 struct libmv_CameraIntrinsics *libmv_CameraIntrinsicsNew(double focal_length, double principal_x, double principal_y,
810                         double k1, double k2, double k3, int width, int height)
811 {
812         libmv::CameraIntrinsics *intrinsics= new libmv::CameraIntrinsics();
813
814         intrinsics->SetFocalLength(focal_length, focal_length);
815         intrinsics->SetPrincipalPoint(principal_x, principal_y);
816         intrinsics->SetRadialDistortion(k1, k2, k3);
817         intrinsics->SetImageSize(width, height);
818
819         return (struct libmv_CameraIntrinsics *) intrinsics;
820 }
821
822 struct libmv_CameraIntrinsics *libmv_CameraIntrinsicsCopy(struct libmv_CameraIntrinsics *libmvIntrinsics)
823 {
824         libmv::CameraIntrinsics *orig_intrinsics = (libmv::CameraIntrinsics *) libmvIntrinsics;
825         libmv::CameraIntrinsics *new_intrinsics= new libmv::CameraIntrinsics(*orig_intrinsics);
826
827         return (struct libmv_CameraIntrinsics *) new_intrinsics;
828 }
829
830 void libmv_CameraIntrinsicsDestroy(struct libmv_CameraIntrinsics *libmvIntrinsics)
831 {
832         libmv::CameraIntrinsics *intrinsics = (libmv::CameraIntrinsics *) libmvIntrinsics;
833
834         delete intrinsics;
835 }
836
837 void libmv_CameraIntrinsicsUpdate(struct libmv_CameraIntrinsics *libmvIntrinsics, double focal_length,
838                         double principal_x, double principal_y, double k1, double k2, double k3, int width, int height)
839 {
840         libmv::CameraIntrinsics *intrinsics = (libmv::CameraIntrinsics *) libmvIntrinsics;
841
842         if (intrinsics->focal_length() != focal_length)
843                 intrinsics->SetFocalLength(focal_length, focal_length);
844
845         if (intrinsics->principal_point_x() != principal_x || intrinsics->principal_point_y() != principal_y)
846                 intrinsics->SetFocalLength(focal_length, focal_length);
847
848         if (intrinsics->k1() != k1 || intrinsics->k2() != k2 || intrinsics->k3() != k3)
849                 intrinsics->SetRadialDistortion(k1, k2, k3);
850
851         if (intrinsics->image_width() != width || intrinsics->image_height() != height)
852                 intrinsics->SetImageSize(width, height);
853 }
854
855 void libmv_CameraIntrinsicsExtract(struct libmv_CameraIntrinsics *libmvIntrinsics, double *focal_length,
856                         double *principal_x, double *principal_y, double *k1, double *k2, double *k3, int *width, int *height) {
857         libmv::CameraIntrinsics *intrinsics= (libmv::CameraIntrinsics *) libmvIntrinsics;
858         *focal_length = intrinsics->focal_length();
859         *principal_x = intrinsics->principal_point_x();
860         *principal_y = intrinsics->principal_point_y();
861         *k1 = intrinsics->k1();
862         *k2 = intrinsics->k2();
863 }
864
865 void libmv_CameraIntrinsicsUndistortByte(struct libmv_CameraIntrinsics *libmvIntrinsics,
866                         unsigned char *src, unsigned char *dst, int width, int height, float overscan, int channels)
867 {
868         libmv::CameraIntrinsics *intrinsics = (libmv::CameraIntrinsics *) libmvIntrinsics;
869
870         intrinsics->Undistort(src, dst, width, height, overscan, channels);
871 }
872
873 void libmv_CameraIntrinsicsUndistortFloat(struct libmv_CameraIntrinsics *libmvIntrinsics,
874                         float *src, float *dst, int width, int height, float overscan, int channels)
875 {
876         libmv::CameraIntrinsics *intrinsics = (libmv::CameraIntrinsics *) libmvIntrinsics;
877
878         intrinsics->Undistort(src, dst, width, height, overscan, channels);
879 }
880
881 void libmv_CameraIntrinsicsDistortByte(struct libmv_CameraIntrinsics *libmvIntrinsics,
882                         unsigned char *src, unsigned char *dst, int width, int height, float overscan, int channels)
883 {
884         libmv::CameraIntrinsics *intrinsics = (libmv::CameraIntrinsics *) libmvIntrinsics;
885         intrinsics->Distort(src, dst, width, height, overscan, channels);
886 }
887
888 void libmv_CameraIntrinsicsDistortFloat(struct libmv_CameraIntrinsics *libmvIntrinsics,
889                         float *src, float *dst, int width, int height, float overscan, int channels)
890 {
891         libmv::CameraIntrinsics *intrinsics = (libmv::CameraIntrinsics *) libmvIntrinsics;
892
893         intrinsics->Distort(src, dst, width, height, overscan, channels);
894 }
895
896 /* ************ distortion ************ */
897
898 void libmv_undistortByte(double focal_length, double principal_x, double principal_y, double k1, double k2, double k3,
899                         unsigned char *src, unsigned char *dst, int width, int height, float overscan, int channels)
900 {
901         libmv::CameraIntrinsics intrinsics;
902
903         intrinsics.SetFocalLength(focal_length, focal_length);
904         intrinsics.SetPrincipalPoint(principal_x, principal_y);
905         intrinsics.SetRadialDistortion(k1, k2, k3);
906
907         intrinsics.Undistort(src, dst, width, height, overscan, channels);
908 }
909
910 void libmv_undistortFloat(double focal_length, double principal_x, double principal_y, double k1, double k2, double k3,
911                         float *src, float *dst, int width, int height, float overscan, int channels)
912 {
913         libmv::CameraIntrinsics intrinsics;
914
915         intrinsics.SetFocalLength(focal_length, focal_length);
916         intrinsics.SetPrincipalPoint(principal_x, principal_y);
917         intrinsics.SetRadialDistortion(k1, k2, k3);
918
919         intrinsics.Undistort(src, dst, width, height, overscan, channels);
920 }
921
922 void libmv_distortByte(double focal_length, double principal_x, double principal_y, double k1, double k2, double k3,
923                         unsigned char *src, unsigned char *dst, int width, int height, float overscan, int channels)
924 {
925         libmv::CameraIntrinsics intrinsics;
926
927         intrinsics.SetFocalLength(focal_length, focal_length);
928         intrinsics.SetPrincipalPoint(principal_x, principal_y);
929         intrinsics.SetRadialDistortion(k1, k2, k3);
930
931         intrinsics.Distort(src, dst, width, height, overscan, channels);
932 }
933
934 void libmv_distortFloat(double focal_length, double principal_x, double principal_y, double k1, double k2, double k3,
935                         float *src, float *dst, int width, int height, float overscan, int channels)
936 {
937         libmv::CameraIntrinsics intrinsics;
938
939         intrinsics.SetFocalLength(focal_length, focal_length);
940         intrinsics.SetPrincipalPoint(principal_x, principal_y);
941         intrinsics.SetRadialDistortion(k1, k2, k3);
942
943         intrinsics.Distort(src, dst, width, height, overscan, channels);
944 }
945
946 /* ************ utils ************ */
947
948 void libmv_applyCameraIntrinsics(double focal_length, double principal_x, double principal_y, double k1, double k2, double k3,
949                         double x, double y, double *x1, double *y1)
950 {
951         libmv::CameraIntrinsics intrinsics;
952
953         intrinsics.SetFocalLength(focal_length, focal_length);
954         intrinsics.SetPrincipalPoint(principal_x, principal_y);
955         intrinsics.SetRadialDistortion(k1, k2, k3);
956
957         if(focal_length) {
958                 /* do a lens undistortion if focal length is non-zero only */
959
960                 intrinsics.ApplyIntrinsics(x, y, x1, y1);
961         }
962 }
963
964 void libmv_InvertIntrinsics(double focal_length, double principal_x, double principal_y, double k1, double k2, double k3,
965                         double x, double y, double *x1, double *y1)
966 {
967         libmv::CameraIntrinsics intrinsics;
968
969         intrinsics.SetFocalLength(focal_length, focal_length);
970         intrinsics.SetPrincipalPoint(principal_x, principal_y);
971         intrinsics.SetRadialDistortion(k1, k2, k3);
972
973         if(focal_length) {
974                 /* do a lens distortion if focal length is non-zero only */
975
976                 intrinsics.InvertIntrinsics(x, y, x1, y1);
977         }
978 }
979
980 /* ************ point clouds ************ */
981
982 void libmvTransformToMat4(libmv::Mat3 &R, libmv::Vec3 &S, libmv::Vec3 &t, double M[4][4])
983 {
984         for (int j = 0; j < 3; ++j)
985                 for (int k = 0; k < 3; ++k)
986                         M[j][k] = R(k, j) * S(j);
987
988         for (int i = 0; i < 3; ++i) {
989                 M[3][0] = t(0);
990                 M[3][1] = t(1);
991                 M[3][2] = t(2);
992
993                 M[0][3] = M[1][3] = M[2][3] = 0;
994         }
995
996         M[3][3] = 1.0;
997 }
998
999 void libmv_rigidRegistration(float (*reference_points)[3], float (*points)[3], int total_points,
1000                              int use_scale, int use_translation, double M[4][4])
1001 {
1002         libmv::Mat3 R;
1003         libmv::Vec3 S;
1004         libmv::Vec3 t;
1005         libmv::vector<libmv::Vec3> reference_points_vector, points_vector;
1006
1007         for (int i = 0; i < total_points; i++) {
1008                 reference_points_vector.push_back(libmv::Vec3(reference_points[i][0],
1009                                                               reference_points[i][1],
1010                                                               reference_points[i][2]));
1011
1012                 points_vector.push_back(libmv::Vec3(points[i][0],
1013                                                     points[i][1],
1014                                                     points[i][2]));
1015         }
1016
1017         if (use_scale && use_translation) {
1018                 libmv::RigidRegistration(reference_points_vector, points_vector, R, S, t);
1019         }
1020         else if (use_translation) {
1021                 S = libmv::Vec3(1.0, 1.0, 1.0);
1022                 libmv::RigidRegistration(reference_points_vector, points_vector, R, t);
1023         }
1024         else {
1025                 S = libmv::Vec3(1.0, 1.0, 1.0);
1026                 t = libmv::Vec3::Zero();
1027                 libmv::RigidRegistration(reference_points_vector, points_vector, R);
1028         }
1029
1030         libmvTransformToMat4(R, S, t, M);
1031 }