fixed Irradiance jacobain calculation, now division by pixel distance

include/msckf_vio/feature.hpp

@@ -157,6 +157,14 @@ struct Feature {
   inline bool initializePosition(
       const CamStateServer& cam_states);
 
+
+cv::Point2f pixelDistanceAt(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  const CameraCalibration& cam,
+                  Eigen::Vector3d& in_p) const;
+
+
 /*
 *  @brief project PositionToCamera Takes a 3d position in a world frame
 *         and projects it into the passed camera frame using pinhole projection
@@ -191,9 +199,7 @@ struct Feature {
                   const StateIDType& cam_state_id,
                   CameraCalibration& cam0,
                   const std::vector<double> photo_r,
-                  std::stringstream& ss,
+                  std::stringstream& ss) const;
-                  cv::Point2f gradientVector,
-                  cv::Point2f residualVector) const;
 
 
   /* @brief takes a pure pixel position (1m from image)
@@ -407,7 +413,10 @@ bool Feature::estimate_FrameIrradiance(
 
   auto anchor = observations.begin();
   if(cam0.moving_window.find(anchor->first) == cam0.moving_window.end())
+  {
+    std::cout << "anchor not in buffer anymore!" << std::endl;
     return false;
+  } 
   
   double anchorExposureTime_ms = cam0.moving_window.find(anchor->first)->second.exposureTime_ms;
   double frameExposureTime_ms = cam0.moving_window.find(cam_state_id)->second.exposureTime_ms;
@@ -550,9 +559,7 @@ bool Feature::VisualizePatch(
                   const StateIDType& cam_state_id,
                   CameraCalibration& cam0,
                   const std::vector<double> photo_r,
-                  std::stringstream& ss,
+                  std::stringstream& ss) const
-                  cv::Point2f gradientVector,
-                  cv::Point2f residualVector) const
 {
 
   double rescale = 1;
@@ -615,7 +622,6 @@ bool Feature::VisualizePatch(
                     CV_FILLED);
 
   // irradiance grid projection
-
   namer.str(std::string());
   namer << "projection";
   cv::putText(irradianceFrame, namer.str(), cvPoint(30, 45+scale*N), 
@@ -682,7 +688,6 @@ bool Feature::VisualizePatch(
                   cv::Point(30+scale*(N/2+0.5)+scale*gradientVector.x, 50+scale*(N+(N/2)+0.5)+scale*gradientVector.y),
                   cv::Scalar(100, 0, 255),
                   1);
-  */
   
   // residual gradient direction
   cv::arrowedLine(irradianceFrame,
@@ -691,11 +696,14 @@ bool Feature::VisualizePatch(
                   cv::Scalar(0, 255, 175),
                   3);
                   
-
+  */
   cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
   
-/*
+    
+
   // visualize position of used observations and resulting feature position
+  /*
+
   cv::Mat positionFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255));
   cv::resize(positionFrame, positionFrame, cv::Size(), rescale, rescale);
 
@@ -728,7 +736,9 @@ bool Feature::VisualizePatch(
 
 
   cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu);
+
   */
+
   // write feature position
   std::stringstream pos_s;
   pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1);
@@ -771,6 +781,44 @@ float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
   return ((float)image.at<uint8_t>(pose.y, pose.x))/255;
 }
 
+cv::Point2f Feature::pixelDistanceAt(
+                  const CAMState& cam_state,
+                  const StateIDType& cam_state_id,
+                  const CameraCalibration& cam,
+                  Eigen::Vector3d& in_p) const
+{
+
+  Eigen::Isometry3d T_c0_w;
+
+  cv::Point2f out_p;
+
+  cv::Point2f cam_p = projectPositionToCamera(cam_state, cam_state_id, cam, in_p);
+
+
+  cv::Point2f surroundingPoint;
+  // create vector of patch in pixel plane
+  surroundingPoint = cv::Point2f(cam_p.x+1, cam_p.y+1);
+
+  cv::Point2f pure_surroundingPoint;
+  image_handler::undistortPoint(surroundingPoint, 
+                                cam.intrinsics, 
+                                cam.distortion_model, 
+                                cam.distortion_coeffs,
+                                pure_surroundingPoint);
+
+  cv::Point2f pure_Point;
+  image_handler::undistortPoint(cam_p, 
+                                cam.intrinsics, 
+                                cam.distortion_model, 
+                                cam.distortion_coeffs,
+                                pure_Point);
+
+  cv::Point2f distance(fabs(pure_surroundingPoint.x - pure_Point.x), fabs(pure_surroundingPoint.y - pure_Point.y));
+
+  return distance;
+}
+
+
 cv::Point2f Feature::projectPositionToCamera(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,

include/msckf_vio/math_utils.hpp

@@ -13,6 +13,13 @@
 
 namespace msckf_vio {
 
+
+inline double absoluted(double a){
+  if(a>0)
+    return a;
+  else return -a;
+}
+
 /*
  *  @brief Create a skew-symmetric matrix from a 3-element vector.
  *  @note Performs the operation:

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="5"/>
+      <param name="patch_size_n" value="3"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 

src/msckf_vio.cpp

@@ -1232,29 +1232,12 @@ void MsckfVio::PhotometricMeasurementJacobian(
   // one line of the NxN Jacobians
   Matrix<double, 1, 6> H_xi;
   Matrix<double, 1, 6> H_xA;
-  Matrix<double, 1, 3> H_fi;
 
   MatrixXd H_xl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 6);
   MatrixXd H_xAl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 6);
-  MatrixXd H_yl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 3);
 
   auto frame = cam0.moving_window.find(cam_state_id)->second.image;
   
-  //observation
-  const Vector4d& z = feature.observations.find(cam_state_id)->second;
-
-  //estimate photometric measurement
-  std::vector<double> estimate_irradiance;
-  std::vector<double> estimate_photo_z;
-  IlluminationParameter estimated_illumination;
-  feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
-  
-  // calculated here, because we need true 'estimate_irradiance' later for jacobi
-  for (auto& estimate_irradiance_j : estimate_irradiance)
-            estimate_photo_z.push_back (estimate_irradiance_j * 
-                    estimated_illumination.frame_gain * estimated_illumination.feature_gain +
-                    estimated_illumination.frame_bias + estimated_illumination.feature_bias);
-
   int count = 0;
   double dx, dy;
 
@@ -1271,19 +1254,17 @@ void MsckfVio::PhotometricMeasurementJacobian(
     Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
     cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
 
-    z_irr_est.push_back(feature.PixelIrradiance(p_in_c0, frame));
-
     Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
-      //calculate photom. residual
-    photo_r.push_back(photo_z[count] - estimate_photo_z[count]);
 
     // add jacobians
 
+    cv::Point2f pixelDistance = feature.pixelDistanceAt(cam_state, cam_state_id, cam0, point);
+  
     // frame derivative calculated convoluting with kernel [-1, 0, 1]
     dx = feature.PixelIrradiance(cv::Point2f(p_in_c0.x+1, p_in_c0.y), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x-1, p_in_c0.y), frame);
     dy = feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y+1), frame) - feature.PixelIrradiance(cv::Point2f(p_in_c0.x, p_in_c0.y-1), frame);
-    dI_dhj(0, 0) = dx;
+    dI_dhj(0, 0) = dx/pixelDistance.x;
-    dI_dhj(0, 1) = dy;
+    dI_dhj(0, 1) = dy/pixelDistance.y;
 
     // Compute the Jacobians.
     Matrix<double, 2, 3> dz_dpc0 = Matrix<double, 2, 3>::Zero();
@@ -1298,17 +1279,16 @@ void MsckfVio::PhotometricMeasurementJacobian(
     dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
     dpc0_dxc.rightCols(3) = -R_w_c0;
 
-    Matrix3d dpc0_dpg = R_w_c0;
-
     H_xi = dI_dhj*dz_dpc0*dpc0_dxc;
-    H_fi = dI_dhj*dz_dpc0*dpc0_dpg;
 
-    dCpij_dGpij = quaternionToRotation(cam_state.orientation);
+    Matrix3d dCpij_dGpij = quaternionToRotation(cam_state.orientation);
 
     //orientation takes camera frame to world frame, we wa
-    dh_dGpij = dz_dpc0 * dCpij_dGpij;
+    Matrix<double, 2, 3> dh_dGpij = dz_dpc0 * dCpij_dGpij;
 
-    double rho = feature.rh
+    double rho = feature.anchor_rho;
+
+    Matrix<double, 3, 6> dGpj_XpAj = Matrix<double, 3, 6>::Zero();
 
     dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
                                  * skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho), 
@@ -1316,42 +1296,43 @@ void MsckfVio::PhotometricMeasurementJacobian(
                                                    1/(rho)));
     dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
 
-    H_xA = dI_dhj*dh_dGpij*dGpj_XpAj
+    H_xA = dI_dhj*dh_dGpij*dGpj_XpAj;
 
     H_xl.block<1, 6>(count, 0) = H_xi;
-    H_yl.block<1, 3>(count, 0) = H_fi;
     H_xAl.block<1, 6>(count, 0) = H_xA;
 
     count++;
   }
 
-  // Compute the residual.
+  // calculate projected irradiance
-  std::vector<float> z_irr;
+  std::vector<double> projectionPatch;
-  cv::Point2f z = cv::Point2f(feature.observations.find(cam_state_id)->second(0), feature.observations.find(cam_state_id)->second(1));
+  for(auto point : feature.anchorPatch_3d)
-  feature.PatchAroundPurePixel(z, N, cam0, cam_state_id, z_irr);
+  {
+    cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
+    projectionPatch.push_back(feature.PixelIrradiance(p_in_c0, frame));
+  }  
   
-  Eigen::VectorXd r_l = Eigen::VectorXd::Zero(count-1);
+  Eigen::VectorXd r_l = Eigen::VectorXd::Zero(count);
+
+  std::vector<double> residual_v;
 
   for(int i = 0; i < r_l.size(); i++)
-    r_l(i) = z_irr[i]- feature.anchorPatch[i];
+  {
-
+    residual_v.push_back(projectionPatch[i]- feature.anchorPatch[i]);
+    r_l(i) = projectionPatch[i] - feature.anchorPatch[i];
+  }
 
   r = r_l;
   H_x = H_xl;
   H_y = H_xAl;
 
 
-  //TODO make this more fluent as well
-  count = 0; 
-  for(auto data : photo_r)
-    r[count++] = data;
-
   std::stringstream ss;
-  ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << "  frame: " << cam_state_cntr;
+  ss << "INFO:"; // << " anchor: " << cam_state_cntr_anchor << "  frame: " << cam_state_cntr;
   if(PRINTIMAGES)
   {  
     feature.MarkerGeneration(marker_pub, state_server.cam_states);
-    feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss, gradientVector, residualVector);
+    feature.VisualizePatch(cam_state, cam_state_id, cam0, residual_v, ss);
   }
 
   return;
@@ -1426,9 +1407,8 @@ void MsckfVio::PhotometricFeatureJacobian(
 
 
     // Stack the Jacobians.
-    H_xi.block(stack_cntr, 21+cam_state_cntr_anchor*7, H_yl.rows(), H_yl.cols()) = -H_yi;
+    H_xi.block(stack_cntr, 21+cam_state_cntr_anchor*7, H_yl.rows(), H_yl.cols()) = H_yl;
-    H_xi.block(stack_cntr, 21+cam_state_cntr*7, H_xl.rows(), H_xl.cols()) = H_xl;
+    H_xi.block(stack_cntr, 21+cam_state_cntr*7, H_xl.rows(), H_xl.cols()) = -H_xl;
-
 
     r_i.segment(stack_cntr, N*N) = r_l;
     stack_cntr += N*N;
@@ -1439,7 +1419,7 @@ void MsckfVio::PhotometricFeatureJacobian(
 
   ofstream myfile;
   myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
-  myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x. * r << endl;
+  myfile << "Hx\n" << H_x << "r\n" << r << "\n x\n" << H_x.colPivHouseholderQr().solve(r) << endl;
   myfile.close();
   cout << "---------- LOGGED -------- " << endl; 
 
@@ -1710,7 +1690,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
 }
 
 bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
-  return true;
   MatrixXd P1 = H * state_server.state_cov * H.transpose();
 
 
@@ -1819,13 +1798,8 @@ void MsckfVio::removeLostFeatures() {
     else
       featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
 
-    cout << "\n" << endl;
-    cout << "H_xj: \n" << H_xj << endl;
-    cout << "res: \n" << endl;
-    cout << r_j << endl; 
 
-
+    if (gatingTest(H_xj, r_j, r_j.size()-1)){ //cam_state_ids.size()-1)) {
-    if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) {
       H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
       r.segment(stack_cntr, r_j.rows()) = r_j;
       stack_cntr += H_xj.rows();
@@ -1987,13 +1961,7 @@ void MsckfVio::pruneCamStateBuffer() {
     else
       featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
     
-    cout << "\n" << endl;
+    if (gatingTest(H_xj, r_j, r_j.size()-1)) { //involved_cam_state_ids.size())) {
-    cout << "H_xj: \n" << H_xj << endl;
-    cout << "res: \n" << endl;
-    cout << r_j << endl; 
-    
-    
-    if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {
       H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
       r.segment(stack_cntr, r_j.rows()) = r_j;
       stack_cntr += H_xj.rows();