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