subprojects/gst-plugins-bad/ext/opencv/gstcameracalibrate.cpp

   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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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
 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 }