12345qiupeng
/
orb_slam3_details
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修复重复跑同一个数据集会崩bug

master
laobai 2022-05-01 23:05:29 +08:00
parent 7369f6baac
commit 8d53ea57b4
4 changed files with 24 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc
View File

@ -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);

7
Examples/ROS/ORB_SLAM3/src/ros_rgbd.cc
View File

@ -63,8 +63,11 @@ int main(int argc, char **argv)
ros::NodeHandle nh;
message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 100);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/depth_registered/image_raw", 100);
// message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 100);
// message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth_registered/image_raw", 100);
// 以下为tum rgbd 数据集的topic
message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_color", 100);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth/image", 100);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,depth_sub);
sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));

12
src/LocalMapping.cc
View File

@ -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<KeyFrame*>::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();

3
src/Tracking.cc
View File

@ -2002,7 +2002,7 @@ void Tracking::Track()
// 如果当前图像时间戳比前一帧图像时间戳小，说明出错了，清除imu数据，创建新的子地图
cerr << "ERROR: Frame with a timestamp older than previous frame detected!" << endl;
unique_lock<mutex> 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;
}