diff --git a/core/detection_wrappers/openpose_detector.cpp b/core/detection_wrappers/openpose_detector.cpp index c9099a5e..3176c0c3 100644 --- a/core/detection_wrappers/openpose_detector.cpp +++ b/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(t1 - t0); - latency = std::chrono::duration(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; -} diff --git a/core/detection_wrappers/openpose_detector.h b/core/detection_wrappers/openpose_detector.h index a90cca24..8cc4f938 100644 --- a/core/detection_wrappers/openpose_detector.h +++ b/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(); }; } \ No newline at end of file diff --git a/core/fusion/fusion.h b/core/fusion/fusion.h index 3b0688c9..447fc1e7 100644 --- a/core/fusion/fusion.h +++ b/core/fusion/fusion.h @@ -101,6 +101,8 @@ class singleJointLatComp { private: cv::KalmanFilter kf; joint vel_accum; + std::deque history_v; + std::deque 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(0) += vel_accum.u; kf.statePost.at(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(0) += history_v[i].u; + kf.statePost.at(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_(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(0) += vel_accum.u; - kf.statePost.at(1) += vel_accum.v; - vel_accum = {0.0, 0.0}; + for(;i < history_ts.size(); i++) + { + kf.statePost.at(0) += history_v[i].u; + kf.statePost.at(1) += history_v[i].v; + } } + vel_accum = {0.0, 0.0}; + history_v.clear(); + history_ts.clear(); prev_pts = ts; + } joint query() { diff --git a/core/motion_estimation/motion_estimation.h b/core/motion_estimation/motion_estimation.h index 9792e804..c23b4985 100644 --- a/core/motion_estimation/motion_estimation.h +++ b/core/motion_estimation/motion_estimation.h @@ -450,8 +450,8 @@ class pwvelocity } - template - void update(const T &packet, double ts) + template + 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(v.y, v.x); - auto &p_eros = eros.at(v.y, v.x); + auto &p_sae = sae.at(v->y, v->x); + auto &p_eros = eros.at(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 + void update(const T &packet, double ts) + { + update(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;