Time as long int instead of float

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();

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)});

tests/config/server_3d_conf.json

+{
+  "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"
+  }
+}

tests/test_stereo.py

+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()