diff --git a/src/server.cpp b/src/server.cpp
index 2af474ce842c97d1d29a4b328d26435b36dce7e1..e23b0355ace8e816d12ce5437952233f18ddcbff 100644
--- a/src/server.cpp
+++ b/src/server.cpp
@@ -85,10 +85,9 @@ namespace ball_tracking {
//3.1) Create a call-back
auto call_back = [ID, this](const Frame& frame) -> void {
- duration<double, std::milli> obs_time = frame.time - this->start_time;
BOOST_LOG_TRIVIAL(debug) << "Obs2D call-back called on camera " << ID <<
" Frame: {cam_id: " << frame.cam_id << ", num:" << frame.num <<
- ", time: " << obs_time.count() << "}";
+ ", time: " << frame.time.time_since_epoch().count() << "}";
Tracker track = this->trackers.at(ID);
vector<cv::KeyPoint> obs2d = track(frame.img);
json obs;
@@ -99,7 +98,7 @@ namespace ball_tracking {
json jframe {
{"cam_id", frame.cam_id},
{"num", frame.num},
- {"time", obs_time.count()},
+ {"time", frame.time.time_since_epoch().count()},
{"obs", obs}
};
BOOST_LOG_TRIVIAL(debug) << "Sending message: " << jframe.dump();
diff --git a/src/stereo.cpp b/src/stereo.cpp
index 32882f05797fdd4bca94f3363c9cc1f264e07963..e80911f33786171599f5d6c2dce237b805bf6f8e 100644
--- a/src/stereo.cpp
+++ b/src/stereo.cpp
@@ -29,7 +29,7 @@ namespace ball_tracking {
class MultiObs2D {
private:
unsigned int num; //!< ID of the observation
- double _time; //!< A consolidated time of the observation
+ long long _time; //!< A consolidated time of the observation
unsigned int _num_obs;
public:
vector<robotics::pt2d> obs2d; //!< List of observations
@@ -45,11 +45,11 @@ namespace ball_tracking {
return _num_obs;
}
- double time() const {
+ long long time() const {
return _time;
}
- void add_obs(unsigned int cam_id, double time, bool is_obs, const robotics::pt2d& x) {
+ void add_obs(unsigned int cam_id, long long time, bool is_obs, const robotics::pt2d& x) {
if (!this->is_obs[cam_id]) {
this->is_obs[cam_id] = is_obs;
_time = time;
@@ -139,7 +139,7 @@ namespace ball_tracking {
//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");
+ long long time = jobs.at("time");
robotics::pt2d x; bool is_obs = false;
if (observations.count(num) == 0) {
observations.insert({num, MultiObs2D(num_cams)});
diff --git a/tests/config/server_3d_conf.json b/tests/config/server_3d_conf.json
new file mode 100644
index 0000000000000000000000000000000000000000..4ad7a24905d84d6027e50ddb4c5b35fc675a884d
--- /dev/null
+++ b/tests/config/server_3d_conf.json
@@ -0,0 +1,38 @@
+{
+ "publisher": "tcp://*:7660",
+ "obs2d": ["tcp://localhost:7650"],
+ "num_cams": 4,
+ "stereo": {
+ "calib": [
+ {
+ "ID": 0,
+ "val": [[-1567.39362382, -33.3690129933, 47.8044304449, 1343.95825491],
+ [-93.797389367, 554.251094363, -1399.18428581, 1156.08458791],
+ [-0.29028677999, -0.990687462996, -0.633844350237, 1.0]]
+ },
+ {
+ "ID": 1,
+ "val": [[-1161.93662474, -638.762854681, -424.816354558, -799.614820568],
+ [78.6136019544, 489.435381802, -1261.47294423, 1032.66675885],
+ [0.391393750933, -0.891927573998, -0.551135528946, 1.0]]
+ },
+ {
+ "ID": 2,
+ "val": [[328.063408018, -14.5000497676, 12.1223018211, 281.932921632],
+ [18.5435065698, -100.013191503, -310.455765145, -120.2653973],
+ [0.0601112874362, 0.179588576061, -0.0935763740174, 1.0]]
+ },
+ {
+ "ID": 3,
+ "val": [[259.286865958, 143.588158876, -74.61764687, 374.753649284],
+ [-23.6615288964, -90.6748563186, -299.006538944, -83.3338272185],
+ [-0.0945259689509, 0.176698553795, -0.0882031045933, 1.0]]
+ }
+ ]
+ },
+ "log": {
+ "auto_flush": true,
+ "file": "/tmp/ball_tracking_3d.log",
+ "severity": "trace"
+ }
+}
diff --git a/tests/test_stereo.py b/tests/test_stereo.py
new file mode 100644
index 0000000000000000000000000000000000000000..62687a2b90db2892c905e7eb83ab1bdeda5cfafb
--- /dev/null
+++ b/tests/test_stereo.py
@@ -0,0 +1,61 @@
+import numpy as np
+import unittest
+import zmq
+import subprocess
+import matplotlib.pyplot as plt
+import os
+import json
+import time
+
+class TestStereo(unittest.TestCase):
+
+ def setUp(self):
+ build_path = '../build/release'
+ ps_path = os.path.join(build_path,'examples/track_server3d')
+ conf_path = './config/server_3d_conf.json'
+ cmd = [ps_path, '-c', conf_path]
+ self.server_process = subprocess.Popen(cmd)
+ self.conf = json.load(file(conf_path,'r'))
+ ctx = zmq.Context()
+ self.obs2d = ctx.socket(zmq.PUB)
+ self.obs2d.bind("tcp://*:7650")
+ self.obs3d = ctx.socket(zmq.SUB)
+ self.obs3d.connect("tcp://localhost:7660")
+ self.obs3d.setsockopt(zmq.SUBSCRIBE, "")
+ time.sleep(0.5)
+ print("Set up succedded")
+
+ def tearDown(self):
+ self.server_process.terminate()
+ print("Sending terminate signal")
+ time.sleep(0.5)
+
+ def test_no_outliers(self):
+ pts_x = np.linspace(-1,1,10)
+ pts_y = np.linspace(-4,0,10)
+ pts_z = np.linspace(-2,0,10)
+ num = 0
+ for x in pts_x:
+ for y in pts_y:
+ for z in pts_z:
+ for c in self.conf["stereo"]["calib"]:
+ c_id = c['ID']
+ C = np.array(c['val'])
+ pt2dh = np.dot(C, np.array([x,y,z,1.0]))
+ pt2d = [pt2dh[0]/pt2dh[2], pt2dh[1]/pt2dh[2]]
+ pt2d_msg = {"cam_id": c_id, "num": num, "obs": pt2d, "time": 0.0}
+ #print(json.dumps(pt2d_msg))
+ self.obs2d.send(json.dumps(pt2d_msg))
+ num += 1
+ pt3d_msg = self.obs3d.recv()
+ pt3d_j = json.loads(pt3d_msg)
+ self.assertLessEqual(np.linalg.norm(
+ np.array(pt3d_j['obs']) - np.array([x,y,z])), 0.02)
+ #print(num)
+
+
+ def test_with_outliers(self):
+ pass
+
+if __name__=='__main__':
+ unittest.main()