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

fixed stop - go functionality via bagcontrol

Browse Source
This commit is contained in:
Raphael Maenle 2019-06-28 09:40:59 +02:00
parent 0712a98c7f
commit 2dac361ba2
2 changed files with 24 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
launch/msckf_vio_tum.launch
View File

@ -21,7 +21,7 @@
<param name="PHOTOMETRIC" value="true"/> <param name="PHOTOMETRIC" value="true"/>
<!-- Debugging Flaggs --> <!-- Debugging Flaggs -->
<param name="PrintImages" value="false"/> <param name="PrintImages" value="true"/>
<param name="GroundTruth" value="false"/> <param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="1"/> <param name="patch_size_n" value="1"/>

33
src/msckf_vio.cpp
View File

@ -404,8 +404,18 @@ void MsckfVio::imageCallback(
const sensor_msgs::ImageConstPtr& cam1_img, const sensor_msgs::ImageConstPtr& cam1_img,
const CameraMeasurementConstPtr& feature_msg) const CameraMeasurementConstPtr& feature_msg)
{ {
if(PRINTIMAGES)
{
std::cout << "stopped playpack" << std::endl;
nh.setParam("/play_bag", false);
}
// Return if the gravity vector has not been set. // Return if the gravity vector has not been set.
if (!is_gravity_set) return; if (!is_gravity_set)
{
nh.setParam("/play_bag", true);
return;
}
// Start the system if the first image is received. // Start the system if the first image is received.
// The frame where the first image is received will be // The frame where the first image is received will be
@ -512,6 +522,13 @@ void MsckfVio::imageCallback(
publish_time, publish_time/processing_time); publish_time, publish_time/processing_time);
} }
if(PRINTIMAGES)
{
std::cout << "stopped playpack" << std::endl;
nh.setParam("/play_bag", true);
}
return; return;
} }
@ -1334,9 +1351,9 @@ void MsckfVio::PhotometricMeasurementJacobian(
//TODO make this more fluent as well //TODO make this more fluent as well
if(PRINTIMAGES) if(PRINTIMAGES)
{ {
std::stringstream ss; //std::stringstream ss;
ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr; //ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr;
feature.MarkerGeneration(marker_pub, state_server.cam_states); //feature.MarkerGeneration(marker_pub, state_server.cam_states);
//feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss); //feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss);
} }
@ -1350,11 +1367,6 @@ void MsckfVio::PhotometricFeatureJacobian(
{ {
// stop playing bagfile if printing images // stop playing bagfile if printing images
if(PRINTIMAGES)
{
std::cout << "stopped playpack" << std::endl;
nh.setParam("/play_bag", false);
}
const auto& feature = map_server[feature_id]; const auto& feature = map_server[feature_id];
@ -1408,6 +1420,7 @@ void MsckfVio::PhotometricFeatureJacobian(
H_x = A_null_space.transpose() * H_xi; H_x = A_null_space.transpose() * H_xi;
r = A_null_space.transpose() * r_i; r = A_null_space.transpose() * r_i;
/*
if(PRINTIMAGES) if(PRINTIMAGES)
{ {
@ -1436,7 +1449,7 @@ void MsckfVio::PhotometricFeatureJacobian(
myfile.close(); myfile.close();
std::cout << "resume playback" << std::endl; std::cout << "resume playback" << std::endl;
nh.setParam("/play_bag", true); nh.setParam("/play_bag", true);
} }*/
return; return;
} }