Initial version of cv-node which should read stereo videos

cv-node/CMakeLists.txt

+cmake_minimum_required (VERSION 3.1.0)
+include (CheckIncludeFile)
+include (CheckFunctionExists)
+
+project (cv-node)
+
+include(CTest)
+enable_testing()
+
+set(THREADS_PREFER_PTHREAD_FLAG ON)
+
+find_package( OpenCV REQUIRED )
+find_package( Threads REQUIRED )
+#find_package(PkgConfig)
+#pkg_check_modules(GTKMM gtkmm-3.0)
+
+# Need to include staged files and libs
+include_directories(${PROJECT_SOURCE_DIR}/include)
+include_directories(${PROJECT_BINARY_DIR})
+
+set(ftl_VERSION_MAJOR "1")
+set(ftl_VERSION_MINOR "0")
+set(ftl_VERSION_PATCH "0")
+set(FTL_VERSION "${ftl_VERSION_MAJOR}.${ftl_VERSION_MINOR}.${ftl_VERSION_PATCH}")
+add_definitions(-DFTL_VERSION=${FTL_VERSION})
+
+if (WIN32)
+	set(FTL_CONFIG_ROOT "$ENV{USERPROFILE}/AppData/ftl")
+	set(FTL_CACHE_ROOT "$ENV{USERPROFILE}/AppData/ftl")
+	set(FTL_DATA_ROOT "$ENV{USERPROFILE}/AppData/ftl")
+	add_definitions(-DWIN32)
+endif (WIN32)
+
+if (UNIX)
+	add_definitions(-DUNIX)
+	set(FTL_CONFIG_ROOT "$ENV{HOME}/.config/ftl")
+	set(FTL_CACHE_ROOT "$ENV{HOME}/.cache/ftl")
+	set(FTL_DATA_ROOT "$ENV{HOME}/.local/share/ftl")
+endif (UNIX)
+
+add_definitions(-DFTL_CONFIG_ROOT=${FTL_CONFIG_ROOT})
+add_definitions(-DFTL_CACHE_ROOT=${FTL_CACHE_ROOT})
+add_definitions(-DFTL_DATA_ROOT=${FTL_DATA_ROOT})
+
+set(CMAKE_CXX_FLAGS "-pthread -std=c++17 -Wall -Wno-deprecated -Werror -Wno-psabi")
+set(CMAKE_CXX_FLAGS_DEBUG "-D_DEBUG -pg -Wall -Werror")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3")
+
+SET(CMAKE_USE_RELATIVE_PATHS ON)
+
+set(CVNODESRC
+	src/main.cpp
+	src/calibrate.cpp
+	src/local.cpp
+	src/sync.cpp
+)
+
+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)
+
+

cv-node/config/in_VID5.xml

+<?xml version="1.0"?>
+<opencv_storage>
+<Settings>
+  <!-- Number of inner corners per a item row and column. (square, circle) -->
+  <BoardSize_Width> 9</BoardSize_Width>
+  <BoardSize_Height>6</BoardSize_Height>
+  
+  <!-- The size of a square in some user defined metric system (pixel, millimeter)-->
+  <Square_Size>50</Square_Size>
+  
+  <!-- The type of input used for camera calibration. One of: CHESSBOARD CIRCLES_GRID ASYMMETRIC_CIRCLES_GRID -->
+  <Calibrate_Pattern>"CHESSBOARD"</Calibrate_Pattern>
+  
+  <!-- The input to use for calibration. 
+		To use an input camera -> give the ID of the camera, like "1"
+		To use an input video  -> give the path of the input video, like "/tmp/x.avi"
+		To use an image list   -> give the path to the XML or YAML file containing the list of the images, like "/tmp/circles_list.xml"
+		-->
+  <Input>"images/CameraCalibration/VID5/VID5.xml"</Input>
+  <!--  If true (non-zero) we flip the input images around the horizontal axis.-->
+  <Input_FlipAroundHorizontalAxis>0</Input_FlipAroundHorizontalAxis>
+  
+  <!-- Time delay between frames in case of camera. -->
+  <Input_Delay>100</Input_Delay>	
+  
+  <!-- How many frames to use, for calibration. -->
+  <Calibrate_NrOfFrameToUse>25</Calibrate_NrOfFrameToUse>
+  <!-- Consider only fy as a free parameter, the ratio fx/fy stays the same as in the input cameraMatrix. 
+	   Use or not setting. 0 - False Non-Zero - True-->
+  <Calibrate_FixAspectRatio> 1 </Calibrate_FixAspectRatio>
+  <!-- If true (non-zero) tangential distortion coefficients  are set to zeros and stay zero.-->
+  <Calibrate_AssumeZeroTangentialDistortion>1</Calibrate_AssumeZeroTangentialDistortion>
+  <!-- If true (non-zero) the principal point is not changed during the global optimization.-->
+  <Calibrate_FixPrincipalPointAtTheCenter> 1 </Calibrate_FixPrincipalPointAtTheCenter>
+  
+  <!-- The name of the output log file. -->
+  <Write_outputFileName>"out_camera_data.xml"</Write_outputFileName>
+  <!-- If true (non-zero) we write to the output file the feature points.-->
+  <Write_DetectedFeaturePoints>1</Write_DetectedFeaturePoints>
+  <!-- If true (non-zero) we write to the output file the extrinsic camera parameters.-->
+  <Write_extrinsicParameters>1</Write_extrinsicParameters>
+  <!-- If true (non-zero) we write to the output file the refined 3D target grid points.-->
+  <Write_gridPoints>1</Write_gridPoints>
+  <!-- If true (non-zero) we show after calibration the undistorted images.-->
+  <Show_UndistortedImage>1</Show_UndistortedImage>
+  <!-- If true (non-zero) will be used fisheye camera model.-->
+  <Calibrate_UseFisheyeModel>0</Calibrate_UseFisheyeModel>
+  <!-- If true (non-zero) distortion coefficient k1 will be equals to zero.-->
+  <Fix_K1>0</Fix_K1>
+  <!-- If true (non-zero) distortion coefficient k2 will be equals to zero.-->
+  <Fix_K2>0</Fix_K2>
+  <!-- If true (non-zero) distortion coefficient k3 will be equals to zero.-->
+  <Fix_K3>0</Fix_K3>
+  <!-- If true (non-zero) distortion coefficient k4 will be equals to zero.-->
+  <Fix_K4>1</Fix_K4>
+  <!-- If true (non-zero) distortion coefficient k5 will be equals to zero.-->
+  <Fix_K5>1</Fix_K5>
+</Settings>
+</opencv_storage>

cv-node/include/ftl/calibrate.hpp

+#ifndef _FTL_CALIBRATION_HPP_
+#define _FTL_CALIBRATION_HPP_
+
+#include <opencv2/opencv.hpp>
+#include <ftl/local.hpp>
+#include <string>
+
+namespace ftl {
+class Calibrate {
+	public:
+	Calibrate(ftl::LocalSource *s, const std::string &cal);
+	
+	bool recalibrate(const std::string &conf);
+	
+	bool undistort(cv::Mat &l, cv::Mat &r);
+	bool rectified(cv::Mat &l, cv::Mat &r);
+	
+	bool isCalibrated();
+	
+	private:
+	bool runCalibration(cv::Mat &img, cv::Mat &cam);
+	
+	private:
+	ftl::LocalSource *local_;
+};
+};
+
+#endif // _FTL_CALIBRATION_HPP_
+

cv-node/include/ftl/local.hpp

+#ifndef _FTL_LOCAL_HPP_
+#define _FTL_LOCAL_HPP_
+
+#include <string>
+
+namespace cv {
+	class Mat;
+	class VideoCapture;
+};
+
+namespace ftl {
+class LocalSource {
+	public:
+	LocalSource();
+	LocalSource(const std::string &vid);
+	
+	//bool left(cv::Mat &m);
+	//bool right(cv::Mat &m);
+	bool get(cv::Mat &l, cv::Mat &r);
+	
+	//void setFramerate(float fps);
+	//float getFramerate() const;
+	
+	double getTimestamp() const;
+	
+	bool isStereo() const;
+	
+	private:
+	double timestamp_;
+	bool stereo_;
+	//float fps_;
+	cv::VideoCapture *camera_a_;
+	cv::VideoCapture *camera_b_;
+};
+};
+
+#endif // _FTL_LOCAL_HPP_
+

cv-node/include/ftl/synched.hpp

+#ifndef _FTL_SYNCHED_HPP_
+#define _FTL_SYNCHED_HPP_
+
+#include <opencv2/opencv.hpp>
+#include <string>
+#include <vector>
+
+namespace ftl {
+
+static const int LEFT = 0;
+static const int RIGHT = 1;
+
+class SyncSource {
+	public:
+	SyncSource();
+	
+	void addChannel(const std::string &c);
+	void feed(int channel, cv::Mat &m, double ts);
+	bool get(int channel, cv::Mat &m);
+	double latency() const;
+	
+	private:
+	std::vector<cv::Mat> channels_;
+};
+};
+
+#endif // _FTL_SYNCHED_HPP_
+

cv-node/src/local.cpp

+#include <ftl/local.hpp>
+
+#include <opencv2/core.hpp>
+#include <opencv2/opencv.hpp>
+#include <string>
+#include <chrono>
+#include <glog/logging.h>
+
+using ftl::LocalSource;
+using cv::Mat;
+using cv::VideoCapture;
+using cv::Rect;
+using std::string;
+using namespace std::chrono;
+
+LocalSource::LocalSource() : timestamp_(0.0) {
+	// Use cameras
+	camera_a_ = new VideoCapture(0);
+	camera_b_ = new VideoCapture(1);
+
+	if (!camera_a_->isOpened()) {
+		delete camera_a_;
+		delete camera_b_;
+		camera_a_ = nullptr;
+		camera_b_ = nullptr;
+		LOG(FATAL) << "No cameras found";
+		return;
+	}
+
+	if (!camera_b_->isOpened()) {
+		delete camera_b_;
+		camera_b_ = nullptr;
+		stereo_ = false;
+		LOG(WARNING) << "Not able to find second camera for stereo";
+	} else {
+		stereo_ = true;
+	}
+}
+
+LocalSource::LocalSource(const string &vid): timestamp_(0.0) {
+	if (vid == "") {
+		LOG(FATAL) << "No video file specified";
+		camera_a_ = nullptr;
+		camera_b_ = nullptr;
+		return;
+	}
+	
+	camera_a_ = new VideoCapture(vid.c_str());
+	camera_b_ = nullptr;
+	
+	if (!camera_a_->isOpened()) {
+		delete camera_a_;
+		camera_a_ = nullptr;
+		LOG(FATAL) << "Unable to load video file";
+		return;
+	}
+	
+	// Read first frame to determine stereo
+	Mat frame;
+	if (!camera_a_->read(frame)) {
+		LOG(FATAL) << "No data in video file";
+	}
+
+	if (frame.cols >= 2*frame.rows) {
+		stereo_ = true;
+	} else {
+		stereo_ = false;
+	}
+}
+
+bool LocalSource::get(cv::Mat &l, cv::Mat &r) {
+	if (!camera_a_) return false;
+	
+	if (!camera_a_->grab()) {
+		LOG(ERROR) << "Unable to grab from camera A";
+		return false;
+	}
+	if (camera_b_ && !camera_b_->grab()) {
+		LOG(ERROR) << "Unable to grab from camera B";
+		return false;
+	}
+	
+	// Record timestamp
+	timestamp_ = duration_cast<duration<double>>(
+			high_resolution_clock::now().time_since_epoch()).count();
+
+	if (camera_b_ || !stereo_) {
+		if (!camera_a_->retrieve(l)) {
+			LOG(ERROR) << "Unable to read frame from camera A";
+			return false;
+		}
+		if (camera_b_ && !camera_b_->retrieve(r)) {
+			LOG(ERROR) << "Unable to read frame from camera B";
+			return false;
+		}
+	} else {
+		Mat frame;
+		if (!camera_a_->retrieve(frame)) {
+			LOG(ERROR) << "Unable to read frame from video";
+			return false;
+		}
+		
+		int resx = frame.cols / 2;
+		l = Mat(frame, Rect(0,0,resx,frame.rows));
+		r = Mat(frame, Rect(resx,0,frame.cols,frame.rows));
+	}
+	
+	return true;
+}
+
+double LocalSource::getTimestamp() const {
+	return timestamp_;
+}
+	
+bool LocalSource::isStereo() const {
+	return stereo_;
+}
+

cv-node/src/main.cpp

+#include <opencv2/opencv.hpp>
+#include <ftl/local.hpp>
+#include <ftl/synched.hpp>
+#include <ftl/calibrate.hpp>
+
+#include <string>
+#include <vector>
+
+using namespace ftl;
+using std::string;
+using std::vector;
+using cv::Mat;
+
+static vector<string> OPTION_peers;
+static vector<string> OPTION_channels;
+static string OPTION_calibration_config;
+static string OPTION_config;
+static bool OPTION_display = false;
+static bool OPTION_calibrate = false;
+
+void handle_options(char ***argv, int *argc) {
+	while (*argc > 0) {
+		string cmd((*argv)[0]);
+		if (cmd[0] != '-') break;
+		
+		if (cmd.find("--calibrate") == 0) {
+			OPTION_calibrate = true;
+		} else if (cmd.find("--peer=") == 0) {
+			cmd = cmd.substr(cmd.find("=")+1);
+			OPTION_peers.push_back(cmd);
+		} else if (cmd.find("--channel=") == 0) {
+			cmd = cmd.substr(cmd.find("=")+1);
+			OPTION_channels.push_back(cmd);
+		} else if (cmd.find("--calibration=") == 0) {
+			cmd = cmd.substr(cmd.find("=")+1);
+			OPTION_calibration_config = cmd;
+		} else if (cmd.find("--config=") == 0) {
+			cmd = cmd.substr(cmd.find("=")+1);
+			OPTION_config = cmd;
+		} else if (cmd.find("--display") == 0) {
+			OPTION_display = true;
+		}
+		
+		(*argc)--;
+		(*argv)++;
+	}
+}
+
+int main(int argc, char **argv) {
+	argc--;
+	argv++;
+	
+	// Process Arguments
+	handle_options(&argv, &argc);
+	
+	// TODO Initiate the network
+	
+	LocalSource *lsrc;
+	
+	if (argc) {
+		// Load video file
+		lsrc = new LocalSource(argv[0]);
+	} else {
+		// Use cameras
+		lsrc = new LocalSource();
+	}
+	
+	auto sync = new SyncSource(); // TODO Pass protocol object
+	// Add any remote channels
+	for (auto c : OPTION_channels) {
+		sync->addChannel(c);
+	}
+	
+	Calibrate calibrate(lsrc, OPTION_calibration_config);
+	
+	while (true) {
+		Mat l, r;
+		
+		calibrate.undistort(l,r);
+		
+		// Feed into sync buffer and network forward
+		sync->feed(LEFT, l,lsrc->getTimestamp());
+		sync->feed(RIGHT, r,lsrc->getTimestamp());
+		
+		// Read back from buffer
+		sync->get(LEFT,l);
+		sync->get(RIGHT,r);
+		//double latency = sync->latency();
+		
+		// TODO Pose and disparity etc here...
+		
+		// TODO Send RGB+D data somewhere
+		
+		cv::imshow("Left",l);
+		if (lsrc->isStereo()) cv::imshow("Right",r);
+		
+		if(cv::waitKey(1) == 27){
+            //exit if ESC is pressed
+            break;
+        }
+	}
+}
+

cv-node/src/sync.cpp

+#include <ftl/synched.hpp>
+
+using ftl::SyncSource;
+using cv::Mat;
+
+SyncSource::SyncSource() {
+	channels_.push_back(Mat());
+	channels_.push_back(Mat());
+}
+	
+void SyncSource::addChannel(const std::string &c) {
+
+}
+
+void SyncSource::feed(int channel, cv::Mat &m, double ts) {
+	if (channel > static_cast<int>(channels_.size())) return;
+	channels_[channel] = m;
+}
+
+bool SyncSource::get(int channel, cv::Mat &m) {
+	if (channel > static_cast<int>(channels_.size())) return false;
+	m = channels_[channel];
+	return true;
+}
+
+double SyncSource::latency() const {
+	return 0.0;
+}
+