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