From cab56d94949cb94d6b2b0baa362427a7ca7ebb94 Mon Sep 17 00:00:00 2001
From: g-spacewhale <raphael@maenle.net>
Date: Wed, 22 May 2019 10:57:31 +0200
Subject: [PATCH] tested bug: filter converges longer without distortion
 effects; reason probably general formulation inconsistencies

---
 include/msckf_vio/feature.hpp | 27 ++++++++-------------------
 src/msckf_vio.cpp             | 14 ++++++--------
 2 files changed, 14 insertions(+), 27 deletions(-)

diff --git a/include/msckf_vio/feature.hpp b/include/msckf_vio/feature.hpp
index 8a6cc27..62a0228 100644
--- a/include/msckf_vio/feature.hpp
+++ b/include/msckf_vio/feature.hpp
@@ -742,27 +742,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 +762,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);
+  for(auto point : anchorPatch_ideal)
     anchorPatch_3d.push_back(projectPixelToPosition(point, cam));
-  }
+
   is_anchored = true;
   return true;
 }
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index ed4ebbc..d8c93e8 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -1232,12 +1232,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
   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;
 
@@ -1316,16 +1310,21 @@ void MsckfVio::PhotometricMeasurementJacobian(
   
   //calculate residual
 
+  //project pixel point to 'pure' position
   cv::Point2f und_p_in_c0;
   image_handler::undistortPoint(p_in_c0,
                                 cam0.intrinsics,
                                 cam0.distortion_model,
-                                0,
+                                cam0.distortion_coeffs,
                                 und_p_in_c0);
 
   r_i[0] = z[0] - und_p_in_c0.x;
   r_i[1] = z[1] - und_p_in_c0.y;
 
+  //cout << "comp\n" << p_c0(0)/p_c0(2) << ":" << und_p_in_c0.x << endl;
+  //cout << p_c0(1)/p_c0(2) << ":" << und_p_in_c0.y << endl;
+
+
   MatrixXd H_xl = MatrixXd::Zero(2, 21+state_server.cam_states.size()*7);
  
   // set anchor Jakobi
@@ -1346,7 +1345,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
   H_x = H_xl;
   H_y = H_rho;
   r = r_i;
-   cout << "h for patch done" << endl;
 
   //TODO make this more fluent as well
   if(PRINTIMAGES)