raphael/msckf_vio
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

upated two calculation, still not working

Browse Source
This commit is contained in:
Raphael Maenle 2019-06-28 10:14:32 +02:00
parent 715ca6a6b4
commit 3ae7bdb13a
2 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
launch/msckf_vio_tum.launch
View File

@ -17,8 +17,8 @@
args='standalone msckf_vio/MsckfVioNodelet' args='standalone msckf_vio/MsckfVioNodelet'
output="screen"> output="screen">
<!-- Photometry Flag--> <!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
<param name="PHOTOMETRIC" value="false"/> <param name="FILTER" value="2"/>
<!-- Debugging Flaggs --> <!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/> <param name="StreamPause" value="true"/>

5
src/msckf_vio.cpp
View File

@ -405,7 +405,7 @@ void MsckfVio::imageCallback(
const sensor_msgs::ImageConstPtr& cam1_img, const sensor_msgs::ImageConstPtr& cam1_img,
const CameraMeasurementConstPtr& feature_msg) const CameraMeasurementConstPtr& feature_msg)
{ {
// stop playing bagfile if printing images // stop playing bagfile if printing images
if(STREAMPAUSE) if(STREAMPAUSE)
nh.setParam("/play_bag", false); nh.setParam("/play_bag", false);
// Return if the gravity vector has not been set. // Return if the gravity vector has not been set.
@ -1454,6 +1454,7 @@ void MsckfVio::twodotFeatureJacobian(
std::cout << "resume playback" << std::endl; std::cout << "resume playback" << std::endl;
nh.setParam("/play_bag", true); nh.setParam("/play_bag", true);
} }
return; return;
} }
@ -2341,7 +2342,9 @@ void MsckfVio::removeLostFeatures() {
cout << "measuring" << endl; cout << "measuring" << endl;
PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j); PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j);
cout << "norm" << endl;
featureJacobian(feature.id, cam_state_ids, H_xj, r_j); featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
cout << "two" << endl;
twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j); twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
cout << "gating" << endl; cout << "gating" << endl;