added live toggle for photometric

include/msckf_vio/feature.hpp

@@ -1008,8 +1008,6 @@ Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCal
 bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
 {
 
-
-
   //initialize patch Size
   int n = (int)(N-1)/2;
 

launch/msckf_vio_euroc.launch

@@ -17,6 +17,16 @@
       args='standalone msckf_vio/MsckfVioNodelet'
       output="screen">
 
+      <!-- Photometry Flag-->
+      <param name="PHOTOMETRIC" value="false"/>
+
+      <!-- Debugging Flaggs -->
+      <param name="StreamPause" value="false"/>
+      <param name="PrintImages" value="false"/>
+      <param name="GroundTruth" value="false"/>
+
+      <param name="patch_size_n" value="5"/>
+
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 

launch/msckf_vio_tinytum.launch

@@ -6,7 +6,7 @@
     default="$(find msckf_vio)/config/camchain-imucam-tum-scaled.yaml"/>
 
   <!-- Image Processor Nodelet  -->
-  <include file="$(find msckf_vio)/launch/image_processor_tum.launch">
+  <include file="$(find msckf_vio)/launch/image_processor_tinytum.launch">
     <arg name="robot" value="$(arg robot)"/>
     <arg name="calibration_file" value="$(arg calibration_file)"/>
   </include>
@@ -18,14 +18,14 @@
       output="screen">
 
       <!-- Photometry Flag-->
-      <param name="PHOTOMETRIC" value="true"/>
+      <param name="PHOTOMETRIC" value="false"/>
 
       <!-- Debugging Flaggs -->
-      <param name="StreamPause" value="true"/>
+      <param name="StreamPause" value="false"/>
-      <param name="PrintImages" value="true"/>
+      <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="5"/>
+      <param name="patch_size_n" value="7"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 
@@ -64,16 +64,13 @@
 
       <remap from="~imu" to="/imu0"/>
       <remap from="~ground_truth" to="/vrpn_client/raw_transform"/>
-      <remap from="~cam0_image" to="/cam0/new_image_raw"/>
+      <remap from="~cam0_image" to="/cam0/image_raw"/>
-      <remap from="~cam1_image" to="/cam1/new_image_raw"/>
+      <remap from="~cam1_image" to="/cam1/image_raw"/>
 
       <remap from="~features" to="image_processor/features"/>
 
     </node>
   </group>
 
-  <node name="scaler" pkg="msckf_vio" type="shrinkImage.py"/>
-  <node name="player" pkg="bagcontrol" type="control.py" />
-
 
 </launch>

launch/msckf_vio_tum.launch

@@ -18,10 +18,10 @@
       output="screen">
 
       <!-- Photometry Flag-->
-      <param name="PHOTOMETRIC" value="false"/>
+      <param name="PHOTOMETRIC" value="true"/>
 
       <!-- Debugging Flaggs -->
-      <param name="StreamPause" value="true"/>
+      <param name="StreamPause" value="false"/>
       <param name="PrintImages" value="false"/>
       <param name="GroundTruth" value="false"/>
 

src/msckf_vio.cpp

@@ -412,6 +412,8 @@ void MsckfVio::imageCallback(
   if(STREAMPAUSE)
     nh.setParam("/play_bag", false);
   
+
+  nh.param<bool>("PHOTOMETRIC", PHOTOMETRIC, false);
   // Start the system if the first image is received.
   // The frame where the first image is received will be
   // the origin.
@@ -448,7 +450,7 @@ void MsckfVio::imageCallback(
   double imu_processing_time = (
       ros::Time::now()-start_time).toSec();
 
-  //cout << "1" << endl;
+  // cout << "1" << endl;
   // Augment the state vector.
   start_time = ros::Time::now();
   //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
@@ -457,7 +459,7 @@ void MsckfVio::imageCallback(
       ros::Time::now()-start_time).toSec();
 
 
-  //cout << "2" << endl;
+  // cout << "2" << endl;
   // Add new observations for existing features or new
   // features in the map server.
   start_time = ros::Time::now();
@@ -466,7 +468,7 @@ void MsckfVio::imageCallback(
       ros::Time::now()-start_time).toSec();
 
 
-  //cout << "3" << endl;
+  // cout << "3" << endl;
   // Add new images to moving window
   start_time = ros::Time::now();
   manageMovingWindow(cam0_img, cam1_img, feature_msg);
@@ -474,20 +476,20 @@ void MsckfVio::imageCallback(
       ros::Time::now()-start_time).toSec();
 
 
-  //cout << "4" << endl;
+  // cout << "4" << endl;
   // Perform measurement update if necessary.
   start_time = ros::Time::now();
   removeLostFeatures();
   double remove_lost_features_time = (
       ros::Time::now()-start_time).toSec();
 
-  //cout << "5" << endl;
+  // cout << "5" << endl;
   start_time = ros::Time::now();
-  pruneLastCamStateBuffer();
+  pruneCamStateBuffer();
   double prune_cam_states_time = (
       ros::Time::now()-start_time).toSec();
 
-  //cout << "6" << endl;
+  // cout << "6" << endl;
   // Publish the odometry.
   start_time = ros::Time::now();
   publish(feature_msg->header.stamp);
@@ -1368,7 +1370,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
   }
 
   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);
+  //MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1);
+  
+  MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1);
 
   // set anchor Jakobi
     // get position of anchor in cam states
@@ -1389,12 +1393,16 @@ void MsckfVio::PhotometricMeasurementJacobian(
   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);
+  //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_yl.block(0, 0, N*N, N*N) = Eigen::MatrixXd::Identity(N*N, N*N);
+  H_yl.block(0, N*N, N*N, 1) = -H_rho;
   
   H_x = H_xl;
   H_y = H_yl;
@@ -1439,7 +1447,10 @@ void MsckfVio::PhotometricFeatureJacobian(
 
   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, N*N+state_server.cam_states.size()+1);
+  MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, N*N+1);
+  
   VectorXd r_i = VectorXd::Zero(jacobian_row_size);
   int stack_cntr = 0;
 
@@ -1460,6 +1471,8 @@ void MsckfVio::PhotometricFeatureJacobian(
     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;
+
+
   }
 
   // Project the residual and Jacobians onto the nullspace
@@ -1799,7 +1812,9 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
 void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
 
   if (H.rows() == 0 || r.rows() == 0)
+  {
     return;
+  }
   // Decompose the final Jacobian matrix to reduce computational
   // complexity as in Equation (28), (29).
   MatrixXd H_thin;
@@ -1840,11 +1855,14 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
   // Compute the error of the state.
 
   VectorXd delta_x = K * r;
-  // cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
-  // cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
   // Update the IMU state.
+
+  
   if (not PHOTOMETRIC) return;
   
+  cout << "msc veloci: " << delta_x[6] << ", " << delta_x[7] << ", " << delta_x[8] << endl;
+  cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
+
   const VectorXd& delta_x_imu = delta_x.head<21>();
 
   if (//delta_x_imu.segment<3>(0).norm() > 0.15 ||
@@ -1900,6 +1918,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
 
 
 bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
+  return 1;
   MatrixXd P1 = H * state_server.state_cov * H.transpose();
 
 
@@ -1908,8 +1927,8 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
 
   double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
 
-    //cout << "gate: " << dof << " " << gamma << " " <<
+    cout << "gate: " << dof << " " << gamma << " " <<
-    //chi_squared_test_table[dof] << endl;
+    chi_squared_test_table[dof] << endl;
 
     if (chi_squared_test_table[dof] == 0)
       return false;
@@ -2008,12 +2027,12 @@ void MsckfVio::removeLostFeatures() {
 
     PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j);
     featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
-    
     if (gatingTest(H_xj, r_j, r_j.size())) { //, 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();
     }  
+    
     if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
       pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
       pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
@@ -2180,7 +2199,6 @@ void MsckfVio::pruneLastCamStateBuffer()
       feature.observations.erase(cam_id);
   }
 
-
   H_x.conservativeResize(stack_cntr, H_x.cols());
   r.conservativeResize(stack_cntr);
   
@@ -2188,7 +2206,6 @@ void MsckfVio::pruneLastCamStateBuffer()
   pr.conservativeResize(pstack_cntr);
   // Perform measurement update.
 
-
   measurementUpdate(H_x, r);
   photometricMeasurementUpdate(pH_x, pr);