--- /dev/null
+ /*
+ * 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);
+}
projects / platform / upstream / gstreamer.git / commitdiff