minor output changes, added arrows for gradient and residual visualization

2019-05-21 13:34:58 +02:00
parent 9c7f67d2fd
commit 0f96c916f1
4 changed files with 151 additions and 80 deletions
--- a/include/msckf_vio/feature.hpp
+++ b/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>
@@ -182,12 +182,14 @@ bool MarkerGeneration(
                  const StateIDType& cam_state_id,
                  CameraCalibration& cam0,
                  const std::vector<double> photo_r,
-                  std::stringstream& ss) const;
+                  std::stringstream& ss,
+                  cv::Point2f gradientVector,
+                  cv::Point2f residualVector) const;
  /*
-  * @brief projectPixelToPosition uses the calcualted pixels
+  * @brief AnchorPixelToPosition uses the calcualted pixels
  *     of the anchor patch to generate 3D positions of all of em
  */
-inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
+inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p,
          const CameraCalibration& cam);

  /*
@@ -533,7 +535,9 @@ bool Feature::VisualizePatch(
                  const StateIDType& cam_state_id,
                  CameraCalibration& cam0,
                  const std::vector<double> photo_r,
-                  std::stringstream& ss) const
+                  std::stringstream& ss,
+                  cv::Point2f gradientVector,
+                  cv::Point2f residualVector) const
 {

  double rescale = 1;
@@ -573,45 +577,107 @@ 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
@@ -703,7 +769,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 +808,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,19 +828,16 @@ 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;
 }
--- a/include/msckf_vio/msckf_vio.h
+++ b/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
--- a/launch/msckf_vio_tum.launch
+++ b/launch/msckf_vio_tum.launch
@@ -24,7 +24,7 @@
      <param name="PrintImages" value="true"/>
      <param name="GroundTruth" value="false"/>

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

--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -1238,6 +1238,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
  //photometric observation
  std::vector<double> photo_z;

+  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();
@@ -1262,9 +1264,31 @@ void MsckfVio::PhotometricMeasurementJacobian(

  auto frame = cam0.moving_window.find(cam_state_id)->second.image;

+  //observation
+  const Vector4d& z = feature.observations.find(cam_state_id)->second;
+
+  //estimate photometric measurement
+  std::vector<double> estimate_irradiance;
+  std::vector<double> estimate_photo_z;
+  IlluminationParameter estimated_illumination;
+  feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
+  
+  // calculated here, because we need true 'estimate_irradiance' later for jacobi
+  for (auto& estimate_irradiance_j : estimate_irradiance)
+            estimate_photo_z.push_back (estimate_irradiance_j * 
+                    estimated_illumination.frame_gain * estimated_illumination.feature_gain +
+                    estimated_illumination.frame_bias + estimated_illumination.feature_bias);
+
  int count = 0;
  double dx, dy;

+  // gradient visualization parameters
+  cv::Point2f gradientVector(0,0);
+
+  // residual change visualization
+  cv::Point2f residualVector(0,0);
+  double res_sum = 0;
+
  for (auto point : feature.anchorPatch_3d)
  {
    Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
@@ -1273,14 +1297,24 @@ void MsckfVio::PhotometricMeasurementJacobian(
    //add observation
    photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));

-    // add jacobian
+      //calculate photom. residual
+    photo_r.push_back(photo_z[count] - estimate_photo_z[count]);
+
+    // add jacobians

    // 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;
+
+    gradientVector.x += dx;
+    gradientVector.y += dy; 
    
+    residualVector.x += dx * photo_r[count];
+    residualVector.y += dy * photo_r[count];
+    res_sum += photo_r[count];
+
    //dh / d{}^Cp_{ij}
    dh_dCpij(0, 0) = 1 / p_c0(2);
    dh_dCpij(1, 1) = 1 / p_c0(2);
@@ -1320,28 +1354,6 @@ void MsckfVio::PhotometricMeasurementJacobian(

    count++;
  }
-  // calculate residual
-
-  //observation
-  const Vector4d& z = feature.observations.find(cam_state_id)->second;
-
-  //estimate photometric measurement
-  std::vector<double> estimate_irradiance;
-  std::vector<double> estimate_photo_z;
-  IlluminationParameter estimated_illumination;
-  feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
-  
-  // calculated here, because we need true 'estimate_irradiance' later for jacobi
-  for (auto& estimate_irradiance_j : estimate_irradiance)
-            estimate_photo_z.push_back (estimate_irradiance_j * 
-                    estimated_illumination.frame_gain * estimated_illumination.feature_gain +
-                    estimated_illumination.frame_bias + estimated_illumination.feature_bias);
-
-  std::vector<double> photo_r;
-  
-  //calculate photom. residual
-  for(int i = 0; i < photo_z.size(); i++)
-    photo_r.push_back(photo_z[i] - estimate_photo_z[i]);

  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);
@@ -1379,12 +1391,13 @@ void MsckfVio::PhotometricMeasurementJacobian(
  count = 0; 
  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);
+    feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss, gradientVector, residualVector);
  }

  return;
@@ -1481,13 +1494,11 @@ void MsckfVio::PhotometricFeatureJacobian(
  H_x = A_null_space.transpose() * H_xi;
  r = A_null_space.transpose() * r_i;

-  
  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 << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x. * r << endl;
  myfile.close();
  cout << "---------- LOGGED -------- " << endl; 
-  

  if(PRINTIMAGES)
  {
@@ -1631,13 +1642,11 @@ void MsckfVio::featureJacobian(
  
  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 << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.ldlt().solve(r) << endl;
  myfile.close();
  cout << "---------- LOGGED -------- " << endl; 
-  
-  nh.setParam("/play_bag", false);
-
  return;
+
 }

 void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
@@ -1648,7 +1657,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();
@@ -1663,8 +1676,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();
@@ -1676,18 +1689,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>();
@@ -1722,7 +1736,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);
@@ -1730,7 +1744,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;
@@ -1744,7 +1758,7 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
 }

 bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
-
+  return true;
  MatrixXd P1 = H * state_server.state_cov * H.transpose();