diff --git a/core/.gitignore b/core/.gitignore index 01f9cb9f..9aaa97c7 100644 --- a/core/.gitignore +++ b/core/.gitignore @@ -1,2 +1,3 @@ build/ -.vscode/ \ No newline at end of file +.vscode/ +*.code-workspace \ No newline at end of file diff --git a/core/fusion/fusion.cpp b/core/fusion/fusion.cpp index 1eaeec77..a76a25d6 100644 --- a/core/fusion/fusion.cpp +++ b/core/fusion/fusion.cpp @@ -6,24 +6,30 @@ bool stateEstimator::initialise(std::vector parameters) { return true; } -void stateEstimator::updateFromVelocity(jointName name, jDot velocity, double dt) +void stateEstimator::updateFromVelocity(jointName name, jDot velocity, double ts) { + double dt = ts - prev_ts; + prev_ts = ts; state[name] += (velocity * dt); } -void stateEstimator::updateFromPosition(jointName name, joint position, double dt) +void stateEstimator::updateFromPosition(jointName name, joint position, double ts) { + prev_ts = ts; state[name] = position; } -void stateEstimator::updateFromVelocity(skeleton13 velocity, double dt) +void stateEstimator::updateFromVelocity(skeleton13 velocity, double ts) { + double dt = ts - prev_ts; + prev_ts = ts; for (int j = 0; j < 13; j++) state[j] += (velocity[j] * dt); } -void stateEstimator::updateFromPosition(skeleton13 position, double dt) +void stateEstimator::updateFromPosition(skeleton13 position, double ts) { + prev_ts = ts; skeleton13_b valid = jointTest(position); for (auto &name : jointNames) { @@ -35,8 +41,9 @@ void stateEstimator::updateFromPosition(skeleton13 position, double dt) // state = position; } -void stateEstimator::set(skeleton13 pose) +void stateEstimator::set(skeleton13 pose, double ts) { + prev_ts = ts; state = pose; pose_initialised = true; } @@ -84,7 +91,7 @@ void kfEstimator::setTimePeriod(double dt) bool kfEstimator::initialise(std::vector parameters) { - if (parameters.size() != 3) + if (parameters.size() < 3) return false; procU = parameters[0]; measUD = parameters[1]; @@ -108,8 +115,11 @@ bool kfEstimator::initialise(std::vector parameters) return true; } -void kfEstimator::updateFromVelocity(jointName name, jDot velocity, double dt) +void kfEstimator::updateFromVelocity(jointName name, jDot velocity, double ts) { + double dt = ts - prev_ts; + prev_ts = ts; + auto &kf = kf_array[name]; joint j_new = state[name] + velocity * dt; setTimePeriod(dt); @@ -120,8 +130,11 @@ void kfEstimator::updateFromVelocity(jointName name, jDot velocity, double dt) state[name] = {kf.statePost.at(0), kf.statePost.at(1)}; } -void kfEstimator::updateFromPosition(jointName name, joint position, double dt) +void kfEstimator::updateFromPosition(jointName name, joint position, double ts) { + double dt = ts - prev_ts; + prev_ts = ts; + auto &kf = kf_array[name]; setTimePeriod(dt); kf.measurementNoiseCov.at(0, 0) = measUD; @@ -131,22 +144,23 @@ void kfEstimator::updateFromPosition(jointName name, joint position, double dt) state[name] = {kf.statePost.at(0), kf.statePost.at(1)}; } -void kfEstimator::updateFromPosition(skeleton13 position, double dt) +void kfEstimator::updateFromPosition(skeleton13 position, double ts) { skeleton13_b valid = jointTest(position); for (auto &name : jointNames) if(valid[name] && position[name].u && position[name].v) - updateFromPosition(name, position[name], dt); + updateFromPosition(name, position[name], ts); } -void kfEstimator::updateFromVelocity(skeleton13 velocity, double dt) +void kfEstimator::updateFromVelocity(skeleton13 velocity, double ts) { for (auto &name : jointNames) - updateFromVelocity(name, velocity[name], dt); + updateFromVelocity(name, velocity[name], ts); } -void kfEstimator::set(skeleton13 pose) +void kfEstimator::set(skeleton13 pose, double ts) { + prev_ts = ts; state = pose; for (jointName name : jointNames) { @@ -157,21 +171,6 @@ void kfEstimator::set(skeleton13 pose) pose_initialised = true; } -bool kfEstimator::poseIsInitialised() -{ - return pose_initialised; -} - -skeleton13 kfEstimator::query() -{ - return state; -} - -joint kfEstimator::query(jointName name) -{ - return state[name]; -} - // ========================================================================== // void constVelKalman::setTimePeriod(double dt) @@ -189,7 +188,7 @@ void constVelKalman::setTimePeriod(double dt) bool constVelKalman::initialise(std::vector parameters) { - if (parameters.size() != 4) + if (parameters.size() < 4) return false; procU = parameters[0]; measU = parameters[1]; @@ -229,8 +228,11 @@ bool constVelKalman::initialise(std::vector parameters) return true; } -void constVelKalman::updateFromVelocity(jointName name, jDot velocity, double dt) +void constVelKalman::updateFromVelocity(jointName name, jDot velocity, double ts) { + double dt = ts - prev_ts; + prev_ts = ts; + auto &kf = kf_array[name]; setTimePeriod(dt); kf.measurementMatrix = measurement_velocity; @@ -239,8 +241,11 @@ void constVelKalman::updateFromVelocity(jointName name, jDot velocity, double dt state[name] = {kf.statePost.at(0), kf.statePost.at(1)}; } -void constVelKalman::updateFromPosition(jointName name, joint position, double dt) +void constVelKalman::updateFromPosition(jointName name, joint position, double ts) { + double dt = ts - prev_ts; + prev_ts = ts; + auto &kf = kf_array[name]; setTimePeriod(dt); kf.measurementMatrix = measurement_position; @@ -249,20 +254,21 @@ void constVelKalman::updateFromPosition(jointName name, joint position, double d state[name] = {kf.statePost.at(0), kf.statePost.at(1)}; } -void constVelKalman::updateFromPosition(skeleton13 position, double dt) +void constVelKalman::updateFromPosition(skeleton13 position, double ts) { for (auto &name : jointNames) - updateFromPosition(name, position[name], dt); + updateFromPosition(name, position[name], ts); } -void constVelKalman::updateFromVelocity(skeleton13 velocity, double dt) +void constVelKalman::updateFromVelocity(skeleton13 velocity, double ts) { for (auto &name : jointNames) - updateFromVelocity(name, velocity[name], dt); + updateFromVelocity(name, velocity[name], ts); } -void constVelKalman::set(skeleton13 pose) +void constVelKalman::set(skeleton13 pose, double ts) { + prev_ts = ts; state = pose; for (jointName name : jointNames) { @@ -275,17 +281,4 @@ void constVelKalman::set(skeleton13 pose) pose_initialised = true; } -bool constVelKalman::poseIsInitialised() -{ - return pose_initialised; -} - -skeleton13 constVelKalman::query() -{ - return state; -} - -joint constVelKalman::query(jointName name) -{ - return state[name]; -} \ No newline at end of file +// ========================================================================== // \ No newline at end of file diff --git a/core/fusion/fusion.h b/core/fusion/fusion.h index 28da994a..79e65d30 100644 --- a/core/fusion/fusion.h +++ b/core/fusion/fusion.h @@ -36,28 +36,31 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ namespace hpecore { +// pure integration class stateEstimator { protected: bool pose_initialised{false}; skeleton13 state{0}; skeleton13_vel velocity{0}; std::deque error[13]; + double prev_ts{0.0}; public: virtual bool initialise(std::vector parameters = {}); - virtual void updateFromVelocity(jointName name, jDot velocity, double dt); - virtual void updateFromPosition(jointName name, joint position, double dt); - virtual void updateFromVelocity(skeleton13 velocity, double dt); - virtual void updateFromPosition(skeleton13 position, double dt); - virtual void set(skeleton13 pose); - bool poseIsInitialised(); - skeleton13 query(); - joint query(jointName name); + virtual void updateFromVelocity(jointName name, jDot velocity, double ts); + virtual void updateFromPosition(jointName name, joint position, double ts); + virtual void updateFromVelocity(skeleton13 velocity, double ts); + virtual void updateFromPosition(skeleton13 position, double ts); + virtual void set(skeleton13 pose, double ts); + virtual bool poseIsInitialised(); + virtual skeleton13 query(); + virtual joint query(jointName name); skeleton13_vel queryVelocity(); void setVelocity(skeleton13_vel vel); std::deque* queryError(); }; +// constant position model class kfEstimator : public stateEstimator { private: std::array kf_array; @@ -67,16 +70,14 @@ class kfEstimator : public stateEstimator { public: bool initialise(std::vector parameters) override; - void updateFromVelocity(skeleton13 velocity, double dt) override; - void updateFromVelocity(jointName name, jDot velocity, double dt) override; - void updateFromPosition(jointName name, joint position, double dt) override; - void updateFromPosition(skeleton13 position, double dt) override; - void set(skeleton13 pose) override; - bool poseIsInitialised(); - skeleton13 query(); - joint query(jointName name); + void updateFromVelocity(skeleton13 velocity, double ts) override; + void updateFromVelocity(jointName name, jDot velocity, double ts) override; + void updateFromPosition(jointName name, joint position, double ts) override; + void updateFromPosition(skeleton13 position, double ts) override; + void set(skeleton13 pose, double ts) override; }; +// constant velocity model class constVelKalman : public stateEstimator { private: std::array kf_array; @@ -87,14 +88,129 @@ class constVelKalman : public stateEstimator { public: bool initialise(std::vector parameters) override; - void updateFromVelocity(skeleton13 velocity, double dt) override; - void updateFromVelocity(jointName name, jDot velocity, double dt) override; - void updateFromPosition(jointName name, joint position, double dt) override; - void updateFromPosition(skeleton13 position, double dt) override; - void set(skeleton13 pose) override; - bool poseIsInitialised(); - skeleton13 query(); - joint query(jointName name); + void updateFromVelocity(skeleton13 velocity, double ts) override; + void updateFromVelocity(jointName name, jDot velocity, double ts) override; + void updateFromPosition(jointName name, joint position, double ts) override; + void updateFromPosition(skeleton13 position, double ts) override; + void set(skeleton13 pose, double ts) override; + +}; + +// latency compensation constant position model +class singleJointLatComp { + private: + cv::KalmanFilter kf; + joint vel_accum; + double measU; + double procU; + double prev_pts; + double prev_vts; + + public: + void initialise(double procU, double measU) { + this->procU = procU; + this->measU = measU; + + // init(state size, measurement size) + kf.init(2, 2); + kf.transitionMatrix = (cv::Mat_(2, 2) << 1, 0, 0, 1); + kf.measurementMatrix = (cv::Mat_(2, 2) << 1, 0, 0, 1); + kf.processNoiseCov = (cv::Mat_(2, 2) << procU, 0, 0, procU); + kf.measurementNoiseCov = (cv::Mat_(2, 2) << measU, 0, 0, measU); + } + + void set(joint position, double ts) { + kf.statePost.at(0) = position.u; + kf.statePost.at(1) = position.v; + prev_pts = ts; + prev_vts = ts; + vel_accum = {0, 0}; + } + + void updateFromVelocity(jDot velocity, double ts) { + vel_accum = vel_accum + velocity * (ts - prev_vts); + prev_vts = ts; + } + + void updateFromPosition(joint position, double ts, bool use_comp = true) + { + //if not using latency compensation we want to integrate before doing + //the position update. + if(!use_comp) + { + kf.statePost.at(0) += vel_accum.u; + kf.statePost.at(1) += vel_accum.v; + vel_accum = {0.0, 0.0}; + } + + // perform an (asynchronous) update of the position from the previous + // position estimated + double dt = ts - prev_pts; + kf.processNoiseCov.at(0, 0) = procU * dt; + kf.processNoiseCov.at(1, 1) = procU * dt; + kf.predict(); + 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(); + if (use_comp) + { + kf.statePost.at(0) += vel_accum.u; + kf.statePost.at(1) += vel_accum.v; + vel_accum = {0.0, 0.0}; + } + } + + joint query() { + return {kf.statePost.at(0) + vel_accum.u, + kf.statePost.at(1) + vel_accum.v}; + } +}; + +class multiJointLatComp : public stateEstimator { + private: + std::array kf_array; + bool use_lc{true}; + + public: + bool initialise(std::vector parameters) override { + if (parameters.size() < 4) + return false; + for (auto &j : kf_array) + j.initialise(parameters[0], parameters[1]); + if(parameters[3] > 0.0) use_lc = true; + return true; + } + + void updateFromVelocity(jointName name, jDot velocity, double ts) override { + kf_array[name].updateFromVelocity(velocity, ts); + state[name] = kf_array[name].query(); + } + + void updateFromVelocity(skeleton13 velocity, double ts) override { + for (auto name : jointNames) + updateFromVelocity(name, velocity[name], ts); + } + + void updateFromPosition(jointName name, joint position, double ts) override { + kf_array[name].updateFromPosition(position, ts, use_lc); + state[name] = kf_array[name].query(); + } + + void updateFromPosition(skeleton13 position, double ts) override { + for (auto name : jointNames) + updateFromPosition(name, position[name], ts); + } + + void set(skeleton13 position, double ts) override { + pose_initialised = true; + state = position; + for (auto name : jointNames) + kf_array[name].set(position[name], ts); + } }; } // namespace hpecore diff --git a/example/movenet/movenet_online.py b/example/movenet/movenet_online.py index a4832d8d..56bc816d 100644 --- a/example/movenet/movenet_online.py +++ b/example/movenet/movenet_online.py @@ -103,13 +103,11 @@ def close(self): def updateModule(self): # synchronous update called every get period seconds. - t0 = datetime.datetime.now() if dev: np_input = cv2.imread(self.files[self.file_counter]) np_input = cv2.cvtColor(np_input, cv2.COLOR_BGR2GRAY) stamp_in = 17.34450 else: - # Preparing input and output image buffers np_input = np.ones((self.image_h, self.image_w), dtype=np.uint8) self.yarp_image.resize(self.image_w, self.image_h) @@ -132,6 +130,7 @@ def updateModule(self): # self.counter += 1 # can be used to interrupt the program self.yarp_image.copy(read_image) + t0 = datetime.datetime.now() input_image = np.copy(np_input) # input_image_resized = np.zeros([1, 3, self.image_h_model, self.image_w_model]) @@ -147,7 +146,7 @@ def updateModule(self): # Predict the pose # pre = self.run_task.predict_online(input_image_resized) pre = self.run_task.predict_online(input_image) - + # Visualize the result # if self.cfg['show_center']: # img = image_show(input_image, pre=pre['joints'], center=pre['center'])