changed to simple irradiance calcualation with derivation of image per frame - not working

include/msckf_vio/feature.hpp

@@ -184,6 +184,11 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
                   const CameraCalibration& cam,
                   Eigen::Vector3d& in_p) const;
 
+double CompleteCvKernel(
+                const cv::Point2f pose,
+                const cv::Mat& frame,
+                std::string type) const;
+
   double cvKernel(
             const cv::Point2f pose,
             std::string type) const;
@@ -488,6 +493,45 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const
   else return false;
 }
 
+double Feature::CompleteCvKernel(
+                const cv::Point2f pose,
+                const cv::Mat& frame,
+                std::string type) const
+{
+
+  double delta = 0;
+
+  cv::Mat xder;
+  cv::Mat yder;
+  cv::Mat deeper_frame;
+  frame.convertTo(deeper_frame,CV_16S);
+  //TODO remove this?
+
+
+  cv::Sobel(deeper_frame, xder, -1, 1, 0, 3);
+  cv::Sobel(deeper_frame, yder, -1, 0, 1, 3);
+
+  xder/=8.;
+  yder/=8.;
+
+  /*
+  cv::Mat norm_abs_xderImage;
+  cv::Mat abs_xderImage2;
+  cv::convertScaleAbs(xder, abs_xderImage2);
+
+  cv::normalize(abs_xderImage2, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1);
+
+  cv::imshow("xder", norm_abs_xderImage);
+  cvWaitKey(0);
+  */
+
+  if(type == "Sobel_x")
+    delta = ((double)xder.at<short>(pose.y, pose.x))/255.;
+  else if (type == "Sobel_y")
+    delta = ((double)yder.at<short>(pose.y, pose.x))/255.;
+  return delta;
+}
+
 double Feature::cvKernel(
                 const cv::Point2f pose,
                 std::string type) const
@@ -823,6 +867,7 @@ bool Feature::VisualizePatch(
   // residual grid projection, positive - red, negative - blue colored 
   namer.str(std::string());
   namer << "residual";
+  std::cout << "-- photo_r -- \n" << photo_r << " -- " << std::endl;
   cv::putText(irradianceFrame, namer.str() , cvPoint(30+scale*N, scale*N/2-5), 
     cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
 

include/msckf_vio/msckf_vio.h

@@ -229,12 +229,14 @@ class MsckfVio {
 
     bool PhotometricPatchPointJacobian(
         const CAMState& cam_state,
+        const StateIDType& cam_state_id,
         const Feature& feature,
         Eigen::Vector3d point,
         int count,
         Eigen::Matrix<double, 1, 1>& H_rhoj,
         Eigen::Matrix<double, 1, 6>& H_plj,
-        Eigen::Matrix<double, 1, 6>& H_pAj);
+        Eigen::Matrix<double, 1, 6>& H_pAj,
+        Eigen::Matrix<double, 1, 2>& dI_dhj);
 
     bool PhotometricMeasurementJacobian(
         const StateIDType& cam_state_id,