Camera tracking integration
[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 typedef struct libmv_Reconstruction {
69         libmv::Reconstruction reconstruction;
70         double error;
71 } libmv_Reconstruction;
72
73 /* ************ Logging ************ */
74
75 void libmv_initLogging(const char *argv0)
76 {
77         google::InitGoogleLogging(argv0);
78         google::SetCommandLineOption("logtostderr", "1");
79         google::SetCommandLineOption("v", "0");
80         google::SetCommandLineOption("stderrthreshold", "7");
81         google::SetCommandLineOption("minloglevel", "7");
82         V3D::optimizerVerbosenessLevel = 0;
83 }
84
85 void libmv_startDebugLogging(void)
86 {
87         google::SetCommandLineOption("logtostderr", "1");
88         google::SetCommandLineOption("v", "0");
89         google::SetCommandLineOption("stderrthreshold", "1");
90         google::SetCommandLineOption("minloglevel", "0");
91         V3D::optimizerVerbosenessLevel = 1;
92 }
93
94 void libmv_setLoggingVerbosity(int verbosity)
95 {
96         char val[10];
97         snprintf(val, sizeof(val), "%d", verbosity);
98
99         google::SetCommandLineOption("v", val);
100         V3D::optimizerVerbosenessLevel = verbosity;
101 }
102
103 /* ************ RegionTracker ************ */
104
105 libmv_RegionTracker *libmv_regionTrackerNew(int max_iterations, int pyramid_level, double tolerance)
106 {
107         libmv::RegionTracker *region_tracker;
108         libmv::TrkltRegionTracker *trklt_region_tracker = new libmv::TrkltRegionTracker;
109
110         trklt_region_tracker->half_window_size = DEFAULT_WINDOW_HALFSIZE;
111         trklt_region_tracker->max_iterations = max_iterations;
112
113         libmv::PyramidRegionTracker *pyramid_region_tracker =
114                 new libmv::PyramidRegionTracker(trklt_region_tracker, pyramid_level);
115
116         region_tracker = new libmv::RetrackRegionTracker(pyramid_region_tracker, tolerance);
117
118         libmv_RegionTracker *configured_region_tracker = new libmv_RegionTracker;
119         configured_region_tracker->trklt_region_tracker = trklt_region_tracker;
120         configured_region_tracker->pyramid_region_tracker = pyramid_region_tracker;
121         configured_region_tracker->region_tracker = region_tracker;
122
123         return configured_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(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), "%02d.png", ++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(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), "%02d.png", ++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 *tracker, const float *ima1, const float *ima2,
265                          int width, int height, int half_window_size,
266                          double x1, double y1, double *x2, double *y2)
267 {
268         libmv::RegionTracker *region_tracker;
269         libmv::TrkltRegionTracker *trklt_region_tracker;
270         libmv::FloatImage old_patch, new_patch;
271
272         trklt_region_tracker = tracker->trklt_region_tracker;
273         region_tracker = tracker->region_tracker;
274
275         trklt_region_tracker->half_window_size = half_window_size;
276
277         floatBufToImage(ima1, width, height, &old_patch);
278         floatBufToImage(ima2, width, height, &new_patch);
279
280 #ifndef DUMP_FAILURE
281         return region_tracker->Track(old_patch, new_patch, x1, y1, x2, y2);
282 #else
283         {
284                 double sx2 = *x2, sy2 = *y2;
285                 int result = region_tracker->Track(old_patch, new_patch, x1, y1, x2, y2);
286
287                 if (!result) {
288                         saveImage(old_patch, x1, y1);
289                         saveImage(new_patch, sx2, sy2);
290                 }
291
292                 return result;
293         }
294 #endif
295 }
296
297 void libmv_regionTrackerDestroy(libmv_RegionTracker *tracker)
298 {
299         delete tracker->region_tracker;
300         delete tracker;
301 }
302
303 /* ************ Tracks ************ */
304
305 libmv_Tracks *libmv_tracksNew(void)
306 {
307         libmv::Tracks *tracks = new libmv::Tracks();
308
309         return (libmv_Tracks *)tracks;
310 }
311
312 void libmv_tracksInsert(struct libmv_Tracks *tracks, int image, int track, double x, double y)
313 {
314         ((libmv::Tracks*)tracks)->Insert(image, track, x, y);
315 }
316
317 void libmv_tracksDestroy(libmv_Tracks *tracks)
318 {
319         delete (libmv::Tracks*)tracks;
320 }
321
322 /* ************ Reconstruction solver ************ */
323
324 libmv_Reconstruction *libmv_solveReconstruction(libmv_Tracks *tracks, int keyframe1, int keyframe2,
325                         double focal_length, double principal_x, double principal_y, double k1, double k2, double k3)
326 {
327         /* Invert the camera intrinsics. */
328         libmv::vector<libmv::Marker> markers = ((libmv::Tracks*)tracks)->AllMarkers();
329         libmv::CameraIntrinsics intrinsics;
330         libmv_Reconstruction *libmv_reconstruction = new libmv_Reconstruction();
331         libmv::Reconstruction *reconstruction = &libmv_reconstruction->reconstruction;
332
333         intrinsics.SetFocalLength(focal_length, focal_length);
334         intrinsics.SetPrincipalPoint(principal_x, principal_y);
335         intrinsics.SetRadialDistortion(k1, k2, k3);
336
337         if(focal_length) {
338                 /* do a lens undistortion if focal length is non-zero only */
339                 for (int i = 0; i < markers.size(); ++i) {
340                         intrinsics.InvertIntrinsics(markers[i].x,
341                                 markers[i].y,
342                                 &(markers[i].x),
343                                 &(markers[i].y));
344                 }
345         }
346
347         libmv::Tracks normalized_tracks(markers);
348
349         libmv::vector<libmv::Marker> keyframe_markers =
350                 normalized_tracks.MarkersForTracksInBothImages(keyframe1, keyframe2);
351
352         libmv::ReconstructTwoFrames(keyframe_markers, reconstruction);
353         libmv::Bundle(normalized_tracks, reconstruction);
354         libmv::CompleteReconstruction(normalized_tracks, reconstruction);
355         libmv_reconstruction->error = libmv::ReprojectionError(*(libmv::Tracks *)tracks, *reconstruction, intrinsics);
356
357         return (libmv_Reconstruction *)libmv_reconstruction;
358 }
359
360 int libmv_reporojectionPointForTrack(libmv_Reconstruction *libmv_reconstruction, int track, double pos[3])
361 {
362         libmv::Reconstruction *reconstruction = &libmv_reconstruction->reconstruction;
363         libmv::Point *point = reconstruction->PointForTrack(track);
364
365         if(point) {
366                 pos[0] = point->X[0];
367                 pos[1] = point->X[2];
368                 pos[2] = point->X[1];
369
370                 return 1;
371         }
372
373         return 0;
374 }
375
376 int libmv_reporojectionCameraForImage(libmv_Reconstruction *libmv_reconstruction, int image, double mat[4][4])
377 {
378         libmv::Reconstruction *reconstruction = &libmv_reconstruction->reconstruction;
379         libmv::Camera *camera = reconstruction->CameraForImage(image);
380
381         if(camera) {
382                 for (int j = 0; j < 3; ++j) {
383                         for (int k = 0; k < 3; ++k) {
384                                 int l = k;
385
386                                 if (k == 1) l = 2;
387                                 else if (k == 2) l = 1;
388
389                                 if (j == 2) mat[j][l] = -camera->R(j,k);
390                                 else mat[j][l] = camera->R(j,k);
391                         }
392                         mat[j][3]= 0.0;
393                 }
394
395                 libmv::Vec3 optical_center = -camera->R.transpose() * camera->t;
396
397                 mat[3][0] = optical_center(0);
398                 mat[3][1] = optical_center(2);
399                 mat[3][2] = optical_center(1);
400
401                 mat[3][3]= 1.0;
402
403                 return 1;
404         }
405
406         return 0;
407 }
408
409 float libmv_reprojectionError(libmv_Reconstruction *libmv_reconstruction)
410 {
411         return libmv_reconstruction->error;
412 }
413
414 void libmv_destroyReconstruction(libmv_Reconstruction *libmv_reconstruction)
415 {
416         delete libmv_reconstruction;
417 }
418
419 /* ************ feature detector ************ */
420
421 struct libmv_Corners *libmv_detectCorners(unsigned char *data, int width, int height, int stride)
422 {
423         std::vector<libmv::Corner> detect= libmv::Detect(data, width, height, stride);
424         std::vector<libmv::Corner> *corners= new std::vector<libmv::Corner>();
425
426         corners->insert(corners->begin(), detect.begin(), detect.end());
427
428         return (libmv_Corners *)corners;
429 }
430
431 int libmv_countCorners(struct libmv_Corners *corners)
432 {
433         return ((std::vector<libmv::Corner> *)corners)->size();
434 }
435
436 void libmv_getCorner(struct libmv_Corners *corners, int number, float *x, float *y, float *score, float *size)
437 {
438         libmv::Corner corner = ((std::vector<libmv::Corner> *)corners)->at(number);
439
440         *x = corner.x;
441         *y = corner.y;
442         *score = corner.score;
443         *size = corner.size;
444 }
445
446 void libmv_destroyCorners(struct libmv_Corners *corners)
447 {
448         delete (std::vector<libmv::Corner> *)corners;
449 }
450
451 /* ************ utils ************ */
452
453 void libmv_applyCameraIntrinsics(double focal_length, double principal_x, double principal_y, double k1, double k2, double k3,
454                         double x, double y, double *x1, double *y1)
455 {
456         libmv::CameraIntrinsics intrinsics;
457
458         intrinsics.SetFocalLength(focal_length, focal_length);
459         intrinsics.SetPrincipalPoint(principal_x, principal_y);
460         intrinsics.SetRadialDistortion(k1, k2, k3);
461
462         if(focal_length) {
463                 /* do a lens undistortion if focal length is non-zero only */
464
465                 intrinsics.ApplyIntrinsics(x, y, x1, y1);
466         }
467 }
468
469 void libmv_InvertIntrinsics(double focal_length, double principal_x, double principal_y, double k1, double k2, double k3,
470                         double x, double y, double *x1, double *y1)
471 {
472         libmv::CameraIntrinsics intrinsics;
473
474         intrinsics.SetFocalLength(focal_length, focal_length);
475         intrinsics.SetPrincipalPoint(principal_x, principal_y);
476         intrinsics.SetRadialDistortion(k1, k2, k3);
477
478         if(focal_length) {
479                 /* do a lens distortion if focal length is non-zero only */
480
481                 intrinsics.InvertIntrinsics(x, y, x1, y1);
482         }
483 }