From 0be0297485ff4e2090e2fcb63ce911da7404ce68 Mon Sep 17 00:00:00 2001
From: Sebastian Gomez-Gonzalez <sgomez@tue.mpg.de>
Date: Wed, 11 Oct 2017 13:50:29 +0200
Subject: [PATCH] Full stereo tracking pipeline seems to work (not well tested
yet)
---
CMakeLists.txt | 7 +
examples/CMakeLists.txt | 8 ++
examples/tracking/gpu_track.cpp | 13 +-
examples/tracking/server3d.cpp | 47 +++++++
examples/tracking/server_3d_conf.json | 29 ++++
examples/tracking/track.cpp | 12 +-
include/ball_tracking/stereo.hpp | 22 +++
src/stereo.cpp | 185 ++++++++++++++++++++++++++
src/tracker.cpp | 58 +++++++-
9 files changed, 372 insertions(+), 9 deletions(-)
create mode 100644 examples/tracking/server3d.cpp
create mode 100644 examples/tracking/server_3d_conf.json
create mode 100644 include/ball_tracking/stereo.hpp
create mode 100644 src/stereo.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c678eb5..2b072e3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -6,11 +6,14 @@ find_package( Boost 1.58 COMPONENTS log REQUIRED )
find_package(OpenCV REQUIRED)
find_package(CUDA 8.0)
+find_package(Armadillo REQUIRED)
find_library(ZMQPP NAMES zmqpp)
find_path(ZMQPP_INCLUDES NAMES zmqpp/zmqpp.hpp)
find_library(CAMLIB NAMES camera)
find_path(CAMLIB_INC NAMES camera.hpp)
+find_library(ROBOTICS NAMES robotics)
+find_path(ROBOTICS_INC NAMES robotics.hpp)
if (CUDA_FOUND)
option (WITH_CUDA "Compile the library with GPU implementations" ON)
@@ -38,6 +41,7 @@ include_directories(include
${OpenCV_INCLUDES}
${ZMQPP_INCLUDES}
${CAMLIB_INC}
+ ${ROBOTICS_INC}
)
add_library(ball_tracking SHARED
@@ -45,6 +49,7 @@ add_library(ball_tracking SHARED
src/utils.cpp
src/tracker.cpp
src/server.cpp
+ src/stereo.cpp
${GPU_CPP_SRC}
)
target_link_libraries(ball_tracking
@@ -53,6 +58,8 @@ target_link_libraries(ball_tracking
${ZMQPP}
${CAMLIB}
${Boost_LIBRARIES}
+ ${ARMADILLO_LIBRARIES}
+ ${ROBOTICS}
)
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index a7994aa..2e49770 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -35,6 +35,14 @@ target_link_libraries(track_server
${Boost_LIBRARIES}
)
+add_executable(track_server3d
+ tracking/server3d.cpp
+ )
+target_link_libraries(track_server3d
+ ball_tracking
+ ${Boost_LIBRARIES}
+ )
+
if (WITH_CUDA)
add_executable(log_reg_approach
img_proc/log_reg_approach.cpp
diff --git a/examples/tracking/gpu_track.cpp b/examples/tracking/gpu_track.cpp
index b5ef35f..457ef28 100644
--- a/examples/tracking/gpu_track.cpp
+++ b/examples/tracking/gpu_track.cpp
@@ -65,11 +65,13 @@ int main(int argc, char** argv) {
Binarizer bin(jobj.at("binarizer"));
ball_tracking::cuda::Binarizer gpu_bin(jobj.at("binarizer"));
FindBallBlob bf(jobj.at("blob_detection"));
+ int subsample = 0;
+ if (jobj.count("subsample")) subsample = jobj.at("subsample");
string fname = vm["input"].as<string>();
VideoCapture in_video(fname);
- Size in_size((int)in_video.get(CV_CAP_PROP_FRAME_WIDTH),
- (int)in_video.get(CV_CAP_PROP_FRAME_HEIGHT));
+ Size in_size((int)in_video.get(CV_CAP_PROP_FRAME_WIDTH) >> subsample,
+ (int)in_video.get(CV_CAP_PROP_FRAME_HEIGHT) >> subsample);
int ex = static_cast<int>(in_video.get(CV_CAP_PROP_FOURCC)); // Use same input codec
// Transform from int to char via Bitwise operators
char codec[] = {(char)(ex & 0XFF) ,
@@ -106,6 +108,13 @@ int main(int argc, char** argv) {
gpu_img.upload(img);
auto end_t = std::chrono::steady_clock::now();
in_time.push_back(std::chrono::duration_cast<std::chrono::microseconds>(end_t - start_t).count());
+ //1.1) Subsample
+ for (int i=0; i<subsample; i++) {
+ cv::cuda::GpuMat gpu_tmp;
+ cv::cuda::pyrDown(gpu_img, gpu_tmp);
+ gpu_img = gpu_tmp;
+ }
+ gpu_img.download(img);
//2) Compute log likelihood image (In GPU)
start_t = std::chrono::steady_clock::now();
diff --git a/examples/tracking/server3d.cpp b/examples/tracking/server3d.cpp
new file mode 100644
index 0000000..636ce07
--- /dev/null
+++ b/examples/tracking/server3d.cpp
@@ -0,0 +1,47 @@
+
+#include <ball_tracking/stereo.hpp>
+#include <ball_tracking/utils.hpp>
+#include <iostream>
+#include <boost/program_options.hpp>
+#include <json.hpp>
+
+using namespace std;
+using namespace ball_tracking;
+using json = nlohmann::json;
+
+int main(int argc, char** argv) {
+ try {
+ namespace po = boost::program_options;
+ po::options_description desc("Options");
+ desc.add_options()
+ ("help", "Produce help message")
+ ("conf,c", po::value<string>(), "path to the JSON configuration");
+ po::variables_map vm;
+ po::store(po::parse_command_line(argc, argv, desc), vm);
+ po::notify(vm);
+
+ if (vm.count("help")) {
+ cout << desc << endl;
+ return 0;
+ }
+ if (!vm.count("conf")) {
+ cerr << "Error: You should provide the video to process" << endl;
+ return 1;
+ }
+
+ json server_conf = load_json(vm.at("conf").as<string>());
+ cout << "Loading server configuration" << endl;
+ StereoServer srv;
+ srv.start(server_conf);
+ cout << "Tracking 3D started. Type q to quit" << endl;
+ char c;
+ do {
+ cin >> c;
+ } while (c != 'q');
+ cout << "Stopping" << endl;
+ srv.stop();
+ } catch (std::exception& ex) {
+ cerr << "Exception: " << ex.what() << endl;
+ return 1;
+ }
+}
diff --git a/examples/tracking/server_3d_conf.json b/examples/tracking/server_3d_conf.json
new file mode 100644
index 0000000..4fa65a4
--- /dev/null
+++ b/examples/tracking/server_3d_conf.json
@@ -0,0 +1,29 @@
+{
+ "publisher": "tcp://*:7660",
+ "obs2d": ["tcp://localhost:7650"],
+ "num_cams": 2,
+ "stereo": {
+ "calib": [
+ {
+ "ID": 0,
+ "val": [
+ [330.818029777, -20.0612367361, 4.36051841227, 267.094073072],
+ [3.18143132853, -129.47891137, -292.718044206, -202.284356759],
+ [0.0636413872527, 0.200475112774, -0.114193103147, 1.0]
+ ]
+ },
+ {
+ "ID": 1,
+ "val": [
+ [226.81497789, 157.971113188, -122.040734067, 342.535383641],
+ [-34.3055014233, -132.114683094, -281.490884508, -123.382602225],
+ [-0.138614275474, 0.161602493631, -0.110710339962, 1.0 ]
+ ]
+ }
+ ]
+ },
+ "log": {
+ "auto_flush": true,
+ "file": "/tmp/ball_tracking.log"
+ }
+}
diff --git a/examples/tracking/track.cpp b/examples/tracking/track.cpp
index 5ad4dd9..c95458a 100644
--- a/examples/tracking/track.cpp
+++ b/examples/tracking/track.cpp
@@ -62,11 +62,13 @@ int main(int argc, char** argv) {
BallLogLikelihood bll(jobj.at("ball_log_lh"));
Binarizer bin(jobj.at("binarizer"));
FindBallBlob bf(jobj.at("blob_detection"));
+ int subsample = 0;
+ if (jobj.count("subsample")) subsample = jobj.at("subsample");
string fname = vm["input"].as<string>();
VideoCapture in_video(fname);
- Size in_size((int)in_video.get(CV_CAP_PROP_FRAME_WIDTH),
- (int)in_video.get(CV_CAP_PROP_FRAME_HEIGHT));
+ Size in_size((int)in_video.get(CV_CAP_PROP_FRAME_WIDTH)>>subsample,
+ (int)in_video.get(CV_CAP_PROP_FRAME_HEIGHT)>>subsample);
int ex = static_cast<int>(in_video.get(CV_CAP_PROP_FOURCC)); // Use same input codec
// Transform from int to char via Bitwise operators
char codec[] = {(char)(ex & 0XFF) ,
@@ -100,6 +102,12 @@ int main(int argc, char** argv) {
if (img.empty()) break;
auto end_t = std::chrono::steady_clock::now();
in_time.push_back(std::chrono::duration_cast<std::chrono::microseconds>(end_t - start_t).count());
+ //1.1) Subsample
+ for (int i=0; i<subsample; i++) {
+ Mat tmp;
+ pyrDown(img, tmp, Size(img.cols/2, img.rows/2));
+ img = tmp;
+ }
//2) Compute log likelihood image
start_t = std::chrono::steady_clock::now();
diff --git a/include/ball_tracking/stereo.hpp b/include/ball_tracking/stereo.hpp
new file mode 100644
index 0000000..8a34495
--- /dev/null
+++ b/include/ball_tracking/stereo.hpp
@@ -0,0 +1,22 @@
+#ifndef BALL_TRACKING_STEREO
+#define BALL_TRACKING_STEREO
+
+#include <memory>
+#include <json.hpp>
+
+namespace ball_tracking {
+
+ class StereoServer {
+ private:
+ class Impl;
+ std::shared_ptr<Impl> _impl;
+ public:
+ StereoServer();
+ ~StereoServer();
+
+ void start(const nlohmann::json& conf);
+ void stop();
+ };
+};
+
+#endif
diff --git a/src/stereo.cpp b/src/stereo.cpp
new file mode 100644
index 0000000..200e25e
--- /dev/null
+++ b/src/stereo.cpp
@@ -0,0 +1,185 @@
+
+#include <ball_tracking/stereo.hpp>
+#include <ball_tracking/utils.hpp>
+#include <robotics/utils.hpp>
+#include <zmqpp/zmqpp.hpp>
+#include <string>
+#include <unordered_map>
+#include <thread>
+#include <chrono>
+#include <armadillo>
+#include <thread>
+
+#include <boost/log/core.hpp>
+#include <boost/log/trivial.hpp>
+#include <boost/log/expressions.hpp>
+#include <boost/log/utility/setup/file.hpp>
+#include <boost/log/utility/setup/console.hpp>
+#include <boost/log/utility/setup/common_attributes.hpp>
+
+namespace logging = boost::log;
+using namespace std;
+using namespace std::chrono;
+using json = nlohmann::json;
+using namespace arma;
+
+namespace ball_tracking {
+
+ class MultiObs2D {
+ private:
+ unsigned int num; //!< ID of the observation
+ double _time; //!< A consolidated time of the observation
+ unsigned int _num_obs;
+ public:
+ vector<robotics::pt2d> obs2d; //!< List of observations
+ vector<bool> is_obs; //!< Weather or not the observation was actually made
+
+ MultiObs2D(unsigned int num_cams) {
+ obs2d = vector<robotics::pt2d>(num_cams);
+ is_obs = vector<bool>(num_cams, false);
+ _num_obs = 0;
+ }
+
+ unsigned int num_obs() const {
+ return _num_obs;
+ }
+
+ double time() const {
+ return _time;
+ }
+
+ void add_obs(unsigned int cam_id, double time, bool is_obs, const robotics::pt2d& x) {
+ if (!this->is_obs[cam_id]) {
+ this->is_obs[cam_id] = is_obs;
+ _time = time;
+ _num_obs++;
+ }
+ obs2d[cam_id] = x;
+ }
+ };
+
+ class Stereo {
+ public:
+ unordered_map<unsigned int, arma::mat> calib;
+
+ Stereo(const json& conf) {
+ json jcalib = conf.at("calib");
+ for (auto elem : jcalib) {
+ unsigned int id = elem.at("ID");
+ mat val = robotics::json2mat( elem.at("val") );
+ calib[id] = val;
+ }
+ }
+
+ bool operator()(robotics::pt3d& ans, const MultiObs2D& obs) {
+ vector<pair<unsigned int,robotics::pt2d>> all_obs;
+ for (unsigned int i=0; i<obs.obs2d.size(); i++) {
+ if (obs.is_obs[i]) {
+ all_obs.push_back(make_pair(i, obs.obs2d[i]));
+ }
+ }
+ if (all_obs.size()<2) return false;
+ ans = robotics::stereo_vision(calib, all_obs);
+ return true;
+ }
+ };
+
+ class StereoServer::Impl {
+ public:
+ unordered_map<unsigned int, mat> calib_mats;
+ unordered_map<unsigned int, MultiObs2D> observations;
+ thread srv_thread;
+ bool running;
+ unsigned int num_cams;
+
+ void start(const json& conf) {
+ running = true;
+ auto method = [this](const json& conf) -> void {
+ this->start_server(conf);
+ };
+ srv_thread = thread(method, conf);
+ }
+
+ void start_server(const json& conf) {
+ // 1) Create client and server sockets
+ zmqpp::context context;
+ zmqpp::socket pos_recv(context, zmqpp::socket_type::sub);
+ zmqpp::socket pos_send(context, zmqpp::socket_type::pub);
+ // 1.1) Subscribe to all position publisher in 2 dimensions (in config)
+ cout << "Creating sockets" << endl;
+ for (const string& url : conf.at("obs2d")) {
+ pos_recv.connect(url);
+ }
+ pos_recv.set(zmqpp::socket_option::subscribe, "a");
+ // 1.2) And create a publisher of 3 dimensional positions
+ string pos_send_url = conf.at("publisher");
+ pos_send.bind(pos_send_url);
+ // 1.3) Read additional configuration
+ cout << "Reading additional configuration" << endl;
+ num_cams = conf.at("num_cams");
+ Stereo stereo(conf.at("stereo"));
+ while (running) {
+ //2) Read the next 2 dim position
+ zmqpp::message msg;
+ pos_recv.receive(msg);
+ string topic, body;
+ msg >> topic >> body;
+ json jobs = json::parse(body);
+
+ //3) Create a new MultiObs2D or modify an existing one
+ unsigned int num = jobs.at("num");
+ unsigned int cam_id = jobs.at("cam_id");
+ double time = jobs.at("time");
+ robotics::pt2d x; bool is_obs = false;
+ if (observations.count(num) == 0) {
+ observations.insert({num, MultiObs2D(num_cams)});
+ }
+ if (!jobs.at("obs").is_null()) {
+ is_obs = true;
+ json jx = jobs.at("obs");
+ x[0] = jx[0]; x[1] = jx[1];
+ }
+ MultiObs2D& curr = observations.at(num);
+ curr.add_obs(cam_id, time, is_obs, x);
+
+ //4) If we have all observations, run stereo
+ if (curr.num_obs() == num_cams) {
+ robotics::pt3d pos3d;
+ bool success = stereo(pos3d, curr);
+ zmqpp::message out;
+ json out_obs;
+ if (success) {
+ for (int i=0; i<3; i++) out_obs.push_back(pos3d[i]);
+ }
+ json out_msg {
+ {"num", num},
+ {"time", time},
+ {"obs", out_obs}
+ };
+ cout << out_msg.dump() << endl;
+ out << "b" << out_msg.dump();
+ }
+ }
+ }
+
+ void stop() {
+ running = false;
+ srv_thread.join();
+ }
+ };
+
+ StereoServer::StereoServer() {
+ _impl = make_shared<Impl>();
+ }
+
+ StereoServer::~StereoServer() = default;
+
+ void StereoServer::start(const nlohmann::json& conf) {
+ _impl->start(conf);
+ }
+
+ void StereoServer::stop() {
+ _impl->stop();
+ }
+
+};
diff --git a/src/tracker.cpp b/src/tracker.cpp
index 59ab004..21aa71b 100644
--- a/src/tracker.cpp
+++ b/src/tracker.cpp
@@ -301,15 +301,40 @@ namespace ball_tracking {
private:
BallLogLikelihood llh;
FindBallBlob blob_detect;
+ unsigned int subsample;
public:
- CPUTracker(const json& conf) {
+ CPUTracker(const json& conf) : subsample(0) {
llh = BallLogLikelihood(conf.at("ball_log_lh"));
blob_detect = FindBallBlob(conf.at("blob_detection"));
+ if (conf.count("subsample")) subsample = conf.at("subsample");
}
- vector<KeyPoint> operator()(cv::InputArray img) {
+ vector<KeyPoint> operator()(cv::InputArray _img) {
+ Mat img = _img.getMat();
+
+ //1) Subsample if necessary
+ double fx=1.0, fy=1.0;
+ for (unsigned int i=0; i<subsample; ++i) {
+ Mat tmp;
+ pyrDown(img, tmp);
+ fx *= ((double)img.cols)/tmp.cols;
+ fy *= ((double)img.rows)/tmp.rows;
+ img = tmp;
+ }
+
+ //2) Process image and get the blobs
Mat llh_img = llh(img);
- return blob_detect(llh_img);
+ auto ans = blob_detect(llh_img);
+
+ //3) Recover the answer to the original problem
+ if (subsample != 0) {
+ for (auto& kpt : ans) {
+ kpt.pt.x *= fx;
+ kpt.pt.y *= fy;
+ kpt.size *= (fx + fy)/2.0;
+ }
+ }
+ return ans;
}
};
@@ -322,16 +347,29 @@ namespace ball_tracking {
ball_tracking::cuda::BallLogLikelihood llh; //!< Fully implemented in GPU
FindBallBlob blob_detect; //!< For the moment only implemented in CPU
cv::cuda::Stream stream;
+ unsigned int subsample;
public:
- GPUTracker(const json& conf) {
+ GPUTracker(const json& conf) : subsample(0) {
BOOST_LOG_TRIVIAL(debug) << "Creating a log-likelihood object in the GPU";
llh = ball_tracking::cuda::BallLogLikelihood(conf.at("ball_log_lh"));
BOOST_LOG_TRIVIAL(debug) << "GPU log-likelihood creation was successful. Now creating CPU blob";
blob_detect = FindBallBlob(conf.at("blob_detection"));
+ if (conf.count("subsample")) subsample = conf.at("subsample");
}
vector<KeyPoint> operator()(cv::InputArray _img) {
Mat img = _img.getMat();
+ //1) Subsample if necessary (in CPU)
+ double fx=1.0, fy=1.0;
+ for (unsigned int i=0; i<subsample; ++i) {
+ Mat tmp;
+ cv::pyrDown(img, tmp);
+ fx *= ((double)img.cols)/tmp.cols;
+ fy *= ((double)img.rows)/tmp.rows;
+ img = tmp;
+ }
+
+ //2) Process image (in GPU) and get blobs (in CPU)
GpuMat gpu_img;
GpuMat gpu_llh_img;
gpu_img.upload(img, stream);
@@ -340,7 +378,17 @@ namespace ball_tracking {
Mat llh_img;
gpu_llh_img.download(llh_img, stream);
stream.waitForCompletion();
- return blob_detect(llh_img);
+ auto ans = blob_detect(llh_img);
+
+ //3) Recover the answer to the original problem
+ if (subsample != 0) {
+ for (auto& kpt : ans) {
+ kpt.pt.x *= fx;
+ kpt.pt.y *= fy;
+ kpt.size *= (fx + fy)/2.0;
+ }
+ }
+ return ans;
}
};
#else
--
GitLab