1 /*
2 * GStreamer
3 * Copyright (C) <2017> Philippe Renon <philippe_renon@yahoo.fr>
4 *
5 * Permission is hereby granted, free of charge, to any person obtaining a
6 * copy of this software and associated documentation files (the "Software"),
7 * to deal in the Software without restriction, including without limitation
8 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
9 * and/or sell copies of the Software, and to permit persons to whom the
10 * Software is furnished to do so, subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be included in
13 * all copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
20 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
21 * DEALINGS IN THE SOFTWARE.
22 *
23 * Alternatively, the contents of this file may be used under the
24 * GNU Lesser General Public License Version 2.1 (the "LGPL"), in
25 * which case the following provisions apply instead of the ones
26 * mentioned above:
27 *
28 * This library is free software; you can redistribute it and/or
29 * modify it under the terms of the GNU Library General Public
30 * License as published by the Free Software Foundation; either
31 * version 2 of the License, or (at your option) any later version.
32 *
33 * This library is distributed in the hope that it will be useful,
34 * but WITHOUT ANY WARRANTY; without even the implied warranty of
35 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
36 * Library General Public License for more details.
37 *
38 * You should have received a copy of the GNU Library General Public
39 * License along with this library; if not, write to the
40 * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
41 * Boston, MA 02110-1301, USA.
42 */
43
44 /**
45 * SECTION:element-cameraundistort
46 *
47 * This element performs camera calibration.
48 *
49 * Once the calibration procedure is done:
50 * - An event, containing the camera correction parameters, is sent upstream
51 * and downstream to be consumed by cameraundistort elements.
52 * - The _settings_ property is set to the camera correction parameters (as
53 * an opaque string of serialized OpenCV objects).
54 * The value of this property can later be used to configure a
55 * cameraundistort element.
56 * - The element becomes idle and can later be restarted [TODO].
57 *
58 * Based on this tutorial: https://docs.opencv.org/2.4/doc/tutorials/calib3d/camera_calibration/camera_calibration.html
59 *
60 * ## Example pipelines
61 *
62 * |[
63 * gst-launch-1.0 -v v4l2src ! videoconvert ! cameraundistort ! cameracalibrate | autovideosink
64 * ]| will correct camera distortion once camera calibration is done.
65 */
66
67 /*
68 * TODO
69 * - signal when calibration is done
70 * - action signal to start calibration
71 * - do pattern detection asynchronously
72 * - do final calibration computation asynchronously
73 * - use cairo for drawing overlay
74 * - use overlay
75 * - implement settings query
76 * - validate user settings (see validate() in tutorial)
77 * - save complete state (see saveCameraParams() in tutorial)
78 */
79
80 #ifdef HAVE_CONFIG_H
81 # include <config.h>
82 #endif
83
84 #include "gstcameracalibrate.h"
85
86 #include <opencv2/imgproc.hpp>
87 #include <opencv2/calib3d.hpp>
88
89 #include <gst/opencv/gstopencvutils.h>
90
91 #include "camerautils.hpp"
92 #include "cameraevent.hpp"
93
94 #include <vector>
95
96 GST_DEBUG_CATEGORY_STATIC (gst_camera_calibrate_debug);
97 #define GST_CAT_DEFAULT gst_camera_calibrate_debug
98
99 #define DEFAULT_CALIBRATON_PATTERN GST_CAMERA_CALIBRATION_PATTERN_CHESSBOARD
100 #define DEFAULT_BOARD_WIDTH 9
101 #define DEFAULT_BOARD_HEIGHT 6
102 #define DEFAULT_SQUARE_SIZE 50
103 #define DEFAULT_ASPECT_RATIO 1.0
104 #define DEFAULT_CORNER_SUB_PIXEL true
105 #define DEFAULT_ZERO_TANGENT_DISTORTION FALSE
106 #define DEFAULT_CENTER_PRINCIPAL_POINT FALSE
107 #define DEFAULT_USE_FISHEYE FALSE
108 #define DEFAULT_FRAME_COUNT 25
109 #define DEFAULT_DELAY 350
110 #define DEFAULT_SHOW_CORNERS true
111
112 enum
113 {
114 PROP_0,
115 PROP_CALIBRATON_PATTERN,
116 PROP_BOARD_WIDTH,
117 PROP_BOARD_HEIGHT,
118 PROP_SQUARE_SIZE,
119 PROP_ASPECT_RATIO,
120 PROP_CORNER_SUB_PIXEL,
121 PROP_ZERO_TANGENT_DISTORTION,
122 PROP_CENTER_PRINCIPAL_POINT,
123 PROP_USE_FISHEYE,
124 PROP_FRAME_COUNT,
125 PROP_DELAY,
126 PROP_SHOW_CORNERS,
127 PROP_SETTINGS
128 };
129
130 enum
131 {
132 DETECTION = 0,
133 CAPTURING = 1,
134 CALIBRATED = 2
135 };
136
137 #define GST_TYPE_CAMERA_CALIBRATION_PATTERN (camera_calibration_pattern_get_type ())
138
139 static GType
camera_calibration_pattern_get_type(void)140 camera_calibration_pattern_get_type (void)
141 {
142 static GType camera_calibration_pattern_type = 0;
143 static const GEnumValue camera_calibration_pattern[] = {
144 {GST_CAMERA_CALIBRATION_PATTERN_CHESSBOARD, "Chessboard", "chessboard"},
145 {GST_CAMERA_CALIBRATION_PATTERN_CIRCLES_GRID, "Circle Grids",
146 "circle_grids"},
147 {GST_CAMERA_CALIBRATION_PATTERN_ASYMMETRIC_CIRCLES_GRID,
148 "Asymmetric Circle Grids", "asymmetric_circle_grids"},
149 {0, NULL, NULL},
150 };
151
152 if (!camera_calibration_pattern_type) {
153 camera_calibration_pattern_type =
154 g_enum_register_static ("GstCameraCalibrationPattern",
155 camera_calibration_pattern);
156 }
157 return camera_calibration_pattern_type;
158 }
159
160 G_DEFINE_TYPE_WITH_CODE (GstCameraCalibrate, gst_camera_calibrate,
161 GST_TYPE_OPENCV_VIDEO_FILTER,
162 GST_DEBUG_CATEGORY_INIT (gst_camera_calibrate_debug, "cameracalibrate", 0,
163 "Performs camera calibration");
164 );
165 GST_ELEMENT_REGISTER_DEFINE (cameracalibrate, "cameracalibrate", GST_RANK_NONE,
166 GST_TYPE_CAMERA_CALIBRATE);
167
168 static void gst_camera_calibrate_dispose (GObject * object);
169 static void gst_camera_calibrate_set_property (GObject * object, guint prop_id,
170 const GValue * value, GParamSpec * pspec);
171 static void gst_camera_calibrate_get_property (GObject * object, guint prop_id,
172 GValue * value, GParamSpec * pspec);
173
174 static GstFlowReturn
175 gst_camera_calibrate_transform_frame_ip (GstOpencvVideoFilter * cvfilter,
176 GstBuffer * frame, cv::Mat img);
177
178 /* clean up */
179 static void
gst_camera_calibrate_finalize(GObject * obj)180 gst_camera_calibrate_finalize (GObject * obj)
181 {
182 G_OBJECT_CLASS (gst_camera_calibrate_parent_class)->finalize (obj);
183 }
184
185 /* initialize the cameracalibration's class */
186 static void
gst_camera_calibrate_class_init(GstCameraCalibrateClass * klass)187 gst_camera_calibrate_class_init (GstCameraCalibrateClass * klass)
188 {
189 GObjectClass *gobject_class = G_OBJECT_CLASS (klass);
190 GstElementClass *element_class = GST_ELEMENT_CLASS (klass);
191 GstOpencvVideoFilterClass *opencvfilter_class =
192 GST_OPENCV_VIDEO_FILTER_CLASS (klass);
193 GstCaps *caps;
194 GstPadTemplate *templ;
195
196 gobject_class->finalize = GST_DEBUG_FUNCPTR (gst_camera_calibrate_finalize);
197 gobject_class->dispose = GST_DEBUG_FUNCPTR (gst_camera_calibrate_dispose);
198 gobject_class->set_property = gst_camera_calibrate_set_property;
199 gobject_class->get_property = gst_camera_calibrate_get_property;
200
201 opencvfilter_class->cv_trans_ip_func =
202 gst_camera_calibrate_transform_frame_ip;
203
204 g_object_class_install_property (gobject_class, PROP_CALIBRATON_PATTERN,
205 g_param_spec_enum ("pattern", "Calibration Pattern",
206 "One of the chessboard, circles, or asymmetric circle pattern",
207 GST_TYPE_CAMERA_CALIBRATION_PATTERN, DEFAULT_CALIBRATON_PATTERN,
208 (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
209
210 g_object_class_install_property (gobject_class, PROP_BOARD_WIDTH,
211 g_param_spec_int ("board-width", "Board Width",
212 "The board width in number of items (e.g. number of squares for chessboard)",
213 1, G_MAXINT, DEFAULT_BOARD_WIDTH,
214 (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
215
216 g_object_class_install_property (gobject_class, PROP_BOARD_HEIGHT,
217 g_param_spec_int ("board-height", "Board Height",
218 "The board height in number of items (e.g. number of squares for chessboard)",
219 1, G_MAXINT, DEFAULT_BOARD_WIDTH,
220 (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
221
222 g_object_class_install_property (gobject_class, PROP_SQUARE_SIZE,
223 g_param_spec_float ("square-size", "Square Size",
224 "The size of a square in your defined unit (point, millimeter, etc.)",
225 0.0, G_MAXFLOAT, DEFAULT_SQUARE_SIZE,
226 (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
227
228 g_object_class_install_property (gobject_class, PROP_ASPECT_RATIO,
229 g_param_spec_float ("aspect-ratio", "Aspect Ratio",
230 "The aspect ratio",
231 0.0, G_MAXFLOAT, DEFAULT_ASPECT_RATIO,
232 (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
233
234 g_object_class_install_property (gobject_class, PROP_CORNER_SUB_PIXEL,
235 g_param_spec_boolean ("corner-sub-pixel", "Corner Sub Pixel",
236 "Improve corner detection accuracy for chessboard",
237 DEFAULT_CORNER_SUB_PIXEL,
238 (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
239
240 g_object_class_install_property (gobject_class, PROP_ZERO_TANGENT_DISTORTION,
241 g_param_spec_boolean ("zero-tangent-distorsion",
242 "Zero Tangent Distorsion", "Assume zero tangential distortion",
243 DEFAULT_ZERO_TANGENT_DISTORTION,
244 (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
245
246 g_object_class_install_property (gobject_class, PROP_CENTER_PRINCIPAL_POINT,
247 g_param_spec_boolean ("center-principal-point", "Center Principal Point",
248 "Fix the principal point at the center",
249 DEFAULT_CENTER_PRINCIPAL_POINT,
250 (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
251
252 g_object_class_install_property (gobject_class, PROP_USE_FISHEYE,
253 g_param_spec_boolean ("use-fisheye", "Use Fisheye",
254 "Use fisheye camera model for calibration",
255 DEFAULT_USE_FISHEYE,
256 (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
257
258 g_object_class_install_property (gobject_class, PROP_DELAY,
259 g_param_spec_int ("delay", "Delay",
260 "Sampling periodicity in ms", 0, G_MAXINT,
261 DEFAULT_DELAY,
262 (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
263
264 g_object_class_install_property (gobject_class, PROP_FRAME_COUNT,
265 g_param_spec_int ("frame-count", "Frame Count",
266 "The number of frames to use from the input for calibration", 1,
267 G_MAXINT, DEFAULT_FRAME_COUNT,
268 (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
269
270 g_object_class_install_property (gobject_class, PROP_SHOW_CORNERS,
271 g_param_spec_boolean ("show-corners", "Show Corners",
272 "Show corners",
273 DEFAULT_SHOW_CORNERS,
274 (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
275
276 g_object_class_install_property (gobject_class, PROP_SETTINGS,
277 g_param_spec_string ("settings", "Settings",
278 "Camera correction parameters (opaque string of serialized OpenCV objects)",
279 NULL, (GParamFlags) (G_PARAM_READABLE | G_PARAM_STATIC_STRINGS)));
280
281 gst_element_class_set_static_metadata (element_class,
282 "cameracalibrate",
283 "Filter/Effect/Video",
284 "Performs camera calibration by having it point at a chessboard pattern "
285 "using upstream/downstream cameraundistort",
286 "Philippe Renon <philippe_renon@yahoo.fr>");
287
288 /* add sink and source pad templates */
289 caps = gst_opencv_caps_from_cv_image_type (CV_8UC4);
290 gst_caps_append (caps, gst_opencv_caps_from_cv_image_type (CV_8UC3));
291 gst_caps_append (caps, gst_opencv_caps_from_cv_image_type (CV_8UC1));
292 templ = gst_pad_template_new ("sink", GST_PAD_SINK, GST_PAD_ALWAYS,
293 gst_caps_ref (caps));
294 gst_element_class_add_pad_template (element_class, templ);
295 templ = gst_pad_template_new ("src", GST_PAD_SRC, GST_PAD_ALWAYS, caps);
296 gst_element_class_add_pad_template (element_class, templ);
297
298 gst_type_mark_as_plugin_api (GST_TYPE_CAMERA_CALIBRATION_PATTERN,
299 (GstPluginAPIFlags) 0);
300 }
301
302 /* initialize the new element
303 * initialize instance structure
304 */
305 static void
gst_camera_calibrate_init(GstCameraCalibrate * calib)306 gst_camera_calibrate_init (GstCameraCalibrate * calib)
307 {
308 calib->calibrationPattern = DEFAULT_CALIBRATON_PATTERN;
309 calib->boardSize.width = DEFAULT_BOARD_WIDTH;
310 calib->boardSize.height = DEFAULT_BOARD_HEIGHT;
311 calib->squareSize = DEFAULT_SQUARE_SIZE;
312 calib->aspectRatio = DEFAULT_ASPECT_RATIO;
313 calib->cornerSubPix = DEFAULT_CORNER_SUB_PIXEL;
314 calib->calibZeroTangentDist = DEFAULT_ZERO_TANGENT_DISTORTION;
315 calib->calibFixPrincipalPoint = DEFAULT_CENTER_PRINCIPAL_POINT;
316 calib->useFisheye = DEFAULT_USE_FISHEYE;
317 calib->nrFrames = DEFAULT_FRAME_COUNT;
318 calib->delay = DEFAULT_DELAY;
319 calib->showCorners = DEFAULT_SHOW_CORNERS;
320
321 calib->flags = cv::CALIB_FIX_K4 | cv::CALIB_FIX_K5;
322 if (calib->calibFixPrincipalPoint)
323 calib->flags |= cv::CALIB_FIX_PRINCIPAL_POINT;
324 if (calib->calibZeroTangentDist)
325 calib->flags |= cv::CALIB_ZERO_TANGENT_DIST;
326 if (calib->aspectRatio)
327 calib->flags |= cv::CALIB_FIX_ASPECT_RATIO;
328
329 if (calib->useFisheye) {
330 /* the fisheye model has its own enum, so overwrite the flags */
331 calib->flags =
332 cv::fisheye::CALIB_FIX_SKEW | cv::fisheye::CALIB_RECOMPUTE_EXTRINSIC |
333 /*cv::fisheye::CALIB_FIX_K1 | */
334 cv::fisheye::CALIB_FIX_K2 | cv::fisheye::CALIB_FIX_K3 | cv::fisheye::
335 CALIB_FIX_K4;
336 }
337
338 calib->mode = CAPTURING; //DETECTION;
339 calib->prevTimestamp = 0;
340
341 calib->imagePoints.clear ();
342 calib->cameraMatrix = 0;
343 calib->distCoeffs = 0;
344
345 calib->settings = NULL;
346
347 gst_opencv_video_filter_set_in_place (GST_OPENCV_VIDEO_FILTER_CAST (calib),
348 TRUE);
349 }
350
351 static void
gst_camera_calibrate_dispose(GObject * object)352 gst_camera_calibrate_dispose (GObject * object)
353 {
354 GstCameraCalibrate *calib = GST_CAMERA_CALIBRATE (object);
355
356 g_free (calib->settings);
357 calib->settings = NULL;
358
359 G_OBJECT_CLASS (gst_camera_calibrate_parent_class)->dispose (object);
360 }
361
362 static void
gst_camera_calibrate_set_property(GObject * object,guint prop_id,const GValue * value,GParamSpec * pspec)363 gst_camera_calibrate_set_property (GObject * object, guint prop_id,
364 const GValue * value, GParamSpec * pspec)
365 {
366 GstCameraCalibrate *calib = GST_CAMERA_CALIBRATE (object);
367
368 switch (prop_id) {
369 case PROP_CALIBRATON_PATTERN:
370 calib->calibrationPattern = g_value_get_enum (value);
371 break;
372 case PROP_BOARD_WIDTH:
373 calib->boardSize.width = g_value_get_int (value);
374 break;
375 case PROP_BOARD_HEIGHT:
376 calib->boardSize.height = g_value_get_int (value);
377 break;
378 case PROP_SQUARE_SIZE:
379 calib->squareSize = g_value_get_float (value);
380 break;
381 case PROP_ASPECT_RATIO:
382 calib->aspectRatio = g_value_get_float (value);
383 break;
384 case PROP_CORNER_SUB_PIXEL:
385 calib->cornerSubPix = g_value_get_boolean (value);
386 break;
387 case PROP_ZERO_TANGENT_DISTORTION:
388 calib->calibZeroTangentDist = g_value_get_boolean (value);
389 break;
390 case PROP_CENTER_PRINCIPAL_POINT:
391 calib->calibFixPrincipalPoint = g_value_get_boolean (value);
392 break;
393 case PROP_USE_FISHEYE:
394 calib->useFisheye = g_value_get_boolean (value);
395 break;
396 case PROP_FRAME_COUNT:
397 calib->nrFrames = g_value_get_int (value);
398 break;
399 case PROP_DELAY:
400 calib->delay = g_value_get_int (value);
401 break;
402 case PROP_SHOW_CORNERS:
403 calib->showCorners = g_value_get_boolean (value);
404 break;
405 default:
406 G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
407 break;
408 }
409 }
410
411 static void
gst_camera_calibrate_get_property(GObject * object,guint prop_id,GValue * value,GParamSpec * pspec)412 gst_camera_calibrate_get_property (GObject * object, guint prop_id,
413 GValue * value, GParamSpec * pspec)
414 {
415 GstCameraCalibrate *calib = GST_CAMERA_CALIBRATE (object);
416
417 switch (prop_id) {
418 case PROP_CALIBRATON_PATTERN:
419 g_value_set_enum (value, calib->calibrationPattern);
420 break;
421 case PROP_BOARD_WIDTH:
422 g_value_set_int (value, calib->boardSize.width);
423 break;
424 case PROP_BOARD_HEIGHT:
425 g_value_set_int (value, calib->boardSize.height);
426 break;
427 case PROP_SQUARE_SIZE:
428 g_value_set_float (value, calib->squareSize);
429 break;
430 case PROP_ASPECT_RATIO:
431 g_value_set_float (value, calib->aspectRatio);
432 break;
433 case PROP_CORNER_SUB_PIXEL:
434 g_value_set_boolean (value, calib->cornerSubPix);
435 break;
436 case PROP_ZERO_TANGENT_DISTORTION:
437 g_value_set_boolean (value, calib->calibZeroTangentDist);
438 break;
439 case PROP_CENTER_PRINCIPAL_POINT:
440 g_value_set_boolean (value, calib->calibFixPrincipalPoint);
441 break;
442 case PROP_USE_FISHEYE:
443 g_value_set_boolean (value, calib->useFisheye);
444 break;
445 case PROP_FRAME_COUNT:
446 g_value_set_int (value, calib->nrFrames);
447 break;
448 case PROP_DELAY:
449 g_value_set_int (value, calib->delay);
450 break;
451 case PROP_SHOW_CORNERS:
452 g_value_set_boolean (value, calib->showCorners);
453 break;
454 case PROP_SETTINGS:
455 g_value_set_string (value, calib->settings);
456 break;
457 default:
458 G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
459 break;
460 }
461 }
462
463 void camera_calibrate_run (GstCameraCalibrate * calib, cv::Mat img);
464
465 /*
466 * Performs the camera calibration
467 */
468 static GstFlowReturn
gst_camera_calibrate_transform_frame_ip(GstOpencvVideoFilter * cvfilter,G_GNUC_UNUSED GstBuffer * frame,cv::Mat img)469 gst_camera_calibrate_transform_frame_ip (GstOpencvVideoFilter * cvfilter,
470 G_GNUC_UNUSED GstBuffer * frame, cv::Mat img)
471 {
472 GstCameraCalibrate *calib = GST_CAMERA_CALIBRATE (cvfilter);
473
474 camera_calibrate_run (calib, img);
475
476 return GST_FLOW_OK;
477 }
478
479 bool camera_calibrate_calibrate (GstCameraCalibrate * calib,
480 cv::Size imageSize, cv::Mat & cameraMatrix, cv::Mat & distCoeffs,
481 std::vector < std::vector < cv::Point2f > >imagePoints);
482
483 void
camera_calibrate_run(GstCameraCalibrate * calib,cv::Mat img)484 camera_calibrate_run (GstCameraCalibrate * calib, cv::Mat img)
485 {
486
487 // For camera only take new samples after delay time
488 if (calib->mode == CAPTURING) {
489 // get_input
490 cv::Size imageSize = img.size ();
491
492 /* find_pattern
493 * FIXME find ways to reduce CPU usage
494 * don't do it on all frames ? will it help ? corner display will be affected.
495 * in a separate frame?
496 * in a separate element that gets composited back into the main stream
497 * (video is tee-d into it and can then be decimated, scaled, etc..) */
498
499 std::vector < cv::Point2f > pointBuf;
500 bool found;
501 int chessBoardFlags =
502 cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE;
503
504 if (!calib->useFisheye) {
505 /* fast check erroneously fails with high distortions like fisheye */
506 chessBoardFlags |= cv::CALIB_CB_FAST_CHECK;
507 }
508
509 /* Find feature points on the input format */
510 switch (calib->calibrationPattern) {
511 case GST_CAMERA_CALIBRATION_PATTERN_CHESSBOARD:
512 found =
513 cv::findChessboardCorners (img, calib->boardSize, pointBuf,
514 chessBoardFlags);
515 break;
516 case GST_CAMERA_CALIBRATION_PATTERN_CIRCLES_GRID:
517 found = cv::findCirclesGrid (img, calib->boardSize, pointBuf);
518 break;
519 case GST_CAMERA_CALIBRATION_PATTERN_ASYMMETRIC_CIRCLES_GRID:
520 found =
521 cv::findCirclesGrid (img, calib->boardSize, pointBuf,
522 cv::CALIB_CB_ASYMMETRIC_GRID);
523 break;
524 default:
525 found = FALSE;
526 break;
527 }
528
529 bool blinkOutput = FALSE;
530 if (found) {
531 /* improve the found corners' coordinate accuracy for chessboard */
532 if (calib->calibrationPattern == GST_CAMERA_CALIBRATION_PATTERN_CHESSBOARD
533 && calib->cornerSubPix) {
534 /* FIXME findChessboardCorners and alike do a cv::COLOR_BGR2GRAY (and a histogram balance)
535 * the color convert should be done once (if needed) and shared
536 * FIXME keep viewGray around to avoid reallocating it each time... */
537 cv::Mat viewGray;
538 cv::cvtColor (img, viewGray, cv::COLOR_BGR2GRAY);
539 cv::cornerSubPix (viewGray, pointBuf, cv::Size (11, 11), cv::Size (-1,
540 -1),
541 cv::TermCriteria (cv::TermCriteria::EPS + cv::TermCriteria::COUNT,
542 30, 0.1));
543 }
544
545 /* take new samples after delay time */
546 if ((calib->mode == CAPTURING)
547 && ((clock () - calib->prevTimestamp) >
548 calib->delay * 1e-3 * CLOCKS_PER_SEC)) {
549 calib->imagePoints.push_back (pointBuf);
550 calib->prevTimestamp = clock ();
551 blinkOutput = true;
552 }
553
554 /* draw the corners */
555 if (calib->showCorners) {
556 cv::drawChessboardCorners (img, calib->boardSize, cv::Mat (pointBuf),
557 found);
558 }
559 }
560
561 /* if got enough frames then stop calibration and show result */
562 if (calib->mode == CAPTURING
563 && calib->imagePoints.size () >= (size_t) calib->nrFrames) {
564
565 if (camera_calibrate_calibrate (calib, imageSize, calib->cameraMatrix,
566 calib->distCoeffs, calib->imagePoints)) {
567 calib->mode = CALIBRATED;
568
569 GstPad *sink_pad = GST_BASE_TRANSFORM_SINK_PAD (calib);
570 GstPad *src_pad = GST_BASE_TRANSFORM_SRC_PAD (calib);
571 GstEvent *sink_event;
572 GstEvent *src_event;
573
574 /* set settings property */
575 g_free (calib->settings);
576 calib->settings =
577 camera_serialize_undistort_settings (calib->cameraMatrix,
578 calib->distCoeffs);
579
580 /* create calibrated event and send upstream and downstream */
581 sink_event = gst_camera_event_new_calibrated (calib->settings);
582 GST_LOG_OBJECT (sink_pad, "Sending upstream event %s.",
583 GST_EVENT_TYPE_NAME (sink_event));
584 if (!gst_pad_push_event (sink_pad, sink_event)) {
585 GST_WARNING_OBJECT (sink_pad,
586 "Sending upstream event %p (%s) failed.", sink_event,
587 GST_EVENT_TYPE_NAME (sink_event));
588 }
589
590 src_event = gst_camera_event_new_calibrated (calib->settings);
591 GST_LOG_OBJECT (src_pad, "Sending downstream event %s.",
592 GST_EVENT_TYPE_NAME (src_event));
593 if (!gst_pad_push_event (src_pad, src_event)) {
594 GST_WARNING_OBJECT (src_pad,
595 "Sending downstream event %p (%s) failed.", src_event,
596 GST_EVENT_TYPE_NAME (src_event));
597 }
598 } else {
599 /* failed to calibrate, go back to detection mode */
600 calib->mode = DETECTION;
601 }
602 }
603
604 if (calib->mode == CAPTURING && blinkOutput) {
605 bitwise_not (img, img);
606 }
607
608 }
609
610 /* output text */
611 /* FIXME ll additional rendering (text, corners, ...) should be done with
612 * cairo or another gst framework.
613 * this will relax the conditions on the input format (RBG only at the moment).
614 * the calibration itself accepts more formats... */
615
616 std::string msg = (calib->mode == CAPTURING) ? "100/100" :
617 (calib->mode == CALIBRATED) ? "Calibrated" : "Waiting...";
618 int baseLine = 0;
619 cv::Size textSize = cv::getTextSize (msg, 1, 1, 1, &baseLine);
620 cv::Point textOrigin (img.cols - 2 * textSize.width - 10,
621 img.rows - 2 * baseLine - 10);
622
623 if (calib->mode == CAPTURING) {
624 msg =
625 cv::format ("%d/%d", (int) calib->imagePoints.size (), calib->nrFrames);
626 }
627
628 const cv::Scalar RED (0, 0, 255);
629 const cv::Scalar GREEN (0, 255, 0);
630
631 cv::putText (img, msg, textOrigin, 1, 1,
632 calib->mode == CALIBRATED ? GREEN : RED);
633 }
634
635 static double
camera_calibrate_calc_reprojection_errors(const std::vector<std::vector<cv::Point3f>> & objectPoints,const std::vector<std::vector<cv::Point2f>> & imagePoints,const std::vector<cv::Mat> & rvecs,const std::vector<cv::Mat> & tvecs,const cv::Mat & cameraMatrix,const cv::Mat & distCoeffs,std::vector<float> & perViewErrors,bool fisheye)636 camera_calibrate_calc_reprojection_errors (const std::vector < std::vector <
637 cv::Point3f > >&objectPoints,
638 const std::vector < std::vector < cv::Point2f > >&imagePoints,
639 const std::vector < cv::Mat > &rvecs, const std::vector < cv::Mat > &tvecs,
640 const cv::Mat & cameraMatrix, const cv::Mat & distCoeffs,
641 std::vector < float >&perViewErrors, bool fisheye)
642 {
643 std::vector < cv::Point2f > imagePoints2;
644 size_t totalPoints = 0;
645 double totalErr = 0, err;
646 perViewErrors.resize (objectPoints.size ());
647
648 for (size_t i = 0; i < objectPoints.size (); ++i) {
649 if (fisheye) {
650 cv::fisheye::projectPoints (objectPoints[i], imagePoints2,
651 rvecs[i], tvecs[i], cameraMatrix, distCoeffs);
652 } else {
653 cv::projectPoints (objectPoints[i], rvecs[i], tvecs[i],
654 cameraMatrix, distCoeffs, imagePoints2);
655 }
656 err = cv::norm (imagePoints[i], imagePoints2, cv::NORM_L2);
657
658 size_t n = objectPoints[i].size ();
659 perViewErrors[i] = (float) std::sqrt (err * err / n);
660 totalErr += err * err;
661 totalPoints += n;
662 }
663
664 return std::sqrt (totalErr / totalPoints);
665 }
666
667 static void
camera_calibrate_calc_corners(cv::Size boardSize,float squareSize,std::vector<cv::Point3f> & corners,gint patternType)668 camera_calibrate_calc_corners (cv::Size boardSize, float squareSize,
669 std::vector < cv::Point3f > &corners, gint patternType /*= CHESSBOARD*/ )
670 {
671 corners.clear ();
672
673 switch (patternType) {
674 case GST_CAMERA_CALIBRATION_PATTERN_CHESSBOARD:
675 case GST_CAMERA_CALIBRATION_PATTERN_CIRCLES_GRID:
676 for (int i = 0; i < boardSize.height; ++i)
677 for (int j = 0; j < boardSize.width; ++j)
678 corners.push_back (cv::Point3f (j * squareSize, i * squareSize, 0));
679 break;
680 case GST_CAMERA_CALIBRATION_PATTERN_ASYMMETRIC_CIRCLES_GRID:
681 for (int i = 0; i < boardSize.height; i++)
682 for (int j = 0; j < boardSize.width; j++)
683 corners.push_back (cv::Point3f ((2 * j + i % 2) * squareSize,
684 i * squareSize, 0));
685 break;
686 default:
687 break;
688 }
689 }
690
691 static bool
camera_calibrate_calibrate_full(GstCameraCalibrate * calib,cv::Size & imageSize,cv::Mat & cameraMatrix,cv::Mat & distCoeffs,std::vector<std::vector<cv::Point2f>> imagePoints,std::vector<cv::Mat> & rvecs,std::vector<cv::Mat> & tvecs,std::vector<float> & reprojErrs,double & totalAvgErr)692 camera_calibrate_calibrate_full (GstCameraCalibrate * calib,
693 cv::Size & imageSize, cv::Mat & cameraMatrix, cv::Mat & distCoeffs,
694 std::vector < std::vector < cv::Point2f > >imagePoints,
695 std::vector < cv::Mat > &rvecs, std::vector < cv::Mat > &tvecs,
696 std::vector < float >&reprojErrs, double &totalAvgErr)
697 {
698 cameraMatrix = cv::Mat::eye (3, 3, CV_64F);
699 if (calib->flags & cv::CALIB_FIX_ASPECT_RATIO) {
700 cameraMatrix.at < double >(0, 0) = calib->aspectRatio;
701 }
702 if (calib->useFisheye) {
703 distCoeffs = cv::Mat::zeros (4, 1, CV_64F);
704 } else {
705 distCoeffs = cv::Mat::zeros (8, 1, CV_64F);
706 }
707
708 std::vector < std::vector < cv::Point3f > >objectPoints (1);
709 camera_calibrate_calc_corners (calib->boardSize, calib->squareSize,
710 objectPoints[0], calib->calibrationPattern);
711
712 objectPoints.resize (imagePoints.size (), objectPoints[0]);
713
714 /* Find intrinsic and extrinsic camera parameters */
715 double rms;
716
717 if (calib->useFisheye) {
718 cv::Mat _rvecs, _tvecs;
719 rms = cv::fisheye::calibrate (objectPoints, imagePoints, imageSize,
720 cameraMatrix, distCoeffs, _rvecs, _tvecs, calib->flags);
721
722 rvecs.reserve (_rvecs.rows);
723 tvecs.reserve (_tvecs.rows);
724 for (int i = 0; i < int (objectPoints.size ()); i++) {
725 rvecs.push_back (_rvecs.row (i));
726 tvecs.push_back (_tvecs.row (i));
727 }
728 } else {
729 rms = cv::calibrateCamera (objectPoints, imagePoints, imageSize,
730 cameraMatrix, distCoeffs, rvecs, tvecs, calib->flags);
731 }
732
733 GST_LOG_OBJECT (calib,
734 "Re-projection error reported by calibrateCamera: %f", rms);
735
736 bool ok = checkRange (cameraMatrix) && checkRange (distCoeffs);
737
738 totalAvgErr =
739 camera_calibrate_calc_reprojection_errors (objectPoints, imagePoints,
740 rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs, calib->useFisheye);
741
742 return ok;
743 }
744
745 bool
camera_calibrate_calibrate(GstCameraCalibrate * calib,cv::Size imageSize,cv::Mat & cameraMatrix,cv::Mat & distCoeffs,std::vector<std::vector<cv::Point2f>> imagePoints)746 camera_calibrate_calibrate (GstCameraCalibrate * calib,
747 cv::Size imageSize, cv::Mat & cameraMatrix, cv::Mat & distCoeffs,
748 std::vector < std::vector < cv::Point2f > >imagePoints)
749 {
750 std::vector < cv::Mat > rvecs, tvecs;
751 std::vector < float >reprojErrs;
752 double totalAvgErr = 0;
753
754 bool ok = camera_calibrate_calibrate_full (calib,
755 imageSize, cameraMatrix, distCoeffs, imagePoints,
756 rvecs, tvecs, reprojErrs, totalAvgErr);
757 GST_LOG_OBJECT (calib, (ok ? "Calibration succeeded" : "Calibration failed"));
758 /* + ". avg re projection error = " + totalAvgErr); */
759
760 return ok;
761 }
762