系统要求
ubuntu18.04, 系统自带opencv3.2.0未改变,若需要改变opencv版本可以参考另一篇文章从零开始的ORB_SLAM3运行(正在更新)
快速安装过程
ORB_SLAM3源码地址https://github.com/UZ-SLAMLab/ORB_SLAM3
前置需求Pangolin,OpenCV,Eigen3,DBoW2 and g2o (Included in Thirdparty folder),Python,ROS (optional)
下面依次安装Pangolin, Eigen3, ros_melodic
Pangolin下载与安装
在 ORB_SLAM3 github 网站中,作者对Pangolin的安装有以下说明:
We use Pangolin for visualization and user interface. Dowload and install instructions can be found at: https://github.com/stevenlovegrove/Pangolin.
具体安装步骤为
git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
mkdir build
cd build
cmake ..
cmake --build .
Eigen3下载与安装
在 ORB_SLAM3 github 网站中,作者对Eigen3的安装有以下说明:
Required by g2o (see below). Download and install instructions can be found at: http://eigen.tuxfamily.org. Required at least 3.1.0.
按照网站右侧提示下载,下载后解压,找到INSTALL文件并根据提示进行安装。
Eigen 3.3.9的下载地址为https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz
官方安装教程为
Let’s call this directory ‘source_dir’ (where this INSTALL file is).
Before starting, create another directory which we will call ‘build_dir’.
Do:
cd build_dir
cmake source_dir
make install
简化版本为:
tar -zxvf eien-3.3.9.tar.gz
cd eigen-3.3.9
mkdir build
cmake ..
sudo make install
ros_melodic的安装(optional)
roswiki中有详细的安装步骤http://wiki.ros.org/melodic/Installation/Ubuntu
其中有几点需要注意:
- 在更换软件源的时候可以选择中国的源,详情可见http://wiki.ros.org/ROS/Installation/UbuntuMirrors
- 在添加环境的环节,注意当前使用的是bash还是zsh,从而选择在.bashrc还是.zshrc中添加source
- 在执行sudo rosdep init时,可能需要科学上网
- 最重要的一点,ROS的安装教程要选择英文版而不是中文版的,中文版的安装教程并没有英文版的全面,在sudo rosdep init之前要先执行sudo apt install python-rosdep才不会报错
ORB_SLAM3的下载与编译
在安装完前置要求之后,开始ORB_SLAM3的编译。从https://github.com/UZ-SLAMLab/ORB_SLAM3下载源码。
git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git
在编译之前需要修改两个文件,具体参考https://www.icode9.com/content-3-961807.html
修改步骤如下
- 将文件~/ORB_SLAM3.0/src/CameraModels/KannalaBrandt8.cpp中第534行void KannalaBrandt8::Triangulate_()函数中出错的这句话注释掉:
x3D = x3D_h.get_minor<3,1>(0,0) / x3D_h(3)
- 将文件~/ORB_SLAM3.0/src/LocalMapping.cc中第393行到749行的void LocalMapping::CreateNewMapPoints()函数修改为:
(其实就是ORBSLAM3上一版的函数直接copy过来)
void LocalMapping::CreateNewMapPoints()
{
// Retrieve neighbor keyframes in covisibility graph
int nn = 10;
// For stereo inertial case
if(mbMonocular)
nn=20;
vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
if (mbInertial)
{
KeyFrame* pKF = mpCurrentKeyFrame;
int count=0;
while((vpNeighKFs.size()<=nn)&&(pKF->mPrevKF)&&(count++<nn))
{
vector<KeyFrame*>::iterator it = std::find(vpNeighKFs.begin(), vpNeighKFs.end(), pKF->mPrevKF);
if(it==vpNeighKFs.end())
vpNeighKFs.push_back(pKF->mPrevKF);
pKF = pKF->mPrevKF;
}
}
float th = 0.6f;
ORBmatcher matcher(th,false);
cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation();
cv::Mat Rwc1 = Rcw1.t();
cv::Mat tcw1 = mpCurrentKeyFrame->GetTranslation();
cv::Mat Tcw1(3,4,CV_32F);
Rcw1.copyTo(Tcw1.colRange(0,3));
tcw1.copyTo(Tcw1.col(3));
cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter();
const float &fx1 = mpCurrentKeyFrame->fx;
const float &fy1 = mpCurrentKeyFrame->fy;
const float &cx1 = mpCurrentKeyFrame->cx;
const float &cy1 = mpCurrentKeyFrame->cy;
const float &invfx1 = mpCurrentKeyFrame->invfx;
const float &invfy1 = mpCurrentKeyFrame->invfy;
const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor;
// Search matches with epipolar restriction and triangulate
for(size_t i=0; i<vpNeighKFs.size(); i++)
{
if(i>0 && CheckNewKeyFrames())// && (mnMatchesInliers>50))
return;
KeyFrame* pKF2 = vpNeighKFs[i];
GeometricCamera* pCamera1 = mpCurrentKeyFrame->mpCamera, *pCamera2 = pKF2->mpCamera;
// Check first that baseline is not too short
cv::Mat Ow2 = pKF2->GetCameraCenter();
cv::Mat vBaseline = Ow2-Ow1;
const float baseline = cv::norm(vBaseline);
if(!mbMonocular)
{
if(baseline<pKF2->mb)
continue;
}
else
{
const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);
const float ratioBaselineDepth = baseline/medianDepthKF2;
if(ratioBaselineDepth<0.01)
continue;
}
// Compute Fundamental Matrix
cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);
// Search matches that fullfil epipolar constraint
vector<pair<size_t,size_t> > vMatchedIndices;
bool bCoarse = mbInertial &&
((!mpCurrentKeyFrame->GetMap()->GetIniertialBA2() && mpCurrentKeyFrame->GetMap()->GetIniertialBA1())||
mpTracker->mState==Tracking::RECENTLY_LOST);
matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false,bCoarse);
cv::Mat Rcw2 = pKF2->GetRotation();
cv::Mat Rwc2 = Rcw2.t();
cv::Mat tcw2 = pKF2->GetTranslation();
cv::Mat Tcw2(3,4,CV_32F);
Rcw2.copyTo(Tcw2.colRange(0,3));
tcw2.copyTo(Tcw2.col(3));
const float &fx2 = pKF2->fx;
const float &fy2 = pKF2->fy;
const float &cx2 = pKF2->cx;
const float &cy2 = pKF2->cy;
const float &invfx2 = pKF2->invfx;
const float &invfy2 = pKF2->invfy;
// Triangulate each match
const int nmatches = vMatchedIndices.size();
for(int ikp=0; ikp<nmatches; ikp++)
{
const int &idx1 = vMatchedIndices[ikp].first;
const int &idx2 = vMatchedIndices[ikp].second;
const cv::KeyPoint &kp1 = (mpCurrentKeyFrame -> NLeft == -1) ? mpCurrentKeyFrame->mvKeysUn[idx1]
: (idx1 < mpCurrentKeyFrame -> NLeft) ? mpCurrentKeyFrame -> mvKeys[idx1]
: mpCurrentKeyFrame -> mvKeysRight[idx1 - mpCurrentKeyFrame -> NLeft];
const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1];
bool bStereo1 = (!mpCurrentKeyFrame->mpCamera2 && kp1_ur>=0);
const bool bRight1 = (mpCurrentKeyFrame -> NLeft == -1 || idx1 < mpCurrentKeyFrame -> NLeft) ? false
: true;
const cv::KeyPoint &kp2 = (pKF2 -> NLeft == -1) ? pKF2->mvKeysUn[idx2]
: (idx2 < pKF2 -> NLeft) ? pKF2 -> mvKeys[idx2]
: pKF2 -> mvKeysRight[idx2 - pKF2 -> NLeft];
const float kp2_ur = pKF2->mvuRight[idx2];
bool bStereo2 = (!pKF2->mpCamera2 && kp2_ur>=0);
const bool bRight2 = (pKF2 -> NLeft == -1 || idx2 < pKF2 -> NLeft) ? false
: true;
if(mpCurrentKeyFrame->mpCamera2 && pKF2->mpCamera2){
if(bRight1 && bRight2){
Rcw1 = mpCurrentKeyFrame->GetRightRotation();
Rwc1 = Rcw1.t();
tcw1 = mpCurrentKeyFrame->GetRightTranslation();
Tcw1 = mpCurrentKeyFrame->GetRightPose();
Ow1 = mpCurrentKeyFrame->GetRightCameraCenter();
Rcw2 = pKF2->GetRightRotation();
Rwc2 = Rcw2.t();
tcw2 = pKF2->GetRightTranslation();
Tcw2 = pKF2->GetRightPose();
Ow2 = pKF2->GetRightCameraCenter();
pCamera1 = mpCurrentKeyFrame->mpCamera2;
pCamera2 = pKF2->mpCamera2;
}
else if(bRight1 && !bRight2){
Rcw1 = mpCurrentKeyFrame->GetRightRotation();
Rwc1 = Rcw1.t();
tcw1 = mpCurrentKeyFrame->GetRightTranslation();
Tcw1 = mpCurrentKeyFrame->GetRightPose();
Ow1 = mpCurrentKeyFrame->GetRightCameraCenter();
Rcw2 = pKF2->GetRotation();
Rwc2 = Rcw2.t();
tcw2 = pKF2->GetTranslation();
Tcw2 = pKF2->GetPose();
Ow2 = pKF2->GetCameraCenter();
pCamera1 = mpCurrentKeyFrame->mpCamera2;
pCamera2 = pKF2->mpCamera;
}
else if(!bRight1 && bRight2){
Rcw1 = mpCurrentKeyFrame->GetRotation();
Rwc1 = Rcw1.t();
tcw1 = mpCurrentKeyFrame->GetTranslation();
Tcw1 = mpCurrentKeyFrame->GetPose();
Ow1 = mpCurrentKeyFrame->GetCameraCenter();
Rcw2 = pKF2->GetRightRotation();
Rwc2 = Rcw2.t();
tcw2 = pKF2->GetRightTranslation();
Tcw2 = pKF2->GetRightPose();
Ow2 = pKF2->GetRightCameraCenter();
pCamera1 = mpCurrentKeyFrame->mpCamera;
pCamera2 = pKF2->mpCamera2;
}
else{
Rcw1 = mpCurrentKeyFrame->GetRotation();
Rwc1 = Rcw1.t();
tcw1 = mpCurrentKeyFrame->GetTranslation();
Tcw1 = mpCurrentKeyFrame->GetPose();
Ow1 = mpCurrentKeyFrame->GetCameraCenter();
Rcw2 = pKF2->GetRotation();
Rwc2 = Rcw2.t();
tcw2 = pKF2->GetTranslation();
Tcw2 = pKF2->GetPose();
Ow2 = pKF2->GetCameraCenter();
pCamera1 = mpCurrentKeyFrame->mpCamera;
pCamera2 = pKF2->mpCamera;
}
}
// Check parallax between rays
cv::Mat xn1 = pCamera1->unprojectMat(kp1.pt);
cv::Mat xn2 = pCamera2->unprojectMat(kp2.pt);
cv::Mat ray1 = Rwc1*xn1;
cv::Mat ray2 = Rwc2*xn2;
const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2));
float cosParallaxStereo = cosParallaxRays+1;
float cosParallaxStereo1 = cosParallaxStereo;
float cosParallaxStereo2 = cosParallaxStereo;
if(bStereo1)
cosParallaxStereo1 = cos(2*atan2(mpCurrentKeyFrame->mb/2,mpCurrentKeyFrame->mvDepth[idx1]));
else if(bStereo2)
cosParallaxStereo2 = cos(2*atan2(pKF2->mb/2,pKF2->mvDepth[idx2]));
cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);
cv::Mat x3D;
if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 ||
(cosParallaxRays<0.9998 && mbInertial) || (cosParallaxRays<0.9998 && !mbInertial)))
{
// Linear Triangulation Method
cv::Mat A(4,4,CV_32F);
A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);
A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);
A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);
A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);
cv::Mat w,u,vt;
cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
x3D = vt.row(3).t();
if(x3D.at<float>(3)==0)
continue;
// Euclidean coordinates
x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}
else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2)
{
x3D = mpCurrentKeyFrame->UnprojectStereo(idx1);
}
else if(bStereo2 && cosParallaxStereo2<cosParallaxStereo1)
{
x3D = pKF2->UnprojectStereo(idx2);
}
else
{
continue; //No stereo and very low parallax
}
cv::Mat x3Dt = x3D.t();
if(x3Dt.empty()) continue;
//Check triangulation in front of cameras
float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at<float>(2);
if(z1<=0)
continue;
float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at<float>(2);
if(z2<=0)
continue;
//Check reprojection error in first keyframe
const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];
const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at<float>(0);
const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at<float>(1);
const float invz1 = 1.0/z1;
if(!bStereo1)
{
cv::Point2f uv1 = pCamera1->project(cv::Point3f(x1,y1,z1));
float errX1 = uv1.x - kp1.pt.x;
float errY1 = uv1.y - kp1.pt.y;
if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1)
continue;
}
else
{
float u1 = fx1*x1*invz1+cx1;
float u1_r = u1 - mpCurrentKeyFrame->mbf*invz1;
float v1 = fy1*y1*invz1+cy1;
float errX1 = u1 - kp1.pt.x;
float errY1 = v1 - kp1.pt.y;
float errX1_r = u1_r - kp1_ur;
if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1)
continue;
}
//Check reprojection error in second keyframe
const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];
const float x2 = Rcw2.row(0).dot(x3Dt)+tcw2.at<float>(0);
const float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at<float>(1);
const float invz2 = 1.0/z2;
if(!bStereo2)
{
cv::Point2f uv2 = pCamera2->project(cv::Point3f(x2,y2,z2));
float errX2 = uv2.x - kp2.pt.x;
float errY2 = uv2.y - kp2.pt.y;
if((errX2*errX2+errY2*errY2)>5.991*sigmaSquare2)
continue;
}
else
{
float u2 = fx2*x2*invz2+cx2;
float u2_r = u2 - mpCurrentKeyFrame->mbf*invz2;
float v2 = fy2*y2*invz2+cy2;
float errX2 = u2 - kp2.pt.x;
float errY2 = v2 - kp2.pt.y;
float errX2_r = u2_r - kp2_ur;
if((errX2*errX2+errY2*errY2+errX2_r*errX2_r)>7.8*sigmaSquare2)
continue;
}
//Check scale consistency
cv::Mat normal1 = x3D-Ow1;
float dist1 = cv::norm(normal1);
cv::Mat normal2 = x3D-Ow2;
float dist2 = cv::norm(normal2);
if(dist1==0 || dist2==0)
continue;
if(mbFarPoints && (dist1>=mThFarPoints||dist2>=mThFarPoints)) // MODIFICATION
continue;
const float ratioDist = dist2/dist1;
const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave]/pKF2->mvScaleFactors[kp2.octave];
if(ratioDist*ratioFactor<ratioOctave || ratioDist>ratioOctave*ratioFactor)
continue;
// Triangulation is succesfull
MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpAtlas->GetCurrentMap());
pMP->AddObservation(mpCurrentKeyFrame,idx1);
pMP->AddObservation(pKF2,idx2);
mpCurrentKeyFrame->AddMapPoint(pMP,idx1);
pKF2->AddMapPoint(pMP,idx2);
pMP->ComputeDistinctiveDescriptors();
pMP->UpdateNormalAndDepth();
mpAtlas->AddMapPoint(pMP);
mlpRecentAddedMapPoints.push_back(pMP);
}
}
}
之后运行目录下的build.sh文件。
cd ORB_SLAM3
chmod +x ./build.sh
./build.sh
没有报错则安装成功。
数据集下载
在 ORB_SLAM3 github 网站中,作者对数据集的下载有以下说明:
EuRoC dataset was recorded with two pinhole cameras and an inertial sensor. We provide an example script to launch EuRoC sequences in all the sensor configurations.
Download a sequence (ASL format) from http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets
Open the script “euroc_examples.sh” in the root of the project. Change pathDatasetEuroc variable to point to the directory where the dataset has been uncompressed.
从http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets下载ASL Dataset Format数据集,并把数据集解压到名为MH01的文件夹内。再将euroc_examples.sh中数据集的地址更改为解压后数据集的绝对地址。
cd ~
wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/machine_hall/MH_01_easy/MH_01_easy.zip
unzip MH_01_easy.zip
mv -rf ./mav0 ~/datasets/MH01
将euroc_examples.sh的第二行进行修改:
pathDatasetEuroc='/home/s/datasets' #Example, it is necesary to change it by the dataset path
运行exampls目录下的euroc_examples.sh即可看到ORB_SLAM3的输出。
ROS_melodic的安装(optional)
roswiki中有详细的安装步骤http://wiki.ros.org/melodic/Installation/Ubuntu
其中有几点需要注意:
- 在更换软件源的时候可以选择中国的源,详情可见http://wiki.ros.org/ROS/Installation/UbuntuMirrors
- 在添加环境的环节,注意当前使用的是bash还是zsh,从而选择在.bashrc还是.zshrc中添加source
- 在执行sudo rosdep init时,可能需要科学上网
- 最重要的一点,ROS的安装教程要选择英文版而不是中文版的,中文版的安装教程并没有英文版的全面,比如在sudo rosdep init之前要先执行sudo apt install python-rosdep才不会报错
ORB_SLAM3中ROS软件包的编译(optional)
- 首先需要添加ros软件包路径
//打开bash环境配置文件
gedit ~/.bashrc
//在文件最下方插入下面这句话(其中PATH改为真实绝对路径)
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM3/Examples/ROS
- 修改~/git/ORB_SLAM3/Examples/ROS/ORB_SLAM3/src/ros_mono文件
//修改第62行中的rostopic话题名称为/cam0/image_raw,与数据集中topic话题名称统一
ros::Subscriber sub = nodeHandler.subscribe("/cam0/image_raw", 1, &ImageGrabber::GrabImage,&igb);
- 编译ros文件夹中的源码
cd ~/git/ORB_SLAM3/Examples/ROS/ORB_SLAM3
mkdir build
cd build
cmake ..
make
- 下载ROS bag格式数据集并运行数据集
cd ~
wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/machine_hall/MH_01_easy/MH_01_easy.bag
//终端1
roscore
//终端2
rosbag play ~/MH_01_easy.bag
//终端3
rosrun ORB_SLAM3 Mono /home/s/git/ORB_SLAM3/Vocabulary/ORBvoc.txt /home/s/git/ORB_SLAM3/Examples/ROS/ORB_SLAM3/Asus.yaml
运行成功示例
错误提示以及修正方案
- 编译ORB_SLAM3出现错误
/home/s/ORB_SLAM3/src/CameraModels/KannalaBrandt8.cpp:534:41: error: no match for ‘operator/’ (operand types are ‘cv::Matx<float, 3, 1>’ and ‘float’)
x3D = x3D_h.get_minor<3,1>(0,0) / x3D_h(3);
参考ORB_SLAM3的下载与编译,修改两个文件即可