opencv: Add disparity-map calculation element

https://bugzilla.gnome.org/show_bug.cgi?id=704760

4 files changed, 914 insertions, 0 deletions

diff --git a/ext/opencv/Makefile.am b/ext/opencv/Makefile.am
index b9f1d072b..3ba1c3493 100644
--- a/ext/opencv/Makefile.am
+++ b/ext/opencv/Makefile.am
@@ -23,6 +23,7 @@ libgstopencv_la_SOURCES = gstopencv.c \
 			gstretinex.c \
 			gstsegmentation.cpp \
 			gstgrabcut.cpp \
+			gstdisparity.cpp \
 			motioncells_wrapper.cpp \
 			MotionCells.cpp
 
@@ -65,6 +66,7 @@ noinst_HEADERS = gstopencvvideofilter.h gstopencvutils.h \
 		gstretinex.h \
 		gstsegmentation.h \
 		gstgrabcut.h \
+		gstdisparity.h \
 		gstmotioncells.h \
 		motioncells_wrapper.h \
 		MotionCells.h
diff --git a/ext/opencv/gstdisparity.cpp b/ext/opencv/gstdisparity.cpp
new file mode 100644
index 000000000..8c94d06cd
--- /dev/null
+++ b/ext/opencv/gstdisparity.cpp
@@ -0,0 +1,792 @@
+ /*
+  * GStreamer
+  * Copyright (C) 2013 Miguel Casas-Sanchez <miguelecasassanchez@gmail.com>
+  *
+  * Permission is hereby granted, free of charge, to any person obtaining a
+  * copy of this software and associated documentation files (the "Software"),
+  * to deal in the Software without restriction, including without limitation
+  * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+  * and/or sell copies of the Software, and to permit persons to whom the
+  * Software is furnished to do so, subject to the following conditions:
+  *
+  * The above copyright notice and this permission notice shall be included in
+  * all copies or substantial portions of the Software.
+  *
+  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+  * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+  * DEALINGS IN THE SOFTWARE.
+  *
+  * Alternatively, the contents of this file may be used under the
+  * GNU Lesser General Public License Version 2.1 (the "LGPL"), in
+  * which case the following provisions apply instead of the ones
+  * mentioned above:
+  *
+  * This library is free software; you can redistribute it and/or
+  * modify it under the terms of the GNU Library General Public
+  * License as published by the Free Software Foundation; either
+  * version 2 of the License, or (at your option) any later version.
+  *
+  * This library is distributed in the hope that it will be useful,
+  * but WITHOUT ANY WARRANTY; without even the implied warranty of
+  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  * Library General Public License for more details.
+  *
+  * You should have received a copy of the GNU Library General Public
+  * License along with this library; if not, write to the
+  * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
+  * Boston, MA 02110-1301, USA.
+  */
+/*
+ * SECTION:element-disparity
+ *
+ * This element computes a disparity map from two stereo images, meaning each one coming from a
+ * different camera, both looking at the same scene and relatively close to each other - more on
+ * this below. The disparity map is a proxy of the depth of a scene as seen from the camera.
+ *
+ * Assumptions: Input images are stereo, rectified and aligned. If these conditions are not met,
+ * results can be poor. Both cameras should be looking parallel to maximize the overlapping
+ * stereo area, and should not have objects too close or too far. The algorithms implemented here
+ * run prefiltering stages to normalize brightness between the inputs, and to maximize texture.
+ *
+ * Note that in general is hard to find correspondences between soft textures, for instance a
+ * block of gloss blue colour. The output is a gray image with values close to white meaning 
+ * closer to the cameras and darker far away. Black means that the pixels were not matched
+ * correctly (not found). The resulting depth map can be transformed into real world coordinates
+ * by means of OpenCV function (reprojectImageTo3D) but for this the camera matrixes need to
+ * be fully known.
+ *
+ * Algorithm 1 is the OpenCV Stereo Block Matching, similar to the one developed by Kurt Konolige
+ * [A] and that works by using small Sum-of-absolute-differenc (SAD) windows to find matching 
+ * points between the left and right rectified images. This algorithm finds only strongly matching
+ * points between both images, this means normally strong textures. In soft textures, such as a
+ * single coloured wall (as opposed to, f.i. a hairy rug), not all pixels might have correspondence.
+ *
+ * Algorithm 2 is the Semi Global Matching (SGM) algorithm [B] which models the scene structure
+ * with a point-wise matching cost and an associated smoothness term. The energy minimization 
+ * is then computed in a multitude of 1D lines. For each point, the disparity corresponding to 
+ * the minimum aggregated cost is selected. In [B] the author proposes to use 8 or 16 different 
+ * independent paths. The SGM approach works well near depth discontinuities, but produces less 
+ * accurate results. Despite its relatively large memory footprint, this method is very fast and 
+ * potentially robust to complicated textured regions.
+ *
+ * Algorithm 3 is the OpenCV implementation of a modification of the variational stereo 
+ * correspondence algorithm, described in [C].
+ *
+ * Algorithm 4 is the Graph Cut stereo vision algorithm (GC) introduced in [D]; it is a global 
+ * stereo vision method. It calculates depth discontinuities by minimizing an energy function
+ * combingin a point-wise matching cost and a smoothness term. The energy function is passed 
+ * to graph and Graph Cut is used to find a lowest-energy cut. GC is computationally intensive due
+ * to its global nature and uses loads of memory, but it can deal with textureless regions and
+ * reflections better than other methods.
+ * Graphcut based technique is CPU intensive hence smaller framesizes are desired.
+ *
+ * Some test images can be found here: http://vision.stanford.edu/~birch/p2p/
+ *
+ * [A] K. Konolige. Small vision system. hardware and implementation. In Proc. International 
+ * Symposium on Robotics Research, pages 111--116, Hayama, Japan, 1997.
+ * [B] H. Hirschmüller, “Accurate and efficient stereo processing by semi-global matching and 
+ * mutual information,” in Proceedings of the IEEE Conference on Computer Vision and Pattern 
+ * Recognition, 2005, pp. 807–814.
+ * [C] S. Kosov, T. Thormaehlen, H.-P. Seidel "Accurate Real-Time Disparity Estimation with
+ * Variational Methods" Proceedings of the 5th International Symposium on Visual Computing, 
+ * Vegas, USA
+ * [D] Scharstein, D. & Szeliski, R. (2001). A taxonomy and evaluation of dense two-frame stereo 
+ * correspondence algorithms, International Journal of Computer Vision 47: 7–42.
+ *
+ * <refsect2>
+ * <title>Example launch line</title>
+ * |[
+ * gst-launch-1.0       videotestsrc ! video/x-raw,width=320,height=240 ! disp0.sink_right      videotestsrc ! video/x-raw,width=320,height=240 ! disp0.sink_left      disparity name=disp0 ! videoconvert ! ximagesink
+ * ]|
+ * Another example, with two png files representing a classical stereo matching,
+ * downloadable from http://vision.middlebury.edu/stereo/submit/tsukuba/im4.png and 
+ * im3.png. Note here they are downloaded in ~ (home).
+ * |[
+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
+ * ]|
+ * Yet another example with two cameras, which should be the same model, aligned etc.
+ * |[
+ 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
+ * ]|
+ * </refsect2>
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <gst/gst.h>
+#include <gst/video/video.h>
+#include <opencv2/contrib/contrib.hpp>
+#include "gstdisparity.h"
+
+GST_DEBUG_CATEGORY_STATIC (gst_disparity_debug);
+#define GST_CAT_DEFAULT gst_disparity_debug
+
+/* Filter signals and args */
+enum
+{
+  /* FILL ME */
+  LAST_SIGNAL
+};
+
+enum
+{
+  PROP_0,
+  PROP_METHOD,
+};
+
+typedef enum
+{
+  METHOD_SBM,
+  METHOD_SGBM,
+  METHOD_VAR,
+  METHOD_GC
+} GstDisparityMethod;
+
+#define DEFAULT_METHOD METHOD_SGBM
+
+#define GST_TYPE_DISPARITY_METHOD (gst_disparity_method_get_type ())
+static GType
+gst_disparity_method_get_type (void)
+{
+  static GType etype = 0;
+  if (etype == 0) {
+    static const GEnumValue values[] = {
+      {METHOD_SBM, "Global block matching algorithm", "sbm"},
+      {METHOD_SGBM, "Semi-global block matching algorithm", "sgbm"},
+      {METHOD_VAR, "Variational matching algorithm", "svar"},
+      {METHOD_GC, "Graph-cut based matching algorithm", "sgc"},
+      {0, NULL, NULL},
+    };
+    etype = g_enum_register_static ("GstDisparityMethod", values);
+  }
+  return etype;
+}
+
+/* the capabilities of the inputs and outputs.
+ */
+static GstStaticPadTemplate sink_factory = GST_STATIC_PAD_TEMPLATE ("sink",
+    GST_PAD_SINK,
+    GST_PAD_ALWAYS,
+    GST_STATIC_CAPS (GST_VIDEO_CAPS_MAKE ("RGB"))
+    );
+
+static GstStaticPadTemplate src_factory = GST_STATIC_PAD_TEMPLATE ("src",
+    GST_PAD_SRC,
+    GST_PAD_ALWAYS,
+    GST_STATIC_CAPS (GST_VIDEO_CAPS_MAKE ("RGB"))
+    );
+
+G_DEFINE_TYPE (GstDisparity, gst_disparity, GST_TYPE_ELEMENT);
+
+static void gst_disparity_finalize (GObject * object);
+static void gst_disparity_set_property (GObject * object, guint prop_id,
+    const GValue * value, GParamSpec * pspec);
+static void gst_disparity_get_property (GObject * object, guint prop_id,
+    GValue * value, GParamSpec * pspec);
+static GstStateChangeReturn gst_disparity_change_state (GstElement * element,
+    GstStateChange transition);
+
+static gboolean gst_disparity_handle_sink_event (GstPad * pad,
+    GstObject * parent, GstEvent * event);
+static gboolean gst_disparity_handle_query (GstPad * pad,
+    GstObject * parent, GstQuery * query);
+static GstFlowReturn gst_disparity_chain_right (GstPad * pad,
+    GstObject * parent, GstBuffer * buffer);
+static GstFlowReturn gst_disparity_chain_left (GstPad * pad, GstObject * parent,
+    GstBuffer * buffer);
+static void gst_disparity_release_all_pointers (GstDisparity * filter);
+
+static void initialise_disparity (GstDisparity * fs, int width, int height,
+    int nchannels);
+static int initialise_sbm (GstDisparity * filter);
+static int run_sbm_iteration (GstDisparity * filter);
+static int run_sgbm_iteration (GstDisparity * filter);
+static int run_svar_iteration (GstDisparity * filter);
+static int run_sgc_iteration (GstDisparity * filter);
+static int finalise_sbm (GstDisparity * filter);
+
+/* initialize the disparity's class */
+static void
+gst_disparity_class_init (GstDisparityClass * klass)
+{
+  GObjectClass *gobject_class;
+  GstElementClass *element_class = GST_ELEMENT_CLASS (klass);
+
+  gobject_class = (GObjectClass *) klass;
+
+  gobject_class->finalize = gst_disparity_finalize;
+  gobject_class->set_property = gst_disparity_set_property;
+  gobject_class->get_property = gst_disparity_get_property;
+
+
+  g_object_class_install_property (gobject_class, PROP_METHOD,
+      g_param_spec_enum ("method",
+          "Stereo matching method to use",
+          "Stereo matching method to use",
+          GST_TYPE_DISPARITY_METHOD, DEFAULT_METHOD,
+          (GParamFlags) (G_PARAM_READWRITE | G_PARAM_STATIC_STRINGS)));
+
+  element_class->change_state = gst_disparity_change_state;
+
+  gst_element_class_set_static_metadata (element_class,
+      "Stereo image disparity (depth) map calculation",
+      "Filter/Effect/Video",
+      "Calculates the stereo disparity map from two (sequences of) rectified and aligned stereo images",
+      "Miguel Casas-Sanchez <miguelecasassanchez@gmail.com>");
+
+  gst_element_class_add_pad_template (element_class,
+      gst_static_pad_template_get (&src_factory));
+  gst_element_class_add_pad_template (element_class,
+      gst_static_pad_template_get (&sink_factory));
+}
+
+/* initialize the new element
+ * instantiate pads and add them to element
+ * set pad callback functions
+ * initialize instance structure
+ */
+static void
+gst_disparity_init (GstDisparity * filter)
+{
+  filter->sinkpad_left =
+      gst_pad_new_from_static_template (&sink_factory, "sink_left");
+  gst_pad_set_event_function (filter->sinkpad_left,
+      GST_DEBUG_FUNCPTR (gst_disparity_handle_sink_event));
+  gst_pad_set_query_function (filter->sinkpad_left,
+      GST_DEBUG_FUNCPTR (gst_disparity_handle_query));
+  gst_pad_set_chain_function (filter->sinkpad_left,
+      GST_DEBUG_FUNCPTR (gst_disparity_chain_left));
+  GST_PAD_SET_PROXY_CAPS (filter->sinkpad_left);
+  gst_element_add_pad (GST_ELEMENT (filter), filter->sinkpad_left);
+
+  filter->sinkpad_right =
+      gst_pad_new_from_static_template (&sink_factory, "sink_right");
+  gst_pad_set_event_function (filter->sinkpad_right,
+      GST_DEBUG_FUNCPTR (gst_disparity_handle_sink_event));
+  gst_pad_set_query_function (filter->sinkpad_right,
+      GST_DEBUG_FUNCPTR (gst_disparity_handle_query));
+  gst_pad_set_chain_function (filter->sinkpad_right,
+      GST_DEBUG_FUNCPTR (gst_disparity_chain_right));
+  GST_PAD_SET_PROXY_CAPS (filter->sinkpad_right);
+  gst_element_add_pad (GST_ELEMENT (filter), filter->sinkpad_right);
+
+  filter->srcpad = gst_pad_new_from_static_template (&src_factory, "src");
+  gst_pad_use_fixed_caps (filter->srcpad);
+  gst_element_add_pad (GST_ELEMENT (filter), filter->srcpad);
+
+  filter->lock = g_mutex_new ();
+  filter->cond = g_cond_new ();
+
+  filter->method = DEFAULT_METHOD;
+}
+
+static void
+gst_disparity_set_property (GObject * object, guint prop_id,
+    const GValue * value, GParamSpec * pspec)
+{
+  GstDisparity *filter = GST_DISPARITY (object);
+  switch (prop_id) {
+    case PROP_METHOD:
+      filter->method = g_value_get_enum (value);
+      break;
+    default:
+      G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
+      break;
+  }
+}
+
+static void
+gst_disparity_get_property (GObject * object, guint prop_id,
+    GValue * value, GParamSpec * pspec)
+{
+  GstDisparity *filter = GST_DISPARITY (object);
+
+  switch (prop_id) {
+    case PROP_METHOD:
+      g_value_set_enum (value, filter->method);
+      break;
+    default:
+      G_OBJECT_WARN_INVALID_PROPERTY_ID (object, prop_id, pspec);
+      break;
+  }
+}
+
+/* GstElement vmethod implementations */
+static GstStateChangeReturn
+gst_disparity_change_state (GstElement * element, GstStateChange transition)
+{
+  GstStateChangeReturn ret = GST_STATE_CHANGE_SUCCESS;
+  GstDisparity *fs = GST_DISPARITY (element);
+  switch (transition) {
+    case GST_STATE_CHANGE_PAUSED_TO_READY:
+      g_mutex_lock (fs->lock);
+      fs->flushing = true;
+      g_cond_signal (fs->cond);
+      g_mutex_unlock (fs->lock);
+      break;
+    case GST_STATE_CHANGE_READY_TO_PAUSED:
+      g_mutex_lock (fs->lock);
+      fs->flushing = false;
+      g_mutex_unlock (fs->lock);
+      break;
+    default:
+      break;
+  }
+
+  ret =
+      GST_ELEMENT_CLASS (gst_disparity_parent_class)->change_state (element,
+      transition);
+
+  switch (transition) {
+    case GST_STATE_CHANGE_PAUSED_TO_READY:
+      g_mutex_lock (fs->lock);
+      fs->flushing = true;
+      g_cond_signal (fs->cond);
+      g_mutex_unlock (fs->lock);
+      break;
+    case GST_STATE_CHANGE_READY_TO_PAUSED:
+      g_mutex_lock (fs->lock);
+      fs->flushing = false;
+      g_mutex_unlock (fs->lock);
+      break;
+    default:
+      break;
+  }
+  return ret;
+}
+
+static gboolean
+gst_disparity_handle_sink_event (GstPad * pad,
+    GstObject * parent, GstEvent * event)
+{
+  gboolean ret = TRUE;
+  GstDisparity *fs = GST_DISPARITY (parent);
+
+  switch (GST_EVENT_TYPE (event)) {
+    case GST_EVENT_CAPS:
+    {
+      GstCaps *caps;
+      GstVideoInfo info;
+      gst_event_parse_caps (event, &caps);
+
+      /* Critical section since both pads handle event sinking simultaneously */
+      g_mutex_lock (fs->lock);
+      gst_video_info_from_caps (&info, caps);
+
+      GST_INFO_OBJECT (pad, " Negotiating caps via event %" GST_PTR_FORMAT,
+          caps);
+      if (!gst_pad_has_current_caps (fs->srcpad)) {
+        /* Init image info (widht, height, etc) and all OpenCV matrices */
+        initialise_disparity (fs, info.width, info.height,
+            info.finfo->n_components);
+
+        /* Initialise and keep the caps. Force them on src pad */
+        fs->caps = gst_video_info_to_caps (&info);
+        gst_pad_set_caps (fs->srcpad, fs->caps);
+
+      } else if (!gst_caps_is_equal (fs->caps, caps)) {
+        ret = FALSE;
+      }
+      g_mutex_unlock (fs->lock);
+
+      GST_INFO_OBJECT (pad,
+          " Negotiated caps (result %d) via event: %" GST_PTR_FORMAT, ret,
+          caps);
+      break;
+    }
+    default:
+      ret = gst_pad_event_default (pad, parent, event);
+      break;
+  }
+  return ret;
+}
+
+static gboolean
+gst_disparity_handle_query (GstPad * pad, GstObject * parent, GstQuery * query)
+{
+  GstDisparity *fs = GST_DISPARITY (parent);
+  gboolean ret = TRUE;
+  GstCaps *template_caps;
+  GstCaps *current_caps;
+
+  switch (GST_QUERY_TYPE (query)) {
+    case GST_QUERY_CAPS:
+      g_mutex_lock (fs->lock);
+      if (!gst_pad_has_current_caps (fs->srcpad)) {
+        template_caps = gst_pad_get_pad_template_caps (pad);
+        gst_query_set_caps_result (query, template_caps);
+        gst_caps_unref (template_caps);
+      } else {
+        current_caps = gst_pad_get_current_caps (fs->srcpad);
+        gst_query_set_caps_result (query, current_caps);
+        gst_caps_unref (current_caps);
+      }
+      g_mutex_unlock (fs->lock);
+      ret = TRUE;
+      break;
+    case GST_QUERY_ALLOCATION:
+      if (pad == fs->sinkpad_right)
+        ret = gst_pad_peer_query (fs->srcpad, query);
+      else
+        ret = FALSE;
+      break;
+    default:
+      ret = gst_pad_query_default (pad, parent, query);
+      break;
+  }
+  return ret;
+}
+
+static void
+gst_disparity_release_all_pointers (GstDisparity * filter)
+{
+  cvReleaseImage (&filter->cvRGB_right);
+  cvReleaseImage (&filter->cvRGB_left);
+  cvReleaseImage (&filter->cvGray_depth_map1);
+  cvReleaseImage (&filter->cvGray_right);
+  cvReleaseImage (&filter->cvGray_left);
+  cvReleaseImage (&filter->cvGray_depth_map2);
+  cvReleaseImage (&filter->cvGray_depth_map1_2);
+
+  finalise_sbm (filter);
+}
+
+static void
+gst_disparity_finalize (GObject * object)
+{
+  GstDisparity *filter;
+  filter = GST_DISPARITY (object);
+  gst_disparity_release_all_pointers (filter);
+
+  gst_caps_unref (filter->caps);
+  filter->caps = NULL;
+
+  g_cond_free (filter->cond);
+  g_mutex_free (filter->lock);
+  G_OBJECT_CLASS (gst_disparity_parent_class)->finalize (object);
+}
+
+
+
+static GstFlowReturn
+gst_disparity_chain_left (GstPad * pad, GstObject * parent, GstBuffer * buffer)
+{
+  GstDisparity *fs;
+  GstMapInfo info;
+
+  fs = GST_DISPARITY (parent);
+  GST_DEBUG_OBJECT (pad, "processing frame from left");
+  g_mutex_lock (fs->lock);
+  if (fs->flushing) {
+    g_mutex_unlock (fs->lock);
+    return GST_FLOW_FLUSHING;
+  }
+  if (fs->buffer_left) {
+    GST_DEBUG_OBJECT (pad, " right is busy, wait and hold");
+    g_cond_wait (fs->cond, fs->lock);
+    GST_DEBUG_OBJECT (pad, " right is free, continuing");
+    if (fs->flushing) {
+      g_mutex_unlock (fs->lock);
+      return GST_FLOW_FLUSHING;
+    }
+  }
+  fs->buffer_left = buffer;
+
+  if (!gst_buffer_map (buffer, &info, (GstMapFlags) GST_MAP_READWRITE)) {
+    return GST_FLOW_ERROR;
+  }
+  if (fs->cvRGB_left)
+    fs->cvRGB_left->imageData = (char *) info.data;
+
+  GST_DEBUG_OBJECT (pad, "signalled right");
+  g_cond_signal (fs->cond);
+  g_mutex_unlock (fs->lock);
+
+  return GST_FLOW_OK;
+}
+
+static GstFlowReturn
+gst_disparity_chain_right (GstPad * pad, GstObject * parent, GstBuffer * buffer)
+{
+  GstDisparity *fs;
+  GstMapInfo info;
+  GstFlowReturn ret;
+
+  fs = GST_DISPARITY (parent);
+  GST_DEBUG_OBJECT (pad, "processing frame from right");
+  g_mutex_lock (fs->lock);
+  if (fs->flushing) {
+    g_mutex_unlock (fs->lock);
+    return GST_FLOW_FLUSHING;
+  }
+  if (fs->buffer_left == NULL) {
+    GST_DEBUG_OBJECT (pad, " left has not provided another frame yet, waiting");
+    g_cond_wait (fs->cond, fs->lock);
+    GST_DEBUG_OBJECT (pad, " left has just provided a frame, continuing");
+    if (fs->flushing) {
+      g_mutex_unlock (fs->lock);
+      return GST_FLOW_FLUSHING;
+    }
+  }
+  if (!gst_buffer_map (buffer, &info, (GstMapFlags) GST_MAP_READWRITE)) {
+    g_mutex_unlock (fs->lock);
+    return GST_FLOW_ERROR;
+  }
+  if (fs->cvRGB_right)
+    fs->cvRGB_right->imageData = (char *) info.data;
+
+
+  /* Here do the business */
+  GST_INFO_OBJECT (pad,
+      "comparing frames, %dB (%dx%d) %d channels", (int) info.size,
+      fs->width, fs->height, fs->actualChannels);
+
+  /* Stereo corresponding using semi-global block matching. According to OpenCV:
+     "" The class implements modified H. Hirschmuller algorithm HH08 . The main 
+     differences between the implemented algorithm and the original one are:
+
+     - by default the algorithm is single-pass, i.e. instead of 8 directions we 
+     only consider 5. Set fullDP=true to run the full variant of the algorithm
+     (which could consume a lot of memory)
+     - the algorithm matches blocks, not individual pixels (though, by setting
+     SADWindowSize=1 the blocks are reduced to single pixels)
+     - mutual information cost function is not implemented. Instead, we use a
+     simpler Birchfield-Tomasi sub-pixel metric from BT96 , though the color 
+     images are supported as well.
+     - we include some pre- and post- processing steps from K. Konolige 
+     algorithm FindStereoCorrespondenceBM , such as pre-filtering 
+     ( CV_STEREO_BM_XSOBEL type) and post-filtering (uniqueness check, quadratic
+     interpolation and speckle filtering) ""
+   */
+  if (METHOD_SGBM == fs->method) {
+    cvCvtColor (fs->cvRGB_left, fs->cvGray_left, CV_RGB2GRAY);
+    cvCvtColor (fs->cvRGB_right, fs->cvGray_right, CV_RGB2GRAY);
+    run_sgbm_iteration (fs);
+    cvNormalize (fs->cvGray_depth_map1, fs->cvGray_depth_map2, 0, 255,
+        CV_MINMAX, NULL);
+    cvCvtColor (fs->cvGray_depth_map2, fs->cvRGB_right, CV_GRAY2RGB);
+  }
+  /* Algorithm 1 is the OpenCV Stereo Block Matching, similar to the one 
+     developed by Kurt Konolige [A] and that works by using small Sum-of-absolute-
+     differences (SAD) window. See the comments on top of the file.
+   */
+  else if (METHOD_SBM == fs->method) {
+    cvCvtColor (fs->cvRGB_left, fs->cvGray_left, CV_RGB2GRAY);
+    cvCvtColor (fs->cvRGB_right, fs->cvGray_right, CV_RGB2GRAY);
+    run_sbm_iteration (fs);
+    cvNormalize (fs->cvGray_depth_map1, fs->cvGray_depth_map2, 0, 255,
+        CV_MINMAX, NULL);
+    cvCvtColor (fs->cvGray_depth_map2, fs->cvRGB_right, CV_GRAY2RGB);
+  }
+  /* The class implements the modified S. G. Kosov algorithm
+     See the comments on top of the file.
+   */
+  else if (METHOD_VAR == fs->method) {
+    cvCvtColor (fs->cvRGB_left, fs->cvGray_left, CV_RGB2GRAY);
+    cvCvtColor (fs->cvRGB_right, fs->cvGray_right, CV_RGB2GRAY);
+    run_svar_iteration (fs);
+    cvCvtColor (fs->cvGray_depth_map2, fs->cvRGB_right, CV_GRAY2RGB);
+  }
+  /* The Graph Cut stereo vision algorithm (GC) introduced in [D] is a global 
+     stereo vision method. It calculates depth discontinuities by minimizing an 
+     energy function combingin a point-wise matching cost and a smoothness term. 
+     See the comments on top of the file.
+   */
+  else if (METHOD_GC == fs->method) {
+    cvCvtColor (fs->cvRGB_left, fs->cvGray_left, CV_RGB2GRAY);
+    cvCvtColor (fs->cvRGB_right, fs->cvGray_right, CV_RGB2GRAY);
+    run_sgc_iteration (fs);
+    cvConvertScale (fs->cvGray_depth_map1, fs->cvGray_depth_map2, -16.0, 0.0);
+    cvCvtColor (fs->cvGray_depth_map2, fs->cvRGB_right, CV_GRAY2RGB);
+  }
+
+
+  GST_DEBUG_OBJECT (pad, " right has finished");
+  gst_buffer_unmap (fs->buffer_left, &info);
+  gst_buffer_unref (fs->buffer_left);
+  fs->buffer_left = NULL;
+  g_cond_signal (fs->cond);
+  g_mutex_unlock (fs->lock);
+
+  ret = gst_pad_push (fs->srcpad, buffer);
+  return ret;
+}
+
+
+
+
+
+/* entry point to initialize the plug-in
+ * initialize the plug-in itself
+ * register the element factories and other features
+ */
+gboolean
+gst_disparity_plugin_init (GstPlugin * disparity)
+{
+  GST_DEBUG_CATEGORY_INIT (gst_disparity_debug, "disparity", 0,
+      "Stereo image disparity (depth) map calculation");
+  return gst_element_register (disparity, "disparity", GST_RANK_NONE,
+      GST_TYPE_DISPARITY);
+}
+
+
+static void
+initialise_disparity (GstDisparity * fs, int width, int height, int nchannels)
+{
+  fs->width = width;
+  fs->height = height;
+  fs->actualChannels = nchannels;
+
+  fs->imgSize = cvSize (fs->width, fs->height);
+  if (fs->cvRGB_right)
+    gst_disparity_release_all_pointers (fs);
+
+  fs->cvRGB_right = cvCreateImageHeader (fs->imgSize, IPL_DEPTH_8U,
+      fs->actualChannels);
+  fs->cvRGB_left = cvCreateImageHeader (fs->imgSize, IPL_DEPTH_8U,
+      fs->actualChannels);
+  fs->cvGray_right = cvCreateImage (fs->imgSize, IPL_DEPTH_8U, 1);
+  fs->cvGray_left = cvCreateImage (fs->imgSize, IPL_DEPTH_8U, 1);
+
+  fs->cvGray_depth_map1 = cvCreateImage (fs->imgSize, IPL_DEPTH_16S, 1);
+  fs->cvGray_depth_map2 = cvCreateImage (fs->imgSize, IPL_DEPTH_8U, 1);
+  fs->cvGray_depth_map1_2 = cvCreateImage (fs->imgSize, IPL_DEPTH_16S, 1);
+
+  /* Stereo Block Matching methods */
+  if ((NULL != fs->cvRGB_right) && (NULL != fs->cvRGB_left)
+      && (NULL != fs->cvGray_depth_map2))
+    initialise_sbm (fs);
+}
+
+int
+initialise_sbm (GstDisparity * filter)
+{
+  filter->img_right_as_cvMat_rgb =
+      (void *) new cv::Mat (filter->cvRGB_right, false);
+  filter->img_left_as_cvMat_rgb =
+      (void *) new cv::Mat (filter->cvRGB_left, false);
+  filter->img_right_as_cvMat_gray =
+      (void *) new cv::Mat (filter->cvGray_right, false);
+  filter->img_left_as_cvMat_gray =
+      (void *) new cv::Mat (filter->cvGray_left, false);
+  filter->depth_map_as_cvMat =
+      (void *) new cv::Mat (filter->cvGray_depth_map1, false);
+  filter->depth_map_as_cvMat2 =
+      (void *) new cv::Mat (filter->cvGray_depth_map2, false);
+
+  filter->sbm = (void *) new cv::StereoBM ();
+  filter->sgbm = (void *) new cv::StereoSGBM ();
+  filter->svar = (void *) new cv::StereoVar ();
+  /* SGC has only two parameters on creation: NumerOfDisparities and MaxIters */
+  filter->sgc = cvCreateStereoGCState (16, 2);
+
+  ((cv::StereoBM *) filter->sbm)->state->SADWindowSize = 9;
+  ((cv::StereoBM *) filter->sbm)->state->numberOfDisparities = 32;
+  ((cv::StereoBM *) filter->sbm)->state->preFilterSize = 9;
+  ((cv::StereoBM *) filter->sbm)->state->preFilterCap = 32;
+  ((cv::StereoBM *) filter->sbm)->state->minDisparity = 0;
+  ((cv::StereoBM *) filter->sbm)->state->textureThreshold = 0;
+  ((cv::StereoBM *) filter->sbm)->state->uniquenessRatio = 0;
+  ((cv::StereoBM *) filter->sbm)->state->speckleWindowSize = 0;
+  ((cv::StereoBM *) filter->sbm)->state->speckleRange = 0;
+  ((cv::StereoBM *) filter->sbm)->state->disp12MaxDiff = 0;
+
+  ((cv::StereoSGBM *) filter->sgbm)->minDisparity = 1;
+  ((cv::StereoSGBM *) filter->sgbm)->numberOfDisparities = 64;
+  ((cv::StereoSGBM *) filter->sgbm)->SADWindowSize = 3;
+  ((cv::StereoSGBM *) filter->sgbm)->P1 = 200;;
+  ((cv::StereoSGBM *) filter->sgbm)->P2 = 255;
+  ((cv::StereoSGBM *) filter->sgbm)->disp12MaxDiff = 0;
+  ((cv::StereoSGBM *) filter->sgbm)->preFilterCap = 0;
+  ((cv::StereoSGBM *) filter->sgbm)->uniquenessRatio = 0;
+  ((cv::StereoSGBM *) filter->sgbm)->speckleWindowSize = 0;
+  ((cv::StereoSGBM *) filter->sgbm)->speckleRange = 0;
+  ((cv::StereoSGBM *) filter->sgbm)->fullDP = true;
+
+  /* From Opencv samples/cpp/stereo_match.cpp */
+  ((cv::StereoVar *) filter->svar)->levels = 3;
+  ((cv::StereoVar *) filter->svar)->pyrScale = 0.5;
+  ((cv::StereoVar *) filter->svar)->nIt = 25;
+  ((cv::StereoVar *) filter->svar)->minDisp = -64;
+  ((cv::StereoVar *) filter->svar)->maxDisp = 0;
+  ((cv::StereoVar *) filter->svar)->poly_n = 3;
+  ((cv::StereoVar *) filter->svar)->poly_sigma = 0.0;
+  ((cv::StereoVar *) filter->svar)->fi = 15.0f;
+  ((cv::StereoVar *) filter->svar)->lambda = 0.03f;
+  ((cv::StereoVar *) filter->svar)->penalization =
+      cv::StereoVar::PENALIZATION_TICHONOV;
+  ((cv::StereoVar *) filter->svar)->cycle = cv::StereoVar::CYCLE_V;
+  ((cv::StereoVar *) filter->svar)->flags = cv::StereoVar::USE_SMART_ID |
+      cv::StereoVar::USE_AUTO_PARAMS |
+      cv::StereoVar::USE_INITIAL_DISPARITY |
+      cv::StereoVar::USE_MEDIAN_FILTERING;
+
+  filter->sgc->Ithreshold = 5;
+  filter->sgc->interactionRadius = 1;
+  filter->sgc->occlusionCost = 10000;
+  filter->sgc->minDisparity = 0;
+  filter->sgc->numberOfDisparities = 16;        /* Coming from constructor too */
+  filter->sgc->maxIters = 1;    /* Coming from constructor too */
+
+  return (0);
+}
+
+int
+run_sbm_iteration (GstDisparity * filter)
+{
+  (*((cv::StereoBM *) filter->
+          sbm)) (*((cv::Mat *) filter->img_left_as_cvMat_gray),
+      *((cv::Mat *) filter->img_right_as_cvMat_gray),
+      *((cv::Mat *) filter->depth_map_as_cvMat));
+  return (0);
+}
+
+int
+run_sgbm_iteration (GstDisparity * filter)
+{
+  (*((cv::StereoSGBM *) filter->
+          sgbm)) (*((cv::Mat *) filter->img_left_as_cvMat_gray),
+      *((cv::Mat *) filter->img_right_as_cvMat_gray),
+      *((cv::Mat *) filter->depth_map_as_cvMat));
+  return (0);
+}
+
+int
+run_svar_iteration (GstDisparity * filter)
+{
+  (*((cv::StereoVar *) filter->
+          svar)) (*((cv::Mat *) filter->img_left_as_cvMat_gray),
+      *((cv::Mat *) filter->img_right_as_cvMat_gray),
+      *((cv::Mat *) filter->depth_map_as_cvMat2));
+  return (0);
+}
+
+int
+run_sgc_iteration (GstDisparity * filter)
+{
+  cvFindStereoCorrespondenceGC (filter->cvGray_left,
+      filter->cvGray_right, filter->cvGray_depth_map1,
+      filter->cvGray_depth_map1_2, filter->sgc, 0);
+  return (0);
+}
+
+int
+finalise_sbm (GstDisparity * filter)
+{
+  delete (cv::Mat *) filter->img_left_as_cvMat_rgb;
+  delete (cv::Mat *) filter->img_right_as_cvMat_rgb;
+  delete (cv::Mat *) filter->depth_map_as_cvMat;
+  delete (cv::Mat *) filter->depth_map_as_cvMat2;
+  delete (cv::Mat *) filter->img_left_as_cvMat_gray;
+  delete (cv::Mat *) filter->img_right_as_cvMat_gray;
+  delete (cv::StereoBM *) filter->sbm;
+  delete (cv::StereoSGBM *) filter->sgbm;
+  delete (cv::StereoVar *) filter->svar;
+  return (0);
+}
diff --git a/ext/opencv/gstdisparity.h b/ext/opencv/gstdisparity.h
new file mode 100644
index 000000000..f11454dad
--- /dev/null
+++ b/ext/opencv/gstdisparity.h
@@ -0,0 +1,116 @@
+/*
+ * GStreamer
+ * Copyright (C) 2013 Miguel Casas-Sanchez <miguelecasassanchez@gmail.com>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ *
+ * Alternatively, the contents of this file may be used under the
+ * GNU Lesser General Public License Version 2.1 (the "LGPL"), in
+ * which case the following provisions apply instead of the ones
+ * mentioned above:
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Library General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Library General Public License for more details.
+ *
+ * You should have received a copy of the GNU Library General Public
+ * License along with this library; if not, write to the
+ * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
+ * Boston, MA 02110-1301, USA.
+ */
+
+#ifndef __GST_DISPARITY_H__
+#define __GST_DISPARITY_H__
+
+#include <gst/gst.h>
+#include <cv.h>
+#include <opencv2/legacy/legacy.hpp>
+
+G_BEGIN_DECLS
+/* #defines don't like whitespacey bits */
+#define GST_TYPE_DISPARITY \
+  (gst_disparity_get_type())
+#define GST_DISPARITY(obj) \
+  (G_TYPE_CHECK_INSTANCE_CAST((obj),GST_TYPE_DISPARITY,GstDisparity))
+#define GST_DISPARITY_CLASS(klass) \
+  (G_TYPE_CHECK_CLASS_CAST((klass),GST_TYPE_DISPARITY,GstDisparityClass))
+#define GST_IS_DISPARITY(obj) \
+  (G_TYPE_CHECK_INSTANCE_TYPE((obj),GST_TYPE_DISPARITY))
+#define GST_IS_DISPARITY_CLASS(klass) \
+  (G_TYPE_CHECK_CLASS_TYPE((klass),GST_TYPE_DISPARITY))
+typedef struct _GstDisparity GstDisparity;
+typedef struct _GstDisparityClass GstDisparityClass;
+
+struct _GstDisparity
+{
+  GstElement element;
+
+  GstPad *sinkpad_left, *sinkpad_right, *srcpad;
+  GstCaps *caps;
+
+  gint method;
+  gboolean display;
+
+  int width;
+  int height;
+  int actualChannels;
+
+  GstBuffer *buffer_left;
+  GMutex *lock;
+  GCond *cond;
+  gboolean flushing;
+
+  CvSize imgSize;
+  IplImage *cvRGB_right;
+  IplImage *cvRGB_left;
+  IplImage *cvGray_right;
+  IplImage *cvGray_left;
+  IplImage *cvGray_depth_map1;  /*IPL_DEPTH_16S */
+  IplImage *cvGray_depth_map2;  /*IPL_DEPTH_8U */
+  IplImage *cvGray_depth_map1_2;        /*IPL_DEPTH_16S */
+
+  void *sbm;                    /* cv::StereoBM */
+  void *sgbm;                   /* cv::StereoSGBM */
+  void *svar;                   /* cv::StereoVar */
+  CvStereoGCState *sgc;         /* This is a C implementation */
+  void *img_right_as_cvMat_rgb; /* cv::Mat */
+  void *img_left_as_cvMat_rgb;  /* cv::Mat */
+  void *depth_map_as_cvMat;     /* cv::Mat */
+  void *depth_map_as_cvMat2;    /* cv::Mat */
+  void *img_right_as_cvMat_gray;        /* cv::Mat */
+  void *img_left_as_cvMat_gray; /* cv::Mat */
+};
+
+struct _GstDisparityClass
+{
+  GstElementClass parent_class;
+};
+
+GType gst_disparity_get_type (void);
+
+gboolean gst_disparity_plugin_init (GstPlugin * disparity);
+
+G_END_DECLS
+#endif /* __GST_DISPARITY_H__ */
diff --git a/ext/opencv/gstopencv.c b/ext/opencv/gstopencv.c
index 1174621d8..31845180c 100644
--- a/ext/opencv/gstopencv.c
+++ b/ext/opencv/gstopencv.c
@@ -41,6 +41,7 @@
 #include "gstretinex.h"
 #include "gstsegmentation.h"
 #include "gstgrabcut.h"
+#include "gstdisparity.h"
 
 static gboolean
 plugin_init (GstPlugin * plugin)
@@ -99,6 +100,9 @@ plugin_init (GstPlugin * plugin)
   if (!gst_grabcut_plugin_init (plugin))
     return FALSE;
 
+  if (!gst_disparity_plugin_init (plugin))
+    return FALSE;
+
   return TRUE;
 }