Merge pull request #10 from xuhao1/master

Fix translation align

FlightAgentX.pro

@@ -3,7 +3,7 @@ QT += widgets
 
 CONFIG += c++17
 DEFINES += QT_DEPRECATED_WARNINGS
-
+QMAKE_CXXFLAGS *= /std:c++17
 INCLUDEPATH += ../third_party/opencv-4.4.0-build/include
 INCLUDEPATH +=../third_party/eigen-3.3.7
 INCLUDEPATH += ../third_party/dlib/include

lib/freetrack/ftnoir_protocol_ft.cpp

@@ -22,8 +22,8 @@ freetrack::~freetrack()
     dummyTrackIR.close();
 }
 
-static_assert(sizeof(LONG) == sizeof(std::int32_t));
-static_assert(sizeof(LONG) == 4u);
+//static_assert(sizeof(LONG) == sizeof(std::int32_t));
+//static_assert(sizeof(LONG) == 4u);
 
 #define TX 0
 #define TY 1
@@ -43,16 +43,16 @@ never_inline void store(float volatile& place, const float value)
 
     value_.f32 = value;
 
-    static_assert(sizeof(value_) == sizeof(float));
-    static_assert(offsetof(decltype(value_), f32) == offsetof(decltype(value_), i32));
+//    static_assert(sizeof(value_) == sizeof(float));
+//    static_assert(offsetof(decltype(value_), f32) == offsetof(decltype(value_), i32));
 
     (void)InterlockedExchange((LONG volatile*)&place, value_.i32);
 }
 
 template<typename t>
 static void store(t volatile& place, t value)
 {
-    static_assert(sizeof(t) == 4u);
+//    static_assert(sizeof(t) == 4u);
     (void)InterlockedExchange((LONG volatile*) &place, (LONG)value);
 }
 
@@ -124,7 +124,7 @@ void freetrack::pose(const double* headpose, const double* raw)
         {
             // FTHeap pMemData happens to be aligned on a page boundary by virtue of
             // memory mapping usage (MS Windows equivalent of mmap(2)).
-            static_assert((offsetof(FTHeap, table) & (sizeof(LONG)-1)) == 0);
+//            static_assert((offsetof(FTHeap, table) & (sizeof(LONG)-1)) == 0);
 
             for (unsigned k = 0; k < 2; k++)
                 store(pMemData->table_ints[k], t.ints[k]);

src/HeadPoseDetector.cpp

@@ -82,7 +82,7 @@ void HeadPoseDetector::loop() {
     }
 
     auto q0_inv = P0.att().inverse();
-    // q0_inv = Eigen::Quaterniond::Identity();
+    q0_inv = Eigen::Quaterniond::Identity();
     this->on_detect_pose6d_raw(t, make_pair(R2ypr(q0_inv*pose_raw.R()), q0_inv*pose_raw.pos()));
 
     t = QDateTime::currentMSecsSinceEpoch()/1000.0 - t0;

src/PoseDataSender.cpp

@@ -20,9 +20,12 @@ void PoseDataSender::send_data_udp(double t, Pose6DoF pose) {
     //Send packet
     //Debug only
     double data[6] = {0};
-    data[0] = pose.second.x()*100;
-    data[1] = - pose.second.y()*100;
-    data[2] = pose.second.z()*100;
+//    data[0] = pose.second.x()*100;
+//    data[1] = - pose.second.y()*100;
+//    data[2] = pose.second.z()*100;
+    data[0] = -pose.second.y()*100;
+    data[1] = -pose.second.z()*100;
+    data[2] = -pose.second.x()*100;
 
     data[3] = eul(0);
     data[4] = eul(1);

src/poseremapper.cpp

@@ -14,13 +14,14 @@ void PoseRemapper::on_pose_data(double t, Pose_ pose_) {
     Pose pose(pose_.second, pose_.first);
     if(!is_inited) {
         qDebug() << "Reset initial pose";
+        std::cout << initial_pose.att().toRotationMatrix() << std::endl;
         initial_pose = pose;
         is_inited = true;
     }
 
     auto Q = initial_pose.att().inverse() * pose.att();
     Eigen::Vector3d T = pose.pos() - initial_pose.pos();
-
+    T = initial_pose.att().inverse() * T;
     auto eul = quat2eulers(Q);
     this->send_mapped_posedata(t, std::make_pair(eul, T));
 }