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 }