mirror of
https://gitlab.freedesktop.org/gstreamer/gstreamer.git
synced 2024-09-09 05:38:58 +00:00
792 lines
29 KiB
C++
792 lines
29 KiB
C++
/*
|
||
* 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);
|
||
|
||
g_mutex_init (&filter->lock);
|
||
g_cond_init (&filter->cond);
|
||
|
||
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_replace (&filter->caps, NULL);
|
||
|
||
g_cond_clear (&filter->cond);
|
||
g_mutex_clear (&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);
|
||
}
|