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