5d37c43f095f6f0d5a484eb4c6c3f798e6b1e2bf
[blender.git] / extern / libmv / libmv-capi.cpp
1 /*
2  * $Id$
3  *
4  * ***** BEGIN GPL LICENSE BLOCK *****
5  *
6  * This program is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * as published by the Free Software Foundation; either version 2
9  * of the License, or (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software Foundation,
18  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19  *
20  * The Original Code is Copyright (C) 2011 Blender Foundation.
21  * All rights reserved.
22  *
23  * Contributor(s): Blender Foundation,
24  *                 Sergey Sharybin
25  *
26  * ***** END GPL LICENSE BLOCK *****
27  */
28
29 /* define this to generate PNG images with content of search areas
30   tracking between which failed */
31 #undef DUMP_FAILURE
32
33 #include "libmv-capi.h"
34
35 #include "glog/logging.h"
36 #include "Math/v3d_optimization.h"
37
38 #include "libmv/tracking/klt_region_tracker.h"
39 #include "libmv/tracking/trklt_region_tracker.h"
40 #include "libmv/tracking/pyramid_region_tracker.h"
41 #include "libmv/tracking/retrack_region_tracker.h"
42
43 #include "libmv/simple_pipeline/tracks.h"
44 #include "libmv/simple_pipeline/initialize_reconstruction.h"
45 #include "libmv/simple_pipeline/bundle.h"
46 #include "libmv/simple_pipeline/detect.h"
47 #include "libmv/simple_pipeline/pipeline.h"
48 #include "libmv/simple_pipeline/camera_intrinsics.h"
49
50 #include <stdlib.h>
51
52 #ifdef DUMP_FAILURE
53 #  include <png.h>
54 #endif
55
56 #ifdef _MSC_VER
57 #  define snprintf _snprintf
58 #endif
59
60 #define DEFAULT_WINDOW_HALFSIZE 5
61
62 typedef struct libmv_RegionTracker {
63         libmv::TrkltRegionTracker *trklt_region_tracker;
64         libmv::PyramidRegionTracker *pyramid_region_tracker;
65         libmv::RegionTracker *region_tracker;
66 } libmv_RegionTracker;
67
68 /* ************ Logging ************ */
69
70 void libmv_initLogging(const char *argv0)
71 {
72         google::InitGoogleLogging(argv0);
73         google::SetCommandLineOption("logtostderr", "1");
74         google::SetCommandLineOption("v", "0");
75         google::SetCommandLineOption("stderrthreshold", "7");
76         google::SetCommandLineOption("minloglevel", "7");
77         V3D::optimizerVerbosenessLevel = 0;
78 }
79
80 void libmv_startDebugLogging(void)
81 {
82         google::SetCommandLineOption("logtostderr", "1");
83         google::SetCommandLineOption("v", "0");
84         google::SetCommandLineOption("stderrthreshold", "1");
85         google::SetCommandLineOption("minloglevel", "0");
86         V3D::optimizerVerbosenessLevel = 1;
87 }
88
89 void libmv_setLoggingVerbosity(int verbosity)
90 {
91         char val[10];
92         snprintf(val, sizeof(val), "%d", verbosity);
93
94         google::SetCommandLineOption("v", val);
95         V3D::optimizerVerbosenessLevel = verbosity;
96 }
97
98 /* ************ RegionTracker ************ */
99
100 libmv_RegionTracker *libmv_regionTrackerNew(int max_iterations, int pyramid_level, double tolerance)
101 {
102         libmv::RegionTracker *region_tracker;
103         libmv::TrkltRegionTracker *trklt_region_tracker = new libmv::TrkltRegionTracker;
104
105         trklt_region_tracker->half_window_size = DEFAULT_WINDOW_HALFSIZE;
106         trklt_region_tracker->max_iterations = max_iterations;
107
108         libmv::PyramidRegionTracker *pyramid_region_tracker =
109                 new libmv::PyramidRegionTracker(trklt_region_tracker, pyramid_level);
110
111         region_tracker = new libmv::RetrackRegionTracker(pyramid_region_tracker, tolerance);
112
113         libmv_RegionTracker *configured_region_tracker = new libmv_RegionTracker;
114         configured_region_tracker->trklt_region_tracker = trklt_region_tracker;
115         configured_region_tracker->pyramid_region_tracker = pyramid_region_tracker;
116         configured_region_tracker->region_tracker = region_tracker;
117
118         return configured_region_tracker;
119 }
120
121 static void floatBufToImage(const float *buf, int width, int height, libmv::FloatImage *image)
122 {
123         int x, y, a = 0;
124
125         image->resize(height, width);
126
127         for (y = 0; y < height; y++) {
128                 for (x = 0; x < width; x++) {
129                         (*image)(y, x, 0) = buf[a++];
130                 }
131         }
132 }
133
134 #ifdef DUMP_FAILURE
135 void savePNGImage(png_bytep *row_pointers, int width, int height, int depth, int color_type, char *file_name)
136 {
137         png_infop info_ptr;
138         png_structp png_ptr;
139         FILE *fp = fopen(file_name, "wb");
140
141         if (!fp)
142                 return;
143
144         /* Initialize stuff */
145         png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
146         info_ptr = png_create_info_struct(png_ptr);
147
148         if (setjmp(png_jmpbuf(png_ptr))) {
149                 fclose(fp);
150                 return;
151         }
152
153         png_init_io(png_ptr, fp);
154
155         /* write header */
156         if (setjmp(png_jmpbuf(png_ptr))) {
157                 fclose(fp);
158                 return;
159         }
160
161         png_set_IHDR(png_ptr, info_ptr, width, height,
162                 depth, color_type, PNG_INTERLACE_NONE,
163                 PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);
164
165         png_write_info(png_ptr, info_ptr);
166
167         /* write bytes */
168         if (setjmp(png_jmpbuf(png_ptr))) {
169                 fclose(fp);
170                 return;
171         }
172
173         png_write_image(png_ptr, row_pointers);
174
175         /* end write */
176         if (setjmp(png_jmpbuf(png_ptr))) {
177                 fclose(fp);
178                 return;
179         }
180
181         png_write_end(png_ptr, NULL);
182
183         fclose(fp);
184 }
185
186 static void saveImage(libmv::FloatImage image, int x0, int y0)
187 {
188         int x, y;
189         png_bytep *row_pointers;
190
191         row_pointers= (png_bytep*)malloc(sizeof(png_bytep)*image.Height());
192
193         for (y = 0; y < image.Height(); y++) {
194                 row_pointers[y]= (png_bytep)malloc(sizeof(png_byte)*4*image.Width());
195
196                 for (x = 0; x < image.Width(); x++) {
197                         if (x0 == x && y0 == y) {
198                                 row_pointers[y][x*4+0]= 255;
199                                 row_pointers[y][x*4+1]= 0;
200                                 row_pointers[y][x*4+2]= 0;
201                                 row_pointers[y][x*4+3]= 255;
202                         }
203                         else {
204                                 float pixel = image(y, x, 0);
205                                 row_pointers[y][x*4+0]= pixel*255;
206                                 row_pointers[y][x*4+1]= pixel*255;
207                                 row_pointers[y][x*4+2]= pixel*255;
208                                 row_pointers[y][x*4+3]= 255;
209                         }
210                 }
211         }
212
213         {
214                 static int a= 0;
215                 char buf[128];
216                 snprintf(buf, sizeof(buf), "%02d.png", ++a);
217                 savePNGImage(row_pointers, image.Width(), image.Height(), 8, PNG_COLOR_TYPE_RGBA, buf);
218         }
219
220         for (y = 0; y < image.Height(); y++) {
221                 free(row_pointers[y]);
222         }
223         free(row_pointers);
224 }
225
226 static void saveBytesImage(unsigned char *data, int width, int height)
227 {
228         int x, y;
229         png_bytep *row_pointers;
230
231         row_pointers= (png_bytep*)malloc(sizeof(png_bytep)*height);
232
233         for (y = 0; y < height; y++) {
234                 row_pointers[y]= (png_bytep)malloc(sizeof(png_byte)*4*width);
235
236                 for (x = 0; x < width; x++) {
237                         char pixel = data[width*y+x];
238                         row_pointers[y][x*4+0]= pixel;
239                         row_pointers[y][x*4+1]= pixel;
240                         row_pointers[y][x*4+2]= pixel;
241                         row_pointers[y][x*4+3]= 255;
242                 }
243         }
244
245         {
246                 static int a= 0;
247                 char buf[128];
248                 snprintf(buf, sizeof(buf), "%02d.png", ++a);
249                 savePNGImage(row_pointers, width, height, 8, PNG_COLOR_TYPE_RGBA, buf);
250         }
251
252         for (y = 0; y < height; y++) {
253                 free(row_pointers[y]);
254         }
255         free(row_pointers);
256 }
257 #endif
258
259 int libmv_regionTrackerTrack(libmv_RegionTracker *tracker, const float *ima1, const float *ima2,
260                          int width, int height, int half_window_size,
261                          double x1, double y1, double *x2, double *y2)
262 {
263         libmv::RegionTracker *region_tracker;
264         libmv::TrkltRegionTracker *trklt_region_tracker;
265         libmv::FloatImage old_patch, new_patch;
266
267         trklt_region_tracker = tracker->trklt_region_tracker;
268         region_tracker = tracker->region_tracker;
269
270         trklt_region_tracker->half_window_size = half_window_size;
271
272         floatBufToImage(ima1, width, height, &old_patch);
273         floatBufToImage(ima2, width, height, &new_patch);
274
275 #ifndef DUMP_FAILURE
276         return region_tracker->Track(old_patch, new_patch, x1, y1, x2, y2);
277 #else
278         {
279                 double sx2 = *x2, sy2 = *y2;
280                 int result = region_tracker->Track(old_patch, new_patch, x1, y1, x2, y2);
281
282                 if (!result) {
283                         saveImage(old_patch, x1, y1);
284                         saveImage(new_patch, sx2, sy2);
285                 }
286
287                 return result;
288         }
289 #endif
290 }
291
292 void libmv_regionTrackerDestroy(libmv_RegionTracker *tracker)
293 {
294         delete tracker->region_tracker;
295         delete tracker;
296 }
297
298 /* ************ Tracks ************ */
299
300 libmv_Tracks *libmv_tracksNew(void)
301 {
302         libmv::Tracks *tracks = new libmv::Tracks();
303
304         return (libmv_Tracks *)tracks;
305 }
306
307 void libmv_tracksInsert(struct libmv_Tracks *tracks, int image, int track, double x, double y)
308 {
309         ((libmv::Tracks*)tracks)->Insert(image, track, x, y);
310 }
311
312 void libmv_tracksDestroy(libmv_Tracks *tracks)
313 {
314         delete (libmv::Tracks*)tracks;
315 }
316
317 /* ************ Reconstruction solver ************ */
318
319 struct libmv_Reconstruction *libmv_solveReconstruction(libmv_Tracks *tracks, int keyframe1, int keyframe2,
320                         double focal_length, double principal_x, double principal_y, double k1, double k2, double k3)
321 {
322         /* Invert the camera intrinsics. */
323         libmv::vector<libmv::Marker> markers = ((libmv::Tracks*)tracks)->AllMarkers();
324         libmv::CameraIntrinsics intrinsics;
325         libmv::Reconstruction *reconstruction = new libmv::Reconstruction();
326
327         intrinsics.SetFocalLength(focal_length, focal_length);
328         intrinsics.SetPrincipalPoint(principal_x, principal_y);
329         intrinsics.SetRadialDistortion(k1, k2, k3);
330
331         if(focal_length) {
332                 /* do a lens undistortion if focal length is non-zero only */
333                 for (int i = 0; i < markers.size(); ++i) {
334                         intrinsics.InvertIntrinsics(markers[i].x,
335                                 markers[i].y,
336                                 &(markers[i].x),
337                                 &(markers[i].y));
338                 }
339         }
340
341         libmv::Tracks normalized_tracks(markers);
342
343         libmv::vector<libmv::Marker> keyframe_markers =
344                 normalized_tracks.MarkersForTracksInBothImages(keyframe1, keyframe2);
345
346         libmv::ReconstructTwoFrames(keyframe_markers, reconstruction);
347         libmv::Bundle(normalized_tracks, reconstruction);
348         libmv::CompleteReconstruction(normalized_tracks, reconstruction);
349         libmv::ReprojectionError(*(libmv::Tracks *)tracks, *reconstruction, intrinsics);
350
351         return (libmv_Reconstruction *)reconstruction;
352 }
353
354 int libmv_reporojectionPointForTrack(libmv_Reconstruction *reconstruction, int track, double pos[3])
355 {
356         libmv::Point *point = ((libmv::Reconstruction *)reconstruction)->PointForTrack(track);
357
358         if(point) {
359                 pos[0] = point->X[0];
360                 pos[1] = point->X[2];
361                 pos[2] = point->X[1];
362
363                 return 1;
364         }
365
366         return 0;
367 }
368
369 int libmv_reporojectionCameraForImage(libmv_Reconstruction *reconstruction, int image, double mat[4][4])
370 {
371         libmv::Camera *camera = ((libmv::Reconstruction *)reconstruction)->CameraForImage(image);
372
373         if(camera) {
374                 for (int j = 0; j < 3; ++j) {
375                         for (int k = 0; k < 3; ++k) {
376                                 int l = k;
377
378                                 if (k == 1) l = 2;
379                                 else if (k == 2) l = 1;
380
381                                 if (j == 2) mat[j][l] = -camera->R(j,k);
382                                 else mat[j][l] = camera->R(j,k);
383                         }
384                         mat[j][3]= 0.0;
385                 }
386
387                 libmv::Vec3 optical_center = -camera->R.transpose() * camera->t;
388
389                 mat[3][0] = optical_center(0);
390                 mat[3][1] = optical_center(2);
391                 mat[3][2] = optical_center(1);
392
393                 mat[3][3]= 1.0;
394
395                 return 1;
396         }
397
398         return 0;
399 }
400
401 void libmv_destroyReconstruction(libmv_Reconstruction *reconstruction)
402 {
403         delete (libmv::Reconstruction *)reconstruction;
404 }
405
406 /* ************ feature detector ************ */
407
408 struct libmv_Corners *libmv_detectCorners(unsigned char *data, int width, int height, int stride)
409 {
410         std::vector<libmv::Corner> detect= libmv::Detect(data, width, height, stride);
411         std::vector<libmv::Corner> *corners= new std::vector<libmv::Corner>();
412
413         corners->insert(corners->begin(), detect.begin(), detect.end());
414
415         return (libmv_Corners *)corners;
416 }
417
418 int libmv_countCorners(struct libmv_Corners *corners)
419 {
420         return ((std::vector<libmv::Corner> *)corners)->size();
421 }
422
423 void libmv_getCorner(struct libmv_Corners *corners, int number, float *x, float *y, float *score, float *size)
424 {
425         libmv::Corner corner = ((std::vector<libmv::Corner> *)corners)->at(number);
426
427         *x = corner.x;
428         *y = corner.y;
429         *score = corner.score;
430         *size = corner.size;
431 }
432
433 void libmv_destroyCorners(struct libmv_Corners *corners)
434 {
435         delete (std::vector<libmv::Corner> *)corners;
436 }
437
438 /* ************ utils ************ */
439
440 void libmv_applyCameraIntrinsics(double focal_length, double principal_x, double principal_y, double k1, double k2, double k3,
441                         double x, double y, double *x1, double *y1)
442 {
443         libmv::CameraIntrinsics intrinsics;
444
445         intrinsics.SetFocalLength(focal_length, focal_length);
446         intrinsics.SetPrincipalPoint(principal_x, principal_y);
447         intrinsics.SetRadialDistortion(k1, k2, k3);
448
449         if(focal_length) {
450                 /* do a lens undistortion if focal length is non-zero only */
451
452                 intrinsics.ApplyIntrinsics(x, y, x1, y1);
453         }
454 }
455
456 void libmv_InvertIntrinsics(double focal_length, double principal_x, double principal_y, double k1, double k2, double k3,
457                         double x, double y, double *x1, double *y1)
458 {
459         libmv::CameraIntrinsics intrinsics;
460
461         intrinsics.SetFocalLength(focal_length, focal_length);
462         intrinsics.SetPrincipalPoint(principal_x, principal_y);
463         intrinsics.SetRadialDistortion(k1, k2, k3);
464
465         if(focal_length) {
466                 /* do a lens distortion if focal length is non-zero only */
467
468                 intrinsics.InvertIntrinsics(x, y, x1, y1);
469         }
470 }