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