From 8d53ea57b4925c9d936f40312fab3fe90d32de9b Mon Sep 17 00:00:00 2001 From: laobai Date: Sun, 1 May 2022 23:05:29 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8D=E9=87=8D=E5=A4=8D=E8=B7=91?= =?UTF-8?q?=E5=90=8C=E4=B8=80=E4=B8=AA=E6=95=B0=E6=8D=AE=E9=9B=86=E4=BC=9A?= =?UTF-8?q?=E5=B4=A9bug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc | 7 +++++-- Examples/ROS/ORB_SLAM3/src/ros_rgbd.cc | 7 +++++-- src/LocalMapping.cc | 12 ++++++++++++ src/Tracking.cc | 3 ++- 4 files changed, 24 insertions(+), 5 deletions(-) diff --git a/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc b/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc index 32f5920..bab8220 100644 --- a/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc +++ b/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc @@ -95,8 +95,11 @@ int main(int argc, char **argv) ImageGrabber igb(&SLAM,&imugb,bEqual); // TODO // Maximum delay, 5 seconds - ros::Subscriber sub_imu = n.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb); - ros::Subscriber sub_img0 = n.subscribe("/camera/image_raw", 100, &ImageGrabber::GrabImage,&igb); + // ros::Subscriber sub_imu = n.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb); + // ros::Subscriber sub_img0 = n.subscribe("/camera/image_raw", 100, &ImageGrabber::GrabImage,&igb); + // 以下为EUROC + ros::Subscriber sub_imu = n.subscribe("/imu0", 1000, &ImuGrabber::GrabImu, &imugb); + ros::Subscriber sub_img0 = n.subscribe("/cam0/image_raw", 100, &ImageGrabber::GrabImage,&igb); std::thread sync_thread(&ImageGrabber::SyncWithImu,&igb); diff --git a/Examples/ROS/ORB_SLAM3/src/ros_rgbd.cc b/Examples/ROS/ORB_SLAM3/src/ros_rgbd.cc index cb68d76..bddf675 100644 --- a/Examples/ROS/ORB_SLAM3/src/ros_rgbd.cc +++ b/Examples/ROS/ORB_SLAM3/src/ros_rgbd.cc @@ -63,8 +63,11 @@ int main(int argc, char **argv) ros::NodeHandle nh; - message_filters::Subscriber rgb_sub(nh, "/camera/rgb/image_raw", 100); - message_filters::Subscriber depth_sub(nh, "camera/depth_registered/image_raw", 100); + // message_filters::Subscriber rgb_sub(nh, "/camera/rgb/image_raw", 100); + // message_filters::Subscriber depth_sub(nh, "/camera/depth_registered/image_raw", 100); + // 以下为tum rgbd 数据集的topic + message_filters::Subscriber rgb_sub(nh, "/camera/rgb/image_color", 100); + message_filters::Subscriber depth_sub(nh, "/camera/depth/image", 100); typedef message_filters::sync_policies::ApproximateTime sync_pol; message_filters::Synchronizer sync(sync_pol(10), rgb_sub,depth_sub); sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2)); diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc index a2e3455..8a8902f 100644 --- a/src/LocalMapping.cc +++ b/src/LocalMapping.cc @@ -1578,6 +1578,8 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) Eigen::Matrix3f Rwg; Eigen::Vector3f dirG; dirG.setZero(); + + int have_imu_num = 0; for(vector::iterator itKF = vpKF.begin(); itKF!=vpKF.end(); itKF++) { if (!(*itKF)->mpImuPreintegrated) @@ -1585,6 +1587,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) if (!(*itKF)->mPrevKF) continue; + have_imu_num++; // 初始化时关于速度的预积分定义Ri.t()*(s*Vj - s*Vi - Rwg*g*tij) dirG -= (*itKF)->mPrevKF->GetImuRotation() * (*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity(); // 求取实际的速度，位移/时间 @@ -1592,6 +1595,15 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) (*itKF)->SetVelocity(_vel); (*itKF)->mPrevKF->SetVelocity(_vel); } + + if (have_imu_num < 6) + { + cout << "imu初始化失败, 由于带有imu预积分信息的关键帧数量太少" << endl; + bInitializing=false; + mbBadImu = true; + return; + } + // dirG = sV1 - sVn + n*Rwg*g*t // 归一化 dirG = dirG/dirG.norm(); diff --git a/src/Tracking.cc b/src/Tracking.cc index 36014ab..18fcc75 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -2002,7 +2002,7 @@ void Tracking::Track() // 如果当前图像时间戳比前一帧图像时间戳小，说明出错了，清除imu数据，创建新的子地图 cerr << "ERROR: Frame with a timestamp older than previous frame detected!" << endl; unique_lock lock(mMutexImuQueue); - mlQueueImuData.clear(); + // mlQueueImuData.clear(); // 创建新地图 CreateMapInAtlas(); return; @@ -3126,6 +3126,7 @@ void Tracking::CreateMapInAtlas() mLastFrame = Frame(); mCurrentFrame = Frame(); mvIniMatches.clear(); + mlQueueImuData.clear(); mbCreatedMap = true; }