subprojects/gst-plugins-bad/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_WITH_CODE (GstDisparity, gst_disparity, GST_TYPE_ELEMENT,
 180     GST_DEBUG_CATEGORY_INIT (gst_disparity_debug, "disparity", 0,
 181         "Stereo image disparity (depth) map calculation");
 182     );
 183 GST_ELEMENT_REGISTER_DEFINE (disparity, "disparity", GST_RANK_NONE,
 184     GST_TYPE_DISPARITY);
 185
 186 static void gst_disparity_finalize (GObject * object);
 187 static void gst_disparity_set_property (GObject * object, guint prop_id,
 188     const GValue * value, GParamSpec * pspec);
 189 static void gst_disparity_get_property (GObject * object, guint prop_id,
 190     GValue * value, GParamSpec * pspec);
 191 static GstStateChangeReturn gst_disparity_change_state (GstElement * element,
 192     GstStateChange transition);
 193
 194 static gboolean gst_disparity_handle_sink_event (GstPad * pad,
 195     GstObject * parent, GstEvent * event);
 196 static gboolean gst_disparity_handle_query (GstPad * pad,
 197     GstObject * parent, GstQuery * query);
 198 static GstFlowReturn gst_disparity_chain_right (GstPad * pad,
 199     GstObject * parent, GstBuffer * buffer);
 200 static GstFlowReturn gst_disparity_chain_left (GstPad * pad, GstObject * parent,
 201     GstBuffer * buffer);
 202
 203 static void initialise_disparity (GstDisparity * fs, int width, int height,
 204     int nchannels);
 205 static int initialise_sbm (GstDisparity * filter);
 206 static int run_sbm_iteration (GstDisparity * filter);
 207 static int run_sgbm_iteration (GstDisparity * filter);
 208
 209 /* initialize the disparity's class */
 210 static void
 211 gst_disparity_class_init (GstDisparityClass * klass)
 212 {
 213   GObjectClass *gobject_class;
 214   GstElementClass *element_class = GST_ELEMENT_CLASS (klass);
 215
 216   gobject_class = (GObjectClass *) klass;
 217
 218   gobject_class->finalize = gst_disparity_finalize;
 219   gobject_class->set_property = gst_disparity_set_property;
 220   gobject_class->get_property = gst_disparity_get_property;
 221
 222
 223   g_object_class_install_property (gobject_class, PROP_METHOD,
 224       g_param_spec_enum ("method",
 225           "Stereo matching method to use",
 226           "Stereo matching method to use",
 227           GST_TYPE_DISPARITY_METHOD, DEFAULT_METHOD,
 228           (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
 229
 230   element_class->change_state = gst_disparity_change_state;
 231
 232   gst_element_class_set_static_metadata (element_class,
 233       "Stereo image disparity (depth) map calculation",
 234       "Filter/Effect/Video",
 235       "Calculates the stereo disparity map from two (sequences of) rectified and aligned stereo images",
 236       "Miguel Casas-Sanchez <miguelecasassanchez@gmail.com>");
 237
 238   gst_element_class_add_static_pad_template (element_class, &src_factory);
 239   gst_element_class_add_static_pad_template (element_class, &sink_factory);
 240
 241   gst_type_mark_as_plugin_api (GST_TYPE_DISPARITY_METHOD, (GstPluginAPIFlags) 0);
 242 }
 243
 244 /* initialize the new element
 245  * instantiate pads and add them to element
 246  * set pad callback functions
 247  * initialize instance structure
 248  */
 249 static void
 250 gst_disparity_init (GstDisparity * filter)
 251 {
 252   filter->sinkpad_left =
 253       gst_pad_new_from_static_template (&sink_factory, "sink_left");
 254   gst_pad_set_event_function (filter->sinkpad_left,
 255       GST_DEBUG_FUNCPTR (gst_disparity_handle_sink_event));
 256   gst_pad_set_query_function (filter->sinkpad_left,
 257       GST_DEBUG_FUNCPTR (gst_disparity_handle_query));
 258   gst_pad_set_chain_function (filter->sinkpad_left,
 259       GST_DEBUG_FUNCPTR (gst_disparity_chain_left));
 260   GST_PAD_SET_PROXY_CAPS (filter->sinkpad_left);
 261   gst_element_add_pad (GST_ELEMENT (filter), filter->sinkpad_left);
 262
 263   filter->sinkpad_right =
 264       gst_pad_new_from_static_template (&sink_factory, "sink_right");
 265   gst_pad_set_event_function (filter->sinkpad_right,
 266       GST_DEBUG_FUNCPTR (gst_disparity_handle_sink_event));
 267   gst_pad_set_query_function (filter->sinkpad_right,
 268       GST_DEBUG_FUNCPTR (gst_disparity_handle_query));
 269   gst_pad_set_chain_function (filter->sinkpad_right,
 270       GST_DEBUG_FUNCPTR (gst_disparity_chain_right));
 271   GST_PAD_SET_PROXY_CAPS (filter->sinkpad_right);
 272   gst_element_add_pad (GST_ELEMENT (filter), filter->sinkpad_right);
 273
 274   filter->srcpad = gst_pad_new_from_static_template (&src_factory, "src");
 275   gst_pad_use_fixed_caps (filter->srcpad);
 276   gst_element_add_pad (GST_ELEMENT (filter), filter->srcpad);
 277
 278   g_mutex_init (&filter->lock);
 279   g_cond_init (&filter->cond);
 280
 281   filter->method = DEFAULT_METHOD;
 282 }
 283
 284 static void
 285 gst_disparity_set_property (GObject * object, guint prop_id,
 286     const GValue * value, GParamSpec * pspec)
 287 {
 288   GstDisparity *filter = GST_DISPARITY (object);
 289   switch (prop_id) {
 290     case PROP_METHOD:
 291       filter->method = g_value_get_enum (value);
 292       break;
 293     default:
 294       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
 295       break;
 296   }
 297 }
 298
 299 static void
 300 gst_disparity_get_property (GObject * object, guint prop_id,
 301     GValue * value, GParamSpec * pspec)
 302 {
 303   GstDisparity *filter = GST_DISPARITY (object);
 304
 305   switch (prop_id) {
 306     case PROP_METHOD:
 307       g_value_set_enum (value, filter->method);
 308       break;
 309     default:
 310       G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
 311       break;
 312   }
 313 }
 314
 315 /* GstElement vmethod implementations */
 316 static GstStateChangeReturn
 317 gst_disparity_change_state (GstElement * element, GstStateChange transition)
 318 {
 319   GstStateChangeReturn ret = GST_STATE_CHANGE_SUCCESS;
 320   GstDisparity *fs = GST_DISPARITY (element);
 321   switch (transition) {
 322     case GST_STATE_CHANGE_PAUSED_TO_READY:
 323       g_mutex_lock (&fs->lock);
 324       fs->flushing = true;
 325       g_cond_signal (&fs->cond);
 326       g_mutex_unlock (&fs->lock);
 327       break;
 328     case GST_STATE_CHANGE_READY_TO_PAUSED:
 329       g_mutex_lock (&fs->lock);
 330       fs->flushing = false;
 331       g_mutex_unlock (&fs->lock);
 332       break;
 333     default:
 334       break;
 335   }
 336
 337   ret =
 338       GST_ELEMENT_CLASS (gst_disparity_parent_class)->change_state (element,
 339       transition);
 340
 341   switch (transition) {
 342     case GST_STATE_CHANGE_PAUSED_TO_READY:
 343       g_mutex_lock (&fs->lock);
 344       fs->flushing = true;
 345       g_cond_signal (&fs->cond);
 346       g_mutex_unlock (&fs->lock);
 347       break;
 348     case GST_STATE_CHANGE_READY_TO_PAUSED:
 349       g_mutex_lock (&fs->lock);
 350       fs->flushing = false;
 351       g_mutex_unlock (&fs->lock);
 352       break;
 353     default:
 354       break;
 355   }
 356   return ret;
 357 }
 358
 359 static gboolean
 360 gst_disparity_handle_sink_event (GstPad * pad,
 361     GstObject * parent, GstEvent * event)
 362 {
 363   gboolean ret = TRUE;
 364   GstDisparity *fs = GST_DISPARITY (parent);
 365
 366   switch (GST_EVENT_TYPE (event)) {
 367     case GST_EVENT_CAPS:
 368     {
 369       GstCaps *caps;
 370       GstVideoInfo info;
 371       gst_event_parse_caps (event, &caps);
 372
 373       /* Critical section since both pads handle event sinking simultaneously */
 374       g_mutex_lock (&fs->lock);
 375       gst_video_info_from_caps (&info, caps);
 376
 377       GST_INFO_OBJECT (pad, " Negotiating caps via event %" GST_PTR_FORMAT,
 378           caps);
 379       if (!gst_pad_has_current_caps (fs->srcpad)) {
 380         /* Init image info (width, height, etc) and all OpenCV matrices */
 381         initialise_disparity (fs, info.width, info.height,
 382             info.finfo->n_components);
 383
 384         /* Initialise and keep the caps. Force them on src pad */
 385         fs->caps = gst_video_info_to_caps (&info);
 386         gst_pad_set_caps (fs->srcpad, fs->caps);
 387
 388       } else if (!gst_caps_is_equal (fs->caps, caps)) {
 389         ret = FALSE;
 390       }
 391       g_mutex_unlock (&fs->lock);
 392
 393       GST_INFO_OBJECT (pad,
 394           " Negotiated caps (result %d) via event: %" GST_PTR_FORMAT, ret,
 395           caps);
 396       break;
 397     }
 398     default:
 399       ret = gst_pad_event_default (pad, parent, event);
 400       break;
 401   }
 402   return ret;
 403 }
 404
 405 static gboolean
 406 gst_disparity_handle_query (GstPad * pad, GstObject * parent, GstQuery * query)
 407 {
 408   GstDisparity *fs = GST_DISPARITY (parent);
 409   gboolean ret = TRUE;
 410   GstCaps *template_caps;
 411   GstCaps *current_caps;
 412
 413   switch (GST_QUERY_TYPE (query)) {
 414     case GST_QUERY_CAPS:
 415       g_mutex_lock (&fs->lock);
 416       current_caps = gst_pad_get_current_caps (fs->srcpad);
 417       if (current_caps == NULL) {
 418         template_caps = gst_pad_get_pad_template_caps (pad);
 419         gst_query_set_caps_result (query, template_caps);
 420         gst_caps_unref (template_caps);
 421       } else {
 422         gst_query_set_caps_result (query, current_caps);
 423         gst_caps_unref (current_caps);
 424       }
 425       g_mutex_unlock (&fs->lock);
 426       ret = TRUE;
 427       break;
 428     case GST_QUERY_ALLOCATION:
 429       if (pad == fs->sinkpad_right)
 430         ret = gst_pad_peer_query (fs->srcpad, query);
 431       else
 432         ret = FALSE;
 433       break;
 434     default:
 435       ret = gst_pad_query_default (pad, parent, query);
 436       break;
 437   }
 438   return ret;
 439 }
 440
 441 static void
 442 gst_disparity_finalize (GObject * object)
 443 {
 444   GstDisparity *filter;
 445
 446   filter = GST_DISPARITY (object);
 447
 448   filter->cvRGB_right.release ();
 449   filter->cvRGB_left.release ();
 450   filter->cvGray_right.release ();
 451   filter->cvGray_left.release ();
 452   filter->cvGray_depth_map1.release ();
 453   filter->cvGray_depth_map2.release ();
 454   filter->cvGray_depth_map1_2.release ();
 455   filter->img_right_as_cvMat_gray.release ();
 456   filter->img_left_as_cvMat_gray.release ();
 457   filter->depth_map_as_cvMat.release ();
 458   filter->sbm.release ();
 459   filter->sgbm.release ();
 460
 461   gst_caps_replace (&filter->caps, NULL);
 462
 463   g_cond_clear (&filter->cond);
 464   g_mutex_clear (&filter->lock);
 465   G_OBJECT_CLASS (gst_disparity_parent_class)->finalize (object);
 466 }
 467
 468
 469
 470 static GstFlowReturn
 471 gst_disparity_chain_left (GstPad * pad, GstObject * parent, GstBuffer * buffer)
 472 {
 473   GstDisparity *fs;
 474   GstMapInfo info;
 475
 476   fs = GST_DISPARITY (parent);
 477   GST_DEBUG_OBJECT (pad, "processing frame from left");
 478   g_mutex_lock (&fs->lock);
 479   if (fs->flushing) {
 480     g_mutex_unlock (&fs->lock);
 481     return GST_FLOW_FLUSHING;
 482   }
 483   if (fs->buffer_left) {
 484     GST_DEBUG_OBJECT (pad, " right is busy, wait and hold");
 485     g_cond_wait (&fs->cond, &fs->lock);
 486     GST_DEBUG_OBJECT (pad, " right is free, continuing");
 487     if (fs->flushing) {
 488       g_mutex_unlock (&fs->lock);
 489       return GST_FLOW_FLUSHING;
 490     }
 491   }
 492   fs->buffer_left = buffer;
 493
 494   if (!gst_buffer_map (buffer, &info, (GstMapFlags) GST_MAP_READWRITE)) {
 495     return GST_FLOW_ERROR;
 496   }
 497   fs->cvRGB_left.data = (unsigned char *) info.data;
 498   fs->cvRGB_left.datastart = (unsigned char *) info.data;
 499
 500   GST_DEBUG_OBJECT (pad, "signalled right");
 501   g_cond_signal (&fs->cond);
 502   g_mutex_unlock (&fs->lock);
 503
 504   return GST_FLOW_OK;
 505 }
 506
 507 static GstFlowReturn
 508 gst_disparity_chain_right (GstPad * pad, GstObject * parent, GstBuffer * buffer)
 509 {
 510   GstDisparity *fs;
 511   GstMapInfo info;
 512   GstFlowReturn ret;
 513
 514   fs = GST_DISPARITY (parent);
 515   GST_DEBUG_OBJECT (pad, "processing frame from right");
 516   g_mutex_lock (&fs->lock);
 517   if (fs->flushing) {
 518     g_mutex_unlock (&fs->lock);
 519     return GST_FLOW_FLUSHING;
 520   }
 521   if (fs->buffer_left == NULL) {
 522     GST_DEBUG_OBJECT (pad, " left has not provided another frame yet, waiting");
 523     g_cond_wait (&fs->cond, &fs->lock);
 524     GST_DEBUG_OBJECT (pad, " left has just provided a frame, continuing");
 525     if (fs->flushing) {
 526       g_mutex_unlock (&fs->lock);
 527       return GST_FLOW_FLUSHING;
 528     }
 529   }
 530   if (!gst_buffer_map (buffer, &info, (GstMapFlags) GST_MAP_READWRITE)) {
 531     g_mutex_unlock (&fs->lock);
 532     return GST_FLOW_ERROR;
 533   }
 534
 535   fs->cvRGB_right.data = (unsigned char *) info.data;
 536   fs->cvRGB_right.datastart = (unsigned char *) info.data;
 537
 538   /* Here do the business */
 539   GST_INFO_OBJECT (pad,
 540       "comparing frames, %dB (%dx%d) %d channels", (int) info.size,
 541       fs->width, fs->height, fs->actualChannels);
 542
 543   /* Stereo corresponding using semi-global block matching. According to OpenCV:
 544      "" The class implements modified H. Hirschmuller algorithm HH08 . The main
 545      differences between the implemented algorithm and the original one are:
 546
 547      - by default the algorithm is single-pass, i.e. instead of 8 directions we
 548      only consider 5. Set fullDP=true to run the full variant of the algorithm
 549      (which could consume a lot of memory)
 550      - the algorithm matches blocks, not individual pixels (though, by setting
 551      SADWindowSize=1 the blocks are reduced to single pixels)
 552      - mutual information cost function is not implemented. Instead, we use a
 553      simpler Birchfield-Tomasi sub-pixel metric from BT96 , though the color
 554      images are supported as well.
 555      - we include some pre- and post- processing steps from K. Konolige
 556      algorithm FindStereoCorrespondenceBM , such as pre-filtering
 557      ( CV_STEREO_BM_XSOBEL type) and post-filtering (uniqueness check, quadratic
 558      interpolation and speckle filtering) ""
 559    */
 560   if (METHOD_SGBM == fs->method) {
 561     cvtColor (fs->cvRGB_left, fs->cvGray_left, COLOR_RGB2GRAY);
 562     cvtColor (fs->cvRGB_right, fs->cvGray_right, COLOR_RGB2GRAY);
 563     run_sgbm_iteration (fs);
 564     normalize (fs->cvGray_depth_map1, fs->cvGray_depth_map2, 0, 255,
 565         NORM_MINMAX, fs->cvGray_depth_map2.type ());
 566     cvtColor (fs->cvGray_depth_map2, fs->cvRGB_right, COLOR_GRAY2RGB);
 567   }
 568   /* Algorithm 1 is the OpenCV Stereo Block Matching, similar to the one
 569      developed by Kurt Konolige [A] and that works by using small Sum-of-absolute-
 570      differences (SAD) window. See the comments on top of the file.
 571    */
 572   else if (METHOD_SBM == fs->method) {
 573     cvtColor (fs->cvRGB_left, fs->cvGray_left, COLOR_RGB2GRAY);
 574     cvtColor (fs->cvRGB_right, fs->cvGray_right, COLOR_RGB2GRAY);
 575     run_sbm_iteration (fs);
 576     normalize (fs->cvGray_depth_map1, fs->cvGray_depth_map2, 0, 255,
 577         NORM_MINMAX, fs->cvGray_depth_map2.type ());
 578     cvtColor (fs->cvGray_depth_map2, fs->cvRGB_right, COLOR_GRAY2RGB);
 579   }
 580
 581
 582   GST_DEBUG_OBJECT (pad, " right has finished");
 583   gst_buffer_unmap (fs->buffer_left, &info);
 584   gst_buffer_unref (fs->buffer_left);
 585   fs->buffer_left = NULL;
 586   g_cond_signal (&fs->cond);
 587   g_mutex_unlock (&fs->lock);
 588
 589   ret = gst_pad_push (fs->srcpad, buffer);
 590   return ret;
 591 }
 592
 593 static void
 594 initialise_disparity (GstDisparity * fs, int width, int height, int nchannels)
 595 {
 596   int cv_type = CV_8UC3;
 597   fs->width = width;
 598   fs->height = height;
 599   fs->actualChannels = nchannels;
 600
 601   fs->imgSize = Size (fs->width, fs->height);
 602   if (fs->actualChannels == 1) {
 603     cv_type = CV_8UC1;
 604   } else if (fs->actualChannels == 2) {
 605     cv_type = CV_8UC2;
 606   }
 607
 608   fs->cvRGB_right.create (fs->imgSize, cv_type);
 609   fs->cvRGB_left.create (fs->imgSize, cv_type);
 610   fs->cvGray_right.create (fs->imgSize, CV_8UC1);
 611   fs->cvGray_left.create (fs->imgSize, CV_8UC1);
 612
 613   fs->cvGray_depth_map1.create (fs->imgSize, CV_16SC1);
 614   fs->cvGray_depth_map2.create (fs->imgSize, CV_8UC1);
 615   fs->cvGray_depth_map1_2.create (fs->imgSize, CV_16SC1);
 616
 617   /* Stereo Block Matching methods */
 618   initialise_sbm (fs);
 619 }
 620
 621 int
 622 initialise_sbm (GstDisparity * filter)
 623 {
 624   filter->img_right_as_cvMat_gray = Mat (filter->cvGray_right);
 625   filter->img_left_as_cvMat_gray = Mat (filter->cvGray_left);
 626   filter->depth_map_as_cvMat = Mat (filter->cvGray_depth_map1);
 627
 628   filter->sbm = StereoBM::create ();
 629   filter->sgbm = StereoSGBM::create (1, 64, 3);
 630
 631   filter->sbm->setBlockSize (9);
 632   filter->sbm->setNumDisparities (32);
 633   filter->sbm->setPreFilterSize (9);
 634   filter->sbm->setPreFilterCap (32);
 635   filter->sbm->setMinDisparity (0);
 636   filter->sbm->setTextureThreshold (0);
 637   filter->sbm->setUniquenessRatio (0);
 638   filter->sbm->setSpeckleWindowSize (0);
 639   filter->sbm->setSpeckleRange (0);
 640   filter->sbm->setDisp12MaxDiff (0);
 641
 642   filter->sgbm->setMinDisparity (1);
 643   filter->sgbm->setNumDisparities (64);
 644   filter->sgbm->setBlockSize (3);
 645   filter->sgbm->setP1 (200);
 646   filter->sgbm->setP2 (255);
 647   filter->sgbm->setDisp12MaxDiff (0);
 648   filter->sgbm->setPreFilterCap (0);
 649   filter->sgbm->setUniquenessRatio (0);
 650   filter->sgbm->setSpeckleWindowSize (0);
 651   filter->sgbm->setSpeckleRange (0);
 652   filter->sgbm->setMode (StereoSGBM::MODE_HH);
 653
 654   return (0);
 655 }
 656
 657 int
 658 run_sbm_iteration (GstDisparity * filter)
 659 {
 660   ((StereoBM *) filter->sbm)->compute (filter->img_left_as_cvMat_gray,
 661       filter->img_right_as_cvMat_gray, filter->depth_map_as_cvMat);
 662
 663   return (0);
 664 }
 665
 666 int
 667 run_sgbm_iteration (GstDisparity * filter)
 668 {
 669   ((StereoSGBM *) filter->sgbm)->compute (filter->img_left_as_cvMat_gray,
 670       filter->img_right_as_cvMat_gray, filter->depth_map_as_cvMat);
 671
 672   return (0);
 673 }