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

include/msckf_vio/feature.hpp

@@ -70,6 +70,11 @@ struct Feature {
     position(Eigen::Vector3d::Zero()),
     is_initialized(false), is_anchored(false) {}
 
+
+void Rhocost(const Eigen::Isometry3d& T_c0_ci,
+    const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2,
+    double& e) const;
+
   /*
    * @brief cost Compute the cost of the camera observations
    * @param T_c0_c1 A rigid body transformation takes
@@ -82,6 +87,13 @@ struct Feature {
       const Eigen::Vector3d& x, const Eigen::Vector2d& z,
       double& e) const;
 
+  bool initializeRho(const CamStateServer& cam_states);
+
+  inline void RhoJacobian(const Eigen::Isometry3d& T_c0_ci,
+    const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2,
+    Eigen::Matrix<double, 2, 1>& J, Eigen::Vector2d& r,
+    double& w) const; 
+
   /*
    * @brief jacobian Compute the Jacobian of the camera observation
    * @param T_c0_c1 A rigid body transformation takes
@@ -97,6 +109,10 @@ struct Feature {
       Eigen::Matrix<double, 2, 3>& J, Eigen::Vector2d& r,
       double& w) const;
 
+  inline double generateInitialDepth(
+    const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1,
+    const Eigen::Vector2d& z2) const;
+
   /*
    * @brief generateInitialGuess Compute the initial guess of
    *    the feature's 3d position using only two views.
@@ -194,7 +210,7 @@ bool MarkerGeneration(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
                   CameraCalibration& cam0,
-                  const std::vector<double> photo_r,
+                  const Eigen::VectorXd& photo_r,
                   std::stringstream& ss,
                   cv::Point2f gradientVector,
                   cv::Point2f residualVector) const;
@@ -263,6 +279,26 @@ typedef std::map<FeatureIDType, Feature, std::less<int>,
         Eigen::aligned_allocator<
         std::pair<const FeatureIDType, Feature> > > MapServer;
 
+void Feature::Rhocost(const Eigen::Isometry3d& T_c0_ci,
+    const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2,
+    double& e) const 
+{
+  // Compute hi1, hi2, and hi3 as Equation (37).
+  const double& rho = x;
+
+  Eigen::Vector3d h = T_c0_ci.linear()*
+    Eigen::Vector3d(z1(0), z1(1), 1.0) + rho*T_c0_ci.translation();
+  double& h1 = h(0);
+  double& h2 = h(1);
+  double& h3 = h(2);
+
+  // Predict the feature observation in ci frame.
+  Eigen::Vector2d z_hat(h1/h3, h2/h3);
+
+  // Compute the residual.
+  e = (z_hat-z2).squaredNorm();
+  return;
+}
 
 void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
     const Eigen::Vector3d& x, const Eigen::Vector2d& z,
@@ -275,9 +311,9 @@ void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
 
   Eigen::Vector3d h = T_c0_ci.linear()*
     Eigen::Vector3d(alpha, beta, 1.0) + rho*T_c0_ci.translation();
-  double& h1 = h(0);
+  double h1 = h(0);
-  double& h2 = h(1);
+  double h2 = h(1);
-  double& h3 = h(2);
+  double h3 = h(2);
 
   // Predict the feature observation in ci frame.
   Eigen::Vector2d z_hat(h1/h3, h2/h3);
@@ -287,6 +323,43 @@ void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
   return;
 }
 
+void Feature::RhoJacobian(const Eigen::Isometry3d& T_c0_ci,
+    const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2,
+    Eigen::Matrix<double, 2, 1>& J, Eigen::Vector2d& r,
+    double& w) const 
+{
+
+  // Compute hi1, hi2, and hi3 as Equation (37).
+  const double& rho = x;
+
+  Eigen::Vector3d h = T_c0_ci.linear()*
+    Eigen::Vector3d(z1(0), z2(1), 1.0) + rho*T_c0_ci.translation();
+  double& h1 = h(0);
+  double& h2 = h(1);
+  double& h3 = h(2);
+
+  // Compute the Jacobian.
+  Eigen::Matrix3d W;
+  W.leftCols<2>() = T_c0_ci.linear().leftCols<2>();
+  W.rightCols<1>() = T_c0_ci.translation();
+
+  J(0,0) = -h1/(h3*h3);
+  J(1,0) = -h2/(h3*h3);
+
+  // Compute the residual.
+  Eigen::Vector2d z_hat(h1/h3, h2/h3);
+  r = z_hat - z2;
+
+  // Compute the weight based on the residual.
+  double e = r.norm();
+  if (e <= optimization_config.huber_epsilon)
+    w = 1.0;
+  else
+    w = optimization_config.huber_epsilon / (2*e);
+
+  return;
+}
+
 void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci,
     const Eigen::Vector3d& x, const Eigen::Vector2d& z,
     Eigen::Matrix<double, 2, 3>& J, Eigen::Vector2d& r,
@@ -326,9 +399,9 @@ void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci,
   return;
 }
 
-void Feature::generateInitialGuess(
+double Feature::generateInitialDepth(
     const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1,
-    const Eigen::Vector2d& z2, Eigen::Vector3d& p) const 
+    const Eigen::Vector2d& z2) const 
 {
   // Construct a least square problem to solve the depth.
   Eigen::Vector3d m = T_c1_c2.linear() * Eigen::Vector3d(z1(0), z1(1), 1.0);
@@ -343,6 +416,15 @@ void Feature::generateInitialGuess(
 
   // Solve for the depth.
   double depth = (A.transpose() * A).inverse() * A.transpose() * b;
+  return depth;
+}
+
+
+void Feature::generateInitialGuess(
+    const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1,
+    const Eigen::Vector2d& z2, Eigen::Vector3d& p) const 
+{
+  double depth = generateInitialDepth(T_c1_c2, z1, z2);
   p(0) = z1(0) * depth;
   p(1) = z1(1) * depth;
   p(2) = depth;
@@ -405,16 +487,11 @@ else if(type == "Sobel_y")
 
 double delta = 0;
 int offs = (int)(kernel.rows()-1)/2;
-for(int i = 0; i < kernel.rows(); i++){
 
+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);
-    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(
@@ -572,7 +649,7 @@ bool Feature::VisualizePatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
                   CameraCalibration& cam0,
-                  const std::vector<double> photo_r,
+                  const Eigen::VectorXd& photo_r,
                   std::stringstream& ss,
                   cv::Point2f gradientVector,
                   cv::Point2f residualVector) const
@@ -685,17 +762,17 @@ bool Feature::VisualizePatch(
 
   for(int i = 0; i<N; i++)
     for(int j = 0; j<N; j++)
-      if(photo_r[i*N+j]>0)
+      if(photo_r(i*N+j)>0)
         cv::rectangle(irradianceFrame, 
                       cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), 
                       cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), 
-                      cv::Scalar(255 - photo_r[i*N+j]*255, 255 - photo_r[i*N+j]*255, 255),  
+                      cv::Scalar(255 - photo_r(i*N+j)*255, 255 - photo_r(i*N+j)*255, 255),  
                       CV_FILLED);
       else
         cv::rectangle(irradianceFrame, 
                       cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)), 
                       cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)), 
-                      cv::Scalar(255, 255 + photo_r[i*N+j]*255, 255 + photo_r[i*N+j]*255), 
+                      cv::Scalar(255, 255 + photo_r(i*N+j)*255, 255 + photo_r(i*N+j)*255), 
                       CV_FILLED);
 
   // gradient arrow
@@ -911,9 +988,160 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
   return true;
 }
 
+bool Feature::initializeRho(const CamStateServer& cam_states) {
+
+  // Organize camera poses and feature observations properly.
+  std::vector<Eigen::Isometry3d,
+    Eigen::aligned_allocator<Eigen::Isometry3d> > cam_poses(0);
+  std::vector<Eigen::Vector2d,
+    Eigen::aligned_allocator<Eigen::Vector2d> > measurements(0);
+
+  for (auto& m : observations) {
+    auto cam_state_iter = cam_states.find(m.first);
+    if (cam_state_iter == cam_states.end()) continue;
+
+    // Add the measurement.
+    measurements.push_back(m.second.head<2>());
+    measurements.push_back(m.second.tail<2>());
+
+    // This camera pose will take a vector from this camera frame
+    // to the world frame.
+    Eigen::Isometry3d cam0_pose;
+    cam0_pose.linear() = quaternionToRotation(
+        cam_state_iter->second.orientation).transpose();
+    cam0_pose.translation() = cam_state_iter->second.position;
+
+    Eigen::Isometry3d cam1_pose;
+    cam1_pose = cam0_pose * CAMState::T_cam0_cam1.inverse();
+
+    cam_poses.push_back(cam0_pose);
+    cam_poses.push_back(cam1_pose);
+  }
+
+  // All camera poses should be modified such that it takes a
+  // vector from the first camera frame in the buffer to this
+  // camera frame.
+  Eigen::Isometry3d T_c0_w = cam_poses[0];
+  T_anchor_w = T_c0_w;
+  for (auto& pose : cam_poses)
+    pose = pose.inverse() * T_c0_w;
+
+  // Generate initial guess
+  double initial_depth = 0;
+  initial_depth = generateInitialDepth(cam_poses[cam_poses.size()-1], measurements[0],
+      measurements[measurements.size()-1]);
+
+  double solution = 1.0/initial_depth;
+
+  // Apply Levenberg-Marquart method to solve for the 3d position.
+  double lambda = optimization_config.initial_damping;
+  int inner_loop_cntr = 0;
+  int outer_loop_cntr = 0;
+  bool is_cost_reduced = false;
+  double delta_norm = 0;
+
+  // Compute the initial cost.
+  double total_cost = 0.0;
+  for (int i = 0; i < cam_poses.size(); ++i) {
+    double this_cost = 0.0;
+    Rhocost(cam_poses[i], solution, measurements[0], measurements[i], this_cost);
+    total_cost += this_cost;
+  }
+
+  // Outer loop.
+  do {
+    Eigen::Matrix<double, 1, 1> A = Eigen::Matrix<double, 1, 1>::Zero();
+    Eigen::Matrix<double, 1, 1>  b = Eigen::Matrix<double, 1, 1>::Zero();
+
+    for (int i = 0; i < cam_poses.size(); ++i) {
+      Eigen::Matrix<double, 2, 1> J;
+      Eigen::Vector2d r;
+      double w;
+
+      RhoJacobian(cam_poses[i], solution, measurements[0], measurements[i], J, r, w);
+
+      if (w == 1) {
+        A += J.transpose() * J;
+        b += J.transpose() * r;
+      } else {
+        double w_square = w * w;
+        A += w_square * J.transpose() * J;
+        b += w_square * J.transpose() * r;
+      }
+    }
+
+    // Inner loop.
+    // Solve for the delta that can reduce the total cost.
+    do {
+      Eigen::Matrix<double, 1, 1> damper = lambda*Eigen::Matrix<double, 1, 1>::Identity();
+      Eigen::Matrix<double, 1, 1> delta = (A+damper).ldlt().solve(b);
+      double new_solution = solution - delta(0,0);
+      delta_norm = delta.norm();
+
+      double new_cost = 0.0;
+      for (int i = 0; i < cam_poses.size(); ++i) {
+        double this_cost = 0.0;
+        Rhocost(cam_poses[i], new_solution, measurements[0], measurements[i], this_cost);
+        new_cost += this_cost;
+      }
+
+      if (new_cost < total_cost) {
+        is_cost_reduced = true;
+        solution = new_solution;
+        total_cost = new_cost;
+        lambda = lambda/10 > 1e-10 ? lambda/10 : 1e-10;
+      } else {
+        is_cost_reduced = false;
+        lambda = lambda*10 < 1e12 ? lambda*10 : 1e12;
+      }
+
+    } while (inner_loop_cntr++ <
+        optimization_config.inner_loop_max_iteration && !is_cost_reduced);
+
+    inner_loop_cntr = 0;
+
+  } while (outer_loop_cntr++ <
+      optimization_config.outer_loop_max_iteration &&
+      delta_norm > optimization_config.estimation_precision);
+
+  // Covert the feature position from inverse depth
+  // representation to its 3d coordinate.
+  Eigen::Vector3d final_position(measurements[0](0)/solution,
+      measurements[0](1)/solution, 1.0/solution);
+
+  // Check if the solution is valid. Make sure the feature
+  // is in front of every camera frame observing it.
+  bool is_valid_solution = true;
+  for (const auto& pose : cam_poses) {
+    Eigen::Vector3d position =
+      pose.linear()*final_position + pose.translation();
+    if (position(2) <= 0) {
+      is_valid_solution = false;
+      break;
+    }
+  }
+
+  //save inverse depth distance from camera
+  anchor_rho = solution;
+
+  // Convert the feature position to the world frame.
+  position = T_c0_w.linear()*final_position + T_c0_w.translation();
+
+  if (is_valid_solution)
+    is_initialized = true;
+
+  return is_valid_solution;
+}
+
+
 bool Feature::initializePosition(const CamStateServer& cam_states) {
 
   // Organize camera poses and feature observations properly.
+
+
+initializeRho(cam_states);
+std::cout << "init rho is: " << anchor_rho << std::endl;
+
   std::vector<Eigen::Isometry3d,
     Eigen::aligned_allocator<Eigen::Isometry3d> > cam_poses(0);
   std::vector<Eigen::Vector2d,
@@ -1051,6 +1279,7 @@ bool Feature::initializePosition(const CamStateServer& cam_states) {
 
   //save inverse depth distance from camera
   anchor_rho = solution(2);
+  std::cout << "from feature: " << anchor_rho << std::endl;
 
   // Convert the feature position to the world frame.
   position = T_c0_w.linear()*final_position + T_c0_w.translation();

launch/msckf_vio_tum.launch

@@ -18,13 +18,13 @@
       output="screen">
 
       <!-- Photometry Flag-->
-      <param name="PHOTOMETRIC" value="true"/>
+      <param name="PHOTOMETRIC" value="false"/>
 
       <!-- Debugging Flaggs -->
-      <param name="PrintImages" value="true"/>
+      <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="1"/>
+      <param name="patch_size_n" value="3"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 

src/msckf_vio.cpp

@@ -1240,9 +1240,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
 
   //photometric observation
   std::vector<double> photo_z;
-
+  VectorXd r_photo = VectorXd::Zero(N*N);
-  std::vector<double> photo_r;
-
   // 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];
+    residualVector.x += dx * r_photo(count);
-    res_sum += photo_r[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; 
+  r = r_photo;
-  for(auto data : photo_r)
-    r[count++] = data;
 
   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];
 
@@ -1663,11 +1682,19 @@ void MsckfVio::featureJacobian(
    r = A.transpose() * r_j;
 
 
+      // 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;
 
 }