/* * GStreamer * Copyright (C) 2013 Miguel Casas-Sanchez * * 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. * * * Example launch line * |[ * 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 * ]| * */ #ifdef HAVE_CONFIG_H #include #endif #include #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 "); 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); }