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

fixed timedelay for publishing structure when bigger time delays than queue allows for

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-27 17:54:16 +02:00
parent af1820a238
commit bfb2a551a7
3 changed files with 19 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/image_processor.cpp
View File

@ -219,18 +219,18 @@ void ImageProcessor::stereoCallback(
cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img,
sensor_msgs::image_encodings::MONO8);
ros::Time start_time = ros::Time::now();
image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
ROS_INFO("Publishing: %f",
(ros::Time::now()-start_time).toSec());
// Build the image pyramids once since they're used at multiple places
createImagePyramids();
// Detect features in the first frame.
if (is_first_img) {
ros::Time start_time = ros::Time::now();
start_time = ros::Time::now();
initializeFirstFrame();
//ROS_INFO("Detection time: %f",
// (ros::Time::now()-start_time).toSec());
@ -243,7 +243,7 @@ void ImageProcessor::stereoCallback(
// (ros::Time::now()-start_time).toSec());
} else {
// Track the feature in the previous image.
ros::Time start_time = ros::Time::now();
start_time = ros::Time::now();
trackFeatures();
//ROS_INFO("Tracking time: %f",
// (ros::Time::now()-start_time).toSec());
@ -273,7 +273,7 @@ void ImageProcessor::stereoCallback(
// (ros::Time::now()-start_time).toSec());
// Publish features in the current image.
ros::Time start_time = ros::Time::now();
start_time = ros::Time::now();
publish();
//ROS_INFO("Publishing: %f",
// (ros::Time::now()-start_time).toSec());

15
src/msckf_vio.cpp
View File

@ -405,12 +405,19 @@ void MsckfVio::imageCallback(
const sensor_msgs::ImageConstPtr& cam1_img,
const CameraMeasurementConstPtr& feature_msg)
{
// Return if the gravity vector has not been set.
if (!is_gravity_set) return;
// stop playing bagfile if printing images
cout << "hello" << endl;
// stop playing bagfile if printing images
if(STREAMPAUSE)
nh.setParam("/play_bag", false);
// Return if the gravity vector has not been set.
if (!is_gravity_set)
{
if(STREAMPAUSE)
nh.setParam("/play_bag", true);
return;
}
// Start the system if the first image is received.
// The frame where the first image is received will be