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