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