<< "Error type" << std::endl;return cv_ptr->image.clone();}}void ImageGrabber::SyncWithImu(){const double maxTimeDiff = 0.01;while(1){cv::Mat imLeft, imRight;double tImLeft = 0, tImRight = 0;if (!imgLeftBuf.empty()&&!imgRightBuf.empty()&&!mpImuGb->imuBuf.empty()){tImLeft = imgLeftBuf.front()->header.stamp.toSec();tImRight = imgRightBuf.front()->header.stamp.toSec();this->mBufMutexRight.lock();while((tImLeft-tImRight)>maxTimeDiff && imgRightBuf.size()>1){imgRightBuf.pop();tImRight = imgRightBuf.front()->header.stamp.toSec();}this->mBufMutexRight.unlock();this->mBufMutexLeft.lock();while((tImRight-tImLeft)>maxTimeDiff && imgLeftBuf.size()>1){imgLeftBuf.pop();tImLeft = imgLeftBuf.front()->header.stamp.toSec();}this->mBufMutexLeft.unlock();if((tImLeft-tImRight)>maxTimeDiff || (tImRight-tImLeft)>maxTimeDiff){// std::cout << "big time difference" << std::endl;continue;}if(tImLeft>mpImuGb->imuBuf.back()->header.stamp.toSec())continue;this->mBufMutexLeft.lock();imLeft = GetImage(imgLeftBuf.front());imgLeftBuf.pop();this->mBufMutexLeft.unlock();this->mBufMutexRight.lock();imRight = GetImage(imgRightBuf.front());imgRightBuf.pop();this->mBufMutexRight.unlock();vector vImuMeas;mpImuGb->mBufMutex.lock();if(!mpImuGb->imuBuf.empty()){// Load imu measurements from buffervImuMeas.clear();while(!mpImuGb->imuBuf.empty() && mpImuGb->imuBuf.front()->header.stamp.toSec()<=tImLeft){double t = mpImuGb->imuBuf.front()->header.stamp.toSec();cv::Point3f acc(mpImuGb->imuBuf.front()->linear_acceleration.x, mpImuGb->imuBuf.front()->linear_acceleration.y, mpImuGb->imuBuf.front()->linear_acceleration.z);cv::Point3f gyr(mpImuGb->imuBuf.front()->angular_velocity.x, mpImuGb->imuBuf.front()->angular_velocity.y, mpImuGb->imuBuf.front()->angular_velocity.z);vImuMeas.push_back(ORB_SLAM3::IMU::Point(acc,gyr,t));mpImuGb->imuBuf.pop();}}mpImuGb->mBufMutex.unlock();if(mbClahe){mClahe->apply(imLeft,imLeft);mClahe->apply(imRight,imRight);}if(do_rectify){cv::remap(imLeft,imLeft,M1l,M2l,cv::INTER_LINEAR);cv::remap(imRight,imRight,M1r,M2r,cv::INTER_LINEAR);}mpSLAM->TrackStereo(imLeft,imRight,tImLeft,vImuMeas);std::chrono::milliseconds tSleep(1);std::this_thread::sleep_for(tSleep);}}}void ImuGrabber::GrabImu(const sensor_msgs::ImuConstPtr &imu_msg){mBufMutex.lock();imuBuf.push(imu_msg);mBufMutex.unlock();return;}
注意自己的topic，如果不确定可以运行三个终端
第一个终端运行：cd catkin_wssource ./devel/setup.bashroscore
第二个终端运行cd catkin_wssource ./devel/setup.bashroslaunch zed_wrapper zed2.launch
第三个终端运行rostopic list
在第三个终端下面查看对应的topic到底是什么，然后输入进去
2. 生成类似Mono的可执行文件
首先编辑-1.0//ROS//.txt文件，在最下面添加
rosbuild_add_executable(zed2_stereo_inertialsrc/zed2_stereo_inertial.cc)target_link_libraries(zed2_stereo_inertial${LIBS})
然后编译，这个步骤类似.sh
cd ORB_SLAM3-master/Examples_old/ROS/ORB_SLAM3mkdir buildcd buildcmake ..make -j8sudo make install
这时候会发现-1.0//ROS/下面多了一个可执行文件,这个文件很重要不能出错 。
3. 重新搭建软链接
更新软连接，在/opt/ros//share目录下，删除原来的软连接（如果有的话）
sudo rm -r ORB_SLAM3
在/opt/ros//share目录下建立软链接
sudo ln -s /home/xxx/Guide_blind/ORB_SLAM3-1.0/Examples_old/ROS/ORB_SLAM3 /opt/ros/melodic/share/ORB_SLAM3
修改后需要重新编译ros：
需要注意是.sh文件需要的是在Examples_old下的，建议打开来一步步执行保证不出错 。sudo ./build_ros.sh