From 6351f5c03100b0e12d1b47a1c8c75b143e18dedd Mon Sep 17 00:00:00 2001
From: gfuse <majidgeravand68@gmail.com>
Date: Sat, 28 Aug 2021 18:11:10 +0200
Subject: [PATCH] conert pitch into roll

---
 GPU_version/vio/include/vio/frame.h        | 11 ++++----
 GPU_version/vio/include/vio/global.h       | 32 ++++++++--------------
 GPU_version/vio/kernel/compute-residual.cl | 28 +++++++++++++------
 GPU_version/vio/src/depth_filter.cpp       | 10 +++----
 4 files changed, 42 insertions(+), 39 deletions(-)

diff --git a/GPU_version/vio/include/vio/frame.h b/GPU_version/vio/include/vio/frame.h
index bc47419..3cdc53a 100644
--- a/GPU_version/vio/include/vio/frame.h
+++ b/GPU_version/vio/include/vio/frame.h
@@ -126,11 +126,12 @@ public:
 
   inline  SE3 getSE3Inv() const{
       //TODO add 15 degree roll orientation
-      Eigen::Matrix<double,3,3> R;
-      R<<T_f_w_.inverse().rotation_matrix()(0,0),1e-19,T_f_w_.inverse().rotation_matrix()(0,1),
-              1e-19,1.0,1e-19,
-              T_f_w_.inverse().rotation_matrix()(1,0),1e-19,T_f_w_.inverse().rotation_matrix()(1,1);
-      SE3 out(R,Vector3d(T_f_w_.inverse().translation()(0),1e-19,T_f_w_.inverse().translation()(1)));
+      SE2 inv=T_f_w_.inverse();
+      Quaterniond q;
+      q = AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real()), Vector3d::UnitX())
+          * AngleAxisd(0.261799, Vector3d::UnitY())
+          * AngleAxisd(0.0, Vector3d::UnitZ());
+      SE3 out(q.toRotationMatrix(),Vector3d(inv.translation()(0),1e-19,inv.translation()(1)));
       return out;
 
   }
diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h
index ff92225..d4305f4 100644
--- a/GPU_version/vio/include/vio/global.h
+++ b/GPU_version/vio/include/vio/global.h
@@ -58,26 +58,18 @@ namespace vio
         }
         SE2_5(SE3&& se3){
             Quaterniond q=se3.unit_quaternion().normalized();
-            double pitch=asin(2.0*(q.w()*q.y()-q.z()*q.x()));
-            if (std::abs(pitch) >= 1)
-                pitch = std::copysign(M_PI / 2.0, pitch); // use 90 degrees if out of range
-            else
-                pitch = std::asin(pitch);
+            auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);//roll,pitch,yaw
             Eigen::Matrix<double, 2,2> R;
-            R<<cos(pitch),sin(pitch),
-               -sin(pitch),cos(pitch);
+            R<<cos(euler(0)),sin(euler(0)),
+               -sin(euler(0)),cos(euler(0));
             T2_ = SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
         }
         SE2_5(SE3& se3){
             Quaterniond q=se3.unit_quaternion().normalized();
-            double pitch=asin(2.0*(q.w()*q.y()-q.z()*q.x()));
-            if (std::abs(pitch) >= 1)
-                pitch = std::copysign(M_PI / 2.0, pitch); // use 90 degrees if out of range
-            else
-                pitch = std::asin(pitch);
-             Eigen::Matrix<double, 2,2> R;
-             R<<cos(pitch),sin(pitch),
-                  -sin(pitch),cos(pitch);
+            auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);//roll,pitch,yaw
+            Eigen::Matrix<double, 2,2> R;
+            R<<cos(euler(0)),sin(euler(0)),
+                    -sin(euler(0)),cos(euler(0));
             T2_=SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
         }
         SE2_5(double y,double z,double roll){
@@ -99,11 +91,11 @@ namespace vio
         }
         SE3 se3() const{
             //Todo add 15 roll orientation
-            Eigen::Matrix<double,3,3> R;
-            R<<T2_.rotation_matrix()(0,0),0.0,T2_.rotation_matrix()(0,1),
-                     0.0,1.0,0.0,
-                    T2_.rotation_matrix()(1,0),0.0,T2_.rotation_matrix()(1,1);
-            return SE3(R,Vector3d(T2_.translation()(0),1e-19,T2_.translation()(1)));
+            Quaterniond q;
+            q = AngleAxisd(pitch(), Vector3d::UnitX())
+                * AngleAxisd(0.261799, Vector3d::UnitY())
+                * AngleAxisd(0.0, Vector3d::UnitZ());
+            return SE3(q.toRotationMatrix(),Vector3d(T2_.translation()(0),1e-19,T2_.translation()(1)));
         }
         ~SE2_5(){}
 
diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl
index 7dc5ad0..d70f6e0 100644
--- a/GPU_version/vio/kernel/compute-residual.cl
+++ b/GPU_version/vio/kernel/compute-residual.cl
@@ -15,15 +15,25 @@ float2 world2cam(float3 feature)
 
 float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature)
 {
-    if(ref.z < 0.0)
-        ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z;
-    else
-        ref.z = ref.z - 3.1415926535897932384626433832795028841971693993;
-    float cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z);
-    float sc_cs = cos(cur.z) * sin(ref.z) + cos(ref.z) * sin(cur.z);
-    return  (float3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z),
-                     ref_feature.y,
-                     -1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z));
+    ref.x*=-1.0;
+    ref.y*=-1.0;
+    ref.z = 3.141592653589793238462643383279502884197169399375 +ref.z;
+    float3 error=ref+cur;
+    float yaw=0.0;
+    float pitch=0.261799;
+    float roll=error.z;
+    float R00=cos(yaw)*cos(pitch);//cos(pitch)
+    float R01=cos(yaw)*sin(pitch)*sin(roll)-sin(yaw)*cos(roll);//0
+    float R02=cos(yaw)*sin(pitch)*cos(roll)+sin(yaw)*sin(roll);//sin(pitch)
+    float R10=sin(yaw)*cos(pitch);//0
+    float R11=sin(yaw)*sin(pitch)*sin(roll)+cos(yaw)*cos(roll);//1
+    float R12=sin(yaw)*sin(pitch)*cos(roll)-cos(yaw)*sin(roll);//0
+    float R20=-sin(pitch);//-sin(pitch)
+    float R21=cos(pitch)*sin(roll);//0
+    float R22=cos(pitch)*cos(roll);//cos(pitch)
+    return  (float3)(R00*ref_feature.x+R01*ref_feature.y+R02*ref_feature.z+error.x,
+                     R10*ref_feature.x+R11*ref_feature.y+R12*ref_feature.z,
+                     R20*ref_feature.x+R21*ref_feature.y+R22*ref_feature.z+error.y);
 }
 
 void jacobian_xyz2uv(float3 xyz_in_f, float* J)
diff --git a/GPU_version/vio/src/depth_filter.cpp b/GPU_version/vio/src/depth_filter.cpp
index b66875d..71ca074 100644
--- a/GPU_version/vio/src/depth_filter.cpp
+++ b/GPU_version/vio/src/depth_filter.cpp
@@ -216,12 +216,12 @@ void DepthFilter::updateSeeds(FramePtr frame)
     }
     // check if point is visible in the current image
     SE2 T(it->ftr->frame->T_f_w_.pitch()-frame->T_f_w_.pitch(),it->ftr->frame->T_f_w_.se2().translation()- frame->T_f_w_.se2().translation());
-    Eigen::Matrix<double,3,3> R;
     ///TODO add 15 degrees roll orientation
-    R<<T.rotation_matrix()(0,0),0.0,T.rotation_matrix()(0,1),
-    0.0,1.0,0.0,
-    T.rotation_matrix()(1,0),0.0,T.rotation_matrix()(1,1);
-    SE3 T_ref_cur(R,Vector3d(T.translation()(0),0.0,T.translation()(1)));
+    Quaterniond q;
+    q = AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitX())
+          * AngleAxisd(0.261799, Vector3d::UnitY())
+          * AngleAxisd(0.0, Vector3d::UnitZ());
+    SE3 T_ref_cur(q.toRotationMatrix(),Vector3d(T.translation()(0),0.0,T.translation()(1)));
     const Vector3d xyz_f(T_ref_cur.inverse()*(1.0/it->mu * it->ftr->f) );
     if(xyz_f.z() < 0.0)  {
       ++it; // behind the camera