From ecab936c627a84678cc5a494dc2aa5041132998a Mon Sep 17 00:00:00 2001
From: g-spacewhale <raphael@maenle.net>
Date: Wed, 19 Jun 2019 09:59:22 +0200
Subject: [PATCH] minor changes in launch file and edited augmentationsize

---
 launch/msckf_vio_tum.launch |  4 ++--
 src/msckf_vio.cpp           | 26 +++++++++-----------------
 2 files changed, 11 insertions(+), 19 deletions(-)

diff --git a/launch/msckf_vio_tum.launch b/launch/msckf_vio_tum.launch
index 54e6c7e..ebf0946 100644
--- a/launch/msckf_vio_tum.launch
+++ b/launch/msckf_vio_tum.launch
@@ -21,7 +21,7 @@
       <param name="PHOTOMETRIC" value="false"/>
 
       <!-- Debugging Flaggs -->
-      <param name="StreamPause" value="true"/>
+      <param name="StreamPause" value="false"/>
       <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
@@ -72,6 +72,6 @@
     </node>
   </group>
 
-  <node name="player" pkg="bagcontrol" type="control.py" />
+  <!--node name="player" pkg="bagcontrol" type="control.py" /-->
 
 </launch>
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index a6fe1f9..02b2879 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -451,13 +451,9 @@ void MsckfVio::imageCallback(
   //cout << "1" << endl;
   // Augment the state vector.
   start_time = ros::Time::now();
-  if(PHOTOMETRIC)
-  {
-    truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
-    PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
-  }
-  else
-    stateAugmentation(feature_msg->header.stamp.toSec());
+  //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
+  PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
+
   double state_augmentation_time = (
       ros::Time::now()-start_time).toSec();
 
@@ -1706,9 +1702,7 @@ 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;
+  int augmentationSize = 7;
 
   // QR decomposition to make stuff faster
   if (H.rows() > H.cols()) {
@@ -1779,11 +1773,13 @@ 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(21+i*augmentationSize, augmentationSize);
+    const VectorXd& delta_x_cam = delta_x.segment(21+i*augmentationSize, 6);
     const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>());
     cam_state_iter->second.orientation = quaternionMultiplication(
         dq_cam, cam_state_iter->second.orientation);
     cam_state_iter->second.position += delta_x_cam.tail<3>();
+    if(PHOTOMETRIC)
+      cam_state_iter->second.illumination.frame_bias += delta_x(21+i*augmentationSize+6);
   }
 
   // Update state covariance.
@@ -1827,9 +1823,7 @@ void MsckfVio::removeLostFeatures() {
   // Remove the features that lost track.
   // BTW, find the size the final Jacobian matrix and residual vector.
   int jacobian_row_size = 0;
-  int augmentationSize = 6;
-  if(PHOTOMETRIC)
-    augmentationSize = 7;
+  int augmentationSize = 7;
 
 
   vector<FeatureIDType> invalid_feature_ids(0);
@@ -2133,9 +2127,7 @@ void MsckfVio::pruneCamStateBuffer() {
 
   // Find the size of the Jacobian matrix.
   int jacobian_row_size = 0;
-    int augmentationSize = 6;
-    if(PHOTOMETRIC)
-      augmentationSize = 7;
+    int augmentationSize = 7;
 
   for (auto& item : map_server) {
     auto& feature = item.second;