subprojects/gst-plugins-bad/ext/opencv/gstcameraundistort.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 distortion correction.
  48  *
  49  * Camera correction settings are obtained by running through
  50  * the camera calibration process with the cameracalibrate element.
  51  *
  52  * It is possible to do live correction and calibration by chaining
  53  * a cameraundistort and a cameracalibrate element. The cameracalibrate
  54  * will send an event with the correction parameters to the the cameraundistort.
  55  *
  56  * Based on this tutorial: https://docs.opencv.org/2.4/doc/tutorials/calib3d/camera_calibration/camera_calibration.html
  57  *
  58  * ## Example pipelines
  59  *
  60  * |[
  61  * gst-launch-1.0 -v v4l2src ! videoconvert ! cameraundistort settings="???" ! autovideosink
  62  * ]| will correct camera distortion based on provided settings.
  63  * |[
  64  * gst-launch-1.0 -v v4l2src ! videoconvert ! cameraundistort ! cameracalibrate ! autovideosink
  65  * ]| will correct camera distortion once camera calibration is done.
  66  */
  67
  68 #ifdef HAVE_CONFIG_H
  69 #  include <config.h>
  70 #endif
  71
  72 #include <vector>
  73
  74 #include "camerautils.hpp"
  75 #include "cameraevent.hpp"
  76
  77 #include "gstcameraundistort.h"
  78
  79 #include <opencv2/imgproc.hpp>
  80 #include <opencv2/calib3d.hpp>
  81
  82 #include <gst/opencv/gstopencvutils.h>
  83
  84 GST_DEBUG_CATEGORY_STATIC (gst_camera_undistort_debug);
  85 #define GST_CAT_DEFAULT gst_camera_undistort_debug
  86
  87 #define DEFAULT_SHOW_UNDISTORTED TRUE
  88 #define DEFAULT_ALPHA 0.0
  89 #define DEFAULT_CROP FALSE
  90
  91 enum
  92 {
  93   PROP_0,
  94   PROP_SHOW_UNDISTORTED,
  95   PROP_ALPHA,
  96   PROP_CROP,
  97   PROP_SETTINGS
  98 };
  99
 100 G_DEFINE_TYPE_WITH_CODE (GstCameraUndistort, gst_camera_undistort,
 101     GST_TYPE_OPENCV_VIDEO_FILTER,
 102     GST_DEBUG_CATEGORY_INIT (gst_camera_undistort_debug, "cameraundistort", 0,
 103         "Performs camera undistortion");
 104     );
 105 GST_ELEMENT_REGISTER_DEFINE (cameraundistort, "cameraundistort", GST_RANK_NONE,
 106     GST_TYPE_CAMERA_UNDISTORT);
 107
 108 static void gst_camera_undistort_dispose (GObject * object);
 109 static void gst_camera_undistort_set_property (GObject * object, guint prop_id,
 110     const GValue * value, GParamSpec * pspec);
 111 static void gst_camera_undistort_get_property (GObject * object, guint prop_id,
 112     GValue * value, GParamSpec * pspec);
 113
 114 static gboolean gst_camera_undistort_set_info (GstOpencvVideoFilter * cvfilter,
 115     gint in_width, gint in_height, int in_cv_type,
 116     gint out_width, gint out_height, int out_cv_type);
 117 static GstFlowReturn gst_camera_undistort_transform_frame (GstOpencvVideoFilter
 118     * cvfilter, GstBuffer * frame, cv::Mat img, GstBuffer * outframe,
 119     cv::Mat outimg);
 120
 121 static gboolean gst_camera_undistort_sink_event (GstBaseTransform * trans,
 122     GstEvent * event);
 123 static gboolean gst_camera_undistort_src_event (GstBaseTransform * trans,
 124     GstEvent * event);
 125
 126 static void camera_undistort_run (GstCameraUndistort * undist, cv::Mat img,
 127     cv::Mat outimg);
 128 static gboolean camera_undistort_init_undistort_rectify_map (GstCameraUndistort
 129     * undist);
 130
 131 /* initialize the cameraundistort's class */
 132 static void
 133 gst_camera_undistort_class_init (GstCameraUndistortClass * klass)
 134 {
 135   GObjectClass *gobject_class = G_OBJECT_CLASS (klass);
 136   GstElementClass *element_class = GST_ELEMENT_CLASS (klass);
 137   GstBaseTransformClass *trans_class = GST_BASE_TRANSFORM_CLASS (klass);
 138   GstOpencvVideoFilterClass *opencvfilter_class =
 139       GST_OPENCV_VIDEO_FILTER_CLASS (klass);
 140
 141   GstCaps *caps;
 142   GstPadTemplate *templ;
 143
 144   gobject_class->dispose = gst_camera_undistort_dispose;
 145   gobject_class->set_property = gst_camera_undistort_set_property;
 146   gobject_class->get_property = gst_camera_undistort_get_property;
 147
 148   trans_class->sink_event = GST_DEBUG_FUNCPTR (gst_camera_undistort_sink_event);
 149   trans_class->src_event = GST_DEBUG_FUNCPTR (gst_camera_undistort_src_event);
 150
 151   opencvfilter_class->cv_set_caps = gst_camera_undistort_set_info;
 152   opencvfilter_class->cv_trans_func = gst_camera_undistort_transform_frame;
 153
 154   g_object_class_install_property (gobject_class, PROP_SHOW_UNDISTORTED,
 155       g_param_spec_boolean ("undistort", "Apply camera corrections",
 156           "Apply camera corrections",
 157           DEFAULT_SHOW_UNDISTORTED,
 158           (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
 159
 160   g_object_class_install_property (gobject_class, PROP_ALPHA,
 161       g_param_spec_float ("alpha", "Pixels",
 162           "Show all pixels (1), only valid ones (0) or something in between",
 163           0.0, 1.0, DEFAULT_ALPHA,
 164           (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
 165
 166   g_object_class_install_property (gobject_class, PROP_SETTINGS,
 167       g_param_spec_string ("settings", "Settings",
 168           "Camera correction parameters (opaque string of serialized OpenCV objects)",
 169           NULL, (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
 170
 171   gst_element_class_set_static_metadata (element_class,
 172       "cameraundistort",
 173       "Filter/Effect/Video",
 174       "Performs camera undistort", "Philippe Renon <philippe_renon@yahoo.fr>");
 175
 176   /* add sink and source pad templates */
 177   caps = gst_opencv_caps_from_cv_image_type (CV_16UC1);
 178   gst_caps_append (caps, gst_opencv_caps_from_cv_image_type (CV_8UC4));
 179   gst_caps_append (caps, gst_opencv_caps_from_cv_image_type (CV_8UC3));
 180   gst_caps_append (caps, gst_opencv_caps_from_cv_image_type (CV_8UC1));
 181   templ = gst_pad_template_new ("sink", GST_PAD_SINK, GST_PAD_ALWAYS,
 182       gst_caps_ref (caps));
 183   gst_element_class_add_pad_template (element_class, templ);
 184   templ = gst_pad_template_new ("src", GST_PAD_SRC, GST_PAD_ALWAYS, caps);
 185   gst_element_class_add_pad_template (element_class, templ);
 186 }
 187
 188 /* initialize the new element
 189  * initialize instance structure
 190  */
 191 static void
 192 gst_camera_undistort_init (GstCameraUndistort * undist)
 193 {
 194   undist->showUndistorted = DEFAULT_SHOW_UNDISTORTED;
 195   undist->alpha = DEFAULT_ALPHA;
 196   undist->crop = DEFAULT_CROP;
 197
 198   undist->doUndistort = FALSE;
 199   undist->settingsChanged = FALSE;
 200
 201   undist->cameraMatrix = 0;
 202   undist->distCoeffs = 0;
 203   undist->map1 = 0;
 204   undist->map2 = 0;
 205
 206   undist->settings = NULL;
 207 }
 208
 209 static void
 210 gst_camera_undistort_dispose (GObject * object)
 211 {
 212   GstCameraUndistort *undist = GST_CAMERA_UNDISTORT (object);
 213
 214   g_free (undist->settings);
 215   undist->settings = NULL;
 216
 217   G_OBJECT_CLASS (gst_camera_undistort_parent_class)->dispose (object);
 218 }
 219
 220
 221 static void
 222 gst_camera_undistort_set_property (GObject * object, guint prop_id,
 223     const GValue * value, GParamSpec * pspec)
 224 {
 225   GstCameraUndistort *undist = GST_CAMERA_UNDISTORT (object);
 226   const char *str;
 227
 228   switch (prop_id) {
 229     case PROP_SHOW_UNDISTORTED:
 230       undist->showUndistorted = g_value_get_boolean (value);
 231       undist->settingsChanged = TRUE;
 232       break;
 233     case PROP_ALPHA:
 234       undist->alpha = g_value_get_float (value);
 235       undist->settingsChanged = TRUE;
 236       break;
 237     case PROP_CROP:
 238       undist->crop = g_value_get_boolean (value);
 239       break;
 240     case PROP_SETTINGS:
 241       if (undist->settings) {
 242         g_free (undist->settings);
 243         undist->settings = NULL;
 244       }
 245       str = g_value_get_string (value);
 246       if (str)
 247         undist->settings = g_strdup (str);
 248       undist->settingsChanged = TRUE;
 249       break;
 250     default:
 251       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
 252       break;
 253   }
 254 }
 255
 256 static void
 257 gst_camera_undistort_get_property (GObject * object, guint prop_id,
 258     GValue * value, GParamSpec * pspec)
 259 {
 260   GstCameraUndistort *undist = GST_CAMERA_UNDISTORT (object);
 261
 262   switch (prop_id) {
 263     case PROP_SHOW_UNDISTORTED:
 264       g_value_set_boolean (value, undist->showUndistorted);
 265       break;
 266     case PROP_ALPHA:
 267       g_value_set_float (value, undist->alpha);
 268       break;
 269     case PROP_CROP:
 270       g_value_set_boolean (value, undist->crop);
 271       break;
 272     case PROP_SETTINGS:
 273       g_value_set_string (value, undist->settings);
 274       break;
 275     default:
 276       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
 277       break;
 278   }
 279 }
 280
 281 gboolean
 282 gst_camera_undistort_set_info (GstOpencvVideoFilter * cvfilter,
 283     gint in_width, gint in_height, G_GNUC_UNUSED int in_cv_type,
 284     G_GNUC_UNUSED gint out_width, G_GNUC_UNUSED gint out_height,
 285     G_GNUC_UNUSED int out_cv_type)
 286 {
 287   GstCameraUndistort *undist = GST_CAMERA_UNDISTORT (cvfilter);
 288
 289   undist->imageSize = cv::Size (in_width, in_height);
 290
 291   return TRUE;
 292 }
 293
 294 /*
 295  * Performs the camera undistort
 296  */
 297 static GstFlowReturn
 298 gst_camera_undistort_transform_frame (GstOpencvVideoFilter * cvfilter,
 299     G_GNUC_UNUSED GstBuffer * frame, cv::Mat img,
 300     G_GNUC_UNUSED GstBuffer * outframe, cv::Mat outimg)
 301 {
 302   GstCameraUndistort *undist = GST_CAMERA_UNDISTORT (cvfilter);
 303
 304   camera_undistort_run (undist, img, outimg);
 305
 306   return GST_FLOW_OK;
 307 }
 308
 309 static void
 310 camera_undistort_run (GstCameraUndistort * undist, cv::Mat img, cv::Mat outimg)
 311 {
 312   /* TODO is settingsChanged handling thread safe ? */
 313   if (undist->settingsChanged) {
 314     /* settings have changed, need to recompute undistort */
 315     undist->settingsChanged = FALSE;
 316     undist->doUndistort = FALSE;
 317     if (undist->showUndistorted && undist->settings) {
 318       if (camera_deserialize_undistort_settings (undist->settings,
 319               undist->cameraMatrix, undist->distCoeffs)) {
 320         undist->doUndistort =
 321             camera_undistort_init_undistort_rectify_map (undist);
 322       }
 323     }
 324   }
 325
 326   if (undist->showUndistorted && undist->doUndistort) {
 327     /* do the undistort */
 328     cv::remap (img, outimg, undist->map1, undist->map2, cv::INTER_LINEAR);
 329
 330     if (undist->crop) {
 331       /* TODO do the cropping */
 332       const cv::Scalar CROP_COLOR (0, 255, 0);
 333       cv::rectangle (outimg, undist->validPixROI, CROP_COLOR);
 334     }
 335   } else {
 336     /* FIXME should use pass through to avoid this copy when not undistorting */
 337     img.copyTo (outimg);
 338   }
 339 }
 340
 341 /* compute undistort */
 342 static gboolean
 343 camera_undistort_init_undistort_rectify_map (GstCameraUndistort * undist)
 344 {
 345   cv::Size newImageSize;
 346   cv::Rect validPixROI;
 347   cv::Mat newCameraMatrix =
 348       cv::getOptimalNewCameraMatrix (undist->cameraMatrix, undist->distCoeffs,
 349       undist->imageSize, undist->alpha, newImageSize, &validPixROI);
 350   undist->validPixROI = validPixROI;
 351
 352   cv::initUndistortRectifyMap (undist->cameraMatrix, undist->distCoeffs,
 353       cv::Mat (), newCameraMatrix, undist->imageSize, CV_16SC2, undist->map1,
 354       undist->map2);
 355
 356   return TRUE;
 357 }
 358
 359 static gboolean
 360 camera_undistort_calibration_event (GstCameraUndistort * undist,
 361     GstEvent * event)
 362 {
 363   g_free (undist->settings);
 364
 365   if (!gst_camera_event_parse_calibrated (event, &(undist->settings))) {
 366     return FALSE;
 367   }
 368
 369   undist->settingsChanged = TRUE;
 370
 371   return TRUE;
 372 }
 373
 374 static gboolean
 375 gst_camera_undistort_sink_event (GstBaseTransform * trans, GstEvent * event)
 376 {
 377   GstCameraUndistort *undist = GST_CAMERA_UNDISTORT (trans);
 378
 379   const GstStructure *structure = gst_event_get_structure (event);
 380
 381   if (GST_EVENT_TYPE (event) == GST_EVENT_CUSTOM_BOTH && structure) {
 382     if (strcmp (gst_structure_get_name (structure),
 383             GST_CAMERA_EVENT_CALIBRATED_NAME) == 0) {
 384       return camera_undistort_calibration_event (undist, event);
 385     }
 386   }
 387
 388   return
 389       GST_BASE_TRANSFORM_CLASS (gst_camera_undistort_parent_class)->sink_event
 390       (trans, event);
 391 }
 392
 393 static gboolean
 394 gst_camera_undistort_src_event (GstBaseTransform * trans, GstEvent * event)
 395 {
 396   GstCameraUndistort *undist = GST_CAMERA_UNDISTORT (trans);
 397
 398   const GstStructure *structure = gst_event_get_structure (event);
 399
 400   if (GST_EVENT_TYPE (event) == GST_EVENT_CUSTOM_BOTH && structure) {
 401     if (strcmp (gst_structure_get_name (structure),
 402             GST_CAMERA_EVENT_CALIBRATED_NAME) == 0) {
 403       return camera_undistort_calibration_event (undist, event);
 404     }
 405   }
 406
 407   return
 408       GST_BASE_TRANSFORM_CLASS (gst_camera_undistort_parent_class)->src_event
 409       (trans, event);
 410 }