diff --git a/cv-node/src/calibrate.cpp b/cv-node/src/calibrate.cpp
index 5591d2ba0945c8e7576ee4f87afb065b4561c7ca..a74f999f44b123732dc923ed12236534d8fd6853 100644
--- a/cv-node/src/calibrate.cpp
+++ b/cv-node/src/calibrate.cpp
@@ -349,7 +349,7 @@ bool Calibrate::_recalibrate(vector<vector<Point2f>> *imagePoints,
     //Mat cameraMatrix, distCoeffs;
     //Size imageSize;
     int mode = CAPTURING;
-    clock_t prevTimestamp = 0;
+    double prevTimestamp = 0.0;
     const Scalar RED(0,0,255), GREEN(0,255,0);
     
     int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
@@ -405,8 +405,9 @@ bool Calibrate::_recalibrate(vector<vector<Point2f>> *imagePoints,
 		found1 = findChessboardCorners( view[0], settings_.boardSize, pointBuf[0], chessBoardFlags);
 		found2 = !local_->isStereo() || findChessboardCorners( view[1], settings_.boardSize, pointBuf[1], chessBoardFlags);
       
-        if (found1 && found2)                // If done with success,
+        if (found1 && found2 && local_->getTimestamp()-prevTimestamp > 0.5)                // If done with success,
         {
+			prevTimestamp = local_->getTimestamp();
               // improve the found corners' coordinate accuracy for chessboard
                     Mat viewGray;
                     cvtColor(view[0], viewGray, COLOR_BGR2GRAY);
@@ -431,7 +432,7 @@ bool Calibrate::_recalibrate(vector<vector<Point2f>> *imagePoints,
 
         imshow("Left", view[0]);
         if (local_->isStereo()) imshow("Right", view[1]);
-        char key = (char)waitKey(settings_.delay);
+        char key = (char)waitKey(50);
 
         if( key  == 27 )
             break;
diff --git a/cv-node/src/main.cpp b/cv-node/src/main.cpp
index 470147f972360fd0e7e5649f1f3c9a83658d38e5..fe13b1c733e78784507d58db4b52dac0d77b234c 100644
--- a/cv-node/src/main.cpp
+++ b/cv-node/src/main.cpp
@@ -3,6 +3,12 @@
 #include <ftl/synched.hpp>
 #include <ftl/calibrate.hpp>
 
+#include "opencv2/imgproc.hpp"
+#include "opencv2/imgcodecs.hpp"
+#include "opencv2/highgui.hpp"
+#include "opencv2/core/utility.hpp"
+#include "opencv2/ximgproc.hpp"
+
 #include <glog/logging.h>
 
 #include <string>
@@ -13,6 +19,9 @@ using std::string;
 using std::vector;
 using cv::Mat;
 
+using namespace cv;
+using namespace cv::ximgproc;
+
 static vector<string> OPTION_peers;
 static vector<string> OPTION_channels;
 static string OPTION_calibration_config = FTL_CONFIG_ROOT "/calibration.xml";
@@ -91,9 +100,25 @@ int main(int argc, char **argv) {
 	if (!calibrate.isCalibrated()) {
 		LOG(WARNING) << "Cameras are not calibrated!";
 	}
+
+	int max_disp = 256;
+	int wsize = 5;
+	Ptr<DisparityWLSFilter> wls_filter;
+
+	Ptr<StereoSGBM> left_matcher  = StereoSGBM::create(50,max_disp,wsize);
+            left_matcher->setP1(24*wsize*wsize);
+            left_matcher->setP2(96*wsize*wsize);
+            left_matcher->setPreFilterCap(63);
+            left_matcher->setMode(StereoSGBM::MODE_SGBM_3WAY);
+            wls_filter = createDisparityWLSFilter(left_matcher);
+            Ptr<StereoMatcher> right_matcher = createRightMatcher(left_matcher);
+	/*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);*/
 	
 	while (true) {
-		Mat l, r;
+		Mat l, r, left_disp, right_disp;
 		
 		calibrate.undistort(l,r);
 		//lsrc->get(l,r);
@@ -109,12 +134,22 @@ int main(int argc, char **argv) {
 		
 		// TODO Pose and disparity etc here...
 		
+        cvtColor(l,  l,  COLOR_BGR2GRAY);
+        cvtColor(r, r, COLOR_BGR2GRAY);
+        //matching_time = (double)getTickCount();
+        left_matcher-> compute(l, r,left_disp);
+        right_matcher->compute(r,l, right_disp);
+       // matching_time = ((double)getTickCount() - matching_time)/getTickFrequency();
+		
 		// TODO Send RGB+D data somewhere
+
+		normalize(left_disp, left_disp, 0, 255, NORM_MINMAX, CV_8U);
+		//normalize(right_disp, right_disp, 0, 255, NORM_MINMAX, CV_8U);
 		
-		cv::imshow("Left",l);
-		if (lsrc->isStereo()) cv::imshow("Right",r);
+		cv::imshow("Left",left_disp);
+		//if (lsrc->isStereo()) cv::imshow("Right",right_disp);
 		
-		if(cv::waitKey(100) == 27){
+		if(cv::waitKey(20) == 27){
             //exit if ESC is pressed
             break;
         }