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