Pixelwise velocity

core/fusion/fusion.cpp

@@ -281,4 +281,4 @@ void constVelKalman::set(skeleton13 pose, double ts)
     pose_initialised = true;
 }
 
-// ========================================================================== //
+// ========================================================================== //

core/fusion/fusion.h

@@ -162,6 +162,7 @@ class singleJointLatComp {
             kf.statePost.at<float>(1) += vel_accum.v;
             vel_accum = {0.0, 0.0};
         }
+        prev_pts = ts;
     }
 
     joint query() {
@@ -173,7 +174,7 @@ class singleJointLatComp {
 class multiJointLatComp : public stateEstimator {
    private:
     std::array<singleJointLatComp, 13> kf_array;
-    bool use_lc{true};
+    bool use_lc{false};
 
    public:
     bool initialise(std::vector<double> parameters) override {

core/motion_estimation/motion_estimation.h

@@ -419,4 +419,217 @@ class surfacedVelocity
         this->tos.getSurface().copyTo(TOSout);
     }
 };
-}
+
+class pwvelocity 
+{
+private:
+
+    //parameters
+    double filter_thresh{0.0}; //seconds
+    int eros_k{7};
+    double eros_d{0.3};
+
+    //internal structure/variables
+    cv::Mat eros;
+    cv::Mat sae;
+    cv::Rect roi_full;
+    double ts_prev{0.0}, ts_curr{0.0};
+    bool use_norm{false}; //invert vector magnitude
+    bool use_om{false};   //observation model
+
+public:
+    void setParameters(cv::Size image_size, int eros_k = 7, double eros_d = 0.3, double filter_threshold = 0) 
+    {
+        eros = cv::Mat(image_size, CV_64F);
+        sae =  cv::Mat(image_size, CV_64F);
+        roi_full = cv::Rect(cv::Point(0, 0), image_size);
+        filter_thresh = filter_threshold;
+        this->eros_k = eros_k % 2 ? eros_k : eros_k + 1;
+        this->eros_d = eros_d >= 1.0 ? 0.99 : eros_d;
+        this->eros_d = eros_d < 0.0 ? 0.01 : eros_d;
+
+    }
+
+    template <typename T>
+    void update(const T &packet, double ts)
+    {
+        //these are static so they get set once and we use the same memory
+        //locations each call
+        static double odecay = pow(eros_d, 1.0 / eros_k);
+        static int half_kernel = eros_k / 2;
+        static cv::Rect roi_raw = cv::Rect(0, 0, eros_k, eros_k);
+
+        ts_curr = ts;
+
+        for(auto &v : packet) 
+        {
+            //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);
+
+            //we manually implement a filter here
+            if(ts < p_sae + filter_thresh)
+                continue;
+
+            //update the sae
+            p_sae = ts;
+
+            //decay the valid region of the eros
+            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;
+        }
+    }
+
+    jDot query_franco(int x, int y, int dRoi = 20, int dNei = 2, jDot pv = {0, 0}, bool circle = false)
+    {
+        static double eros_valid = 1.0 - eros_d;
+        jDot out = {0, 0};
+        int n = 0;
+
+        int xl = std::max(x - dRoi, dNei); 
+        int xh = std::min(x + dRoi, sae.cols - dNei);
+        int yl = std::max(y - dRoi, dNei);
+        int yh = std::min(y + dRoi, sae.rows - dNei);
+        for (int yi = yl; yi <= yh; yi++)
+        {
+            for (int xi = xl; xi <= xh; xi++)
+            {
+
+                //keep searching if not a new events
+                auto &ts = sae.at<double>(yi, xi);
+                if(ts < ts_prev)
+                    continue;
+
+                //search neighbouring events to calc: distance / time
+                int nn = 0;
+                double xdot = 0.0, ydot = 0.0;
+                for (auto yj = yi - dNei; yj <= yi + dNei; yj++)
+                {
+                    for (auto xj = xi - dNei; xj <= xi + dNei; xj++)
+                    {
+                        // if (eros.at<double>(yj, xi) >= eros_valid) {
+                        if (eros.at<double>(yj, xj) >= eros_valid && sae.at<double>(yj, xj) != ts)
+                        {
+                            double inv_dt = 1.0 / (ts - sae.at<double>(yj, xj));
+                            int dx = xi - xj;
+                            int dy = yi - yj;
+                            xdot += dx * inv_dt;
+                            ydot += dy * inv_dt;
+                            nn++;
+                        }
+                    }
+                }
+                // calculate the average for this event
+                if (nn > 1)
+                {
+                    double inv_nn = 1.0 / nn;
+                    out.u += xdot * inv_nn;
+                    out.v += ydot * inv_nn;
+                    n++;
+                }
+            }
+        }
+
+        if (n)
+        {
+            double inv_n = 1.0 / n;
+            out.u *= inv_n;
+            out.v *= inv_n;
+        }
+
+        //deal with inversion
+
+        //add observation model
+
+
+
+        return out;
+    }
+
+    // jDot query_fullroi(int x, int y, int d1 = 20, int d2 = 2, jDot pv = {0, 0})
+    // {
+
+    //     static double eros_valid = 1.0 - eros_d;
+    //     jDot out = {0, 0};
+    //     int nx = 0, ny = 0;
+
+    //     int xl = std::max(x - d1, d2);
+    //     int xh = std::min(x + d1, sae.cols - d2);
+    //     int yl = std::max(y - d1, d2);
+    //     int yh = std::min(x + d1, sae.rows - d2);
+    //     for(int yi = yl; yi < yh; y++) {
+    //         for(int xi = xl; xi < xh; x++) {
+
+    //             //keep searching if not a new events
+    //             auto &ts = sae.at<double>(yi, xi);
+    //             if(ts <= ts_prev)
+    //                 continue;
+
+    //             //search neighbouring events to calc: distance / time
+    //             int nnx = 0, nny = 0;
+    //             double dtdx = 0.0, dtdy = 0.0;
+    //             for(auto yj = yi - d2; yj < yi + d2; jy++) {
+    //                 for(auto xj = xi - d2; xj < xi + d2; xj++) {
+    //                     if (eros.at<double>(yj, xi) >= eros_valid) 
+    //                     {
+    //                         double dt = ts - sae.at<double>(yj, xj);
+    //                         int dx = xi - xj;
+    //                         int dy = yi - yj;
+    //                         if(dx) {dtdx += dt / dx; nnx++;}
+    //                         if(dy) {dtdy += dy / dy; nny++;}
+    //                     }
+    //                 }
+    //             }
+    //             // calculate the average for this event
+    //             if(nnx) {out.u += 
+    //             if (nn > 1) {
+    //                 double inv_nn = 1.0 / nn;
+    //                 out.u += xdot * inv_nn;
+    //                 out.v += ydot * inv_nn;
+    //                 n++;
+    //             }
+    //         }
+    //     }
+
+    //     if (n) {
+    //         double inv_n = 1.0 / n; 
+    //         out.u *= inv_n;
+    //         out.v *= inv_n;
+    //     }
+
+    //     //deal with inversion
+
+    //     //add observation model
+
+
+    //     ts_prev = ts_curr;
+
+    //     return out;
+    // }
+
+    skeleton13_vel query(skeleton13 points, int dRoi = 20, int dNei = 2, skeleton13_vel pv = {0}, bool circle = false)
+    {
+        skeleton13_vel out;
+        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;
+        return out;
+    }
+
+    const cv::Mat& querySAE()
+    {
+        return sae;
+    } 
+
+    const cv::Mat& queryEROS()
+    {
+        return eros;
+    }
+
+};
+
+}

core/utility/utility.h

@@ -236,7 +236,7 @@ inline void drawSkeleton(cv::Mat &image, const skeleton13 pose, std::array<int,
     // if(jb[head] && jb[shoulderL] && jb[shoulderR]) cv::line(image, (jv[shoulderL] + jv[shoulderR])/2, jv[head] + cv::Point(0, 20), colorS, th);
     if(jb[head] && jb[shoulderL] && jb[shoulderR])
     {
-        int dist = cv::norm(jv[shoulderL]-jv[shoulderR])/3;
+        int dist = cv::norm(jv[shoulderL]-jv[hipR])/6;
         cv::circle(image, jv[head] + cv::Point(0, 0.0), dist, colorS, th);
         cv::line(image, (jv[shoulderL] + jv[shoulderR])/2, jv[head] + cv::Point(0, dist), colorS, th);
     } 
@@ -254,6 +254,21 @@ inline void drawSkeleton(cv::Mat &image, const skeleton13 pose, std::array<int,
     if(jb[kneeR] && jb[footR]) cv::line(image, jv[kneeR], jv[footR], colorS, th);
 }
 
+
+inline void drawVel(cv::Mat &image, const skeleton13 pose, const skeleton13_vel vel, std::array<int, 3> color = {0, 0, 200}, int th =1) 
+{
+    skeleton13_b jb = jointTest(pose);
+    skeleton13_v jv = jointConvert(pose);
+    skeleton13_v jvel = jointConvert(vel);
+    auto colorS = CV_RGB(color[0], color[1], color[2]);
+
+    // plot detected joints
+    for (size_t i = 0; i < pose.size(); i++)
+        if (jb[i])
+            cv::arrowedLine(image, jv[i], jv[i]+jvel[i], colorS, th);
+
+}
+
 template <typename T>
 inline skeleton13 extractSkeletonFromYARP(const T &gt_container)
 {
@@ -295,8 +310,8 @@ class writer {
                 // write the full_buffer and clear it
                 for(auto &i : c_buf) 
                 {
-                    fileio << std::setprecision(5);
-                    fileio << i.timestamp << " " << i.delay;
+                    fileio << std::setprecision(5) << std::fixed;
+                    fileio << i.timestamp << " " << std::scientific << i.delay << std::fixed;
                     for(auto &j : i.pose)
                         fileio << std::setprecision(2) << " " << j.u << " " << j.v;
                     fileio << std::endl;

example/movenet/eros-framer.cpp

@@ -132,4 +132,4 @@ int main(int argc, char * argv[])
     /* create the module */
     eroser instance;
     return instance.runModule(rf);
-}
+}

example/movenet/movenet_online.py

@@ -92,11 +92,13 @@ def getPeriod(self):
     def interruptModule(self):
         # interrupting all the ports
         self.input_port.interrupt()
+        self.output_port.interrupt()
         return True
 
     def close(self):
         # closing ports
         self.input_port.close()
+        self.output_port.close()
         cv2.destroyAllWindows()
         return True
 
@@ -130,8 +132,8 @@ 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)
+        t0 = datetime.datetime.now()
 
         # input_image_resized = np.zeros([1, 3, self.image_h_model, self.image_w_model])
         # # print(input_image_resized.shape)