added direct levenberg marqhart estimation for rho, was previously calculated from feature position

src/msckf_vio.cpp

@@ -1240,9 +1240,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
   //photometric observation
   std::vector<double> photo_z;
-
-  std::vector<double> photo_r;
-
+  VectorXd r_photo = VectorXd::Zero(N*N);
   // individual Jacobians
   Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
   Matrix<double, 2, 3> dh_dCpij = Matrix<double, 2, 3>::Zero();
@@ -1303,8 +1301,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
     photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
 
       //calculate photom. residual
-    photo_r.push_back((photo_z[count] - estimate_photo_z[count]));
-
+    r_photo(count) = photo_z[count] - estimate_photo_z[count];
     //cout << "residual: " << photo_r.back() << endl;
 
     // add jacobians
@@ -1325,9 +1322,10 @@ void MsckfVio::PhotometricMeasurementJacobian(
     gradientVector.x += dx;
     gradientVector.y += dy; 
     
-    residualVector.x += dx * photo_r[count];
-    residualVector.y += dy * photo_r[count];
-    res_sum += photo_r[count];
+
+    residualVector.x += dx * r_photo(count);
+    residualVector.y += dy * r_photo(count);
+    res_sum += r_photo(count);
 
     //dh / d{}^Cp_{ij}
     dh_dCpij(0, 0) = 1 / p_c0(2);
@@ -1348,7 +1346,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
     // d{}^Gp_P{ij} / \rho_i
     double rho = feature.anchor_rho;
-
     // Isometry T_anchor_w takes a vector in anchor frame to world frame
     dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho));
 
@@ -1363,6 +1360,12 @@ void MsckfVio::PhotometricMeasurementJacobian(
     H_plj =  dI_dhj * dh_dXplj; // 1 x 6
     H_pAj =  dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
 
+    /*
+    cout << endl;
+    std::cout << H_plj << endl;
+    cout << r_photo.segment(count, 1) << endl;
+    std::cout << H_plj.colPivHouseholderQr().solve(r_photo.segment(count, 1)) << std::endl;
+    */
     H_rho.block<1, 1>(count, 0) = H_rhoj;
     H_pl.block<1, 6>(count, 0) = H_plj;
     H_pA.block<1, 6>(count, 0) = H_pAj;
@@ -1375,6 +1378,14 @@ void MsckfVio::PhotometricMeasurementJacobian(
     count++;
   }
 
+  /*
+  std::cout << "\n\n frame change through patch" << std::endl;
+  std::cout << H_pl << std::endl;
+  std::cout << r_photo << std::endl;
+  std::cout << endl;
+  std::cout << H_pl.colPivHouseholderQr().solve(r_photo) << std::endl;
+  */
+
   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,16 +1419,14 @@ void MsckfVio::PhotometricMeasurementJacobian(
   H_y = H_yl;
 
   //TODO make this more fluent as well
-  count = 0; 
-  for(auto data : photo_r)
-    r[count++] = data;
+  r = r_photo;
 
   std::stringstream ss;
   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, r_photo, ss, gradientVector, residualVector);
   }
 
   return;
@@ -1463,6 +1472,8 @@ void MsckfVio::PhotometricFeatureJacobian(
     if (feature.observations.find(cam_id) ==
         feature.observations.end()) continue;
 
+    if (feature.observations.find(cam_id) == feature.observations.begin())
+      continue;
     valid_cam_state_ids.push_back(cam_id);
   }
 
@@ -1519,6 +1530,7 @@ 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 << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
     myfile.close();
     cout << "---------- LOGGED -------- " << endl; 
@@ -1601,6 +1613,13 @@ void MsckfVio::featureJacobian(
     const std::vector<StateIDType>& cam_state_ids,
     MatrixXd& H_x, VectorXd& r) 
 {
+    // stop playing bagfile if printing images
+  if(PRINTIMAGES)
+  {
+    std::cout << "stopped playpack" << std::endl;
+    nh.setParam("/play_bag", false);
+  }
+
 
   const auto& feature = map_server[feature_id];
 
@@ -1662,12 +1681,20 @@ void MsckfVio::featureJacobian(
    H_x = A.transpose() * H_xj;
    r = A.transpose() * r_j;
 
-  
-  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.colPivHouseholderQr().solve(r) << endl;
-  myfile.close();
-  cout << "---------- LOGGED -------- " << endl; 
+
+      // stop playing bagfile if printing images
+  if(PRINTIMAGES)
+  {
+
+    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.colPivHouseholderQr().solve(r) << endl;
+    myfile.close();
+    cout << "---------- LOGGED -------- " << endl; 
+    std::cout << "stopped playpack" << std::endl;
+    nh.setParam("/play_bag", true);
+  }
+
   return;
 
 }