changed some parameters

include/msckf_vio/feature.hpp

@@ -935,7 +935,7 @@ bool Feature::VisualizePatch(
   //cv::imwrite(loc.str(), cam0.featureVisu);
 
   cv::imshow("patch", cam0.featureVisu);
-  cvWaitKey(1);
+  cvWaitKey(0);
 }
 
 float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const

launch/image_processor_tinytum.launch

@@ -14,8 +14,8 @@
       <rosparam command="load" file="$(arg calibration_file)"/>
       <param name="grid_row" value="4"/>
       <param name="grid_col" value="4"/>
-      <param name="grid_min_feature_num" value="5"/>
+      <param name="grid_min_feature_num" value="3"/>
-      <param name="grid_max_feature_num" value="10"/>
+      <param name="grid_max_feature_num" value="4"/>
       <param name="pyramid_levels" value="3"/>
       <param name="patch_size" value="15"/>
       <param name="fast_threshold" value="10"/>
@@ -24,9 +24,9 @@
       <param name="ransac_threshold" value="3"/>
       <param name="stereo_threshold" value="5"/>
 
-      <remap from="~imu" to="/imu0"/>
+      <remap from="~imu" to="/imu02"/>
-      <remap from="~cam0_image" to="/cam0/image_raw"/>
+      <remap from="~cam0_image" to="/cam0/image_raw2"/>
-      <remap from="~cam1_image" to="/cam1/image_raw"/>
+      <remap from="~cam1_image" to="/cam1/image_raw2"/>
 
     </node>
   </group>

launch/msckf_vio_tinytum.launch

@@ -22,10 +22,10 @@
 
       <!-- Debugging Flaggs -->
       <param name="StreamPause" value="true"/>
-      <param name="PrintImages" value="false"/>
+      <param name="PrintImages" value="true"/>
       <param name="GroundTruth" value="false"/>
 
-      <param name="patch_size_n" value="3"/>
+      <param name="patch_size_n" value="5"/>
       <!-- Calibration parameters -->
       <rosparam command="load" file="$(arg calibration_file)"/>
 
@@ -33,7 +33,7 @@
       <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="12"/>
+      <param name="max_cam_state_size" value="20"/>
       <param name="position_std_threshold" value="8.0"/>
 
       <param name="rotation_threshold" value="0.2618"/>

src/msckf_vio.cpp

@@ -50,7 +50,7 @@ Isometry3d CAMState::T_cam0_cam1 = Isometry3d::Identity();
 
 // Static member variables in Feature class.
 FeatureIDType Feature::next_id = 0;
-double Feature::observation_noise = 0.05;
+double Feature::observation_noise = 0.2;
 Feature::OptimizationConfig Feature::optimization_config;
 
 map<int, double> MsckfVio::chi_squared_test_table;
@@ -1623,7 +1623,7 @@ bool MsckfVio::PhotometricPatchPointResidual(
     new_anchorPatch_3d = feature.anchorPatch_3d;
   }
 
-  for(auto point : new_anchorPatch_3d)
+  for(auto point : feature.anchorPatch_3d)
   {
 
     cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
@@ -1869,6 +1869,7 @@ bool MsckfVio::PhotometricMeasurementJacobian(
     // visualizing functions
     feature.MarkerGeneration(marker_pub, state_server.cam_states);
     feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss);
+    cout << "r\n" << r_photo << endl;
     //feature.VisualizeKernel(cam_state, cam_state_id, cam0);
   }
 
@@ -2782,7 +2783,6 @@ void MsckfVio::removeLostFeatures() {
     MatrixXd p2H_xj;
     VectorXd p2r_j;
 
-    cout << "5: " << endl;
     if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j, N))
     {
         if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) {
@@ -2792,7 +2792,7 @@ void MsckfVio::removeLostFeatures() {
       }
     }
     
-    
+    /*
     cout << "3: " << endl;
     if(PhotometricFeatureJacobian(feature.id, cam_state_ids, p2H_xj, p2r_j, 3))
     {
@@ -2803,7 +2803,7 @@ void MsckfVio::removeLostFeatures() {
         p2stack_cntr += p2H_xj.rows();
       }
     }    
-    
+    */
     featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
     twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
 
@@ -2981,7 +2981,6 @@ void MsckfVio::pruneLastCamStateBuffer()
     for (const auto& cam_state : state_server.cam_states)
       involved_cam_state_ids.push_back(cam_state.first);
 
-        cout << "5: " << endl;
     if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j, N))
     {
         if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) {
@@ -2991,6 +2990,7 @@ void MsckfVio::pruneLastCamStateBuffer()
       }
     }
 
+    /*
     cout << "3: " << endl;
     if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3))
     {
@@ -3000,6 +3000,7 @@ void MsckfVio::pruneLastCamStateBuffer()
           p2stack_cntr += p2H_xj.rows();
         }
     }
+    */
     featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
     twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
 
@@ -3179,7 +3180,6 @@ void MsckfVio::pruneCamStateBuffer() {
 
     if (involved_cam_state_ids.size() == 0) continue;
 
-    cout << "5: " << endl;
     if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j, N))
     { 
         if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) {// involved_cam_state_ids.size())) {
@@ -3189,6 +3189,7 @@ void MsckfVio::pruneCamStateBuffer() {
       }
     }
 
+    /*
     cout << "3: " << endl;
     if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3))
     {
@@ -3198,7 +3199,7 @@ void MsckfVio::pruneCamStateBuffer() {
         p2stack_cntr += p2H_xj.rows();
       }    
     }
-    
+    */
 
     featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
     twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);