added sobel kernel for image gradient calcualtion

include/msckf_vio/feature.hpp

@@ -168,6 +168,11 @@ struct Feature {
                   const CameraCalibration& cam,
                   Eigen::Vector3d& in_p) const;
 
+  double Kernel(
+            const cv::Point2f pose,
+            const cv::Mat frame,
+            std::string type) const;
+
   /*
   * @brief IrradianceAnchorPatch_Camera returns irradiance values
   *        of the Anchor Patch position in a camera frame
@@ -387,6 +392,31 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const
   else return false;
 }
 
+double Feature::Kernel(
+                const cv::Point2f pose,
+                const cv::Mat frame,
+                std::string type) const
+{
+Eigen::Matrix<double, 3, 3> kernel = Eigen::Matrix<double, 3, 3>::Zero();
+if(type == "Sobel_x")
+  kernel  << -1., 0., 1.,-2., 0., 2. , -1., 0., 1.;
+else if(type == "Sobel_y")
+  kernel << -1., -2., -1., 0., 0., 0., 1., 2., 1.;
+
+double delta = 0;
+int offs = (int)(kernel.rows()-1)/2;
+for(int i = 0; i < kernel.rows(); i++){
+
+  for(int j = 0; j < kernel.cols(); j++)
+  { 
+    std::cout << "i: " << i << ":" << "j: " << j << ":" << kernel(i,j) << std::endl;
+    std::cout <<"pose: " << pose.y+i-offs << " : " << pose.x+j-offs << std::endl;
+    delta += ((float)frame.at<uint8_t>(pose.y+i-offs , pose.x+j-offs))/255 * (float)kernel(i,j);
+  }
+}
+std::cout << "delta  " << delta << std::endl;
+return delta;
+}
 bool Feature::estimate_FrameIrradiance(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,