diff --git a/cv-node/CMakeLists.txt b/cv-node/CMakeLists.txt
index 0f32a0c45abf0206ca75bf041e068fac1bcc9332..1531cc233eee7057a9e1a06eb7746fdcf7452788 100644
--- a/cv-node/CMakeLists.txt
+++ b/cv-node/CMakeLists.txt
@@ -76,6 +76,7 @@ set(CVNODESRC
src/calibrate.cpp
src/local.cpp
src/sync.cpp
+ src/display.cpp
src/disparity.cpp
src/middlebury.cpp
src/algorithms/rtcensus.cpp
diff --git a/cv-node/include/ftl/display.hpp b/cv-node/include/ftl/display.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..62e2aec263a9bd0467f7b12f487acdc00ec9f0bb
--- /dev/null
+++ b/cv-node/include/ftl/display.hpp
@@ -0,0 +1,28 @@
+#ifndef _FTL_DISPLAY_HPP_
+#define _FTL_DISPLAY_HPP_
+
+#include <nlohmann/json.hpp>
+#include <opencv2/opencv.hpp>
+#include <ftl/calibrate.hpp>
+#include "opencv2/highgui.hpp"
+
+namespace ftl {
+ class Display {
+ public:
+ Display(const Calibrate &cal, nlohmann::json &config);
+ ~Display();
+
+ bool render(const cv::Mat &rgb, const cv::Mat &depth);
+
+ bool active() const;
+
+ private:
+ const ftl::Calibrate &calibrate_;
+ nlohmann::json config_;
+ cv::viz::Viz3d *window_;
+ bool active_;
+ };
+};
+
+#endif // _FTL_DISPLAY_HPP_
+
diff --git a/cv-node/src/display.cpp b/cv-node/src/display.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0c1d13a7bab252539a24bc13b04596bef33508c1
--- /dev/null
+++ b/cv-node/src/display.cpp
@@ -0,0 +1,76 @@
+#include <ftl/display.hpp>
+
+using ftl::Display;
+using namespace cv;
+
+Display::Display(const Calibrate &cal, nlohmann::json &config) : calibrate_(cal), config_(config) {
+ window_ = new cv::viz::Viz3d("FTL");
+ window_->setBackgroundColor(cv::viz::Color::white());
+ active_ = true;
+}
+
+Display::~Display() {
+ delete window_;
+}
+
+bool Display::render(const cv::Mat &rgb, const cv::Mat &depth) {
+ Mat idepth;
+
+ if (config_["points"]) {
+ cv::Mat Q_32F; // = (Mat_<double>(4,4) << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); //(4,4,CV_32F);
+ calibrate_.getQ().convertTo(Q_32F,CV_32F);
+ cv::Mat_<cv::Vec3f> XYZ(depth.rows,depth.cols); // Output point cloud
+ reprojectImageTo3D(depth, XYZ, Q_32F, false);
+
+ //cv::imshow("Points",XYZ);
+
+ cv::viz::WCloud cloud_widget = cv::viz::WCloud( XYZ, rgb );
+ cloud_widget.setRenderingProperty( cv::viz::POINT_SIZE, 2 );
+
+ /* Rotation using rodrigues */
+ /// Rotate around (1,1,1)
+ /*rot_vec.at<float>(0,0) = 0.0f;
+ rot_vec.at<float>(0,1) = 0.0f;
+ rot_vec.at<float>(0,2) = CV_PI * 1.0f;*/
+
+ //Mat rot_mat;
+ //Rodrigues(rot_vec, rot_mat);
+
+ /// Construct pose
+ //Affine3f pose(rot_mat, Vec3f(0, 20.0, 0));
+ window_->showWidget( "coosys", viz::WCoordinateSystem() );
+ window_->showWidget( "Depth", cloud_widget );
+ //window_->setWidgetPose("Depth", pose);
+
+ window_->spinOnce( 1, true );
+ }
+
+ if (config_["depth"]) {
+ /*Mat depth32F = (focal * (float)l.cols * base_line) / depth;
+ normalize(depth32F, depth32F, 0, 255, NORM_MINMAX, CV_8U);
+ cv::imshow("Depth", depth32F);
+ if(cv::waitKey(10) == 27){
+ //exit if ESC is pressed
+ active_ = false;
+ }*/
+ } else if (config_["disparity"]) {
+ //if (disparity32F.type() == CV_32FC1) {
+ //disparity32F = disparity32F / (float)config["disparity"]["maximum"];
+ depth.convertTo(idepth,CV_8U,255.0f / 208.0f); //(float)config_["maximum"]);
+ //}
+ //normalize(disparity32F, disparity32F, 0, 255, NORM_MINMAX, CV_8U);
+ applyColorMap(idepth, idepth, cv::COLORMAP_JET);
+ cv::imshow("Disparity", idepth);
+ if(cv::waitKey(10) == 27){
+ //exit if ESC is pressed
+ active_ = false;
+ }
+ }
+
+ return true;
+}
+
+bool Display::active() const {
+ return active_ && !window_->wasStopped();
+}
+
diff --git a/cv-node/src/main.cpp b/cv-node/src/main.cpp
index 9e3689bdb39120e1e3349ddba1ed1c7e33a5ee42..ed2aa9bc25f3bdc5a456ba813c026b98d6be6fc6 100644
--- a/cv-node/src/main.cpp
+++ b/cv-node/src/main.cpp
@@ -4,6 +4,7 @@
#include <ftl/calibrate.hpp>
#include <ftl/disparity.hpp>
#include <ftl/middlebury.hpp>
+#include <ftl/display.hpp>
#include <nlohmann/json.hpp>
#include "opencv2/imgproc.hpp"
@@ -137,14 +138,13 @@ static void run(const string &file) {
Mat l, r, disparity32F, depth32F, lbw, rbw;
- cv::viz::Viz3d myWindow("FTL");
- myWindow.setBackgroundColor(cv::viz::Color::white());
+ Display display(calibrate, config["display"]);
float base_line = (float)config["camera"]["base_line"];
float focal = (float)(config["camera"]["focal_length"]) / (float)(config["camera"]["sensor_width"]);
Mat rot_vec = Mat::zeros(1,3,CV_32F);
- while (!myWindow.wasStopped()) {
+ while (display.active()) {
// Read calibrated images.
calibrate.undistort(l,r);
@@ -156,64 +156,15 @@ static void run(const string &file) {
sync->get(LEFT,l);
sync->get(RIGHT,r);
- // TODO Check other algorithms convert to BW
disparity->compute(l,r,disparity32F);
- //disparity32F += 10.0f; // TODO REMOVE
-
// Clip the left edge
//Rect rect((int)config["disparity"]["maximum"],7,disparity32F.cols-(int)config["disparity"]["maximum"],disparity32F.rows-14);
- if (config["display"]["points"]) {
- cv::Mat Q_32F; // = (Mat_<double>(4,4) << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); //(4,4,CV_32F);
- calibrate.getQ().convertTo(Q_32F,CV_32F);
- cv::Mat_<cv::Vec3f> XYZ(disparity32F.rows,disparity32F.cols); // Output point cloud
- reprojectImageTo3D(disparity32F, XYZ, Q_32F, false);
-
- //cv::imshow("Points",XYZ);
-
- cv::viz::WCloud cloud_widget = cv::viz::WCloud( XYZ, l );
- cloud_widget.setRenderingProperty( cv::viz::POINT_SIZE, 2 );
-
- /* Rotation using rodrigues */
- /// Rotate around (1,1,1)
- rot_vec.at<float>(0,0) = 0.0f;
- rot_vec.at<float>(0,1) = 0.0f;
- rot_vec.at<float>(0,2) = CV_PI * 1.0f;
-
- Mat rot_mat;
- Rodrigues(rot_vec, rot_mat);
-
- /// Construct pose
- Affine3f pose(rot_mat, Vec3f(0, 20.0, 0));
- myWindow.showWidget( "coosys", viz::WCoordinateSystem() );
- myWindow.showWidget( "Depth", cloud_widget );
- myWindow.setWidgetPose("Depth", pose);
-
- myWindow.spinOnce( 1, true );
- }
-
- if (config["display"]["depth"]) {
- depth32F = (focal * (float)l.cols * base_line) / disparity32F;
- normalize(depth32F, depth32F, 0, 255, NORM_MINMAX, CV_8U);
- cv::imshow("Depth", depth32F);
- if(cv::waitKey(10) == 27){
- //exit if ESC is pressed
- break;
- }
- } else if (config["display"]["disparity"]) {
- //if (disparity32F.type() == CV_32FC1) {
- //disparity32F = disparity32F / (float)config["disparity"]["maximum"];
- disparity32F.convertTo(disparity32F,CV_8U,255.0f / (float)config["disparity"]["maximum"]);
- //}
- //normalize(disparity32F, disparity32F, 0, 255, NORM_MINMAX, CV_8U);
- applyColorMap(disparity32F, disparity32F, cv::COLORMAP_JET);
- cv::imshow("Disparity", disparity32F);
- if(cv::waitKey(10) == 27){
- //exit if ESC is pressed
- break;
- }
- }
+ // Send RGB+Depth images for local rendering
+ display.render(l, disparity32F);
+
+ //streamer.send(l, disparity32F);
}
}