From 670f33401e9b45aee30c0e4e8d60eee917ed40e2 Mon Sep 17 00:00:00 2001
From: gfuse <majidgeravand68@gmail.com>
Date: Sat, 28 Aug 2021 13:51:50 +0200
Subject: [PATCH] commmit for add 15 degrees roll

---
 GPU_version/vio/include/vio/frame.h        |  1 +
 GPU_version/vio/include/vio/global.h       |  1 +
 GPU_version/vio/kernel/compute-residual.cl | 28 +++++++++++++++-------
 GPU_version/vio/src/depth_filter.cpp       |  1 +
 4 files changed, 22 insertions(+), 9 deletions(-)

diff --git a/GPU_version/vio/include/vio/frame.h b/GPU_version/vio/include/vio/frame.h
index 7408805..bc47419 100644
--- a/GPU_version/vio/include/vio/frame.h
+++ b/GPU_version/vio/include/vio/frame.h
@@ -125,6 +125,7 @@ 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,
diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h
index 86cc64a..eb5375c 100644
--- a/GPU_version/vio/include/vio/global.h
+++ b/GPU_version/vio/include/vio/global.h
@@ -97,6 +97,7 @@ namespace vio
             return atan2(T2_.rotation_matrix()(0,1),T2_.rotation_matrix()(0,0));
         }
         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,
diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl
index 7dc5ad0..158937a 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=error.z;
+    float roll=15.0;
+    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 bef435e..b66875d 100644
--- a/GPU_version/vio/src/depth_filter.cpp
+++ b/GPU_version/vio/src/depth_filter.cpp
@@ -217,6 +217,7 @@ 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);