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

CMakeLists.txt

@@ -24,6 +24,7 @@ find_package(catkin REQUIRED COMPONENTS
   pcl_conversions
   pcl_ros
   std_srvs
+  visualization_msgs
 )
 
 ## System dependencies are found with CMake's conventions

include/msckf_vio/feature.hpp

@@ -15,6 +15,10 @@
 #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>
 
 #include "image_handler.h"
 
@@ -22,6 +26,7 @@
 #include "imu_state.h"
 #include "cam_state.h"
 
+
 namespace msckf_vio {
 
 /*
@@ -116,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
@@ -126,7 +140,7 @@ struct Feature {
    */
 
   bool initializeAnchor(
-   const CameraCalibration& cam);
+   const CameraCalibration& cam, int N);
 
 
   /*
@@ -143,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
@@ -165,20 +187,31 @@ struct Feature {
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
                   CameraCalibration& cam0,
-                  std::vector<float>& anchorPatch_estimate) const;
+                  std::vector<double>& anchorPatch_estimate,
+                  IlluminationParameter& estimatedIllumination) const;
 
-  bool FrameIrradiance(
+  bool MarkerGeneration(
+                  ros::Publisher& marker_pub,
+                  const CamStateServer& cam_states) const;
+
+  bool VisualizePatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
                   CameraCalibration& cam0,
-                  std::vector<float>& anchorPatch_measurement) const;
+                  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
@@ -203,8 +236,10 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
 
   // NxN Patch of Anchor Image
   std::vector<double> anchorPatch;
+  std::vector<cv::Point2f> anchorPatch_ideal;
+  std::vector<cv::Point2f> anchorPatch_real;
 
-  // Position of NxN Patch in 3D space
+  // Position of NxN Patch in 3D space in anchor camera frame
   std::vector<Eigen::Vector3d> anchorPatch_3d;
 
   // Anchor Isometry
@@ -239,7 +274,8 @@ typedef std::map<FeatureIDType, Feature, std::less<int>,
 
 void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
     const Eigen::Vector3d& x, const Eigen::Vector2d& z,
-    double& e) const {
+    double& e) const 
+{
   // Compute hi1, hi2, and hi3 as Equation (37).
   const double& alpha = x(0);
   const double& beta = x(1);
@@ -262,7 +298,8 @@ void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
 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,
-    double& w) const {
+    double& w) const 
+{
 
   // Compute hi1, hi2, and hi3 as Equation (37).
   const double& alpha = x(0);
@@ -299,7 +336,8 @@ void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci,
 
 void Feature::generateInitialGuess(
     const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1,
-    const Eigen::Vector2d& z2, Eigen::Vector3d& p) const {
+    const Eigen::Vector2d& z2, Eigen::Vector3d& p) 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);
 
@@ -319,8 +357,8 @@ void Feature::generateInitialGuess(
   return;
 }
 
-bool Feature::checkMotion(
-    const CamStateServer& cam_states) const {
+bool Feature::checkMotion(const CamStateServer& cam_states) const 
+{
 
   const StateIDType& first_cam_id = observations.begin()->first;
   const StateIDType& last_cam_id = (--observations.end())->first;
@@ -366,7 +404,8 @@ bool Feature::estimate_FrameIrradiance(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
                   CameraCalibration& cam0,
-                  std::vector<float>& anchorPatch_estimate) const
+                  std::vector<double>& anchorPatch_estimate,
+                  IlluminationParameter& estimated_illumination) const
 {
   // get irradiance of patch in anchor frame
   // subtract estimated b and divide by a of anchor frame
@@ -374,77 +413,404 @@ 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;
 
 
   double a_A = anchorExposureTime_ms;
   double b_A = 0;
-  double a_l =frameExposureTime_ms;
+  double a_l = frameExposureTime_ms;
   double b_l = 0;
-  
+
+  estimated_illumination.frame_gain = a_l;
+  estimated_illumination.frame_bias = b_l;
+  estimated_illumination.feature_gain = 1;
+  estimated_illumination.feature_bias = 0;
   //printf("frames: %lld, %lld\n", anchor->first, cam_state_id);
   //printf("exposure: %f, %f\n", a_A, a_l);
   
   for (double anchorPixel : anchorPatch)
   {
-    float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l;
+    float irradiance = (anchorPixel - b_A) / a_A ;
     anchorPatch_estimate.push_back(irradiance);
   }
-
 }
 
-bool Feature::FrameIrradiance(
+// generates markers for every camera position/observation
+// and estimated feature/path position
+bool Feature::MarkerGeneration(
+                  ros::Publisher& marker_pub,
+                  const CamStateServer& cam_states) const
+{
+  visualization_msgs::MarkerArray ma;
+
+  // add all camera states used for estimation
+  int count = 0;
+
+  for(auto observation : observations)
+  {
+    visualization_msgs::Marker marker;
+    marker.header.frame_id = "world";
+    marker.header.stamp = ros::Time::now();
+
+    marker.ns = "cameras";
+    marker.id = count++;
+    marker.type =  visualization_msgs::Marker::ARROW;
+    marker.action = visualization_msgs::Marker::ADD;
+
+    marker.pose.position.x = cam_states.find(observation.first)->second.position(0);
+    marker.pose.position.y = cam_states.find(observation.first)->second.position(1);
+    marker.pose.position.z = cam_states.find(observation.first)->second.position(2);
+
+    // rotate form x to z axis
+    Eigen::Vector4d q = quaternionMultiplication(Eigen::Vector4d(0, -0.707, 0, 0.707), cam_states.find(observation.first)->second.orientation);
+
+    marker.pose.orientation.x = q(0);
+    marker.pose.orientation.y = q(1);
+    marker.pose.orientation.z = q(2);
+    marker.pose.orientation.w = q(3);
+
+    marker.scale.x = 0.15;
+    marker.scale.y = 0.05;
+    marker.scale.z = 0.05;
+
+    if(count == 1)
+    {
+        marker.color.r = 0.0f;
+        marker.color.g = 0.0f;
+        marker.color.b = 1.0f;
+    }
+    else
+    {
+        marker.color.r = 0.0f;
+        marker.color.g = 1.0f;
+        marker.color.b = 0.0f;
+    }
+    marker.color.a = 1.0;
+
+    marker.lifetime = ros::Duration(0);
+
+    ma.markers.push_back(marker);
+  }
+
+  // 'delete' any existing cameras (make invisible)
+  for(int i = count; i < 20; i++)
+  {
+    visualization_msgs::Marker marker;
+
+    marker.header.frame_id = "world";
+    marker.header.stamp = ros::Time::now();
+
+    marker.ns = "cameras";
+    marker.id = i;
+    marker.type =  visualization_msgs::Marker::ARROW;
+    marker.action = visualization_msgs::Marker::ADD;
+
+    marker.pose.orientation.w = 1;
+
+    marker.color.a = 0.0;
+
+    marker.lifetime = ros::Duration(1);
+
+    ma.markers.push_back(marker);
+  }
+
+  //generate feature patch points position
+  visualization_msgs::Marker marker;
+  
+  marker.header.frame_id = "world";
+  marker.header.stamp = ros::Time::now();
+
+  marker.ns = "patch";
+  marker.id = 0;
+  marker.type = visualization_msgs::Marker::POINTS;
+  marker.action = visualization_msgs::Marker::ADD;
+
+  marker.pose.orientation.w = 1;
+
+  marker.scale.x = 0.02;
+  marker.scale.y = 0.02;
+
+  marker.color.r = 1.0f;
+  marker.color.g = 0.0f;
+  marker.color.b = 0.0f;
+  marker.color.a = 1.0;
+
+  for(auto point : anchorPatch_3d)
+  {
+    geometry_msgs::Point p;
+    p.x = point(0);
+    p.y = point(1);
+    p.z = point(2);
+    marker.points.push_back(p);
+  }
+  ma.markers.push_back(marker);
+
+  marker_pub.publish(ma);
+}
+
+bool Feature::VisualizePatch(
                   const CAMState& cam_state,
                   const StateIDType& cam_state_id,
                   CameraCalibration& cam0,
-                  std::vector<float>& anchorPatch_measurement) const
+                  const std::vector<double> photo_r,
+                  std::stringstream& ss) const
 {
 
-  // visu - feature
-  /*cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
-  cv::Mat dottedFrame(current_image.size(), CV_8UC3);
-  cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
-  */
+  double rescale = 1;
 
-  // project every point in anchorPatch_3d.
-  for (auto point : anchorPatch_3d)
+  //visu - anchor
+  auto anchor = observations.begin();
+  cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
+  cv::Mat dottedFrame(anchorImage.size(), CV_8UC3);
+  cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB);
+
+  // visualize the true anchor points (the surrounding of the original measurements)
+  for(auto point : anchorPatch_real)
   {
-
-    cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
-
     // visu - feature
-    /*cv::Point xs(p_in_c0.x, p_in_c0.y);
+    cv::Point xs(point.x, point.y);
+    cv::Point ys(point.x, point.y);
+    cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255));
+  }  
+  cam0.featureVisu = dottedFrame.clone(); 
+
+  // visu - feature
+  cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
+  cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
+  
+  // set position in frame 
+  // save irradiance of projection
+  std::vector<double> projectionPatch;
+  for(auto point : anchorPatch_3d)
+  {
+    cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
+    projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image));
+    // visu - feature
+    cv::Point xs(p_in_c0.x, p_in_c0.y);
     cv::Point ys(p_in_c0.x, p_in_c0.y);
     cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
-  */
-    float irradiance = PixelIrradiance(p_in_c0, cam0.moving_window.find(cam_state_id)->second.image);
-    anchorPatch_measurement.push_back(irradiance);
-    
-    // testing
-    //if(cam_state_id == observations.begin()->first)
-      //if(count++ == 4)
-        //printf("dist:\n \tpos: %f, %f\n\ttrue pos: %f, %f\n\n", p_in_c0.x, p_in_c0.y, anchor_center_pos.x, anchor_center_pos.y);
-    
+  }  
+
+  cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
+
+
+  // patches visualization
+  int N = sqrt(anchorPatch_3d.size());
+  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(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(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);
+    }
   }
 
-  // visu - feature
-  //cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2));
-  /*if(cam0.featureVisu.empty())
-    cam0.featureVisu = dottedFrame.clone();
-  else
-    cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
+  // 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(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(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);
+
+  // 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);
+
+
+  // draw world zero
+  cv::line(positionFrame,
+          cv::Point(20,20),
+          cv::Point(20,30),
+          cv::Scalar(0,0,255),
+          CV_FILLED);
+
+  cv::line(positionFrame,
+          cv::Point(20,20),
+          cv::Point(30,20),
+          cv::Scalar(255,0,0),
+          CV_FILLED);
+  // for every observation, get cam state
+  for(auto obs : observations)
+  {
+      cv::line(positionFrame,
+        cv::Point(20,20),
+        cv::Point(30,20),
+        cv::Scalar(255,0,0),
+        CV_FILLED);
+  }
+
+  // draw, x y position and arrow with direction - write z next to it
+
+  cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale);
+
+
+  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);
+  cv::putText(cam0.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30), 
+    cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(200,200,250), 1, CV_AA);
+  // create line?
+
+  //save image
+
+  std::stringstream loc;
+ // loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
+  //cv::imwrite(loc.str(), cam0.featureVisu);
+
+  cv::imshow("patch", cam0.featureVisu);
+  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.x, pose.y);
+
+  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,
@@ -477,27 +843,23 @@ 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
   // save resulting NxN positions for this feature
 
   Eigen::Vector3d PositionInCamera(in_p.x/anchor_rho, in_p.y/anchor_rho, 1/anchor_rho);
-  Eigen::Vector3d PositionInWorld= T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation();
+  Eigen::Vector3d PositionInWorld = T_anchor_w.linear()*PositionInCamera + T_anchor_w.translation();
   return PositionInWorld;
   //printf("%f, %f, %f\n",PositionInWorld[0], PositionInWorld[1], PositionInWorld[2]);
 }
 
 //@test center projection must always be initial feature projection
-bool Feature::initializeAnchor(
-   const CameraCalibration& cam)
+bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
 {
 
   //initialize patch Size
-  //TODO make N size a ros parameter
-  int N = 3;
   int n = (int)(N-1)/2;
 
   auto anchor = observations.begin();
@@ -516,52 +878,45 @@ bool Feature::initializeAnchor(
   int count = 0;
 
   // get feature in undistorted pixel space
+  // this only reverts from 'pure' space into undistorted pixel space using camera matrix
   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
-  std::vector<cv::Point2f> vec = image_handler::distortPoints(und_v,
-                                                             cam.intrinsics,
-                                                             cam.distortion_model,
-                                                             cam.distortion_coeffs);
+                                cam.distortion_coeffs,
+                                anchorPatch_ideal);
 
 
   // save anchor position for later visualisaztion
-  anchor_center_pos = vec[4];
-  for(auto point : vec)
-  {
+  anchor_center_pos = anchorPatch_real[(N*N-1)/2];
+
+  // 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 : vec)
-    anchorPatch.push_back((double)anchorImage.at<uint8_t>((int)point.x,(int)point.y));
 
-  // project patch pixel to 3D space
-  for(auto point : und_v)
-    anchorPatch_3d.push_back(projectPixelToPosition(point, cam));
+  for(auto point : anchorPatch_real)
+    anchorPatch.push_back(PixelIrradiance(point, anchorImage));
+
+  // project patch pixel to 3D space in camera  coordinate system
+  for(auto point : anchorPatch_ideal)
+    anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam));
 
   is_anchored = true;
   return true;
 }
-
-bool Feature::initializePosition(
-    const CamStateServer& cam_states) {
+/*
+bool Feature::initializeRho(const CamStateServer& cam_states) {
 
   // Organize camera poses and feature observations properly.
   std::vector<Eigen::Isometry3d,
@@ -710,6 +1065,157 @@ bool Feature::initializePosition(
 
   return is_valid_solution;
 }
+*/
+bool Feature::initializePosition(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;
+}
+
 } // namespace msckf_vio
 
 #endif // MSCKF_VIO_FEATURE_H

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:
@@ -43,6 +50,50 @@ inline void quaternionNormalize(Eigen::Vector4d& q) {
   return;
 }
 
+/*
+ * @brief invert rotation of passed quaternion through conjugation
+ *        and return conjugation
+ */
+inline Eigen::Vector4d quaternionConjugate(Eigen::Vector4d& q)
+{
+  Eigen::Vector4d p;
+  p(0) = -q(0);
+  p(1) = -q(1);
+  p(2) = -q(2);
+  p(3) =  q(3);
+  quaternionNormalize(p);
+  return p;
+}
+
+/*
+ * @brief converts a vector4 to a vector3, dropping (3)
+ *        this is typically used to get the vector part of a quaternion
+          for eq small angle approximation
+*/
+inline Eigen::Vector3d v4tov3(const Eigen::Vector4d& q)
+{
+  Eigen::Vector3d p;
+  p(0) = q(0);
+  p(1) = q(1);
+  p(2) = q(2);
+  return p;
+}
+
+/*
+ * @brief Perform q1 * q2
+ */
+
+inline Eigen::Vector4d QtoV(const Eigen::Quaterniond& q)
+{
+  Eigen::Vector4d p;
+  p(0) = q.x();
+  p(1) = q.y();
+  p(2) = q.z();
+  p(3) = q.w();
+  return p;
+}
+
+
 /*
  * @brief Perform q1 * q2
  */

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
@@ -107,6 +109,15 @@ class MsckfVio {
      */
     void imuCallback(const sensor_msgs::ImuConstPtr& msg);
 
+    /*
+     * @brief truthCallback
+     *      Callback function for ground truth navigation information
+     * @param TransformStamped msg
+     */    
+    void truthCallback(
+        const geometry_msgs::TransformStampedPtr& msg);
+
+
     /*
      * @brief imageCallback
      *    Callback function for feature measurements
@@ -144,11 +155,26 @@ class MsckfVio {
     bool resetCallback(std_srvs::Trigger::Request& req,
         std_srvs::Trigger::Response& res);
 
+    // Saves the exposure time and the camera measurementes
     void manageMovingWindow(
         const sensor_msgs::ImageConstPtr& cam0_img,
         const sensor_msgs::ImageConstPtr& cam1_img,
         const CameraMeasurementConstPtr& feature_msg);
 
+
+    void calcErrorState();
+
+    // Debug related Functions
+    // Propagate the true state
+    void batchTruthProcessing(
+        const double& time_bound);
+
+
+    void processTruthtoIMU(const double& time,
+        const Eigen::Vector4d& m_rot,
+        const Eigen::Vector3d& m_trans);
+
+
     // Filter related functions
     // Propogate the state
     void batchImuProcessing(
@@ -160,8 +186,12 @@ class MsckfVio {
         const Eigen::Vector3d& gyro,
         const Eigen::Vector3d& acc);
 
+    // groundtruth state augmentation
+    void truePhotometricStateAugmentation(const double& time);
+
     // Measurement update
     void stateAugmentation(const double& time);
+    void PhotometricStateAugmentation(const double& time);
     void addFeatureObservations(const CameraMeasurementConstPtr& msg);
     // This function is used to compute the measurement Jacobian
     // for a single feature observed at a single camera frame.
@@ -180,9 +210,9 @@ class MsckfVio {
     void PhotometricMeasurementJacobian(
     const StateIDType& cam_state_id,
     const FeatureIDType& feature_id,
-    Eigen::Matrix<double, 4, 6>& H_x,
-    Eigen::Matrix<double, 4, 3>& H_f,
-    Eigen::Vector4d& r);
+    Eigen::MatrixXd& H_x,
+    Eigen::MatrixXd& H_y,
+    Eigen::VectorXd& r);
 
     void PhotometricFeatureJacobian(
     const FeatureIDType& feature_id,
@@ -200,11 +230,32 @@ class MsckfVio {
     // Reset the system online if the uncertainty is too large.
     void onlineReset();
 
+    // Photometry flag
+    bool PHOTOMETRIC;
+
+    // debug flag
+    bool PRINTIMAGES;
+    bool GROUNDTRUTH;
+
+    bool nan_flag;
+    bool play;
+    double last_time_bound;
+
+    // Patch size for Photometry
+    int N;
+
     // Chi squared test table.
     static std::map<int, double> chi_squared_test_table;
 
     // State vector
     StateServer state_server;
+
+    // Ground truth state vector 
+    StateServer true_state_server;
+
+    // error state based on ground truth
+    StateServer err_state_server;
+
     // Maximum number of camera states
     int max_cam_state_size;
 
@@ -216,6 +267,8 @@ class MsckfVio {
     // transfer delay between IMU and Image messages.
     std::vector<sensor_msgs::Imu> imu_msg_buffer;
 
+    // Ground Truth message data
+    std::vector<geometry_msgs::TransformStamped> truth_msg_buffer;
     // Moving Window buffer
     movingWindow cam0_moving_window;
     movingWindow cam1_moving_window;
@@ -224,6 +277,8 @@ class MsckfVio {
     CameraCalibration cam0;
     CameraCalibration cam1;
 
+    // covariance data
+    double irradiance_frame_bias;
 
 
     ros::Time image_save_time;
@@ -255,7 +310,9 @@ class MsckfVio {
 
     // Subscribers and publishers
     ros::Subscriber imu_sub;
+    ros::Subscriber truth_sub;
     ros::Publisher odom_pub;
+    ros::Publisher marker_pub;
     ros::Publisher feature_pub;
     tf::TransformBroadcaster tf_pub;
     ros::ServiceServer reset_srv;
@@ -287,6 +344,9 @@ class MsckfVio {
     ros::Publisher mocap_odom_pub;
     geometry_msgs::TransformStamped raw_mocap_odom_msg;
     Eigen::Isometry3d mocap_initial_frame;
+
+    Eigen::Vector4d mocap_initial_orientation;
+    Eigen::Vector3d mocap_initial_position;
 };
 
 typedef MsckfVio::Ptr MsckfVioPtr;

launch/msckf_vio_debug_tum.launch

@@ -0,0 +1,75 @@
+<launch>
+
+  <arg name="robot" default="firefly_sbx"/>
+  <arg name="fixed_frame_id" default="world"/>
+  <arg name="calibration_file"
+    default="$(find msckf_vio)/config/camchain-imucam-tum.yaml"/>
+
+  <!-- Image Processor Nodelet  -->
+  <include file="$(find msckf_vio)/launch/image_processor_tum.launch">
+    <arg name="robot" value="$(arg robot)"/>
+    <arg name="calibration_file" value="$(arg calibration_file)"/>
+  </include>
+
+  <!-- Msckf Vio Nodelet  -->
+  <group ns="$(arg robot)">
+    <node pkg="nodelet" type="nodelet" name="vio"
+      args='standalone msckf_vio/MsckfVioNodelet'
+      output="screen"
+      launch-prefix="xterm -e gdb --args">
+
+      <!-- Photometry Flag-->
+      <param name="PHOTOMETRIC" value="true"/>
+
+      <!-- Debugging Flaggs -->
+      <param name="PrintImages" value="false"/>
+      <param name="GroundTruth" value="false"/>
+
+      <param name="patch_size_n" value="7"/>
+
+      <!-- Calibration parameters -->
+      <rosparam command="load" file="$(arg calibration_file)"/>
+
+      <param name="publish_tf" value="true"/>
+      <param name="frame_rate" value="20"/>
+      <param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
+      <param name="child_frame_id" value="odom"/>
+      <param name="max_cam_state_size" value="20"/>
+      <param name="position_std_threshold" value="8.0"/>
+
+      <param name="rotation_threshold" value="0.2618"/>
+      <param name="translation_threshold" value="0.4"/>
+      <param name="tracking_rate_threshold" value="0.5"/>
+
+      <!-- Feature optimization config -->
+      <param name="feature/config/translation_threshold" value="-1.0"/>
+
+      <!-- These values should be standard deviation -->
+      <param name="noise/gyro" value="0.005"/>
+      <param name="noise/acc" value="0.05"/>
+      <param name="noise/gyro_bias" value="0.001"/>
+      <param name="noise/acc_bias" value="0.01"/>
+      <param name="noise/feature" value="0.035"/>
+
+      <param name="initial_state/velocity/x" value="0.0"/>
+      <param name="initial_state/velocity/y" value="0.0"/>
+      <param name="initial_state/velocity/z" value="0.0"/>
+
+      <!-- These values should be covariance -->
+      <param name="initial_covariance/velocity" value="0.25"/>
+      <param name="initial_covariance/gyro_bias" value="0.01"/>
+      <param name="initial_covariance/acc_bias" value="0.01"/>
+      <param name="initial_covariance/extrinsic_rotation_cov" value="3.0462e-4"/>
+      <param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>
+      <param name="initial_covariance/irradiance_frame_bias" value="0.1"/>
+
+      <remap from="~imu" to="/imu0"/>
+      <remap from="~cam0_image" to="/cam0/image_raw"/>
+      <remap from="~cam1_image" to="/cam1/image_raw"/>
+
+      <remap from="~features" to="image_processor/features"/>
+
+    </node>
+  </group>
+
+</launch>

launch/msckf_vio_tum.launch

@@ -17,6 +17,14 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       output="screen">
 
+      <!-- Photometry Flag-->
+      <param name="PHOTOMETRIC" value="true"/>
+
+      <!-- Debugging Flaggs -->
+      <param name="PrintImages" value="true"/>
+      <param name="GroundTruth" value="false"/>
+
+      <param name="patch_size_n" value="7"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 
@@ -51,8 +59,10 @@
       <param name="initial_covariance/acc_bias" value="0.01"/>
       <param name="initial_covariance/extrinsic_rotation_cov" value="3.0462e-4"/>
       <param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>
+      <param name="initial_covariance/irradiance_frame_bias" value="0.1"/>
 
       <remap from="~imu" to="/imu0"/>
+      <remap from="~ground_truth" to="/vrpn_client/raw_transform"/>
       <remap from="~cam0_image" to="/cam0/image_raw"/>
       <remap from="~cam1_image" to="/cam1/image_raw"/>
 

package.xml

@@ -18,6 +18,7 @@
   <depend>nav_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
   <depend>eigen_conversions</depend>
   <depend>tf_conversions</depend>
   <depend>random_numbers</depend>

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,