VI-SLAM技术讨论

当前，为了在实际场景中获得更加精确和稳定的效果，通常采用多传感器融合的方式。其中，采用低成本的IMU与相机融合为视觉惯性SLAM 系统的研究得到了越来越多的关注。IMU传感器可以得到三个轴的角速度以及加速度测量，其测量过程不受光照等因素的影响，与相机具有高度互补性：一方面，可以利用IMU较高的采样频率提高系统的输出频率，进而提高视觉对动态环境和复杂环境的鲁棒性；另一方面，IMU可以借助视觉信息消除自身的累积积分误差。

本文将从目前市面上的传感器类别来分析VI-SLAM的各个系统研究现状，然后分别对比各个传感器的优劣，最后从实际场景的应用进行对比展开。

一、VI-SLAM与VSLAM的对比

1、视觉SLAM

近年来，仅使用相机的SLAM由于传感器配置简单且技术难度高于其他方法而得到广泛关注。由于这种SLAM的输入仅为视觉信息，因此该技术被专门称为视觉SLAM（VSLAM）。VSLAM 算法在计算机视觉、机器人和增强现实领域收到广泛关注。特别是VSLAM 算法适用于移动设备的增强现实系统中对相机进行位姿估计，其系统的配置容易便捷优点使带有摄像头的平板电脑或智能手机就可以轻松实现增强现实。

2、视觉SLAM的类别

当前根据相机的种类进行分类，主要有单目、双目和深度相机三种类别。单目相机运用视觉SLAM上仅使用一个摄像头作为传感器完成同步定位与地图创建操作，具有系统结构简单、成本低且易实现等优点。但是在单张图片中无法确定一个物体的真实大小，具有尺度不确定性；双目相机类似于人眼的原理，通过基线来估计每个像素的空间位置，双目相机的基线距离越大，能够测量的距离就越远，可以将视觉SLAM运用到室内和室外，然而计算上消耗资源，并且和单目相机一样容易受到运动速度和光照变化的影响；深度相机能够节省计算资源直接获取深度信息，在室内能够有很好的表现，但是遇到投射或黑色材质的物体无法测量，在室外易受到日光的干扰。

传感器类别	优势	劣势
单目相机	系统结构简单、成本低且易实现	尺度不确定性
双目相机	容易处理绝对尺度问题，鲁棒性更好	计算负载大
RGB-D相机	使用物理测距方法测量深度，因此在没有光照、快速运动等情况下都可以测距	测量范围较窄且易受日光干扰，通常只能用于室内场景，在遇到透射材料、反光表面以及黑色物体等情况下表现较差，造成深度图缺失

3、视觉惯性SLAM

仅利用视觉信息进行位姿估计时，在遇到纹理弱，光线变化大和高速运动时，定位效果不能满足场景需要，所以在视觉SLAM的基础上，将惯性测量单元的计算结果融合组成鲁棒性更好的VI-SLAM的研究收到越来越多的关注。

4、视觉惯性SLAM的类别

目前的视觉惯性SLAM系统通常存在较高的计算负载，对硬件的要求比较高，需要平衡算法精度和计算效率两者的关系。相机与IMU融合为将来SLAM的小型化与低成本化提供了一个有效的方向。单目相机与IMU融合能够解决初始化尺度问题，但是在局部平面上，单目IMU融合SLAM容易漂移；双目与IMU融合对尺度额外可观，追踪中提供较好的初始化位姿，初始化速度更快；深度相机融合IMU的测量范围依旧受限，极大限制了其应用，所以目前多开源的视觉SLAM系统，更多采用双目相机与IMU融合的方式。

传感器类别	优势	劣势
单目相机+ IMU	解决初始化尺度问题	容易漂移
双目相机 + IMU	追踪中提供较好的初始化位姿，初始化速度更快	标定复杂
RGB-D相机 + IMU	建图效果更好	测量范围依旧受限

二、VI-SLAM算法在实际场景中的应用

本文将使用ORB-SLAM3算法进行实验，ORB-SLAM3是第一个基于特征的VI-SLAM系统，通过视觉信息与惯性处理单元融合的紧耦合系统。ORB-SLAM系列算法是特征点法VI-SLAM的代表，当前最新发展的ORB-SLAM3已经将相机模型抽象化，适用范围非常广，能更好的为增强现实的算法实现落地。ORB-SLAM3是一个支持视觉，视觉惯导融合和混合地图的SLAM系统，可以在单目、双目和RGB-D相机上利用针孔模型或者鱼眼模型运行，不管是大、小场景，室内外都能够鲁棒的运行。其相较之前发展的仅利用最新的几帧数据的里程计ORB-SLAM-VI算法更鲁棒，并且精度提高两倍以上，ORB-SLAM3使用子地图让系统更不容易跟踪丢失，回环检测加入最大后验估计能够持续稳定的运行，跟踪线程丢失特征后，子地图会迅速搜索最近建立的子图帮助重新定位，所以ORB-SLAM3系统比之前的任何版本变现的都更加鲁棒。

1、单目视觉惯性实验

实验使用单目摄像头型号：DF100-720P，IMU：MPU6050姿态传感器

// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true);//

//执行重点，输入图片帧，和对应图片帧的imu数据（10个左右），这两个数据在此文件中处理读出
SLAM.TrackMonocular(im,tframe,vImuMeas); // TODO change to monocular_inertial

   // 如果是单目+imu模式，把IMU数据存储到mlQueueImuData中
   if (mSensor == System::IMU_MONOCULAR)
       for(size_t i_imu = 0; i_imu < vImuMeas.size(); i_imu++)
           mpTracker->GrabImuData(vImuMeas[i_imu]);
   // 开始跟踪，返回相机位姿
   Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed,timestamp,filename);//mpTracker为三个主线程之一
   unique_lock<mutex> lock2(mMutexState);
   mTrackingState = mpTracker->mState;//记录跟踪状态
   mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;//当前帧的地图点
   mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;//当前帧的去畸变后关键点
   return Tcw;//返回世界坐标系到相机的位姿

//开始跟踪 单目
Sophus::SE3f Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp, string filename)
{
    //将彩色图像转为灰度图像 mImGray

    //构造Frame，同时完成特征点的提取、计算词袋等操作，mCurrentFrame用图片帧里的特征点来表征一帧图像
    //imu模式的Frame构造函数  比纯视觉多了&mLastFrame,*mpImuCalib两项
	mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mpCamera,mDistCoef,mbf,mThDepth,&mLastFrame,*mpImuCalib);

    mCurrentFrame.mNameFile = filename;
    mCurrentFrame.mnDataset = mnNumDataset;

    lastID = mCurrentFrame.mnId;
    Track();//最主要的函数

    return mCurrentFrame.GetPose();
}
();//imu模式下进行imu预积分
if(mState==NOT_INITIALIZED){
	MonocularInitialization();//
}else{//已经初始化完成，正常定位模式
	if(mState==OK){
		CheckReplacedInLastFrame();//检查上一帧被替换的地图点，将地图点替换成新的地图点
			//如果运动模型是空且imu未初始化 || 刚刚完成重定位
		if((!mbVelocity && !pCurrentMap->isImuInitialized()) || mCurrentFrame.mnId<mnLastRelocFrameId+2)
		    bOK = TrackReferenceKeyFrame(); //跟踪参考关键帧  估计参考关键帧和当前帧之间的位姿，都没有IMU的参与 
		else
		    bOK = TrackWithMotionModel(); //否则用恒速模型进行跟踪，IMU,失败则再次跟踪参考关键帧
		    if(!bOK)
		        bOK = TrackReferenceKeyFrame();
		if (!bOK)//如果跟踪失败，bOK==false      
				mState = LOST;
				mState = RECENTLY_LOST;//地图中关键帧的数量>10
	}
	else if(mState == RECENTLY_LOST){
		PredictStateIMU();//利用IMU计算位姿
		//有两种情况会用到此函数：（a）视觉跟丢时用imu预测位姿；（b）imu模式下，恒速模型跟踪时提供位姿初始值
		//imu进行跟踪，最多跟5s，还失败就设置为LOST
	}
	else if(mState == LOST){
		mpSystem->ResetActiveMap();//如果关键帧数量小于10，则重置地图
		CreateMapInAtlas();//否则新建地图
	}
}
TrackLocalMap();//优化当前帧的位姿（跟踪成功，则更新局部地图，寻找更多的匹配）
UpdateLocalMap();//更新局部地图，共视关键帧，共视地图点
SearchLocalPoints();
//imu没有初始化，或者刚刚重定位
PoseOptimization(&mCurrentFrame);
//地图未更新时（与上一帧距离近、误差小）用PoseInertialOptimizationLastFrame()
PoseInertialOptimizationLastFrame(&mCurrentFrame); 
//地图更新时用（关键帧优化了，和上一阵相比误差更小）PoseInertialOptimizationLastKeyFrame()
PoseInertialOptimizationLastKeyFrame(&mCurrentFrame);

//在IMUType.cc中有Preintegrated构造函数
IMU::Preintegrated* pImuPreintegratedFromLastFrame = new IMU::Preintegrated(mLastFrame.mImuBias,mCurrentFrame.mImuCalib);
//对于n个imu数据，要进行n-1次计算得到两帧之间的预积分量
for(int i=0; i<n; i++){
 	 //开始计算预积分 IntegrateNewMeasurement()
	 mpImuPreintegratedFromLastKF->IntegrateNewMeasurement(acc,angVel,tstep);//上一关键帧到当前帧的预积分mpImuPreintegratedFromLastKF
	 pImuPreintegratedFromLastFrame->IntegrateNewMeasurement(acc,angVel,tstep);//上一帧到当前帧的预积分pImuPreintegratedFromLastFrame
	 }
//计算结果加入当前帧
mCurrentFrame.mpImuPreintegratedFrame = pImuPreintegratedFromLastFrame;
mCurrentFrame.mpImuPreintegrated = mpImuPreintegratedFromLastKF;
mCurrentFrame.mpLastKeyFrame = mpLastKeyFrame;

修改IMU的加速度阈值，可以看出轨迹如下：

2、双目视觉惯性实验

实验使用双目摄像头型号：MYNT EYE小觅双目摄像机，内置六轴IMU传感器

分别读取左右相机的内参矩阵、畸变系数、旋转矩阵以及投影矩阵，这里ORB-SLAM3是自己根据我们给出的左右相机的各种参数，在内部拼接了一个校正的变换，然后用它去校正双目相机影像

int main(int argc,char **argv)
{
//Create SLAM system,It initializes all system threads and gets ready to process frames.ORB SLAM3::System SLAM(argv[1],argv[2],ORB SLAM3::System::STEREO, true);
cv::Mat imLeft,imRight;
for(seg=0;seq<num seg; seg++)
// Seg loop
double tresize0;
double trect-0;
double ttrack=0;int num_rect.0;int proccIm=0:
for(int ni=0;ni<nImagessegl; ni++,proccIm++)
'/Read left and right images from file
imLeft = cv: :imread(vstrImageLeft[seq][ni],CV::IMREAD_UNCHANGED);//,CV::IMREAD UNCHANGED);imRight = cv::imread(vstrImageRight[seq][ni],CV::IMREAD UNCHANGED);//,CV::IMREAD UNCHANGED);
if(imLeft.empty())
cerr<<endl<<"Failed to load image at:
<< string(vstrImageLeft[seg][ni])<< endl;
return 1;
if(imRight.empty())
cerr<<endl<<"Falled to load image at:"
<<string(vstrImageRight[seq][ni])<< endl;
return 1;

double tframe =vTimestampsCam[seg][ni];
#ifdef COMPILEDWITHC11
std::chrono::steady clock::time point t1 = std::chrono::steady clock::now();
#else
std::chrono::monotonic clock::time_point t1= std::chrono::monotonic clock: :now();
#endif
//Pass the images to the SLAM system
SLAM.Trackstereo(imleft,imRieht,tframe, vector<ORB SLAM3::IMU::Point>(),vstrImageleft[segl[nil);

}

直接传入的就是读取的影像。当然，这并不代表ORB-SLAM3不做双目校正了。ORB-SLAM3用的是OpenCV的stereorectify()函数，在这个ORB_SLAM3/src/Settings.cc文件中

void Settings::readOtherParameters(cv::FileStorage& fSettings){

void Settings::precomputeRectificationMaps(){
//Precompute rectification maps,new calibrations,...cv::Mat K1=static cast<Pinhole*>(calibration1 )->toK();K1.convertTo(K1,CV_64F);
cv::Mat K2=static_cast<Pinhole*>(calibration2_)->toK();K2.convertTo(K2,CV_64F);
cv::Mat cvTlr;
cv::eigen2cv(Tlr_.inverse().matrix3x4(),cvTlr);cv::Mat R12 =cvTlr.rowRange(8,3).colRange(0,3);R12.convertTo(R12,CV 64F);cv::Mat t12cvTlr.rowRange(0,3).col(3);t12.convertTo(t12,CV 64F);
cv::Mat R_r1_ul, R_r2_u2;
cv::Mat P1,P2,Q;

cv::stereoRectify(K1,camera1DistortionCoef(),K2,camera2DistortionCoef(),newImSize_,
R12, t12,
R rI ul'R r2 u2,P1,P2,Q,CV::CALIB ZERO DISPARITY,-1,newImsize );cv: :initUndistortRectifyMap(K1, camera1DistortionCoef(),R r1 u1,P1.rowRange(0,3).colRange(0, 3),newImSize_,CV 32F，M11_,M21_);cv::initUndistortRectifyMap(K2, camera2DistortionCoef(),R_r2_u2,P2.rowRange(0, 3).colRange(0, 3),newImsizeCV32F，M1r,M2r );
//Update calibration
calibration1->setParameter(p1.at<double>(0,0)，0);calibration1->setParameter(P1.at<double>(1,1)，1);calibration1->setParameter(P1.at<double>(0,2)2);
calibration1->setParameter(P1.at<double>(1,2)3);

修改IMU的加速度阈值，可以看出轨迹如下：

三、总结

相机传感器融合imu传感器的好处是解决了高速运动中间数据处理的问题。相机运动过程中间出现模糊，两帧之间重叠区域太少，因此很难做特征点匹配。相机自己又能在慢速运动中间解决 imu的漂移问题，这两者是互补的。

IMU加进视觉里面，并不是提高了视觉的定位精度，比如在ORB-SLAM3的纯视觉定位精度是高于视觉+IMU的定位精度的，纯视觉是更高精度的里程计，过程相当于一个高精度融合低精度的，所以最后多模态的定位精度没有纯视觉的高，所以视觉+IMU提高的是系统的鲁棒性，并非提高精度。