EV2 update

core/detection_wrappers/openpose_detector.cpp

@@ -159,11 +159,7 @@ void openposethread::run()
         m.lock();
         if (stop)
             return;
-        auto t0 = std::chrono::high_resolution_clock::now();
         pose.pose = detop.detect(image);
-        auto t1 = std::chrono::high_resolution_clock::now();
-        std::chrono::microseconds ms = std::chrono::duration_cast<std::chrono::microseconds>(t1 - t0);
-        latency = std::chrono::duration<double>(ms).count() * 1e3;
         data_ready = true;
     }
 }
@@ -203,6 +199,7 @@ bool openposethread::update(cv::Mat next_image, double image_timestamp, hpecore:
 
     // else set the result to the provided stampedPose
     previous_result = pose;
+    previous_result.delay = image_timestamp - pose.timestamp;
 
     // set the timestamp
     pose.timestamp = image_timestamp;
@@ -224,8 +221,3 @@ bool openposethread::update(cv::Mat next_image, double image_timestamp, hpecore:
     data_ready = false;
     return true;
 }
-
-double openposethread::getLatency()
-{
-    return latency;
-}

core/detection_wrappers/openpose_detector.h

@@ -53,7 +53,6 @@ class openposethread
     hpecore::OpenPoseDetector detop;
     hpecore::stampedPose pose{0.0, -1.0, 0.0};
     cv::Mat image;
-    double latency = 0;
 
     bool stop{false};
     bool data_ready{true};
@@ -67,7 +66,6 @@ class openposethread
     void close();
     bool update(cv::Mat next_image, double image_timestamp, 
                 hpecore::stampedPose &previous_result);
-    double getLatency();
 };
 
 }

core/fusion/fusion.h

@@ -101,6 +101,8 @@ class singleJointLatComp {
    private:
     cv::KalmanFilter kf;
     joint vel_accum;
+    std::deque<joint> history_v;
+    std::deque<double> history_ts;
     double measU;
     double procU;
     double prev_pts;
@@ -128,19 +130,32 @@ class singleJointLatComp {
     }
 
     void updateFromVelocity(jDot velocity, double ts) {
-        vel_accum = vel_accum + velocity * (ts - prev_vts);
+        history_v.push_back(velocity * (ts - prev_vts));
+        history_ts.push_back(ts);
+        vel_accum = vel_accum + history_v.back();
         prev_vts = ts;
     }
 
     void updateFromPosition(joint position, double ts, bool use_comp = true)
     {
+        size_t i = 0; //position in history
         //if not using latency compensation we want to integrate before doing
         //the position update.
-        if(!use_comp) 
+        if(!use_comp)
         {
             kf.statePost.at<float>(0) += vel_accum.u;
             kf.statePost.at<float>(1) += vel_accum.v;
-            vel_accum = {0.0, 0.0};
+        }
+        //if we use latency compensation we want to integrate only up to the
+        //point the detection was made
+        else
+        {
+            for(;i < history_ts.size(); i++)
+            {
+                if(history_ts[i] > ts) break;
+                kf.statePost.at<float>(0) += history_v[i].u;
+                kf.statePost.at<float>(1) += history_v[i].v;
+            }
         }
 
         // perform an (asynchronous) update of the position from the previous
@@ -152,17 +167,20 @@ class singleJointLatComp {
         if(position.u > 0 || position.v > 0)
             kf.correct((cv::Mat_<float>(2, 1) << position.u, position.v));
 
-        // add the current period velocity accumulation to the state
-        // vel_accum.u *= 5; vel_accum.v *= 5;
-        // std::cout << vel_accum.u << " " << vel_accum.v << std::endl;
-        //std::cout.flush();
+        // add the remaining current period velocity accumulation to the state
         if (use_comp)
         {
-            kf.statePost.at<float>(0) += vel_accum.u;
-            kf.statePost.at<float>(1) += vel_accum.v;
-            vel_accum = {0.0, 0.0};
+            for(;i < history_ts.size(); i++)
+            {
+                kf.statePost.at<float>(0) += history_v[i].u;
+                kf.statePost.at<float>(1) += history_v[i].v;
+            }
         }
+        vel_accum = {0.0, 0.0};
+        history_v.clear();
+        history_ts.clear();
         prev_pts = ts;
+
     }
 
     joint query() {

core/motion_estimation/motion_estimation.h

@@ -450,8 +450,8 @@ class pwvelocity
 
     }
 
-    template <typename T>
-    void update(const T &packet, double ts)
+    template<typename T>
+    void update(const T &begin, const T &end, double ts)
     {
         //these are static so they get set once and we use the same memory
         //locations each call
@@ -461,11 +461,11 @@ class pwvelocity
 
         ts_curr = ts;
 
-        for(auto &v : packet) 
+        for(auto v = begin; v != end; v++)
         {
             //get references to the indexed pixel locations
-            auto &p_sae  =  sae.at<double>(v.y, v.x);
-            auto &p_eros = eros.at<double>(v.y, v.x);
+            auto &p_sae  =  sae.at<double>(v->y, v->x);
+            auto &p_eros = eros.at<double>(v->y, v->x);
 
             //we manually implement a filter here
             if(ts < p_sae + filter_thresh)
@@ -475,13 +475,20 @@ class pwvelocity
             p_sae = ts;
 
             //decay the valid region of the eros
-            roi_raw.x = v.x - half_kernel;
-            roi_raw.y = v.y - half_kernel;
+            roi_raw.x = v->x - half_kernel;
+            roi_raw.y = v->y - half_kernel;
             eros(roi_raw & roi_full) *= odecay;
 
             //set the eros position to max
             p_eros = 1.0;
         }
+
+    }
+
+    template <typename T>
+    void update(const T &packet, double ts)
+    {
+        update<T::iterator>(packet.begin(), packet.end(), ts);
     }
 
     jDot query_franco(int x, int y, int dRoi = 20, int dNei = 2, jDot pv = {0, 0}, bool circle = false)
@@ -613,7 +620,7 @@ class pwvelocity
 
     skeleton13_vel query(skeleton13 points, int dRoi = 20, int dNei = 2, skeleton13_vel pv = {0}, bool circle = false)
     {
-        skeleton13_vel out;
+        skeleton13_vel out = {0};
         for(size_t i = 0; i < points.size(); i++)
             out[i] = query_franco(points[i].u, points[i].v, dRoi, dNei, pv[i], circle);
 	    // ts_prev = ts_curr;