diff --git a/cv-node/CMakeLists.txt b/cv-node/CMakeLists.txt
index a847840be1bea2086e6f5855a7808b55a4129f80..9bc20a4f37da65253d8d07db333cd19181b9ce7e 100644
--- a/cv-node/CMakeLists.txt
+++ b/cv-node/CMakeLists.txt
@@ -8,6 +8,7 @@ include(CTest)
 enable_testing()
 
 set(THREADS_PREFER_PTHREAD_FLAG ON)
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/")
 
 if (WIN32)
 find_package( glog REQUIRED )
@@ -15,6 +16,8 @@ include_directories(${glog_DIR})
 endif (WIN32)
 find_package( OpenCV REQUIRED )
 find_package( Threads REQUIRED )
+find_package( LibSGM )
+find_package( CUDA )
 #find_package(PkgConfig)
 #pkg_check_modules(GTKMM gtkmm-3.0)
 
@@ -63,10 +66,16 @@ set(CVNODESRC
 	src/disparity.cpp
 	src/algorithms/rtcensus.cpp
 	src/algorithms/opencv_sgbm.cpp
+	src/algorithms/opencv_bm.cpp
 )
 
+if (LIBSGM_FOUND)
+	list(APPEND CVNODESRC "src/algorithms/fixstars_sgm.cpp")
+	message("Cuda libraries at ${CUDA_LIBRARIES}")
+endif (LIBSGM_FOUND)
+
 add_executable(cv-node ${CVNODESRC})
 target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include)
-target_link_libraries(cv-node Threads::Threads ${OpenCV_LIBS} glog)
+target_link_libraries(cv-node Threads::Threads ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} glog)
 
 
diff --git a/cv-node/cmake/FindLibSGM.cmake b/cv-node/cmake/FindLibSGM.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..ac950c25175cc43df740f5c3ee76e09bce4545e3
--- /dev/null
+++ b/cv-node/cmake/FindLibSGM.cmake
@@ -0,0 +1,33 @@
+###############################################################################
+# Find LibSGM
+#
+# This sets the following variables:
+# LIBSGM_FOUND - True if LIBSGM was found.
+# LIBSGM_INCLUDE_DIRS - Directories containing the LIBSGM include files.
+# LIBSGM_LIBRARY - Libraries needed to use LIBSGM.
+
+# Find lib
+set(LIBSGM_FOUND FALSE CACHE BOOL "" FORCE)
+find_library(LIBSGM_LIBRARY
+    NAMES sgm libsgm
+    PATH_SUFFIXES lib/
+)
+
+# Find include
+find_path(LIBSGM_INCLUDE_DIRS
+    NAMES libsgm.h
+    PATH_SUFFIXES include/
+)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(LibSGM DEFAULT_MSG LIBSGM_LIBRARY LIBSGM_INCLUDE_DIRS)
+
+message(STATUS "(LIBSGM_FOUND : ${LIBSGM_FOUND} include: ${LIBSGM_INCLUDE_DIRS}, lib: ${LIBSGM_LIBRARY})")
+
+mark_as_advanced(LIBSGM_FOUND)
+
+if(LIBSGM_FOUND)
+    set(LIBSGM_FOUND TRUE CACHE BOOL "" FORCE)
+    set(LIBSGM_LIBRARIES ${LIBSGM_LIBRARY})
+    message(STATUS "LibSGM found ( include: ${LIBSGM_INCLUDE_DIRS}, lib: ${LIBSGM_LIBRARY})")
+endif()
diff --git a/cv-node/config/config.json b/cv-node/config/config.json
index 4e4a982b7e8fa0c09f71f07b651fc2ddfd97fecb..9a768db7d2359bb35a592c0704a3d7cf54dcae6c 100644
--- a/cv-node/config/config.json
+++ b/cv-node/config/config.json
@@ -34,7 +34,10 @@
 		"minimum": 0,
 		"maximum": 208,
 		"tau": 0.0,
-		"gamma": 0.0
+		"gamma": 0.0,
+		"window_size": 5,
+		"sigma": 1.5,
+		"lambda": 8000.0
 	}
 }
 
diff --git a/cv-node/include/ftl/algorithms/fixstars_sgm.hpp b/cv-node/include/ftl/algorithms/fixstars_sgm.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..1afa4dc46ef1233d84a32fecc00689ca8f75c41c
--- /dev/null
+++ b/cv-node/include/ftl/algorithms/fixstars_sgm.hpp
@@ -0,0 +1,26 @@
+#ifndef _FTL_ALGORITHMS_FIXSTARS_SGM_HPP_
+#define _FTL_ALGORITHMS_FIXSTARS_SGM_HPP_
+
+#include <opencv2/core.hpp>
+#include <opencv2/opencv.hpp>
+#include <libsgm.h>
+#include <ftl/disparity.hpp>
+
+namespace ftl {
+namespace algorithms {
+class FixstarsSGM : public ftl::Disparity {
+	public:
+	FixstarsSGM(nlohmann::json &config);
+	
+	void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
+
+	static inline Disparity *create(nlohmann::json &config) { return new FixstarsSGM(config); }
+	
+	private:
+	sgm::StereoSGM *ssgm_;
+};
+};
+};
+
+#endif // _FTL_ALGORITHMS_FIXSTARS_SGM_HPP_
+
diff --git a/cv-node/include/ftl/algorithms/opencv_bm.hpp b/cv-node/include/ftl/algorithms/opencv_bm.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..defa3a105b2b11cb54bd5f8903ad3d7b61258610
--- /dev/null
+++ b/cv-node/include/ftl/algorithms/opencv_bm.hpp
@@ -0,0 +1,29 @@
+#ifndef _FTL_ALGORITHMS_OPENCV_BM_HPP_
+#define _FTL_ALGORITHMS_OPENCV_BM_HPP_
+
+#include <opencv2/core.hpp>
+#include <opencv2/opencv.hpp>
+#include "opencv2/ximgproc.hpp"
+#include <opencv2/calib3d.hpp>
+#include <ftl/disparity.hpp>
+
+namespace ftl {
+namespace algorithms {
+class OpenCVBM : public ftl::Disparity {
+	public:
+	OpenCVBM(nlohmann::json &config);
+	
+	void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
+
+	static inline Disparity *create(nlohmann::json &config) { return new OpenCVBM(config); }
+	
+	private:
+	cv::Ptr<cv::StereoBM> left_matcher_;
+	cv::Ptr<cv::StereoMatcher> right_matcher_;
+	cv::Ptr<cv::ximgproc::DisparityWLSFilter> wls_filter_;
+};
+};
+};
+
+#endif // _FTL_ALGORITHMS_OPENCV_SGBM_HPP_
+
diff --git a/cv-node/src/algorithms/fixstars_sgm.cpp b/cv-node/src/algorithms/fixstars_sgm.cpp
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..216cf8e939b9c1c65b08e547992935874e9fabb3 100644
--- a/cv-node/src/algorithms/fixstars_sgm.cpp
+++ b/cv-node/src/algorithms/fixstars_sgm.cpp
@@ -0,0 +1,28 @@
+#include <ftl/algorithms/fixstars_sgm.hpp>
+
+using ftl::algorithms::FixstarsSGM;
+using namespace cv;
+
+static ftl::Disparity::Register fixstarssgm("libsgm", FixstarsSGM::create);
+
+FixstarsSGM::FixstarsSGM(nlohmann::json &config) : Disparity(config) {
+	ssgm_ = nullptr;
+}
+
+void FixstarsSGM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
+	Mat left_disp;
+	Mat right_disp;
+	
+	if (!ssgm_) {
+		ssgm_ = new sgm::StereoSGM(l.cols, l.rows, max_disp_, 8, 8, sgm::EXECUTE_INOUT_HOST2HOST);
+	}
+	
+	if (disp.cols != l.cols || disp.rows != l.rows) {
+		disp = Mat(cv::Size(l.cols, l.rows), CV_8UC1);
+	}
+	
+	ssgm_->execute(l.data, r.data, disp.data);
+}
+
+
+
diff --git a/cv-node/src/algorithms/opencv_bm.cpp b/cv-node/src/algorithms/opencv_bm.cpp
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..acb9793e601f82edcfaac3e9e5b6088a49e25936 100644
--- a/cv-node/src/algorithms/opencv_bm.cpp
+++ b/cv-node/src/algorithms/opencv_bm.cpp
@@ -0,0 +1,31 @@
+#include <ftl/algorithms/opencv_bm.hpp>
+
+using ftl::algorithms::OpenCVBM;
+using namespace cv::ximgproc;
+using namespace cv;
+
+static ftl::Disparity::Register opencvbm("bm", OpenCVBM::create);
+
+OpenCVBM::OpenCVBM(nlohmann::json &config) : Disparity(config) {
+	int wsize = config.value("windows_size", 5);
+	float sigma = config.value("sigma", 1.5);
+	float lambda = config.value("lambda", 8000.0);
+
+	left_matcher_  = StereoBM::create(max_disp_,wsize);
+    wls_filter_ = createDisparityWLSFilter(left_matcher_);
+    right_matcher_ = createRightMatcher(left_matcher_);
+    
+    wls_filter_->setLambda(lambda);
+    wls_filter_->setSigmaColor(sigma);
+}
+
+void OpenCVBM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
+	Mat left_disp;
+	Mat right_disp;
+	left_matcher_-> compute(l, r,left_disp);
+    right_matcher_->compute(r,l, right_disp);
+    wls_filter_->filter(left_disp,l,disp,right_disp);
+}
+
+
+
diff --git a/cv-node/src/algorithms/opencv_sgbm.cpp b/cv-node/src/algorithms/opencv_sgbm.cpp
index 102add505827af879a2fe8aac48545d9b359b0a0..9853e4f01f995dfd886a9cfde89d777ed4cd19a5 100644
--- a/cv-node/src/algorithms/opencv_sgbm.cpp
+++ b/cv-node/src/algorithms/opencv_sgbm.cpp
@@ -7,10 +7,9 @@ using namespace cv;
 static ftl::Disparity::Register opencvsgbm("sgbm", OpenCVSGBM::create);
 
 OpenCVSGBM::OpenCVSGBM(nlohmann::json &config) : Disparity(config) {
-	int wsize = 5;
-	float sigma = 1.5;
-	float lambda = 8000.0;
-	
+	int wsize = config.value("windows_size", 5);
+	float sigma = config.value("sigma", 1.5);
+	float lambda = config.value("lambda", 8000.0);
 
 	left_matcher_  = StereoSGBM::create(min_disp_,max_disp_,wsize);
             left_matcher_->setP1(24*wsize*wsize);
diff --git a/cv-node/src/main.cpp b/cv-node/src/main.cpp
index 4fbe18c48921d5305c088c7da14a4694d5796c2d..c4c804dc391bfff7cb8daf1e388343a3128051ca 100644
--- a/cv-node/src/main.cpp
+++ b/cv-node/src/main.cpp
@@ -23,12 +23,9 @@ using std::map;
 using cv::Mat;
 using json = nlohmann::json;
 using std::ifstream;
-
 using namespace cv;
-//using namespace cv::ximgproc;
-
-static string OPTION_calibration_config = FTL_CONFIG_ROOT "/calibration.xml";
 
+// Store loaded configuration
 static json config;
 
 /**
@@ -42,6 +39,9 @@ static bool findConfiguration(const string &file) {
 	return true;
 }
 
+/**
+ * Generate a map from command line option to value
+ */
 map<string,string> read_options(char ***argv, int *argc) {
 	map<string,string> opts;
 	
@@ -63,6 +63,10 @@ map<string,string> read_options(char ***argv, int *argc) {
 	return opts;
 }
 
+/**
+ * Put command line options into json config. If config element does not exist
+ * or is of a different type then report an error.
+ */
 static void process_options(const map<string,string> &opts) {
 	for (auto opt : opts) {
 		if (opt.first == "config") continue;
@@ -95,7 +99,6 @@ int main(int argc, char **argv) {
 	// TODO Initiate the network
 	
 	LocalSource *lsrc;
-	
 	if (argc) {
 		// Load video file
 		lsrc = new LocalSource(argv[0], config["source"]);
@@ -114,16 +117,9 @@ int main(int argc, char **argv) {
 	Calibrate calibrate(lsrc, config["calibration"]);
 	if (config["calibrate"]) calibrate.recalibrate();
 	if (!calibrate.isCalibrated()) LOG(WARNING) << "Cameras are not calibrated!";
-
-	/*Ptr<StereoBM> left_matcher = StereoBM::create(max_disp,wsize);
-	//left_matcher->setPreFilterCap(63);
-            wls_filter = createDisparityWLSFilter(left_matcher);
-            Ptr<StereoMatcher> right_matcher = createRightMatcher(left_matcher);*/
     
     // Choose and configure disparity algorithm
     auto disparity = Disparity::create(config["disparity"]);
-    
-    //double fact = 4.051863857;
 	
 	Mat l, r, filtered_disp;