added kernel calculation visualization; changed sobel filter to cv implementation, added octave export

include/msckf_vio/feature.hpp

@@ -184,6 +184,10 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
                   const CameraCalibration& cam,
                   Eigen::Vector3d& in_p) const;
 
+  double cvKernel(
+            const cv::Point2f pose,
+            std::string type) const;
+
   double Kernel(
             const cv::Point2f pose,
             const cv::Mat frame,
@@ -269,6 +273,14 @@ inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p,
   bool is_initialized;
   bool is_anchored;
 
+
+  cv::Mat abs_xderImage;
+  cv::Mat abs_yderImage;
+
+  cv::Mat xderImage;
+  cv::Mat yderImage;
+
+  cv::Mat anchorImage_blurred;
   cv::Point2f anchor_center_pos;
   cv::Point2f undist_anchor_center_pos;
   // Noise for a normalized feature measurement.
@@ -478,6 +490,27 @@ bool Feature::checkMotion(const CamStateServer& cam_states) const
   else return false;
 }
 
+double Feature::cvKernel(
+                const cv::Point2f pose,
+                std::string type) const
+{
+
+  double delta = 0;
+
+  if(type == "Sobel_x")
+  {
+    std::cout << "image value x" << ((double)xderImage.at<short>(pose.y, pose.x))/255. << std::endl;
+    delta = ((double)xderImage.at<short>(pose.y, pose.x))/255.;
+  }
+  else if (type == "Sobel_y")
+  {
+    std::cout << "image value y" << ((double)yderImage.at<short>(pose.y, pose.x))/255. << std::endl;
+    delta = ((double)yderImage.at<short>(pose.y, pose.x))/255.;
+  }
+  return delta;
+
+}
+
 double Feature::Kernel(
                 const cv::Point2f pose,
                 const cv::Mat frame,
@@ -485,9 +518,9 @@ double Feature::Kernel(
 {
 Eigen::Matrix<double, 3, 3> kernel = Eigen::Matrix<double, 3, 3>::Zero();
 if(type == "Sobel_x")
-  kernel  << -3., 0., 3.,-10., 0., 10. , -3., 0., 3.;
+  kernel  << -1., 0., 1.,-2., 0., 2. , -1., 0., 1.;
 else if(type == "Sobel_y")
-  kernel << -3., -10., -3., 0., 0., 0., 3., 10., 3.;
+  kernel << -1., -2., -1., 0., 0., 0., 1., 2., 1.;
 
 double delta = 0;
 int offs = (int)(kernel.rows()-1)/2;
@@ -658,29 +691,29 @@ bool Feature::VisualizeKernel(
   auto anchor = observations.begin();
   cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
 
-  cv::Mat xderImage;
+  //cv::Mat xderImage;
-  cv::Mat yderImage;
+  //cv::Mat yderImage;
 
-  cv::Sobel(anchorImage, xderImage, CV_8UC1, 1, 0, 3);
+  //cv::Sobel(anchorImage, xderImage, CV_8UC1, 1, 0, 3);
-  cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3);
+  //cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3);
 
 
-  cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, yderImage.type());
+  cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
-  cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, yderImage.type());
+  cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
 
-  cv::imshow("xder", xderImage);
+  cv::imshow("xder", abs_xderImage);
-  //cv::imshow("yder", yderImage);
+  cv::imshow("yder", abs_yderImage);
 
   for(int i = 1; i < anchorImage.rows-1; i++)
     for(int j = 1; j < anchorImage.cols-1; j++)
-      xderImage2.at<uint8_t>(j,i) = 255*Kernel(cv::Point2f(i,j), anchorImage, "Sobel_x");
+      xderImage2.at<uint8_t>(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_x"));
 
   for(int i = 1; i < anchorImage.rows-1; i++)
     for(int j = 1; j < anchorImage.cols-1; j++)
-      yderImage2.at<uint8_t>(j,i) = 255*Kernel(cv::Point2f(i,j), anchorImage, "Sobel_y");
+      yderImage2.at<uint8_t>(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_y"));
   cv::imshow("anchor", anchorImage);
   cv::imshow("xder2", xderImage2);
-  //cv::imshow("yder2", yderImage2);
+  cv::imshow("yder2", yderImage2);
 
   cvWaitKey(0);
 } 
@@ -824,12 +857,13 @@ bool Feature::VisualizePatch(
   */
 
   // residual gradient direction
+      /*
   cv::arrowedLine(irradianceFrame,
                   cv::Point(40+scale*(N+N/2+0.5), 15+scale*((N-0.5))),
                   cv::Point(40+scale*(N+N/2+0.5)+scale*residualVector.x, 15+scale*(N-0.5)+scale*residualVector.y),
                   cv::Scalar(0, 255, 175),
                   3);
-                  
+       */           
 
   cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
       
@@ -921,7 +955,7 @@ cv::Point2f Feature::pixelDistanceAt(
   Eigen::Vector3d p_c0 = R_w_c0 * (in_p-t_c0_w);
 
   // returns the distance between the pixel points in space
-  cv::Point2f distance(fabs(pure[0].x - pure[1].x)*p_c0[2], fabs(pure[2].y - pure[3].y)*p_c0[2]);
+  cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y));
 
   return distance;
 }
@@ -976,6 +1010,8 @@ Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCal
 bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
 {
 
+
+
   //initialize patch Size
   int n = (int)(N-1)/2;
 
@@ -984,6 +1020,18 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
     return false;
 
   cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image;
+  cv::Mat anchorImage_deeper;
+  anchorImage.convertTo(anchorImage_deeper,CV_16S);
+  //TODO remove this?
+
+
+  cv::Sobel(anchorImage_deeper, xderImage, -1, 1, 0, 3);
+  cv::Sobel(anchorImage_deeper, yderImage, -1, 0, 1, 3);
+
+  cv::convertScaleAbs(xderImage, abs_xderImage);
+  cv::convertScaleAbs(yderImage, abs_yderImage);
+  
+  cv::GaussianBlur(anchorImage, anchorImage_blurred, cv::Size(3,3), 0, 0, cv::BORDER_DEFAULT);
 
   auto u = anchor->second(0);//*cam.intrinsics[0] + cam.intrinsics[2];
   auto v = anchor->second(1);//*cam.intrinsics[1] + cam.intrinsics[3];

src/msckf_vio.cpp

@@ -1314,14 +1314,14 @@ void MsckfVio::PhotometricMeasurementJacobian(
     // calculate derivation for anchor frame, use position for derivation calculation
     // frame derivative calculated convoluting with kernel [-1, 0, 1]
 
-    dx = feature.Kernel(p_in_anchor, anchor_frame, "Sobel_x");
+    dx = feature.cvKernel(p_in_anchor, "Sobel_x");
-    dy = feature.Kernel(p_in_anchor, anchor_frame, "Sobel_y");
+    dy = feature.cvKernel(p_in_anchor, "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, 0) = dx/(pixelDistance.x);
-    dI_dhj(0, 1) = dy;// /(pixelDistance.y);
+    dI_dhj(0, 1) = dy/(pixelDistance.y);
 
     gradientVector.x += dx;
     gradientVector.y += dy; 
@@ -1423,8 +1423,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
   if(PRINTIMAGES)
   {  
     feature.MarkerGeneration(marker_pub, state_server.cam_states);
-    //feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector);
+    feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector);
-    feature.VisualizeKernel(cam_state, cam_state_id, cam0);
+    // feature.VisualizeKernel(cam_state, cam_state_id, cam0);
   }
 
   return;
@@ -1496,7 +1496,7 @@ void MsckfVio::PhotometricFeatureJacobian(
   H_x = A_null_space.transpose() * H_xi;
   r = A_null_space.transpose() * r_i;
 
-  cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r)  << endl;
+  //cout << "\nx\n" << H_x.colPivHouseholderQr().solve(r)  << endl;
 
   if(PRINTIMAGES)
   {
@@ -1519,8 +1519,34 @@ void MsckfVio::PhotometricFeatureJacobian(
       myfile << A_null_space.block(i, 0, 1, A_null_space.cols()) << ";";
 
 
+    myfile.close();
+
+    //octave
+    myfile.open("/home/raphael/dev/octave/log2octave");
+    myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
+           << "# name: Hx\n"
+           << "# type: matrix\n"
+           << "# rows: " << H_xi.rows() << "\n"
+           << "# columns: " << H_xi.cols() << "\n"
+           << H_xi << endl;
+
+    myfile << "# name: Hy\n"
+           << "# type: matrix\n"
+           << "# rows: " << H_yi.rows() << "\n"  
+           << "# columns: " << H_yi.cols() << "\n"
+           << H_yi << endl;
+
+
+    myfile << "# name: r\n"
+           << "# type: matrix\n"
+           << "# rows: " << r_i.rows() << "\n"  
+           << "# columns: " << 1 << "\n"
+           << r_i << endl;
+
     myfile.close();
     cout << "---------- LOGGED -------- " << endl; 
+    cvWaitKey(0);
+
   }
   if(PRINTIMAGES)
   {
@@ -1808,6 +1834,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
   cout << dof << " " << gamma << " " <<
     chi_squared_test_table[dof] << endl;
 
+    if (chi_squared_test_table[dof] == 0)
+      return true;
   if (gamma < chi_squared_test_table[dof]) {
     //cout << "passed" << endl;
     return true;