calculate average of jacobis and residual to reduce to single point in caclualtion - does not produce desired result

include/msckf_vio/feature.hpp

@@ -15,7 +15,7 @@
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
 #include <Eigen/StdVector>
-
+#include <math.h>
 #include <visualization_msgs/Marker.h>
 #include <visualization_msgs/MarkerArray.h>
 #include <geometry_msgs/Point.h>
@@ -121,6 +121,15 @@ struct Feature {
   inline bool checkMotion(
       const CamStateServer& cam_states) const;
 
+
+  /*
+   * @brief AnchorPixelToPosition projects an undistorted point in the
+   *        anchor frame back into the real world using the rho calculated 
+   *        based on featur position
+   */
+  inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam);
+
+
   /*
    * @brief InitializeAnchor generates the NxN patch around the
    *        feature in the Anchor image
@@ -148,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
@@ -173,7 +190,7 @@ struct Feature {
                   std::vector<double>& anchorPatch_estimate,
                   IlluminationParameter& estimatedIllumination) const;
 
-bool MarkerGeneration(
+  bool MarkerGeneration(
                   ros::Publisher& marker_pub,
                   const CamStateServer& cam_states) const;
 
@@ -183,12 +200,18 @@ bool MarkerGeneration(
                   CameraCalibration& cam0,
                   const std::vector<double> photo_r,
                   std::stringstream& ss) const;
-  /*
-  * @brief projectPixelToPosition uses the calcualted pixels
-  *     of the anchor patch to generate 3D positions of all of em
+
+
+  /* @brief takes a pure pixel position (1m from image)
+  * converts to actual pixel value and returns patch irradiance
+  * around this pixel
   */
-inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
-          const CameraCalibration& cam);
+  void PatchAroundPurePixel(cv::Point2f p, 
+                               int N,
+                               const CameraCalibration& cam, 
+                               const StateIDType& cam_state_id,
+                               std::vector<float>& return_i) const;
+
 
   /*
   * @brief Irradiance returns irradiance value of a pixel
@@ -390,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;
@@ -573,48 +599,111 @@ bool Feature::VisualizePatch(
 
   cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
 
-  // irradiance grid anchor
+
+  // patches visualization
   int N = sqrt(anchorPatch_3d.size());
-  int scale = 20;
+  int scale = 30;
   cv::Mat irradianceFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255));
   cv::resize(irradianceFrame, irradianceFrame, cv::Size(), rescale, rescale);
+  
+  // irradiance grid anchor
+  std::stringstream namer;
+  namer << "anchor";
+  cv::putText(irradianceFrame, namer.str() , cvPoint(30, 25), 
+    cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
+
+
   for(int i = 0; i<N; i++)
     for(int j = 0; j<N; j++)
       cv::rectangle(irradianceFrame, 
-                    cv::Point(10+scale*(i+1), 10+scale*j), 
-                    cv::Point(10+scale*i, 10+scale*(j+1)), 
+                    cv::Point(30+scale*(i+1), 30+scale*j), 
+                    cv::Point(30+scale*i, 30+scale*(j+1)), 
                     cv::Scalar(anchorPatch[i*N+j]*255, anchorPatch[i*N+j]*255, anchorPatch[i*N+j]*255),  
                     CV_FILLED);
 
   // irradiance grid projection
+  namer.str(std::string());
+  namer << "projection";
+  cv::putText(irradianceFrame, namer.str(), cvPoint(30, 45+scale*N), 
+    cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
+
   for(int i = 0; i<N; i++)
     for(int j = 0; j<N ; j++)
       cv::rectangle(irradianceFrame, 
-                    cv::Point(10+scale*(i+1), 20+scale*(N+j)), 
-                    cv::Point(10+scale*(i), 20+scale*(N+j+1)), 
+                    cv::Point(30+scale*(i+1), 50+scale*(N+j)), 
+                    cv::Point(30+scale*(i), 50+scale*(N+j+1)), 
                     cv::Scalar(projectionPatch[i*N+j]*255, projectionPatch[i*N+j]*255, projectionPatch[i*N+j]*255),  
                     CV_FILLED);
 
+  // true irradiance at feature
+  // get current observation
+
+  namer.str(std::string());
+  namer << "feature";
+  cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N), 
+    cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
+
+  cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
+  // move to real pixels
+  p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs);
+
+  for(int i = 0; i<N; i++)
+  {
+    for(int j = 0; j<N ; j++)
+    {
+      float irr = PixelIrradiance(cv::Point2f(p_f.x + (i-(N-1)/2), p_f.y + (j-(N-1)/2)), current_image);
+      cv::rectangle(irradianceFrame, 
+                    cv::Point(30+scale*(i+1), 70+scale*(2*N+j)), 
+                    cv::Point(30+scale*(i), 70+scale*(2*N+j+1)), 
+                    cv::Scalar(irr*255, irr*255, irr*255),  
+                    CV_FILLED);
+    }
+  }
+
   // residual grid projection, positive - red, negative - blue colored 
+  namer.str(std::string());
+  namer << "residual";
+  cv::putText(irradianceFrame, namer.str() , cvPoint(30+scale*N, scale*N/2-5), 
+    cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
+
   for(int i = 0; i<N; i++)
     for(int j = 0; j<N; j++)
       if(photo_r[i*N+j]>0)
         cv::rectangle(irradianceFrame, 
-                      cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)), 
-                      cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)), 
+                      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_FILLED);
       else
         cv::rectangle(irradianceFrame, 
-                      cv::Point(20+scale*(N+i+1), 15+scale*(N/2+j)), 
-                      cv::Point(20+scale*(N+i), 15+scale*(N/2+j+1)), 
+                      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_FILLED);
 
-      cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
+  // gradient arrow
+  /*
+  cv::arrowedLine(irradianceFrame,
+                  cv::Point(30+scale*(N/2 +0.5), 50+scale*(N+(N/2)+0.5)),
+                  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,
+                  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);
+  
+    
 
-/*
   // 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);
 
@@ -647,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);
@@ -665,12 +756,61 @@ bool Feature::VisualizePatch(
   cvWaitKey(0);
 }
 
+void Feature::PatchAroundPurePixel(cv::Point2f p, 
+                               int N,
+                               const CameraCalibration& cam, 
+                               const StateIDType& cam_state_id,
+                               std::vector<float>& return_i) const
+{
+  int n = (int)(N-1)/2;
+  cv::Mat image = cam.moving_window.find(cam_state_id)->second.image;
+  cv::Point2f img_p = image_handler::distortPoint(p, 
+                                                cam.intrinsics, 
+                                                cam.distortion_model, 
+                                                cam.distortion_coeffs);
+
+  for(double u_run = -n; u_run <= n; u_run++)
+    for(double v_run = -n; v_run <= n; v_run++)
+      return_i.push_back(PixelIrradiance(cv::Point2f(img_p.x+u_run, img_p.y+v_run), image));
+
+}
+
 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
+{
+
+  cv::Point2f cam_p = projectPositionToCamera(cam_state, cam_state_id, cam, in_p);
+
+  // create vector of patch in pixel plane
+  std::vector<cv::Point2f> surroundingPoints;
+  surroundingPoints.push_back(cv::Point2f(cam_p.x+1, cam_p.y));
+  surroundingPoints.push_back(cv::Point2f(cam_p.x-1, cam_p.y));
+  surroundingPoints.push_back(cv::Point2f(cam_p.x, cam_p.y+1));
+  surroundingPoints.push_back(cv::Point2f(cam_p.x, cam_p.y-1));
+
+  std::vector<cv::Point2f> pure;
+  image_handler::undistortPoints(surroundingPoints, 
+                                cam.intrinsics, 
+                                cam.distortion_model, 
+                                cam.distortion_coeffs, 
+                                pure); 
+
+  // returns the absolute pixel distance at pixels one metres away
+  cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y));
+
+  return distance;
+}
+
+
 cv::Point2f Feature::projectPositionToCamera(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
@@ -703,7 +843,7 @@ cv::Point2f Feature::projectPositionToCamera(
   return my_p;
 }
 
-Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam)
+Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam)
 {
   // use undistorted position of point of interest
   // project it back into 3D space using pinhole model
@@ -742,27 +882,19 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
   cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v), 
                                                     cam.intrinsics, 
                                                     cam.distortion_model, 
-                                                    0);
+                                                    cam.distortion_coeffs);
   // create vector of patch in pixel plane
-  std::vector<cv::Point2f>und_pix_v;
   for(double u_run = -n; u_run <= n; u_run++)
     for(double v_run = -n; v_run <= n; v_run++)
-      und_pix_v.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run));
+      anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run));
 
 
   //create undistorted pure points    
-  std::vector<cv::Point2f> und_v;
-  image_handler::undistortPoints(und_pix_v,
+  image_handler::undistortPoints(anchorPatch_real,
                                 cam.intrinsics,
                                 cam.distortion_model,
-                                0,
-                                und_v);
-
-  //create distorted pixel points
-  anchorPatch_real = image_handler::distortPoints(und_v,
-                                                   cam.intrinsics,
-                                                   cam.distortion_model,
-                                                   cam.distortion_coeffs);
+                                cam.distortion_coeffs,
+                                anchorPatch_ideal);
 
 
   // save anchor position for later visualisaztion
@@ -770,23 +902,170 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
 
   // save true pixel Patch position
   for(auto point : anchorPatch_real)
-  {
     if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1))
       return false;
-  }
+
   for(auto point : anchorPatch_real)
     anchorPatch.push_back(PixelIrradiance(point, anchorImage));
 
   // project patch pixel to 3D space in camera  coordinate system
-  for(auto point : und_v)
-  {
-    anchorPatch_ideal.push_back(point);
-    anchorPatch_3d.push_back(projectPixelToPosition(point, cam));
-  }
+  for(auto point : anchorPatch_ideal)
+    anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam));
+
   is_anchored = true;
   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) {
+    // TODO: This should be handled properly. Normally, the
+    //    required camera states should all be available in
+    //    the input cam_states buffer.
+    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
+  Eigen::Vector3d initial_position(0.0, 0.0, 0.0);
+  generateInitialGuess(cam_poses[cam_poses.size()-1], measurements[0],
+      measurements[measurements.size()-1], initial_position);
+  Eigen::Vector3d solution(
+      initial_position(0)/initial_position(2),
+      initial_position(1)/initial_position(2),
+      1.0/initial_position(2));
+
+  // 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;
+    cost(cam_poses[i], solution, measurements[i], this_cost);
+    total_cost += this_cost;
+  }
+
+  // Outer loop.
+  do {
+    Eigen::Matrix3d A = Eigen::Matrix3d::Zero();
+    Eigen::Vector3d b = Eigen::Vector3d::Zero();
+
+    for (int i = 0; i < cam_poses.size(); ++i) {
+      Eigen::Matrix<double, 2, 3> J;
+      Eigen::Vector2d r;
+      double w;
+
+      jacobian(cam_poses[i], solution, 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::Matrix3d damper = lambda * Eigen::Matrix3d::Identity();
+      Eigen::Vector3d delta = (A+damper).ldlt().solve(b);
+      Eigen::Vector3d new_solution = solution - delta;
+      delta_norm = delta.norm();
+
+      double new_cost = 0.0;
+      for (int i = 0; i < cam_poses.size(); ++i) {
+        double this_cost = 0.0;
+        cost(cam_poses[i], new_solution, 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(solution(0)/solution(2),
+      solution(1)/solution(2), 1.0/solution(2));
+
+  // 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(2);
+
+  // 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.

include/msckf_vio/image_handler.h

@@ -36,6 +36,16 @@ cv::Point2f distortPoint(
     const cv::Vec4d& intrinsics,
     const std::string& distortion_model,
     const cv::Vec4d& distortion_coeffs);
+
+void undistortPoint(
+    const cv::Point2f& pt_in,
+    const cv::Vec4d& intrinsics,
+    const std::string& distortion_model,
+    const cv::Vec4d& distortion_coeffs,
+    cv::Point2f& pt_out,
+    const cv::Matx33d &rectification_matrix = cv::Matx33d::eye(),
+    const cv::Vec4d &new_intrinsics = cv::Vec4d(1,1,0,0));
+
 }
 }
 #endif

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:

include/msckf_vio/msckf_vio.h

@@ -14,7 +14,7 @@
 #include <string>
 #include <Eigen/Dense>
 #include <Eigen/Geometry>
-
+#include <math.h>
 #include <boost/shared_ptr.hpp>
 #include <opencv2/opencv.hpp>
 #include <opencv2/video.hpp>
@@ -38,6 +38,8 @@
 #include <message_filters/subscriber.h>
 #include <message_filters/time_synchronizer.h>
 
+#define PI 3.14159265
+
 namespace msckf_vio {
 /*
  * @brief MsckfVio Implements the algorithm in

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="7"/>

src/image_handler.cpp

@@ -14,6 +14,47 @@ namespace msckf_vio {
 namespace image_handler {
 
 
+void undistortPoint(
+    const cv::Point2f& pt_in,
+    const cv::Vec4d& intrinsics,
+    const std::string& distortion_model,
+    const cv::Vec4d& distortion_coeffs,
+    cv::Point2f& pt_out,
+    const cv::Matx33d &rectification_matrix,
+    const cv::Vec4d &new_intrinsics) {
+
+
+  std::vector<cv::Point2f> pts_in;
+  std::vector<cv::Point2f> pts_out;
+  pts_in.push_back(pt_in);
+  if (pts_in.size() == 0) return;
+
+  const cv::Matx33d K(
+      intrinsics[0], 0.0, intrinsics[2],
+      0.0, intrinsics[1], intrinsics[3],
+      0.0, 0.0, 1.0);
+
+  const cv::Matx33d K_new(
+      new_intrinsics[0], 0.0, new_intrinsics[2],
+      0.0, new_intrinsics[1], new_intrinsics[3],
+      0.0, 0.0, 1.0);
+
+  if (distortion_model == "radtan") {
+    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
+                        rectification_matrix, K_new);
+  } else if (distortion_model == "equidistant") {
+    cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
+                                 rectification_matrix, K_new);
+  } else {
+    ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...",
+                  distortion_model.c_str());
+    cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
+                        rectification_matrix, K_new);
+  }
+  pt_out = pts_out[0];
+  return;
+}
+
 void undistortPoints(
     const std::vector<cv::Point2f>& pts_in,
     const cv::Vec4d& intrinsics,

src/msckf_vio.cpp

@@ -428,21 +428,18 @@ void MsckfVio::imageCallback(
   
   // save true state in seperate state vector
 
-  //if(ErrState)
-  //{
-    batchTruthProcessing(feature_msg->header.stamp.toSec());
+  batchTruthProcessing(feature_msg->header.stamp.toSec());
   
-    if(GROUNDTRUTH)
-    {
-      state_server.imu_state.position = true_state_server.imu_state.position;
-      state_server.imu_state.orientation = true_state_server.imu_state.orientation;
-      state_server.imu_state.position_null = true_state_server.imu_state.position_null;
-      state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null;
+  if(GROUNDTRUTH)
+  {
+    state_server.imu_state.position = true_state_server.imu_state.position;
+    state_server.imu_state.orientation = true_state_server.imu_state.orientation;
+    state_server.imu_state.position_null = true_state_server.imu_state.position_null;
+    state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null;
 
-      state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0;
-      state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu;
-    }
-  //}
+    state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0;
+    state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu;
+  }
   double imu_processing_time = (
       ros::Time::now()-start_time).toSec();
 
@@ -1228,83 +1225,90 @@ void MsckfVio::PhotometricMeasurementJacobian(
   const CAMState& cam_state = state_server.cam_states[cam_state_id];
   const Feature& feature = map_server[feature_id];
 
+  const StateIDType anchor_state_id = feature.observations.begin()->first;
+  const CAMState anchor_state = state_server.cam_states[anchor_state_id];
+
   // Cam0 pose.
   Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
   const Vector3d& t_c0_w = cam_state.position;
 
-  // Cam1 pose.
-  Matrix3d R_c0_c1 = CAMState::T_cam0_cam1.linear();
-  Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0;
-  Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation();
-
-
-  //photometric observation
-  std::vector<double> photo_z;
-
-  // individual Jacobians
-  Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
-  Matrix<double, 2, 3> dh_dCpij = Matrix<double, 2, 3>::Zero();
-  Matrix<double, 2, 3> dh_dGpij = Matrix<double, 2, 3>::Zero();
-  Matrix<double, 2, 6> dh_dXplj = Matrix<double, 2, 6>::Zero();
-  Matrix<double, 3, 1> dGpj_drhoj = Matrix<double, 3, 1>::Zero();
-  Matrix<double, 3, 6> dGpj_XpAj = Matrix<double, 3, 6>::Zero();
-
-  Matrix<double, 3, 3> dCpij_dGpij = Matrix<double, 3, 3>::Zero();
-  Matrix<double, 3, 3> dCpij_dCGtheta = Matrix<double, 3, 3>::Zero();
-  Matrix<double, 3, 3> dCpij_dGpC = Matrix<double, 3, 3>::Zero();
-
   // one line of the NxN Jacobians
-  Eigen::Matrix<double, 1, 1> H_rhoj;
-  Eigen::Matrix<double, 1, 6> H_plj;
-  Eigen::Matrix<double, 1, 6> H_pAj;
-
-  // combined Jacobians
-  Eigen::MatrixXd H_rho(N*N, 1);
-  Eigen::MatrixXd H_pl(N*N, 6);
-  Eigen::MatrixXd H_pA(N*N, 6);
+  Matrix<double, 1, 1> H_rho;
+  Matrix<double, 1, 6> H_xi;
+  Matrix<double, 1, 6> H_xA;
+/*
+  MatrixXd H_xl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 6);
+  MatrixXd H_xAl = MatrixXd::Zero(feature.anchorPatch_3d.size(), 6);
+  MatrixXd H_rhol = MatrixXd::Zero(feature.anchorPatch_3d.size(), 1);
+*/
+  MatrixXd H_xl = MatrixXd::Zero(1, 6);
+  MatrixXd H_xAl = MatrixXd::Zero(1, 6);
+  MatrixXd H_rhol = MatrixXd::Zero(1, 1);
 
   auto frame = cam0.moving_window.find(cam_state_id)->second.image;
   
   int count = 0;
   double dx, dy;
 
+  std::vector<float> z_irr_est;
+  // gradient visualization parameters
+  cv::Point2f gradientVector(0,0);
+
+  // residual change visualization
+  cv::Point2f residualVector(0,0);
+  double res_sum = 0;
+
+  // get patch around feature in current image
+  cv::Point2f p_f(feature.observations.find(cam_state_id)->second(0),feature.observations.find(cam_state_id)->second(1));
+  // move to real pixels
+  p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs);
+
+  std::vector<double> cur_image_irr;
+  std::vector<cv::Point2f> cur_image_p;
+  for(int i = 0; i<N; i++)
+  {
+    for(int j = 0; j<N ; j++)
+    {
+      cur_image_p.push_back(cv::Point2f(p_f.x + (i-(N-1)/2), p_f.y + (j-(N-1)/2)));
+      cur_image_irr.push_back(feature.PixelIrradiance(cv::Point2f(p_f.x + (i-(N-1)/2), p_f.y + (j-(N-1)/2)), frame));
+    }
+  }
   for (auto point : feature.anchorPatch_3d)
   {
     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);
     
-    //add observation
-    photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
-
-    // add jacobian
+    // add jacobians
+    cv::Point2f pixelDistance = feature.pixelDistanceAt(cam_state, cam_state_id, cam0, point);
 
+    // calculate derivation for anchor frame, use position for derivation calculation
     // 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, 1) = dy;
+    Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
+    dx = feature.PixelIrradiance(cv::Point2f(cur_image_p[count].x+1, cur_image_p[count].y), frame) - feature.PixelIrradiance(cv::Point2f(cur_image_p[count].x-1, cur_image_p[count].y), frame);
+    dy = feature.PixelIrradiance(cv::Point2f(cur_image_p[count].x, cur_image_p[count].y+1), frame) - feature.PixelIrradiance(cv::Point2f(cur_image_p[count].x, cur_image_p[count].y-1), frame);
+    dI_dhj(0, 0) = (double)dx/pixelDistance.x;
+    dI_dhj(0, 1) = (double)dy/pixelDistance.y;
 
-    //dh / d{}^Cp_{ij}
-    dh_dCpij(0, 0) = 1 / p_c0(2);
-    dh_dCpij(1, 1) = 1 / p_c0(2);
-    dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2));
-    dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2));
+    // Compute the Jacobians.
+    Matrix<double, 2, 3> dz_dpc0 = Matrix<double, 2, 3>::Zero();
+    dz_dpc0(0, 0) = 1 / p_c0(2);
+    dz_dpc0(1, 1) = 1 / p_c0(2);
+    dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2));
+    dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2)*p_c0(2));
 
-    dCpij_dGpij = quaternionToRotation(cam_state.orientation);
+    Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero();
+    
+    // original jacobi
+    dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
+    dpc0_dxc.rightCols(3) = -R_w_c0;
+
+    Matrix3d dCpij_dGpij = quaternionToRotation(cam_state.orientation);
 
     //orientation takes camera frame to world frame, we wa
-    dh_dGpij = dh_dCpij * dCpij_dGpij;
+    Matrix<double, 2, 3> dh_dGpij = dz_dpc0 * dCpij_dGpij;
 
-    //dh / d X_{pl}
-    dCpij_dCGtheta = skewSymmetric(p_c0);
-    dCpij_dGpC = -quaternionToRotation(cam_state.orientation);
-    dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta;
-    dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC;
-
-    //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));
+
+    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), 
@@ -1312,82 +1316,85 @@ void MsckfVio::PhotometricMeasurementJacobian(
                                                    1/(rho)));
     dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
 
-    // Intermediate Jakobians
-    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
 
-    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;
+    // Isometry T_anchor_w takes a vector in anchor frame to world frame
+    Matrix<double, 3, 1> dGpj_drhoj = Matrix<double, 3, 1>::Zero();
+    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));
+
+    H_xi = dI_dhj * dz_dpc0 * dpc0_dxc;
+    H_xA = dI_dhj * dh_dGpij * dGpj_XpAj;
+    H_rho = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1
+
+/*
+    H_xl.block<1, 6>(count, 0) = H_xi;
+    H_xAl.block<1, 6>(count, 0) = H_xA;
+    H_rhol.block<1, 1>(count, 0) = H_rho;
+*/
+    H_xl += H_xi;
+    H_xAl += H_xA;
+    H_rhol += H_rho;
     
     count++;
   }
-  // calculate residual
 
-  //observation
-  const Vector4d& z = feature.observations.find(cam_state_id)->second;
+  // calculate projected irradiance
+  std::vector<double> projectionPatch;
+  for(auto point : feature.anchorPatch_3d)
+  {
+    cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
+    projectionPatch.push_back(feature.PixelIrradiance(p_in_c0, frame));
+  }  
 
-  //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);
+  //Eigen::VectorXd r_l = Eigen::VectorXd::Zero(count);
+Eigen::VectorXd r_l = Eigen::VectorXd::Zero(1);
 
-  std::vector<double> photo_r;
+  std::vector<double> residual_v;
+  double residual_v_sum = 0;
+  for(int i = 0; i < N*N; i++)
+  {
+    residual_v_sum += cur_image_irr[i] - projectionPatch[i];
+    residual_v.push_back(cur_image_irr[i] - projectionPatch[i]);
+    //r_l(i) = cur_image_irr[i] - projectionPatch[i];
+  }
 
-  //calculate photom. residual
-  for(int i = 0; i < photo_z.size(); i++)
-    photo_r.push_back(photo_z[i] - estimate_photo_z[i]);
+  r_l(0) = residual_v_sum/(N*N);
+  H_xl /=(N*N);
+  H_xAl /= (N*N);
+  H_rhol /= (N*N);
 
-  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);
+  cout << "dI/dxl\n" << H_xl << endl;
+  cout << "dI/dAl\n" << H_xAl << endl;
+  cout << "dI/drhol\n" << H_rhol << endl;
+
+
+  r = r_l;
+
+  MatrixXd H_xt = MatrixXd::Zero(1, 21+state_server.cam_states.size()*7);
 
   // set anchor Jakobi
-    // get position of anchor in cam states
-
+  // get position of anchor in cam states
   auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first);
   int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor);
-  H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA; 
+  //H_xt.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = H_xAl; 
+  H_xt.block(0, 21+cam_state_cntr_anchor*7, 1, 6) = H_xAl; 
 
   // set frame Jakobi
-    //get position of current frame in cam states
+  //get position of current frame in cam states
   auto cam_state_iter = state_server.cam_states.find(cam_state_id);
   int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter);
+  //H_xt.block(0, 21+cam_state_cntr*7, N*N, 6) = H_xl;
+  H_xt.block(0, 21+cam_state_cntr*7, 1, 6) = H_xl;
   
-    // set jakobi of state
-  H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl;
+  H_x = H_xt;
+  H_y = H_rhol;
 
-    // set ones for irradiance bias
-  H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N);
-
-  // set irradiance error Block
-  H_yl.block(0, 0,N*N, N*N) = estimated_illumination.feature_gain * estimated_illumination.frame_gain * Eigen::MatrixXd::Identity(N*N, N*N);
-  
-  // TODO make this calculation more fluent
-  for(int i = 0; i< N*N; i++)
-    H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i];
-  H_yl.block(0, N*N+state_server.cam_states.size(), N*N, 1) = -H_rho;
-
-  H_x = H_xl;
-  H_y = H_yl;
-
-  //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);
+    feature.VisualizePatch(cam_state, cam_state_id, cam0, residual_v, ss);
   }
 
   return;
@@ -1433,15 +1440,18 @@ void MsckfVio::PhotometricFeatureJacobian(
     if (feature.observations.find(cam_id) ==
         feature.observations.end()) continue;
 
-    valid_cam_state_ids.push_back(cam_id);
+    // if not anchor frame
+    if (cam_id != feature.observations.begin()->first)
+      valid_cam_state_ids.push_back(cam_id);
   }
 
   int jacobian_row_size = 0;
-  jacobian_row_size = N * N * valid_cam_state_ids.size();
+  //jacobian_row_size = N * N * valid_cam_state_ids.size();
+jacobian_row_size = valid_cam_state_ids.size();
 
   MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
       21+state_server.cam_states.size()*7);
-  MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+state_server.cam_states.size()+1);
+  MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1);
   VectorXd r_i = VectorXd::Zero(jacobian_row_size);
   int stack_cntr = 0;
 
@@ -1457,35 +1467,34 @@ void MsckfVio::PhotometricFeatureJacobian(
     int cam_state_cntr = std::distance(
         state_server.cam_states.begin(), cam_state_iter);
 
+    auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first);
+    int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor);
+
+
     // Stack the Jacobians.
     H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl;
-    H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl;
-    r_i.segment(stack_cntr, N*N) = r_l;
-    stack_cntr += N*N;
+    H_yi.block(stack_cntr,0, H_yl.rows(), H_yl.cols()) = H_yl;
+
+
+    //r_i.segment(stack_cntr, N*N) = r_l;
+    //stack_cntr += N*N;
+    r_i.segment(stack_cntr, 1) = r_l;
+    stack_cntr += 1;
   }
 
-  // Project the residual and Jacobians onto the nullspace
-  // of H_yj.
-
-  // get Nullspace
   FullPivLU<MatrixXd> lu(H_yi.transpose());
-  MatrixXd A_null_space = lu.kernel();
-  /*
-  JacobiSVD<MatrixXd> svd_helper(H_yi, ComputeFullU | ComputeThinV);
-  
-  int sv_size = 0;
-  Eigen::VectorXd singularValues = svd_helper.singularValues();
-  for(int i = 0; i < singularValues.size(); i++)
-    if(singularValues[i] > 1e-12)
-      sv_size++;
-
-  MatrixXd A = svd_helper.matrixU().rightCols(jacobian_row_size - singularValues.size());
-  */
-  H_x = A_null_space.transpose() * H_xi;
-  r = A_null_space.transpose() * r_i;
+  MatrixXd A = lu.kernel();
+  H_x = A.transpose() * H_xi;
+  r = A.transpose() * r_i;
 
   if(PRINTIMAGES)
   {
+    ofstream myfile;
+    myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
+    myfile << "Hyi\n" << H_yi << "Hxi\n" << H_xi << "Hx\n" << H_x << "r\n" << r << "\n x\n" << H_x.colPivHouseholderQr().solve(r) << endl;
+    myfile.close();
+    cout << "---------- LOGGED -------- " << endl; 
+
     std::cout << "resume playback" << std::endl;
     nh.setParam("/play_bag", true);
   }
@@ -1495,8 +1504,7 @@ void MsckfVio::PhotometricFeatureJacobian(
 void MsckfVio::measurementJacobian(
     const StateIDType& cam_state_id,
     const FeatureIDType& feature_id,
-    Matrix<double, 4, 6>& H_x, Matrix<double, 4, 3>& H_f, Vector4d& r)
-{
+    Matrix<double, 4, 6>& H_x, Matrix<double, 4, 3>& H_f, Vector4d& r) {
 
   // Prepare all the required data.
   const CAMState& cam_state = state_server.cam_states[cam_state_id];
@@ -1535,12 +1543,7 @@ void MsckfVio::measurementJacobian(
   dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2));
 
   Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero();
-  
-  // original jacobi
-  //dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
-  // my version of calculation
   dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
-  //dpc0_dxc.leftCols(3) = - skewSymmetric(R_w_c0.transpose() * (t_c0_w - p_w)) * R_w_c0;
   dpc0_dxc.rightCols(3) = -R_w_c0;
 
   Matrix<double, 3, 6> dpc1_dxc = Matrix<double, 3, 6>::Zero();
@@ -1553,6 +1556,17 @@ void MsckfVio::measurementJacobian(
   H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc;
   H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg;
 
+  // Modifty the measurement Jacobian to ensure
+  // observability constrain.
+  Matrix<double, 4, 6> A = H_x;
+  Matrix<double, 6, 1> u = Matrix<double, 6, 1>::Zero();
+  u.block<3, 1>(0, 0) = quaternionToRotation(
+      cam_state.orientation_null) * IMUState::gravity;
+  u.block<3, 1>(3, 0) = skewSymmetric(
+      p_w-cam_state.position_null) * IMUState::gravity;
+  H_x = A - A*u*(u.transpose()*u).inverse()*u.transpose();
+  H_f = -H_x.block<4, 3>(0, 3);
+
   // Compute the residual.
   r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2),
       p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));
@@ -1560,11 +1574,14 @@ void MsckfVio::measurementJacobian(
   return;
 }
 
+
 void MsckfVio::featureJacobian(
     const FeatureIDType& feature_id,
     const std::vector<StateIDType>& cam_state_ids,
     MatrixXd& H_x, VectorXd& r) 
 {
+  if(PRINTIMAGES)
+    nh.setParam("/play_bag", false);
 
   const auto& feature = map_server[feature_id];
 
@@ -1626,16 +1643,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 << "-- residual -- \n" << r << "\n---- H ----\n" << H_x << "\n---- state cov ----\n" << state_server.state_cov <<endl;
-  myfile.close();
-  cout << "---------- LOGGED -------- " << endl; 
-  */
-  nh.setParam("/play_bag", false);
   
+  if(PRINTIMAGES)
+  {
+    ofstream myfile;
+    myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
+    myfile << H_x.colPivHouseholderQr().solve(r) << endl;
+    myfile.close();
+    cout << "---------- LOGGED -------- " << endl; 
+
+    std::cout << "resume playback" << std::endl;
+    nh.setParam("/play_bag", true);
+  }
   return;
+
 }
 
 void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
@@ -1646,7 +1667,11 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
   // complexity as in Equation (28), (29).
   MatrixXd H_thin;
   VectorXd r_thin;
+  int augmentationSize = 6;
+  if(PHOTOMETRIC)
+    augmentationSize = 7;
 
+  /*
   if (H.rows() > H.cols()) {
     // Convert H to a sparse matrix.
     SparseMatrix<double> H_sparse = H.sparseView();
@@ -1661,8 +1686,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
     (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp);
     (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp);
 
-    H_thin = H_temp.topRows(21+state_server.cam_states.size()*6);
-    r_thin = r_temp.head(21+state_server.cam_states.size()*6);
+    H_thin = H_temp.topRows(21+state_server.cam_states.size()*augmentationSize);
+    r_thin = r_temp.head(21+state_server.cam_states.size()*augmentationSize);
 
     //HouseholderQR<MatrixXd> qr_helper(H);
     //MatrixXd Q = qr_helper.householderQ();
@@ -1674,18 +1699,19 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
     H_thin = H;
     r_thin = r;
   }
+  */
 
   // Compute the Kalman gain.
   const MatrixXd& P = state_server.state_cov;
-  MatrixXd S = H_thin*P*H_thin.transpose() +
+  MatrixXd S = H*P*H.transpose() +
       Feature::observation_noise*MatrixXd::Identity(
-        H_thin.rows(), H_thin.rows());
-  //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H_thin*P);
-  MatrixXd K_transpose = S.ldlt().solve(H_thin*P);
+        H.rows(), H.rows());
+  //MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P);
+  MatrixXd K_transpose = S.ldlt().solve(H*P);
   MatrixXd K = K_transpose.transpose();
 
   // Compute the error of the state.
-  VectorXd delta_x = K * r_thin;
+  VectorXd delta_x = K * r;
 
   // Update the IMU state.
   const VectorXd& delta_x_imu = delta_x.head<21>();
@@ -1720,7 +1746,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
   auto cam_state_iter = state_server.cam_states.begin();
   for (int i = 0; i < state_server.cam_states.size();
       ++i, ++cam_state_iter) {
-    const VectorXd& delta_x_cam = delta_x.segment<6>(21+i*6);
+    const VectorXd& delta_x_cam = delta_x.segment(21+i*augmentationSize, augmentationSize);
     const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>());
     cam_state_iter->second.orientation = quaternionMultiplication(
         dq_cam, cam_state_iter->second.orientation);
@@ -1728,7 +1754,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
   }
 
   // Update state covariance.
-  MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin;
+  MatrixXd I_KH = MatrixXd::Identity(K.rows(), H.cols()) - K*H;
   //state_server.state_cov = I_KH*state_server.state_cov*I_KH.transpose() +
   //  K*K.transpose()*Feature::observation_noise;
   state_server.state_cov = I_KH*state_server.state_cov;
@@ -1742,7 +1768,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
 }
 
 bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
-
   MatrixXd P1 = H * state_server.state_cov * H.transpose();
 
 
@@ -1851,7 +1876,8 @@ void MsckfVio::removeLostFeatures() {
     else
       featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
 
-    if (gatingTest(H_xj, r_j, cam_state_ids.size()-1)) {
+
+    if (gatingTest(H_xj, r_j, state_server.cam_states.size() - 1)){ //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();
@@ -2013,7 +2039,7 @@ void MsckfVio::pruneCamStateBuffer() {
     else
       featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
     
-    if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {
+    if (gatingTest(H_xj, r_j, involved_cam_state_ids.size() - 1)) { //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();