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