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