ext/opencv/gstdisparity.cpp

   1  /*
   2   * GStreamer
   3   * Copyright (C) 2013 Miguel Casas-Sanchez <miguelecasassanchez@gmail.com>
   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  * SECTION:element-disparity
  45  *
  46  * This element computes a disparity map from two stereo images, meaning each one coming from a
  47  * different camera, both looking at the same scene and relatively close to each other - more on
  48  * this below. The disparity map is a proxy of the depth of a scene as seen from the camera.
  49  *
  50  * Assumptions: Input images are stereo, rectified and aligned. If these conditions are not met,
  51  * results can be poor. Both cameras should be looking parallel to maximize the overlapping
  52  * stereo area, and should not have objects too close or too far. The algorithms implemented here
  53  * run prefiltering stages to normalize brightness between the inputs, and to maximize texture.
  54  *
  55  * Note that in general is hard to find correspondences between soft textures, for instance a
  56  * block of gloss blue colour. The output is a gray image with values close to white meaning
  57  * closer to the cameras and darker far away. Black means that the pixels were not matched
  58  * correctly (not found). The resulting depth map can be transformed into real world coordinates
  59  * by means of OpenCV function (reprojectImageTo3D) but for this the camera matrixes need to
  60  * be fully known.
  61  *
  62  * Algorithm 1 is the OpenCV Stereo Block Matching, similar to the one developed by Kurt Konolige
  63  * [A] and that works by using small Sum-of-absolute-differenc (SAD) windows to find matching
  64  * points between the left and right rectified images. This algorithm finds only strongly matching
  65  * points between both images, this means normally strong textures. In soft textures, such as a
  66  * single coloured wall (as opposed to, f.i. a hairy rug), not all pixels might have correspondence.
  67  *
  68  * Algorithm 2 is the Semi Global Matching (SGM) algorithm [B] which models the scene structure
  69  * with a point-wise matching cost and an associated smoothness term. The energy minimization
  70  * is then computed in a multitude of 1D lines. For each point, the disparity corresponding to
  71  * the minimum aggregated cost is selected. In [B] the author proposes to use 8 or 16 different
  72  * independent paths. The SGM approach works well near depth discontinuities, but produces less
  73  * accurate results. Despite its relatively large memory footprint, this method is very fast and
  74  * potentially robust to complicated textured regions.
  75  *
  76  * Algorithm 3 is the OpenCV implementation of a modification of the variational stereo
  77  * correspondence algorithm, described in [C].
  78  *
  79  * Algorithm 4 is the Graph Cut stereo vision algorithm (GC) introduced in [D]; it is a global
  80  * stereo vision method. It calculates depth discontinuities by minimizing an energy function
  81  * combingin a point-wise matching cost and a smoothness term. The energy function is passed
  82  * to graph and Graph Cut is used to find a lowest-energy cut. GC is computationally intensive due
  83  * to its global nature and uses loads of memory, but it can deal with textureless regions and
  84  * reflections better than other methods.
  85  * Graphcut based technique is CPU intensive hence smaller framesizes are desired.
  86  *
  87  * Some test images can be found here: http://vision.stanford.edu/~birch/p2p/
  88  *
  89  * [A] K. Konolige. Small vision system. hardware and implementation. In Proc. International
  90  * Symposium on Robotics Research, pages 111--116, Hayama, Japan, 1997.
  91  * [B] H. Hirschmüller, “Accurate and efficient stereo processing by semi-global matching and
  92  * mutual information,” in Proceedings of the IEEE Conference on Computer Vision and Pattern
  93  * Recognition, 2005, pp. 807–814.
  94  * [C] S. Kosov, T. Thormaehlen, H.-P. Seidel "Accurate Real-Time Disparity Estimation with
  95  * Variational Methods" Proceedings of the 5th International Symposium on Visual Computing,
  96  * Vegas, USA
  97  * [D] Scharstein, D. & Szeliski, R. (2001). A taxonomy and evaluation of dense two-frame stereo
  98  * correspondence algorithms, International Journal of Computer Vision 47: 7–42.
  99  *
 100  * ## Example launch line
 101  *
 102  * |[
 103  * gst-launch-1.0 videotestsrc ! video/x-raw,width=320,height=240 ! videoconvert ! disp0.sink_right videotestsrc ! video/x-raw,width=320,height=240 ! videoconvert ! disp0.sink_left disparity name=disp0 ! videoconvert ! ximagesink
 104  * ]|
 105  * Another example, with two png files representing a classical stereo matching,
 106  * downloadable from http://vision.middlebury.edu/stereo/submit/tsukuba/im4.png and
 107  * im3.png. Note here they are downloaded in ~ (home).
 108  * |[
 109 gst-launch-1.0    multifilesrc  location=~/im3.png ! pngdec ! videoconvert  ! disp0.sink_right     multifilesrc  location=~/im4.png ! pngdec ! videoconvert ! disp0.sink_left disparity   name=disp0 method=sbm     disp0.src ! videoconvert ! ximagesink
 110  * ]|
 111  * Yet another example with two cameras, which should be the same model, aligned etc.
 112  * |[
 113  gst-launch-1.0    v4l2src device=/dev/video1 ! video/x-raw,width=320,height=240 ! videoconvert  ! disp0.sink_right     v4l2src device=/dev/video0 ! video/x-raw,width=320,height=240 ! videoconvert ! disp0.sink_left disparity   name=disp0 method=sgbm     disp0.src ! videoconvert ! ximagesink
 114  * ]|
 115  */
 116
 117 #ifdef HAVE_CONFIG_H
 118 #include <config.h>
 119 #endif
 120
 121 #include "gstdisparity.h"
 122 #include <opencv2/imgproc.hpp>
 123
 124 GST_DEBUG_CATEGORY_STATIC (gst_disparity_debug);
 125 #define GST_CAT_DEFAULT gst_disparity_debug
 126
 127 using namespace cv;
 128 /* Filter signals and args */
 129 enum
 130 {
 131   /* FILL ME */
 132   LAST_SIGNAL
 133 };
 134
 135 enum
 136 {
 137   PROP_0,
 138   PROP_METHOD,
 139 };
 140
 141 typedef enum
 142 {
 143   METHOD_SBM,
 144   METHOD_SGBM
 145 } GstDisparityMethod;
 146
 147 #define DEFAULT_METHOD METHOD_SGBM
 148
 149 #define GST_TYPE_DISPARITY_METHOD (gst_disparity_method_get_type ())
 150 static GType
 151 gst_disparity_method_get_type (void)
 152 {
 153   static GType etype = 0;
 154   if (etype == 0) {
 155     static const GEnumValue values[] = {
 156       {METHOD_SBM, "Global block matching algorithm", "sbm"},
 157       {METHOD_SGBM, "Semi-global block matching algorithm", "sgbm"},
 158       {0, NULL, NULL},
 159     };
 160     etype = g_enum_register_static ("GstDisparityMethod", values);
 161   }
 162   return etype;
 163 }
 164
 165 /* the capabilities of the inputs and outputs.
 166  */
 167 static GstStaticPadTemplate sink_factory = GST_STATIC_PAD_TEMPLATE ("sink",
 168     GST_PAD_SINK,
 169     GST_PAD_ALWAYS,
 170     GST_STATIC_CAPS (GST_VIDEO_CAPS_MAKE ("RGB"))
 171     );
 172
 173 static GstStaticPadTemplate src_factory = GST_STATIC_PAD_TEMPLATE ("src",
 174     GST_PAD_SRC,
 175     GST_PAD_ALWAYS,
 176     GST_STATIC_CAPS (GST_VIDEO_CAPS_MAKE ("RGB"))
 177     );
 178
 179 G_DEFINE_TYPE (GstDisparity, gst_disparity, GST_TYPE_ELEMENT);
 180
 181 static void gst_disparity_finalize (GObject * object);
 182 static void gst_disparity_set_property (GObject * object, guint prop_id,
 183     const GValue * value, GParamSpec * pspec);
 184 static void gst_disparity_get_property (GObject * object, guint prop_id,
 185     GValue * value, GParamSpec * pspec);
 186 static GstStateChangeReturn gst_disparity_change_state (GstElement * element,
 187     GstStateChange transition);
 188
 189 static gboolean gst_disparity_handle_sink_event (GstPad * pad,
 190     GstObject * parent, GstEvent * event);
 191 static gboolean gst_disparity_handle_query (GstPad * pad,
 192     GstObject * parent, GstQuery * query);
 193 static GstFlowReturn gst_disparity_chain_right (GstPad * pad,
 194     GstObject * parent, GstBuffer * buffer);
 195 static GstFlowReturn gst_disparity_chain_left (GstPad * pad, GstObject * parent,
 196     GstBuffer * buffer);
 197
 198 static void initialise_disparity (GstDisparity * fs, int width, int height,
 199     int nchannels);
 200 static int initialise_sbm (GstDisparity * filter);
 201 static int run_sbm_iteration (GstDisparity * filter);
 202 static int run_sgbm_iteration (GstDisparity * filter);
 203
 204 /* initialize the disparity's class */
 205 static void
 206 gst_disparity_class_init (GstDisparityClass * klass)
 207 {
 208   GObjectClass *gobject_class;
 209   GstElementClass *element_class = GST_ELEMENT_CLASS (klass);
 210
 211   gobject_class = (GObjectClass *) klass;
 212
 213   gobject_class->finalize = gst_disparity_finalize;
 214   gobject_class->set_property = gst_disparity_set_property;
 215   gobject_class->get_property = gst_disparity_get_property;
 216
 217
 218   g_object_class_install_property (gobject_class, PROP_METHOD,
 219       g_param_spec_enum ("method",
 220           "Stereo matching method to use",
 221           "Stereo matching method to use",
 222           GST_TYPE_DISPARITY_METHOD, DEFAULT_METHOD,
 223           (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
 224
 225   element_class->change_state = gst_disparity_change_state;
 226
 227   gst_element_class_set_static_metadata (element_class,
 228       "Stereo image disparity (depth) map calculation",
 229       "Filter/Effect/Video",
 230       "Calculates the stereo disparity map from two (sequences of) rectified and aligned stereo images",
 231       "Miguel Casas-Sanchez <miguelecasassanchez@gmail.com>");
 232
 233   gst_element_class_add_static_pad_template (element_class, &src_factory);
 234   gst_element_class_add_static_pad_template (element_class, &sink_factory);
 235 }
 236
 237 /* initialize the new element
 238  * instantiate pads and add them to element
 239  * set pad callback functions
 240  * initialize instance structure
 241  */
 242 static void
 243 gst_disparity_init (GstDisparity * filter)
 244 {
 245   filter->sinkpad_left =
 246       gst_pad_new_from_static_template (&sink_factory, "sink_left");
 247   gst_pad_set_event_function (filter->sinkpad_left,
 248       GST_DEBUG_FUNCPTR (gst_disparity_handle_sink_event));
 249   gst_pad_set_query_function (filter->sinkpad_left,
 250       GST_DEBUG_FUNCPTR (gst_disparity_handle_query));
 251   gst_pad_set_chain_function (filter->sinkpad_left,
 252       GST_DEBUG_FUNCPTR (gst_disparity_chain_left));
 253   GST_PAD_SET_PROXY_CAPS (filter->sinkpad_left);
 254   gst_element_add_pad (GST_ELEMENT (filter), filter->sinkpad_left);
 255
 256   filter->sinkpad_right =
 257       gst_pad_new_from_static_template (&sink_factory, "sink_right");
 258   gst_pad_set_event_function (filter->sinkpad_right,
 259       GST_DEBUG_FUNCPTR (gst_disparity_handle_sink_event));
 260   gst_pad_set_query_function (filter->sinkpad_right,
 261       GST_DEBUG_FUNCPTR (gst_disparity_handle_query));
 262   gst_pad_set_chain_function (filter->sinkpad_right,
 263       GST_DEBUG_FUNCPTR (gst_disparity_chain_right));
 264   GST_PAD_SET_PROXY_CAPS (filter->sinkpad_right);
 265   gst_element_add_pad (GST_ELEMENT (filter), filter->sinkpad_right);
 266
 267   filter->srcpad = gst_pad_new_from_static_template (&src_factory, "src");
 268   gst_pad_use_fixed_caps (filter->srcpad);
 269   gst_element_add_pad (GST_ELEMENT (filter), filter->srcpad);
 270
 271   g_mutex_init (&filter->lock);
 272   g_cond_init (&filter->cond);
 273
 274   filter->method = DEFAULT_METHOD;
 275 }
 276
 277 static void
 278 gst_disparity_set_property (GObject * object, guint prop_id,
 279     const GValue * value, GParamSpec * pspec)
 280 {
 281   GstDisparity *filter = GST_DISPARITY (object);
 282   switch (prop_id) {
 283     case PROP_METHOD:
 284       filter->method = g_value_get_enum (value);
 285       break;
 286     default:
 287       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
 288       break;
 289   }
 290 }
 291
 292 static void
 293 gst_disparity_get_property (GObject * object, guint prop_id,
 294     GValue * value, GParamSpec * pspec)
 295 {
 296   GstDisparity *filter = GST_DISPARITY (object);
 297
 298   switch (prop_id) {
 299     case PROP_METHOD:
 300       g_value_set_enum (value, filter->method);
 301       break;
 302     default:
 303       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
 304       break;
 305   }
 306 }
 307
 308 /* GstElement vmethod implementations */
 309 static GstStateChangeReturn
 310 gst_disparity_change_state (GstElement * element, GstStateChange transition)
 311 {
 312   GstStateChangeReturn ret = GST_STATE_CHANGE_SUCCESS;
 313   GstDisparity *fs = GST_DISPARITY (element);
 314   switch (transition) {
 315     case GST_STATE_CHANGE_PAUSED_TO_READY:
 316       g_mutex_lock (&fs->lock);
 317       fs->flushing = true;
 318       g_cond_signal (&fs->cond);
 319       g_mutex_unlock (&fs->lock);
 320       break;
 321     case GST_STATE_CHANGE_READY_TO_PAUSED:
 322       g_mutex_lock (&fs->lock);
 323       fs->flushing = false;
 324       g_mutex_unlock (&fs->lock);
 325       break;
 326     default:
 327       break;
 328   }
 329
 330   ret =
 331       GST_ELEMENT_CLASS (gst_disparity_parent_class)->change_state (element,
 332       transition);
 333
 334   switch (transition) {
 335     case GST_STATE_CHANGE_PAUSED_TO_READY:
 336       g_mutex_lock (&fs->lock);
 337       fs->flushing = true;
 338       g_cond_signal (&fs->cond);
 339       g_mutex_unlock (&fs->lock);
 340       break;
 341     case GST_STATE_CHANGE_READY_TO_PAUSED:
 342       g_mutex_lock (&fs->lock);
 343       fs->flushing = false;
 344       g_mutex_unlock (&fs->lock);
 345       break;
 346     default:
 347       break;
 348   }
 349   return ret;
 350 }
 351
 352 static gboolean
 353 gst_disparity_handle_sink_event (GstPad * pad,
 354     GstObject * parent, GstEvent * event)
 355 {
 356   gboolean ret = TRUE;
 357   GstDisparity *fs = GST_DISPARITY (parent);
 358
 359   switch (GST_EVENT_TYPE (event)) {
 360     case GST_EVENT_CAPS:
 361     {
 362       GstCaps *caps;
 363       GstVideoInfo info;
 364       gst_event_parse_caps (event, &caps);
 365
 366       /* Critical section since both pads handle event sinking simultaneously */
 367       g_mutex_lock (&fs->lock);
 368       gst_video_info_from_caps (&info, caps);
 369
 370       GST_INFO_OBJECT (pad, " Negotiating caps via event %" GST_PTR_FORMAT,
 371           caps);
 372       if (!gst_pad_has_current_caps (fs->srcpad)) {
 373         /* Init image info (widht, height, etc) and all OpenCV matrices */
 374         initialise_disparity (fs, info.width, info.height,
 375             info.finfo->n_components);
 376
 377         /* Initialise and keep the caps. Force them on src pad */
 378         fs->caps = gst_video_info_to_caps (&info);
 379         gst_pad_set_caps (fs->srcpad, fs->caps);
 380
 381       } else if (!gst_caps_is_equal (fs->caps, caps)) {
 382         ret = FALSE;
 383       }
 384       g_mutex_unlock (&fs->lock);
 385
 386       GST_INFO_OBJECT (pad,
 387           " Negotiated caps (result %d) via event: %" GST_PTR_FORMAT, ret,
 388           caps);
 389       break;
 390     }
 391     default:
 392       ret = gst_pad_event_default (pad, parent, event);
 393       break;
 394   }
 395   return ret;
 396 }
 397
 398 static gboolean
 399 gst_disparity_handle_query (GstPad * pad, GstObject * parent, GstQuery * query)
 400 {
 401   GstDisparity *fs = GST_DISPARITY (parent);
 402   gboolean ret = TRUE;
 403   GstCaps *template_caps;
 404   GstCaps *current_caps;
 405
 406   switch (GST_QUERY_TYPE (query)) {
 407     case GST_QUERY_CAPS:
 408       g_mutex_lock (&fs->lock);
 409       current_caps = gst_pad_get_current_caps (fs->srcpad);
 410       if (current_caps == NULL) {
 411         template_caps = gst_pad_get_pad_template_caps (pad);
 412         gst_query_set_caps_result (query, template_caps);
 413         gst_caps_unref (template_caps);
 414       } else {
 415         gst_query_set_caps_result (query, current_caps);
 416         gst_caps_unref (current_caps);
 417       }
 418       g_mutex_unlock (&fs->lock);
 419       ret = TRUE;
 420       break;
 421     case GST_QUERY_ALLOCATION:
 422       if (pad == fs->sinkpad_right)
 423         ret = gst_pad_peer_query (fs->srcpad, query);
 424       else
 425         ret = FALSE;
 426       break;
 427     default:
 428       ret = gst_pad_query_default (pad, parent, query);
 429       break;
 430   }
 431   return ret;
 432 }
 433
 434 static void
 435 gst_disparity_finalize (GObject * object)
 436 {
 437   GstDisparity *filter;
 438
 439   filter = GST_DISPARITY (object);
 440
 441   filter->cvRGB_right.release ();
 442   filter->cvRGB_left.release ();
 443   filter->cvGray_right.release ();
 444   filter->cvGray_left.release ();
 445   filter->cvGray_depth_map1.release ();
 446   filter->cvGray_depth_map2.release ();
 447   filter->cvGray_depth_map1_2.release ();
 448   filter->img_right_as_cvMat_gray.release ();
 449   filter->img_left_as_cvMat_gray.release ();
 450   filter->depth_map_as_cvMat.release ();
 451   filter->sbm.release ();
 452   filter->sgbm.release ();
 453
 454   gst_caps_replace (&filter->caps, NULL);
 455
 456   g_cond_clear (&filter->cond);
 457   g_mutex_clear (&filter->lock);
 458   G_OBJECT_CLASS (gst_disparity_parent_class)->finalize (object);
 459 }
 460
 461
 462
 463 static GstFlowReturn
 464 gst_disparity_chain_left (GstPad * pad, GstObject * parent, GstBuffer * buffer)
 465 {
 466   GstDisparity *fs;
 467   GstMapInfo info;
 468
 469   fs = GST_DISPARITY (parent);
 470   GST_DEBUG_OBJECT (pad, "processing frame from left");
 471   g_mutex_lock (&fs->lock);
 472   if (fs->flushing) {
 473     g_mutex_unlock (&fs->lock);
 474     return GST_FLOW_FLUSHING;
 475   }
 476   if (fs->buffer_left) {
 477     GST_DEBUG_OBJECT (pad, " right is busy, wait and hold");
 478     g_cond_wait (&fs->cond, &fs->lock);
 479     GST_DEBUG_OBJECT (pad, " right is free, continuing");
 480     if (fs->flushing) {
 481       g_mutex_unlock (&fs->lock);
 482       return GST_FLOW_FLUSHING;
 483     }
 484   }
 485   fs->buffer_left = buffer;
 486
 487   if (!gst_buffer_map (buffer, &info, (GstMapFlags) GST_MAP_READWRITE)) {
 488     return GST_FLOW_ERROR;
 489   }
 490   fs->cvRGB_left.data = (unsigned char *) info.data;
 491   fs->cvRGB_left.datastart = (unsigned char *) info.data;
 492
 493   GST_DEBUG_OBJECT (pad, "signalled right");
 494   g_cond_signal (&fs->cond);
 495   g_mutex_unlock (&fs->lock);
 496
 497   return GST_FLOW_OK;
 498 }
 499
 500 static GstFlowReturn
 501 gst_disparity_chain_right (GstPad * pad, GstObject * parent, GstBuffer * buffer)
 502 {
 503   GstDisparity *fs;
 504   GstMapInfo info;
 505   GstFlowReturn ret;
 506
 507   fs = GST_DISPARITY (parent);
 508   GST_DEBUG_OBJECT (pad, "processing frame from right");
 509   g_mutex_lock (&fs->lock);
 510   if (fs->flushing) {
 511     g_mutex_unlock (&fs->lock);
 512     return GST_FLOW_FLUSHING;
 513   }
 514   if (fs->buffer_left == NULL) {
 515     GST_DEBUG_OBJECT (pad, " left has not provided another frame yet, waiting");
 516     g_cond_wait (&fs->cond, &fs->lock);
 517     GST_DEBUG_OBJECT (pad, " left has just provided a frame, continuing");
 518     if (fs->flushing) {
 519       g_mutex_unlock (&fs->lock);
 520       return GST_FLOW_FLUSHING;
 521     }
 522   }
 523   if (!gst_buffer_map (buffer, &info, (GstMapFlags) GST_MAP_READWRITE)) {
 524     g_mutex_unlock (&fs->lock);
 525     return GST_FLOW_ERROR;
 526   }
 527
 528   fs->cvRGB_right.data = (unsigned char *) info.data;
 529   fs->cvRGB_right.datastart = (unsigned char *) info.data;
 530
 531   /* Here do the business */
 532   GST_INFO_OBJECT (pad,
 533       "comparing frames, %dB (%dx%d) %d channels", (int) info.size,
 534       fs->width, fs->height, fs->actualChannels);
 535
 536   /* Stereo corresponding using semi-global block matching. According to OpenCV:
 537      "" The class implements modified H. Hirschmuller algorithm HH08 . The main
 538      differences between the implemented algorithm and the original one are:
 539
 540      - by default the algorithm is single-pass, i.e. instead of 8 directions we
 541      only consider 5. Set fullDP=true to run the full variant of the algorithm
 542      (which could consume a lot of memory)
 543      - the algorithm matches blocks, not individual pixels (though, by setting
 544      SADWindowSize=1 the blocks are reduced to single pixels)
 545      - mutual information cost function is not implemented. Instead, we use a
 546      simpler Birchfield-Tomasi sub-pixel metric from BT96 , though the color
 547      images are supported as well.
 548      - we include some pre- and post- processing steps from K. Konolige
 549      algorithm FindStereoCorrespondenceBM , such as pre-filtering
 550      ( CV_STEREO_BM_XSOBEL type) and post-filtering (uniqueness check, quadratic
 551      interpolation and speckle filtering) ""
 552    */
 553   if (METHOD_SGBM == fs->method) {
 554     cvtColor (fs->cvRGB_left, fs->cvGray_left, COLOR_RGB2GRAY);
 555     cvtColor (fs->cvRGB_right, fs->cvGray_right, COLOR_RGB2GRAY);
 556     run_sgbm_iteration (fs);
 557     normalize (fs->cvGray_depth_map1, fs->cvGray_depth_map2, 0, 255,
 558         NORM_MINMAX, fs->cvGray_depth_map2.type ());
 559     cvtColor (fs->cvGray_depth_map2, fs->cvRGB_right, COLOR_GRAY2RGB);
 560   }
 561   /* Algorithm 1 is the OpenCV Stereo Block Matching, similar to the one
 562      developed by Kurt Konolige [A] and that works by using small Sum-of-absolute-
 563      differences (SAD) window. See the comments on top of the file.
 564    */
 565   else if (METHOD_SBM == fs->method) {
 566     cvtColor (fs->cvRGB_left, fs->cvGray_left, COLOR_RGB2GRAY);
 567     cvtColor (fs->cvRGB_right, fs->cvGray_right, COLOR_RGB2GRAY);
 568     run_sbm_iteration (fs);
 569     normalize (fs->cvGray_depth_map1, fs->cvGray_depth_map2, 0, 255,
 570         NORM_MINMAX, fs->cvGray_depth_map2.type ());
 571     cvtColor (fs->cvGray_depth_map2, fs->cvRGB_right, COLOR_GRAY2RGB);
 572   }
 573
 574
 575   GST_DEBUG_OBJECT (pad, " right has finished");
 576   gst_buffer_unmap (fs->buffer_left, &info);
 577   gst_buffer_unref (fs->buffer_left);
 578   fs->buffer_left = NULL;
 579   g_cond_signal (&fs->cond);
 580   g_mutex_unlock (&fs->lock);
 581
 582   ret = gst_pad_push (fs->srcpad, buffer);
 583   return ret;
 584 }
 585
 586
 587
 588
 589
 590 /* entry point to initialize the plug-in
 591  * initialize the plug-in itself
 592  * register the element factories and other features
 593  */
 594 gboolean
 595 gst_disparity_plugin_init (GstPlugin * disparity)
 596 {
 597   GST_DEBUG_CATEGORY_INIT (gst_disparity_debug, "disparity", 0,
 598       "Stereo image disparity (depth) map calculation");
 599   return gst_element_register (disparity, "disparity", GST_RANK_NONE,
 600       GST_TYPE_DISPARITY);
 601 }
 602
 603
 604 static void
 605 initialise_disparity (GstDisparity * fs, int width, int height, int nchannels)
 606 {
 607   int cv_type = CV_8UC3;
 608   fs->width = width;
 609   fs->height = height;
 610   fs->actualChannels = nchannels;
 611
 612   fs->imgSize = Size (fs->width, fs->height);
 613   if (fs->actualChannels == 1) {
 614     cv_type = CV_8UC1;
 615   } else if (fs->actualChannels == 2) {
 616     cv_type = CV_8UC2;
 617   }
 618
 619   fs->cvRGB_right.create (fs->imgSize, cv_type);
 620   fs->cvRGB_left.create (fs->imgSize, cv_type);
 621   fs->cvGray_right.create (fs->imgSize, CV_8UC1);
 622   fs->cvGray_left.create (fs->imgSize, CV_8UC1);
 623
 624   fs->cvGray_depth_map1.create (fs->imgSize, CV_16SC1);
 625   fs->cvGray_depth_map2.create (fs->imgSize, CV_8UC1);
 626   fs->cvGray_depth_map1_2.create (fs->imgSize, CV_16SC1);
 627
 628   /* Stereo Block Matching methods */
 629   initialise_sbm (fs);
 630 }
 631
 632 int
 633 initialise_sbm (GstDisparity * filter)
 634 {
 635   filter->img_right_as_cvMat_gray = Mat (filter->cvGray_right);
 636   filter->img_left_as_cvMat_gray = Mat (filter->cvGray_left);
 637   filter->depth_map_as_cvMat = Mat (filter->cvGray_depth_map1);
 638
 639   filter->sbm = StereoBM::create ();
 640   filter->sgbm = StereoSGBM::create (1, 64, 3);
 641
 642   filter->sbm->setBlockSize (9);
 643   filter->sbm->setNumDisparities (32);
 644   filter->sbm->setPreFilterSize (9);
 645   filter->sbm->setPreFilterCap (32);
 646   filter->sbm->setMinDisparity (0);
 647   filter->sbm->setTextureThreshold (0);
 648   filter->sbm->setUniquenessRatio (0);
 649   filter->sbm->setSpeckleWindowSize (0);
 650   filter->sbm->setSpeckleRange (0);
 651   filter->sbm->setDisp12MaxDiff (0);
 652
 653   filter->sgbm->setMinDisparity (1);
 654   filter->sgbm->setNumDisparities (64);
 655   filter->sgbm->setBlockSize (3);
 656   filter->sgbm->setP1 (200);
 657   filter->sgbm->setP2 (255);
 658   filter->sgbm->setDisp12MaxDiff (0);
 659   filter->sgbm->setPreFilterCap (0);
 660   filter->sgbm->setUniquenessRatio (0);
 661   filter->sgbm->setSpeckleWindowSize (0);
 662   filter->sgbm->setSpeckleRange (0);
 663   filter->sgbm->setMode (StereoSGBM::MODE_HH);
 664
 665   return (0);
 666 }
 667
 668 int
 669 run_sbm_iteration (GstDisparity * filter)
 670 {
 671   ((StereoBM *) filter->sbm)->compute (filter->img_left_as_cvMat_gray,
 672       filter->img_right_as_cvMat_gray, filter->depth_map_as_cvMat);
 673
 674   return (0);
 675 }
 676
 677 int
 678 run_sgbm_iteration (GstDisparity * filter)
 679 {
 680   ((StereoSGBM *) filter->sgbm)->compute (filter->img_left_as_cvMat_gray,
 681       filter->img_right_as_cvMat_gray, filter->depth_map_as_cvMat);
 682
 683   return (0);
 684 }