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