gstreamer/ext/opencv/gstdisparity.cpp
2013-08-23 12:01:07 +02:00

792 lines
29 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

/*
* 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_unref (filter->caps);
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);
}