added sobel kernel for image gradient calcualtion

.vscode/c_cpp_properties.json

@@ -0,0 +1,16 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c11",
+            "cppStandard": "c++14",
+            "intelliSenseMode": "clang-x64"
+        }
+    ],
+    "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,6 @@
+{
+    "files.associations": {
+        "core": "cpp",
+        "sparsecore": "cpp"
+    }
+}

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,

launch/msckf_vio_tum.launch

@@ -24,7 +24,7 @@
       <param name="PrintImages" value="true"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="7"/>
+      <param name="patch_size_n" value="1"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 

src/msckf_vio.cpp

@@ -1312,8 +1312,13 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
     // calculate derivation for anchor frame, use position for derivation calculation
     // frame derivative calculated convoluting with kernel [-1, 0, 1]
-    dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame);
-    dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame);
+
+    dx = feature.Kernel(p_in_anchor, anchor_frame, "Sobel_x");
+    dy = feature.Kernel(p_in_anchor, anchor_frame, "Sobel_y");
+    
+    // dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame);
+    // dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame);
+    
     dI_dhj(0, 0) = dx/(pixelDistance.x);
     dI_dhj(0, 1) = dy/(pixelDistance.y);