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