ORBSLAM3 主程序循环是如何实现的

非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()一样一直循环,而是在程序走到它时,运行一次上面的代码之后就接着往下走了。

  • 1
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

My.科研小菜鸡

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值