非ROS程序
通过循环函数,顺序读入图像和imu数据。
for (seq = 0; seq<num_seq; seq++)
{
// Seq loop
vector<ORB_SLAM3::IMU::Point> vImuMeas;
double t_rect = 0.f;
double t_resize = 0.f;
double t_track = 0.f;
int num_rect = 0;
int proccIm = 0;
// Step 5开始逐帧追踪
for(int ni=0; ni<nImages[seq]; ni++, proccIm++)
{
// 5.1Read left and right images from file
imLeft = cv::imread(vstrImageLeft[seq][ni],cv::IMREAD_UNCHANGED);
imRight = cv::imread(vstrImageRight[seq][ni],cv::IMREAD_UNCHANGED);
double tframe = vTimestampsCam[seq][ni];
// 5.2Load imu measurements from previous frame
vImuMeas.clear();
if(ni>0)
while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni]) // while(vTimestampsImu[first_imu]<=vTimestampsCam[ni])
{
vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z,
vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z,
vTimestampsImu[seq][first_imu[seq]]));
first_imu[seq]++;
}
// ! 5.3 Pass the images to the SLAM system
SLAM.TrackStereo(imLeft,imRight,tframe,vImuMeas);
#ifdef COMPILEDWITHC11
std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif
#ifdef REGISTER_TIMES
t_track = t_rect + t_resize + std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t2 - t1).count();
SLAM.InsertTrackTime(t_track);
#endif
double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
vTimesTrack[ni]=ttrack;
// 5.4 Wait to load the next frame
double T=0;
if(ni<nImages[seq]-1)
T = vTimestampsCam[seq][ni+1]-tframe;
else if(ni>0)
T = tframe-vTimestampsCam[seq][ni-1];
if(ttrack<T)
usleep((T-ttrack)*1e6); // 1e6
}
ROS
ROS则通过 ros::spin(),重复执行回调函数,将数据读入。sync_thread线程持续循环接受数据。
// Step 3 接受imu数据、图像数据,通过回调函数进行数据预处理。
// Maximum delay, 5 seconds
//最大延迟5s
ros::Subscriber sub_imu = n.subscribe("/mynteye/imu/data_raw", 1000, &ImuGrabber::GrabImu, &imugb);
ros::Subscriber sub_img_left = n.subscribe("/mynteye/left/image_raw", 100, &ImageGrabber::GrabImageLeft, &igb);
ros::Subscriber sub_img_right = n.subscribe("/mynteye/right/image_raw", 100, &ImageGrabber::GrabImageRight, &igb);
// Step 4 开启同步线程,进行imu数据和图像匹配,追踪
std::thread sync_thread(&ImageGrabber::SyncWithImu, &igb);
ros::spin();
ros::spin()与ros::spinonce()
ros::spin()与ros::spinonce(),都是ROS消息回调处理函数。这两个函数需要结合ros::Subscriber()(ROS消息订阅函数)来看。消息回调处理的意思是调用回调函数处理订阅到的消息。首先,使用ros::Subscriber()进行消息订阅,但此处需要注意的是,我们写的诸如下面这句代码只是一个声明,程序并没有真正地执行回调函数。直到遇到ros::spin()或ros::spinonce(),程序
ros::Subscriber sub = nh.subscribe("topic_name", 100, Callback);
才算真正地执行上面这句代码:先订阅一条topic_name话题当前时间戳的消息,然后调用Callback回调函数。如果你的代码中没有写ros::spin()或ros::spinonce(),那么上面这行代码是毫无意义的,因为它并不会执行。
ros::spin()与ros::spinonce()的区别是,当程序遇到ros::spin()时,会一直执行上面这行代码,而且只会执行这一行代码(比如说你的ros::spin()与上面一行代码中间有代码,那也只会在开始的时候执行一次,遇到ros::spin()之后就不会再执行了,而不是像我们常看到的执行完这一句执行下一句),不停地订阅消息、调用回调函数、订阅消息、调用回调函数…直到它退出(可以是程序退出、程序员自己加上退出条件等等)。ros::spinonce()也很简单,它不会像ros::spin()一样一直循环,而是在程序走到它时,运行一次上面的代码之后就接着往下走了。