new jacobian init

GPU_version/vio/include/vio/atan_camera.h

@@ -32,6 +32,7 @@ private:
   double fx_inv_, fy_inv_;              //!< inverse focal length
   double cx_, cy_;                      //!< projection center
   double s_, s_inv_;                    //!< distortion model coeff
+  mutable double r_;
   double tans_;                         //!< distortion model coeff
   double tans_inv_;                     //!< distortion model coeff
   bool distortion_;                     //!< use distortion model?
@@ -92,6 +93,7 @@ public:
       param_[2]=cx_;
       param_[3]=cy_;
       param_[4]=s_;
+      param_[5]=r_;
      return  param_;
   }
 };

GPU_version/vio/include/vio/frame.h

@@ -164,12 +164,34 @@ namespace vio {
                 const Vector3d& pos,
                 Matrix<double,2,3>& J)
         {
-            cam_->params();
+            double fx = cam_->params()[0];
-            T_f_w_.se2().translation();
+            double fy = cam_->params()[1];
-            T_f_w_.pitch();
+            double s = cam_->params()[4];
-            dz;
+            double r = cam_->params()[5];
-            dz_n_k;
+            double x_n = pos.x();
-            dp_;
+            double y_n = pos.y();
+            double z_n = pos.z();
+            double x_c = T_f_w_.se2().translation().x();
+            double z_c = T_f_w_.se2().translation().z();
+            double theta = T_f_w_.pitch();
+
+            double alpha = (fx*(theta/r))-(fx*((x_n*x_n)/(r*r))*theta)+((1+3*s*theta*theta)/((r*r)+1))*((fx*x_n*x_n)/(r*r));
+            double beta  =               -(fx*((x_n*y_n)/(r*r))*theta)+((1+3*s*theta*theta)/((r*r)+1))*((fx*x_n*y_n)/(r*r));
+            double gamma =               -(fy*((x_n*y_n)/(r*r))*theta)+((1+3*s*theta*theta)/((r*r)+1))*((fy*x_n*y_n)/(r*r));
+            double lamda = (fy*(theta/r))-(fy*((y_n*y_n)/(r*r))*theta)+((1+3*s*theta*theta)/((r*r)+1))*((fy*y_n*y_n)/(r*r));
+
+	        double Xf_Xc = x_n - x_c;
+	        double Zf_Zc = z_n - z_c;
+	        double n1 = -1*sin(theta)*Xf_Xc + cos(theta)*Zf_Zc;
+	        double n2 = -1*cos(theta)*Xf_Xc - sin(theta)*Zf_Zc;
+
+            J(0,0) = ((-1*cos(theta)/z_n)*alpha)-((sin(theta)/z_n)*beta)-(((x_n/(z_n*z_n))*alpha + (y_n/(z_n*z_n))*beta)*n1);
+            J(0,1) = 0;
+            J(0,2) = ((sin(theta)/z_n)*alpha)-((cos(theta)/z_n)*beta)-(((x_n/(z_n*z_n))*alpha + (y_n/(z_n*z_n))*beta)*n2);
+
+            J(1,0) = ((-1*cos(theta)/z_n)*gamma)-((sin(theta)/z_n)*lamda)-(((x_n/(z_n*z_n))*gamma + (y_n/(z_n*z_n))*lamda)*n1);
+            J(1,1) = 0;
+            J(1,2) = ((sin(theta)/z_n)*gamma)-((cos(theta)/z_n)*lamda)-(((x_n/(z_n*z_n))*gamma + (y_n/(z_n*z_n))*lamda)*n2);
         }
     };
 

GPU_version/vio/src/atan_camera.cpp

@@ -21,7 +21,7 @@ ATANCamera(double width, double height,
            fx_(width*fx), fy_(height*fy),
            fx_inv_(1.0/fx_), fy_inv_(1.0/fy_),
            cx_(cx*width - 0.5), cy_(cy*height - 0.5),
-           s_(s), s_inv_(1.0/s_)
+           s_(s), s_inv_(1.0/s_), r_(0.0)
 {
   if(s_ != 0.0)
   {
@@ -76,6 +76,7 @@ Vector2d ATANCamera::
 world2cam(const Vector2d& uv) const
 {
   double r = uv.norm();
+  r_ = r;
   double factor = rtrans_factor(r);
   Vector2d dist_cam = factor * uv;
   return Vector2d(cx_ + fx_ * dist_cam[0],