gstreamer/ext/opencv/gstdisparity.cpp

793 lines
29 KiB
C++
Raw Normal View History

/*
* 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. 807814.
* [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: 742.
*
* <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);
}