added kernel visualization

include/msckf_vio/feature.hpp

@@ -206,6 +206,11 @@ bool MarkerGeneration(
                   ros::Publisher& marker_pub,
                   const CamStateServer& cam_states) const;
 
+bool VisualizeKernel(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  CameraCalibration& cam0) const;
+
   bool VisualizePatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
@@ -480,16 +485,16 @@ double Feature::Kernel(
 {
 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.;
+  kernel  << -3., 0., 3.,-10., 0., 10. , -3., 0., 3.;
 else if(type == "Sobel_y")
-  kernel << -1., -2., -1., 0., 0., 0., 1., 2., 1.;
+  kernel << -3., -10., -3., 0., 0., 0., 3., 10., 3.;
 
 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++)
-    delta += ((float)frame.at<uint8_t>(pose.y+j-offs , pose.x+i-offs))/255 * (float)kernel(j,i);
+    delta += ((float)frame.at<uint8_t>(pose.y+j-offs , pose.x+i-offs))/255. * (float)kernel(j,i);
 
 return delta;
 }
@@ -644,6 +649,41 @@ bool Feature::MarkerGeneration(
   marker_pub.publish(ma);
 }
 
+
+bool Feature::VisualizeKernel(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  CameraCalibration& cam0) const
+{
+  auto anchor = observations.begin();
+  cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
+
+  cv::Mat xderImage;
+  cv::Mat yderImage;
+
+  cv::Sobel(anchorImage, xderImage, CV_8UC1, 1, 0, 3);
+  cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3);
+
+
+  cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, yderImage.type());
+  cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, yderImage.type());
+
+  cv::imshow("xder", xderImage);
+  //cv::imshow("yder", 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");
+
+  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");
+  cv::imshow("anchor", anchorImage);
+  cv::imshow("xder2", xderImage2);
+  //cv::imshow("yder2", yderImage2);
+
+  cvWaitKey(0);
+} 
 bool Feature::VisualizePatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
@@ -848,7 +888,7 @@ bool Feature::VisualizePatch(
 float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
 {
 
-  return ((float)image.at<uint8_t>(pose.y, pose.x))/255;
+  return ((float)image.at<uint8_t>(pose.y, pose.x))/255.;
 }
 
 cv::Point2f Feature::pixelDistanceAt(

launch/msckf_vio_debug_tum.launch

@@ -22,10 +22,10 @@
       <param name="PHOTOMETRIC" value="true"/>
 
       <!-- Debugging Flaggs -->
-      <param name="PrintImages" value="false"/>
+      <param name="PrintImages" value="true"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="7"/>
+      <param name="patch_size_n" value="3"/>
 
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>

launch/msckf_vio_tum.launch

@@ -21,7 +21,7 @@
       <param name="PHOTOMETRIC" value="true"/>
 
       <!-- Debugging Flaggs -->
-      <param name="PrintImages" value="false"/>
+      <param name="PrintImages" value="true"/>
       <param name="GroundTruth" value="false"/>
 
       <param name="patch_size_n" value="3"/>

log

@@ -0,0 +1,73 @@
+# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>
+# name: Hx
+# type: matrix
+# rows: 18
+# columns: 49
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 313.795 -58.7912 139.778 -46.7616 144.055 86.9644 0 -314.123 55.6434 -140.648 46.7616 -144.055 -86.9644 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 441.06 -94.50069999999999 174.424 -53.7653 204.822 120.248 0 -441.685 90.1101 -175.657 53.7653 -204.822 -120.248 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 225.35 -54.5629 77.60599999999999 -21.1425 105.886 60.3706 0 -225.756 52.3373 -78.2406 21.1425 -105.886 -60.3706 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 175.128 20.6203 175.127 -79.63939999999999 73.245 62.1868 0 -174.573 -22.5235 -175.576 79.63939999999999 -73.245 -62.1868 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 296.962 43.5469 311.307 -143.667 123.399 108.355 0 -295.905 -46.7952 -312.063 143.667 -123.399 -108.355 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 126.283 117.889 311.864 -161.264 38.8521 71.8019 0 -124.464 -119.541 -312.118 161.264 -38.8521 -71.8019 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 49.2502 63.7166 155.071 -80.81950000000001 12.7732 32.1826 0 -48.2934 -64.4113 -155.157 80.81950000000001 -12.7732 -32.1826 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 69.59699999999999 154.579 335.384 -179.355 9.212580000000001 62.0364 0 -67.35599999999999 -155.735 -335.462 179.355 -9.212580000000001 -62.0364 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -66.6965 304.947 500.218 -285.589 -71.31010000000001 55.5058 0 70.8009 -305.077 -499.831 285.589 71.31010000000001 -55.5058 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 323.404 -62.2043 141.092 -46.6015 148.737 89.1108 0 0 0 0 0 0 0 0 -324.336 57.8552 -141.991 46.6015 -148.737 -89.1108 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 454.208 -99.3095 175.986 -53.4094 211.276 123.174 0 0 0 0 0 0 0 0 -455.779 93.0992 -177.158 53.4094 -211.276 -123.174 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 231.884 -57.0266 78.2559 -20.8926 109.118 61.8183 0 0 0 0 0 0 0 0 -232.824 53.8025 -78.80719999999999 20.8926 -109.118 -61.8183 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 181.715 18.8525 177.045 -80.08499999999999 76.3716 63.8254 0 0 0 0 0 0 0 0 -181.07 -20.839 -177.959 80.08499999999999 -76.3716 -63.8254 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 308.249 40.5812 314.711 -144.494 128.766 111.207 0 0 0 0 0 0 0 0 -306.972 -43.8825 -316.328 144.494 -128.766 -111.207 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 133.309 116.865 315.474 -162.598 42.0763 73.8353 0 0 0 0 0 0 0 0 -130.603 -117.454 -316.931 162.598 -42.0763 -73.8353 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 52.4426 63.3607 156.902 -81.5288 14.2139 33.1222 0 0 0 0 0 0 0 0 -50.9976 -63.4393 -157.607 81.5288 -14.2139 -33.1222 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 75.5508 154.234 339.369 -180.995 11.859 63.8956 0 0 0 0 0 0 0 0 -72.1041 -153.816 -340.865 180.995 -11.859 -63.8956 1 0 0 0 0 0 0 0
+ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -62.0551 306.357 506.351 -288.542 -69.50409999999999 57.4562 0 0 0 0 0 0 0 0 68.6262 -303.028 -508.423 288.542 69.50409999999999 -57.4562 1 0 0 0 0 0 0 0
+
+
+# name: Hy
+# type: matrix
+# rows: 18
+# columns: 14
+ 1.56267 0 0 0 0 0 0 0 0 0 0.252873 0 0 0.110387
+ 0 1.56267 0 0 0 0 0 0 0 0 0.453168 0 0 0.162961
+ 0 0 1.56267 0 0 0 0 0 0 0 0.638441 0 0 0.0873758
+ 0 0 0 1.56267 0 0 0 0 0 0 0.21031 0 0 0.0321517
+ 0 0 0 0 1.56267 0 0 0 0 0 0.375554 0 0 0.0505151
+ 0 0 0 0 0 1.56267 0 0 0 0 0.638441 0 0 -0.0336034
+ 0 0 0 0 0 0 1.56267 0 0 0 0.157733 0 0 -0.0232704
+ 0 0 0 0 0 0 0 1.56267 0 0 0.212814 0 0 -0.0688343
+ 0 0 0 0 0 0 0 0 1.56267 0 0.360532 0 0 -0.187745
+ 1.56267 0 0 0 0 0 0 0 0 0 0 0.252873 0 0.322811
+ 0 1.56267 0 0 0 0 0 0 0 0 0 0.453168 0 0.467319
+ 0 0 1.56267 0 0 0 0 0 0 0 0 0.638441 0 0.245907
+ 0 0 0 1.56267 0 0 0 0 0 0 0 0.21031 0 0.135725
+ 0 0 0 0 1.56267 0 0 0 0 0 0 0.375554 0 0.225277
+ 0 0 0 0 0 1.56267 0 0 0 0 0 0.638441 0 0.012926
+ 0 0 0 0 0 0 1.56267 0 0 0 0 0.157733 0 -0.0108962
+ 0 0 0 0 0 0 0 1.56267 0 0 0 0.212814 0 -0.06974909999999999
+ 0 0 0 0 0 0 0 0 1.56267 0 0 0.360532 0 -0.318846
+
+
+# name: r
+# type: matrix
+# rows: 18
+# columns: 1
+ 0.0354809
+ 0.0153183
+ 0.0570191
+ -0.0372801
+ 0.0878601
+ 0.06811780000000001
+ -0.00426164
+ 5.162985999041026e-321
+ 6.927779999999998e-310
+ 0
+ 2.121999999910509e-314
+ 0
+ 0
+ 0
+ 3.6073900000086e-313
+ 4.446590812571219e-323
+ 3.952525166729972e-323
+ 3.952525166729972e-323
+
+

src/msckf_vio.cpp

@@ -1290,6 +1290,10 @@ void MsckfVio::PhotometricMeasurementJacobian(
   cv::Point2f residualVector(0,0);
   double res_sum = 0;
 
+
+  ofstream myfile;
+  myfile.open ("/home/raphael/dev/MSCKF_ws/log_jacobi.txt");
+
   for (auto point : feature.anchorPatch_3d)
   {
     //cout << "____feature-measurement_____\n" << endl;
@@ -1316,8 +1320,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
     // 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; 
@@ -1359,6 +1363,16 @@ void MsckfVio::PhotometricMeasurementJacobian(
     H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1
     H_plj =  dI_dhj * dh_dXplj; // 1 x 6
     H_pAj =  dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
+    myfile << " --------- \n" << endl;
+    myfile << "H_rhoj\n" << H_rhoj << endl;
+    myfile << "H_plj\n" << H_plj << endl;
+    myfile << "H_pAj\n" << H_pAj << endl;
+    myfile << "\n" << endl;
+    myfile << "dI_dhj\n" << dI_dhj << endl;
+    myfile << "dh_dGpij\n" << dh_dGpij << endl;
+    myfile << "dGpj_XpAj\n" << dGpj_XpAj << endl;
+
+    // myfile << "pixel pos change based on residual:\n" << dI_dhj.colPivHouseholderQr().solve(r_photo(count)) << endl;
 
     H_rho.block<1, 1>(count, 0) = H_rhoj;
     H_pl.block<1, 6>(count, 0) = H_plj;
@@ -1367,6 +1381,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
     count++;
   }
 
+  myfile.close();
 
   MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7);
   MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1);
@@ -1408,7 +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);
   }
 
   return;
@@ -1486,10 +1502,24 @@ void MsckfVio::PhotometricFeatureJacobian(
   {
     ofstream myfile;
     myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
-    myfile << "Hxi\n" << H_xi << "ri\n" << r_i << "Hyi\n" << H_yi << endl;
+    myfile << "\nHxi" << endl;
-    myfile << "kernel\n" << A_null_space << endl;
+    for(int i = 0; i < H_xi.rows(); i++)
+      myfile << H_xi.block(i, 0, 1, H_xi.cols()) << ";";
+
+    myfile << "\nr" << endl;
+    for(int i = 0; i < r_i.rows(); i++)
+      myfile << r.segment(i, 1) << ";";
+
+    myfile << "\nHyi" << endl;
+    for(int i = 0; i < H_yi.rows(); i++)
+      myfile << H_yi.block(i, 0, 1, H_yi.cols()) << ";";
+
+    myfile << "A_null_space" << endl;
+    for(int i = 0; i < A_null_space.rows(); i++)
+      myfile << A_null_space.block(i, 0, 1, A_null_space.cols()) << ";";
+
+
     myfile.close();
-    myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
     cout << "---------- LOGGED -------- " << endl; 
   }
   if(PRINTIMAGES)