conert pitch into roll

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;
+      SE2 inv=T_f_w_.inverse();
-      R<<T_f_w_.inverse().rotation_matrix()(0,0),1e-19,T_f_w_.inverse().rotation_matrix()(0,1),
+      Quaterniond q;
-              1e-19,1.0,1e-19,
+      q = AngleAxisd(atan2(inv.so2().unit_complex().imag(),inv.so2().unit_complex().real()), Vector3d::UnitX())
-              T_f_w_.inverse().rotation_matrix()(1,0),1e-19,T_f_w_.inverse().rotation_matrix()(1,1);
+          * AngleAxisd(0.261799, Vector3d::UnitY())
-      SE3 out(R,Vector3d(T_f_w_.inverse().translation()(0),1e-19,T_f_w_.inverse().translation()(1)));
+          * AngleAxisd(0.0, Vector3d::UnitZ());
+      SE3 out(q.toRotationMatrix(),Vector3d(inv.translation()(0),1e-19,inv.translation()(1)));
       return out;
 
   }

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()));
+            auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);//roll,pitch,yaw
-            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),
+            R<<cos(euler(0)),sin(euler(0)),
-               -sin(pitch),cos(pitch);
+               -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()));
+            auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);//roll,pitch,yaw
-            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),
+            R<<cos(euler(0)),sin(euler(0)),
-                  -sin(pitch),cos(pitch);
+                    -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;
+            Quaterniond q;
-            R<<T2_.rotation_matrix()(0,0),0.0,T2_.rotation_matrix()(0,1),
+            q = AngleAxisd(pitch(), Vector3d::UnitX())
-                     0.0,1.0,0.0,
+                * AngleAxisd(0.261799, Vector3d::UnitY())
-                    T2_.rotation_matrix()(1,0),0.0,T2_.rotation_matrix()(1,1);
+                * AngleAxisd(0.0, Vector3d::UnitZ());
-            return SE3(R,Vector3d(T2_.translation()(0),1e-19,T2_.translation()(1)));
+            return SE3(q.toRotationMatrix(),Vector3d(T2_.translation()(0),1e-19,T2_.translation()(1)));
         }
         ~SE2_5(){}
 

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.x*=-1.0;
-        ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z;
+    ref.y*=-1.0;
-    else
+    ref.z = 3.141592653589793238462643383279502884197169399375 +ref.z;
-        ref.z = ref.z - 3.1415926535897932384626433832795028841971693993;
+    float3 error=ref+cur;
-    float cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z);
+    float yaw=0.0;
-    float sc_cs = cos(cur.z) * sin(ref.z) + cos(ref.z) * sin(cur.z);
+    float pitch=0.261799;
-    return  (float3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z),
+    float roll=error.z;
-                     ref_feature.y,
+    float R00=cos(yaw)*cos(pitch);//cos(pitch)
-                     -1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z));
+    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)

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),
+    Quaterniond q;
-    0.0,1.0,0.0,
+    q = AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitX())
-    T.rotation_matrix()(1,0),0.0,T.rotation_matrix()(1,1);
+          * AngleAxisd(0.261799, Vector3d::UnitY())
-    SE3 T_ref_cur(R,Vector3d(T.translation()(0),0.0,T.translation()(1)));
+          * 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