diff --git a/cv-node/CMakeLists.txt b/cv-node/CMakeLists.txt
index 9c8b3d96a0a6a71461fd070de6830f7554085137..a847840be1bea2086e6f5855a7808b55a4129f80 100644
--- a/cv-node/CMakeLists.txt
+++ b/cv-node/CMakeLists.txt
@@ -60,7 +60,9 @@ set(CVNODESRC
src/calibrate.cpp
src/local.cpp
src/sync.cpp
- src/rtcensus.cpp
+ src/disparity.cpp
+ src/algorithms/rtcensus.cpp
+ src/algorithms/opencv_sgbm.cpp
)
add_executable(cv-node ${CVNODESRC})
diff --git a/cv-node/include/ftl/algorithms/opencv_sgbm.hpp b/cv-node/include/ftl/algorithms/opencv_sgbm.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..641457b1b1b874a9cf4319632a15fed22d478131
--- /dev/null
+++ b/cv-node/include/ftl/algorithms/opencv_sgbm.hpp
@@ -0,0 +1,29 @@
+#ifndef _FTL_ALGORITHMS_OPENCV_SGBM_HPP_
+#define _FTL_ALGORITHMS_OPENCV_SGBM_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 OpenCVSGBM : public ftl::Disparity {
+ public:
+ OpenCVSGBM();
+
+ void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
+
+ static inline Disparity *create() { return new OpenCVSGBM(); }
+
+ private:
+ cv::Ptr<cv::StereoSGBM> 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/include/ftl/algorithms/rtcensus.hpp b/cv-node/include/ftl/algorithms/rtcensus.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..63b14d85b15cd8c38694c0b86d92f65c82dceb26
--- /dev/null
+++ b/cv-node/include/ftl/algorithms/rtcensus.hpp
@@ -0,0 +1,29 @@
+#ifndef _FTL_ALGORITHMS_RTCENSUS_HPP_
+#define _FTL_ALGORITHMS_RTCENSUS_HPP_
+
+#include <opencv2/core.hpp>
+#include <opencv2/opencv.hpp>
+#include <ftl/disparity.hpp>
+
+namespace ftl {
+namespace algorithms {
+class RTCensus : public ftl::Disparity {
+ public:
+ RTCensus();
+
+ void setGamma(float gamma) { gamma_ = gamma; }
+ void setTau(float tau) { tau_ = tau; }
+
+ void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
+
+ static inline Disparity *create() { return new RTCensus(); }
+
+ private:
+ float gamma_;
+ float tau_;
+};
+};
+};
+
+#endif // _FTL_ALGORITHMS_RTCENSUS_HPP_
+
diff --git a/cv-node/include/ftl/disparity.hpp b/cv-node/include/ftl/disparity.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a34febdffc9d7b3d67f416dae769ed3387c730f5
--- /dev/null
+++ b/cv-node/include/ftl/disparity.hpp
@@ -0,0 +1,38 @@
+#ifndef _FTL_DISPARITY_HPP_
+#define _FTL_DISPARITY_HPP_
+
+#include <opencv2/opencv.hpp>
+
+namespace ftl {
+class Disparity {
+ public:
+ Disparity();
+
+ virtual void setMinDisparity(size_t min) { min_disp_ = min; }
+ virtual void setMaxDisparity(size_t max) { max_disp_ = max; }
+
+ virtual void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp)=0;
+
+ class Register {
+ public:
+ Register(const std::string &n, std::function<Disparity*()> f) {
+ Disparity::_register(n,f);
+ };
+ };
+
+ static Disparity *create(const std::string &n);
+
+ protected:
+ static void _register(const std::string &n, std::function<Disparity*()> f);
+
+ protected:
+ size_t min_disp_;
+ size_t max_disp_;
+
+ private:
+ static std::map<std::string,std::function<Disparity*()>> algorithms__;
+};
+};
+
+#endif // _FTL_DISPARITY_HPP_
+
diff --git a/cv-node/include/ftl/rtcensus.hpp b/cv-node/include/ftl/rtcensus.hpp
deleted file mode 100644
index 5aec99da914ccf2c6bbf0e7ae91f5411c01bd61d..0000000000000000000000000000000000000000
--- a/cv-node/include/ftl/rtcensus.hpp
+++ /dev/null
@@ -1,15 +0,0 @@
-#ifndef _FTL_RTCENSUS_HPP_
-#define _FTL_RTCENSUS_HPP_
-
-#include <opencv2/core.hpp>
-#include <opencv2/opencv.hpp>
-
-namespace ftl {
-class RTCensus {
- public:
- void disparity(cv::Mat &l, cv::Mat &r, cv::Mat &disp, size_t num_disp=32, float gamma=0.0f, float tau=0.0f);
-};
-};
-
-#endif // _FTL_RTCENSUS_HPP_
-
diff --git a/cv-node/src/algorithms/fixstars_sgm.cpp b/cv-node/src/algorithms/fixstars_sgm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/cv-node/src/algorithms/opencv_bm.cpp b/cv-node/src/algorithms/opencv_bm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/cv-node/src/algorithms/opencv_sgbm.cpp b/cv-node/src/algorithms/opencv_sgbm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5ecd1a632a4425fbd55c2dc943cd139529a99712
--- /dev/null
+++ b/cv-node/src/algorithms/opencv_sgbm.cpp
@@ -0,0 +1,36 @@
+#include <ftl/algorithms/opencv_sgbm.hpp>
+
+using ftl::algorithms::OpenCVSGBM;
+using namespace cv::ximgproc;
+using namespace cv;
+
+static ftl::Disparity::Register opencvsgbm("sgbm", OpenCVSGBM::create);
+
+OpenCVSGBM::OpenCVSGBM() {
+ int wsize = 5;
+ float sigma = 1.5;
+ float lambda = 8000.0;
+
+
+ left_matcher_ = StereoSGBM::create(min_disp_,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_);
+ right_matcher_ = createRightMatcher(left_matcher_);
+
+ wls_filter_->setLambda(lambda);
+ wls_filter_->setSigmaColor(sigma);
+}
+
+void OpenCVSGBM::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/rtcensus.cpp b/cv-node/src/algorithms/rtcensus.cpp
similarity index 81%
rename from cv-node/src/rtcensus.cpp
rename to cv-node/src/algorithms/rtcensus.cpp
index 91b2e78e355e6e87bd2750eedf88cb80d3f19ee9..8671863ef66ba69bd69351070c9cd45edb4fd0f2 100644
--- a/cv-node/src/rtcensus.cpp
+++ b/cv-node/src/algorithms/rtcensus.cpp
@@ -1,11 +1,11 @@
-#include <ftl/rtcensus.hpp>
+#include <ftl/algorithms/rtcensus.hpp>
#include <vector>
#include <tuple>
#include <bitset>
#include <cmath>
#include <glog/logging.h>
-using ftl::RTCensus;
+using ftl::algorithms::RTCensus;
using std::vector;
using cv::Mat;
using cv::Point;
@@ -15,9 +15,11 @@ using std::get;
using std::make_tuple;
using std::bitset;
+static ftl::Disparity::Register rtcensus("rtcensus", RTCensus::create);
+
#define XHI(P1,P2) ((P1 <= P2) ? 0 : 1)
-static vector<uint64_t> sparse_census_16x16(Mat &arr) {
+static vector<uint64_t> sparse_census_16x16(const Mat &arr) {
vector<uint64_t> result;
result.resize(arr.cols*arr.rows,0);
@@ -45,14 +47,15 @@ static vector<uint64_t> sparse_census_16x16(Mat &arr) {
return bitset<64>(n1^n2).count();
}*/
-static vector<uint16_t> dsi_ca(vector<uint64_t> &census_R, vector<uint64_t> &census_L, size_t w, size_t h, size_t d_start, size_t d_stop, int sign=1) {
+static void dsi_ca(vector<uint16_t> &result, const vector<uint64_t> &census_R, const vector<uint64_t> &census_L, size_t w, size_t h, size_t d_start, size_t d_stop, int sign=1) {
// TODO Add asserts
assert( census_R.size() == w*h);
assert( census_L.size() == w*h);
assert( d_stop-d_start > 0 );
auto ds = d_stop - d_start;
- vector<uint16_t> result(census_R.size()*ds, 0);
+ //vector<uint16_t> result(census_R.size()*ds, 0);
+ result.resize(census_R.size()*ds, 0);
const size_t eu = (sign>0) ? w-2-ds : w-2;
@@ -64,12 +67,12 @@ static vector<uint16_t> dsi_ca(vector<uint64_t> &census_R, vector<uint64_t> &cen
const auto u_ = u + n;
for (int m=-2; m<=2; m++) {
-
- for (size_t d=0; d<ds; d++) {
- const auto d_ = d * sign;
- //if (u_+d_ < 0 || u_+d_ >= w) continue;
const auto v_ = (v + m)*w;
auto r = census_R[u_+v_];
+
+ for (size_t d=0; d<ds; d++) {
+ const auto d_ = d * sign;
+
auto l = census_L[v_+(u_+d_)];
result[ix+d] += bitset<64>(r^l).count(); //hamming(r,l);
}
@@ -79,7 +82,7 @@ static vector<uint16_t> dsi_ca(vector<uint64_t> &census_R, vector<uint64_t> &cen
}
}
- return result;
+ //return result;
}
static size_t arrmin(vector<uint16_t> &a, size_t ix, size_t len) {
@@ -148,9 +151,11 @@ static cv::Mat consistency(cv::Mat &d_sub_r, cv::Mat &d_sub_l) {
return result;
}
-void RTCensus::disparity(cv::Mat &l, cv::Mat &r, cv::Mat &disp, size_t num_disp, float gamma, float tau) {
- size_t d_min = 0;
- size_t d_max = num_disp;
+RTCensus::RTCensus() : gamma_(0.0f), tau_(0.0f) {}
+
+void RTCensus::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
+ size_t d_min = min_disp_;
+ size_t d_max = max_disp_;
auto start = std::chrono::high_resolution_clock::now();
auto census_R = sparse_census_16x16(r);
@@ -159,8 +164,9 @@ void RTCensus::disparity(cv::Mat &l, cv::Mat &r, cv::Mat &disp, size_t num_disp,
LOG(INFO) << "Census in " << elapsed.count() << "s";
start = std::chrono::high_resolution_clock::now();
- auto dsi_ca_R = dsi_ca(census_R, census_L, l.cols, l.rows, d_min, d_max);
- auto dsi_ca_L = dsi_ca(census_L, census_R, l.cols, l.rows, d_min, d_max, -1);
+ vector<uint16_t> dsi_ca_R,dsi_ca_L;
+ dsi_ca(dsi_ca_R,census_R, census_L, l.cols, l.rows, d_min, d_max);
+ dsi_ca(dsi_ca_L,census_L, census_R, l.cols, l.rows, d_min, d_max, -1);
elapsed = std::chrono::high_resolution_clock::now() - start;
LOG(INFO) << "DSI in " << elapsed.count() << "s";
diff --git a/cv-node/src/disparity.cpp b/cv-node/src/disparity.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c11cfba52ad2b8c802a995f952b834797df44ccf
--- /dev/null
+++ b/cv-node/src/disparity.cpp
@@ -0,0 +1,17 @@
+#include <ftl/disparity.hpp>
+
+using ftl::Disparity;
+
+std::map<std::string,std::function<Disparity*()>> Disparity::algorithms__;
+
+Disparity::Disparity() : min_disp_(0), max_disp_(208) {}
+
+Disparity *Disparity::create(const std::string &n) {
+ if (algorithms__.count(n) != 1) return nullptr;
+ return algorithms__[n]();
+}
+
+void Disparity::_register(const std::string &n, std::function<Disparity*()> f) {
+ algorithms__[n] = f;
+}
+
diff --git a/cv-node/src/main.cpp b/cv-node/src/main.cpp
index 9e2034f1fe9ca70a87c1d08a6593dbf6f47a6d79..2867d4b278dc0e66cce539e0a17fc3dbfa75a17f 100644
--- a/cv-node/src/main.cpp
+++ b/cv-node/src/main.cpp
@@ -2,13 +2,12 @@
#include <ftl/local.hpp>
#include <ftl/synched.hpp>
#include <ftl/calibrate.hpp>
-#include <ftl/rtcensus.hpp>
+#include <ftl/disparity.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>
@@ -21,12 +20,13 @@ using std::vector;
using cv::Mat;
using namespace cv;
-using namespace cv::ximgproc;
+//using namespace cv::ximgproc;
static vector<string> OPTION_peers;
static vector<string> OPTION_channels;
static string OPTION_calibration_config = FTL_CONFIG_ROOT "/calibration.xml";
static string OPTION_config;
+static string OPTION_algorithm = "rtcensus";
static bool OPTION_display = false;
static bool OPTION_calibrate = false;
static bool OPTION_flip = false;
@@ -52,6 +52,9 @@ void handle_options(char ***argv, int *argc) {
} else if (cmd.find("--config=") == 0) {
cmd = cmd.substr(cmd.find("=")+1);
OPTION_config = cmd;
+ } else if (cmd.find("--algorithm=") == 0) {
+ cmd = cmd.substr(cmd.find("=")+1);
+ OPTION_algorithm = cmd;
} else if (cmd.find("--display") == 0) {
OPTION_display = true;
} else if (cmd.find("--flip") == 0) {
@@ -92,41 +95,27 @@ int main(int argc, char **argv) {
sync->addChannel(c);
}
+ // Perform or load calibration intrinsics + extrinsics
Calibrate calibrate(lsrc, OPTION_calibration_config);
-
- if (OPTION_calibrate) {
- calibrate.recalibrate();
- }
-
- if (!calibrate.isCalibrated()) {
- LOG(WARNING) << "Cameras are not calibrated!";
- }
-
- /*int max_disp = 256; //208;
- int wsize = 5;
- float sigma = 1.5;
- float lambda = 8000.0;
- Ptr<DisparityWLSFilter> wls_filter;
+ if (OPTION_calibrate) calibrate.recalibrate();
+ if (!calibrate.isCalibrated()) LOG(WARNING) << "Cameras are not calibrated!";
- 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);*/
-
- ftl::RTCensus rtc;
+
+ // Choose and configure disparity algorithm
+ auto disparity = Disparity::create(OPTION_algorithm);
+ disparity->setMaxDisparity(208);
+
+ //double fact = 4.051863857;
+
+ Mat l, r, filtered_disp;
while (true) {
- Mat l, r, left_disp, right_disp, filtered_disp;
-
+ // Read calibrated images.
calibrate.undistort(l,r);
- //lsrc->get(l,r);
// Feed into sync buffer and network forward
sync->feed(LEFT, l,lsrc->getTimestamp());
@@ -135,38 +124,27 @@ int main(int argc, char **argv) {
// Read back from buffer
sync->get(LEFT,l);
sync->get(RIGHT,r);
- //double latency = sync->latency();
- // TODO Pose and disparity etc here...
-
- // Downscale to half
+ // Downscale
//cv::resize(l, l, cv::Size(l.cols * 0.75,l.rows * 0.75), 0, 0, INTER_LINEAR);
//cv::resize(r, r, cv::Size(r.cols * 0.75,r.rows * 0.75), 0, 0, INTER_LINEAR);
+ // Black and white
cvtColor(l, l, COLOR_BGR2GRAY);
cvtColor(r, r, COLOR_BGR2GRAY);
-
- /*left_matcher-> compute(l, r,left_disp);
- right_matcher->compute(r,l, right_disp);
-
- wls_filter->setLambda(lambda);
- wls_filter->setSigmaColor(sigma);
- //filtering_time = (double)getTickCount();
- wls_filter->filter(left_disp,l,filtered_disp,right_disp);
- //filtering_time = ((double)getTickCount() - filtering_time)/getTickFrequency();*/
- rtc.disparity(l,r,filtered_disp,200);
- LOG(INFO) << "Disparity complete";
+ disparity->compute(l,r,filtered_disp);
+ LOG(INFO) << "Disparity complete ";
// TODO Send RGB+D data somewhere
+
+ //left_disp = (fact * (double)l.cols * 0.1) / filtered_disp;
normalize(filtered_disp, filtered_disp, 0, 255, NORM_MINMAX, CV_8U);
- //normalize(right_disp, right_disp, 0, 255, NORM_MINMAX, CV_8U);
- cv::imshow("Left",filtered_disp);
- //if (lsrc->isStereo()) cv::imshow("Right",right_disp);
+ cv::imshow("Disparity",filtered_disp);
- if(cv::waitKey(1000) == 27){
+ if(cv::waitKey(10) == 27){
//exit if ESC is pressed
break;
}