Better filer in after pose remap.

inc/HeadPoseDetector.h

@@ -20,7 +20,6 @@
 #include <opencv2/tracking/tracker.hpp>
 #include <PS3EYEDriver/src/ps3eye.h>
 #include <PS3EYEDriver/src/ps3eye_capi.h>
-#include <filter_accela.h>
 
 class MainWindow;
 
@@ -71,7 +70,6 @@ class HeadPoseDetector: public QObject {
     LandmarkDetector * lmd = nullptr;
 
      ExtendKalmanFilter12DOF_13 ekf;
-     accela _accela, _accela2;
 //    ExtendKalmanFilter12DOF_19 ekf;
 
 
@@ -93,7 +91,6 @@ class HeadPoseDetector: public QObject {
 
     QThread mainThread;
     QTimer * main_loop_timer;
-    QTimer * pose_callback_timer;
     QThread detectThread;
 
     cv::Mat rvec_init, tvec_init;
@@ -147,8 +144,7 @@ class HeadPoseDetector: public QObject {
 public:
     MainWindow * main_window;
 
-    HeadPoseDetector(): last_roi(0, 0, 0, 0), _accela(&(settings->accela_s)), 
-        _accela2(&(settings->accela_s)) {
+    HeadPoseDetector(): last_roi(0, 0, 0, 0) {
         cv::setNumThreads(1);
         log.open(settings->app_path + "/debug.txt", std::ofstream::out);
         is_running = false;
@@ -185,6 +181,7 @@ class HeadPoseDetector: public QObject {
 
     void run_detect_thread();
 
+    void pose_callback(double t, Pose pose);
 
 public:
     cv::Mat & get_preview_image() {
@@ -203,7 +200,6 @@ class HeadPoseDetector: public QObject {
 
 private slots:
     void loop();
-    void pose_callback_loop();
     void start_slot();
     void stop_slot();
 

inc/filterconfig.h

@@ -21,10 +21,10 @@ class FilterConfig : public QWidget
     Q_OBJECT
 
     double rot_smooth_min = 0.01;
-    double rot_smooth_max = 0.2;
+    double rot_smooth_max = 2.5;
 
     double rot_deadzone_min = 0.01;
-    double rot_deadzone_max = 3.0;
+    double rot_deadzone_max = 0.3;
 
     double trans_smooth_min = 0.01;
     double trans_smooth_max = 0.2;

inc/poseremapper.h

@@ -3,15 +3,23 @@
 
 #include <QObject>
 #include "fagx_datatype.h"
+#include <filter_accela.h>
+#include <QDateTime>
 
 class PoseRemapper : public QObject
 {
     Q_OBJECT
 
     Pose initial_pose;
     bool is_inited = false;
+    accela _accela, _accela2;
 
     Eigen::Matrix3d Rcam;
+    QTimer * pose_callback_timer;
+    double t_last;
+    Eigen::Vector3d eul_last, T_last;
+    double t0;
+
 public:
     explicit PoseRemapper(QObject *parent = nullptr);
 
@@ -20,6 +28,9 @@ class PoseRemapper : public QObject
 public slots:
     void on_pose_data(double t, Pose_ pose);
     void reset_center();
+
+private slots:
+    void pose_callback_loop();
 };
 
 #endif // POSEREMAPPER_H

lib/accela_filter/filter_accela.cpp

@@ -57,20 +57,17 @@ static void do_deltas(const double* deltas, double* output, F&& fun)
     }
 }
 
-Pose accela::filter(Pose raw, double dt)
+std::pair<Eigen::Vector3d, Eigen::Vector3d> accela::filter(Eigen::Vector3d eul, Eigen::Vector3d T, double dt)
 {
     double input[6] = {0};
     double output[6] = {0};
 
-    auto T = raw.pos();
-    auto ypr_deg = quat2eulers(raw.att(), true);
-
     input[TX] = T[0];
     input[TY] = T[1];
     input[TZ] = T[2];
-    input[Yaw] = ypr_deg(0);
-    input[Pitch] = ypr_deg(1);
-    input[Roll] = ypr_deg(2);
+    input[Yaw] = eul(0);
+    input[Pitch] = eul(1);
+    input[Roll] = eul(2);
 
     static constexpr double full_turn = 360.0;	
     static constexpr double half_turn = 180.0;	
@@ -86,7 +83,7 @@ Pose accela::filter(Pose raw, double dt)
             last_output[i] = f;
         }
 
-        return raw;
+        return std::make_pair(eul, T);
     }
 
     const double rot_thres{s->rot_smoothing};
@@ -146,7 +143,6 @@ Pose accela::filter(Pose raw, double dt)
     }
 
     Eigen::Vector3d ret_T(output[TX], output[TY], output[TZ]);
-    Eigen::Vector3d ret_att(output[Yaw]/57.3, output[Pitch]/57.3, output[Roll]/57.3);
-    Pose ret(ret_T, ret_att);
-    return ret;
+    Eigen::Vector3d ret_att(output[Yaw], output[Pitch], output[Roll]);
+    return std::make_pair(ret_att, ret_T);
 }

lib/accela_filter/filter_accela.h

@@ -27,7 +27,7 @@ enum Axis : int
 struct accela
 {
     accela(settings_accela * _s);
-    Pose filter(Pose raw, double dt);
+    std::pair<Eigen::Vector3d, Eigen::Vector3d> filter(Eigen::Vector3d eul, Eigen::Vector3d T, double dt);
     void center() { first_run = true; }
 private:
     settings_accela * s = nullptr;

src/HeadPoseDetector.cpp

@@ -96,34 +96,23 @@ void HeadPoseDetector::loop() {
     // log << -omg(1)*l << "," <<  omg(0)*ly << "," << ret.face_ground_speed(0) << "," << ret.face_ground_speed(1) << ","
     //     << spd(0) << "," << spd(1)  << "," << spd(2) << "," << _T(0) << "," << _T(1) << "," << _T(2) << ","
     //     << T(0) << "," << T(1) << "," << T(2) << std::endl;
-
+    pose_callback(t, pose);
     if (settings->enable_preview) {
         _show.copyTo(preview_image);
     }
 }
 
 
-void HeadPoseDetector::pose_callback_loop() {
-    Pose pose;
+void HeadPoseDetector::pose_callback(double t, Pose pose) {
     if (!inited) {
         return;
     }
 
-    double t = QDateTime::currentMSecsSinceEpoch()/1000.0 - t0;
-    double dt = t - t_last;
-    t_last = t;
     auto q0_inv = Eigen::Quaterniond::Identity();
 
-    if (settings->use_accela) {
-        pose = _accela.filter(pose_last, dt);
-        if (settings->double_accela) {
-            pose = _accela2.filter(pose, dt);
-        }
-    } else if(settings->use_ekf) {
+    if(settings->use_ekf) {
         pose = ekf.predict(t);
-    } else {
-        pose = pose_last;
-    }
+    } 
 
     auto R = pose.R();
     auto q = pose.att();
@@ -251,10 +240,6 @@ void HeadPoseDetector::run_thread() {
     connect(main_loop_timer, SIGNAL(timeout()), this, SLOT(loop()));
     main_loop_timer->start(1.0/(settings->fps + 10)*1000);
 
-    pose_callback_timer = new QTimer;
-    pose_callback_timer->moveToThread(&mainThread);
-    connect(pose_callback_timer, SIGNAL(timeout()), this, SLOT(pose_callback_loop()));
-    pose_callback_timer->start(1000/POSE_OUTPUT_FREQ);
 
 }
 

src/poseremapper.cpp

@@ -3,11 +3,19 @@
 #include <QDebug>
 #include <FlightAgxSettings.h>
 
-PoseRemapper::PoseRemapper(QObject *parent) : QObject(parent)
+PoseRemapper::PoseRemapper(QObject *parent) : QObject(parent), _accela(&(settings->accela_s)), 
+        _accela2(&(settings->accela_s))
 {
     Rcam << 0, 0, -1,
            -1, 0, 0,
             0, 1, 0;
+
+
+    pose_callback_timer = new QTimer;
+    connect(pose_callback_timer, SIGNAL(timeout()), this, SLOT(pose_callback_loop()));
+    pose_callback_timer->start(1000/POSE_OUTPUT_FREQ);
+
+    t0 = QDateTime::currentMSecsSinceEpoch()/1000.0;
 }
 
 double double_constrain(double v, double vmin, double vmax) {
@@ -40,6 +48,28 @@ double remap(double v, double input_bound, double output_bound, double _expo) {
     //return v/input_bound*output_bound;
 }
 
+void PoseRemapper::pose_callback_loop() {
+    if (settings->use_ft || settings->use_npclient) {
+        double t = QDateTime::currentMSecsSinceEpoch()/1000.0 - t0;
+        double dt = t - t_last;
+        t_last = t;
+
+        Eigen::Vector3d eul, T;
+        if (settings->use_accela) {
+            auto ret = _accela.filter(eul_last, T_last, dt);
+            eul = ret.first;
+            T = ret.second;
+            if (settings->double_accela) {
+                auto ret = _accela2.filter(eul, T, dt);
+                eul = ret.first;
+                T = ret.second;
+            }
+        }
+
+        this->send_mapped_posedata(t, std::make_pair(eul, T));
+    }
+}
+
 void PoseRemapper::on_pose_data(double t, Pose_ pose_) {
     Pose pose(pose_.second, pose_.first);
     if(!is_inited) {
@@ -65,9 +95,12 @@ void PoseRemapper::on_pose_data(double t, Pose_ pose_) {
 
         eul.y() = remap(eul.y(), settings->inp_bound_eul.y(), settings->out_bound_eul.y(), settings->expo_eul.y());
         eul.z() = remap(eul.z(), settings->inp_bound_eul.z(), settings->out_bound_eul.z(), settings->expo_eul.z());
+        eul_last = eul;
+        T_last = T;
+    } else {
+        this->send_mapped_posedata(t, std::make_pair(eul, T));
     }
 
-    this->send_mapped_posedata(t, std::make_pair(eul, T));
 }
 
 void PoseRemapper::reset_center() {