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