Skip to content

Use cuda bundle adjustment in ORB SLAM2

Jump to bottom
Akihiro Takagi edited this page Feb 6, 2024 · 4 revisions

Use cuda-bundle-adjustment in ORB-SLAM2

Introduction

This page describes how to incorporate cuda-bundle-adjustment into ORB-SLAM2.
We try to replace g2o::SparseOptimizer with cuba::CudaBundleAdjustment in Optimizer::BundleAdjustment().
A patch file containing all fixes is provided here.

Environment

We have confirmed operation in the following environment.

Package Name Version
Ubuntu 16.04
gcc/g++ 7.5
CUDA Toolkit 11.0.3
Eigen 3.2.0
OpenCV 3.4.20
Pangolin 0.5
cuda-bundle-adjustment 2.1.1

note:
ORB-SLAM2 has stopped being updated for several years, so we have to choose old versions as some dependent libraries. This makes building the environment difficult.
As a reference, we provide the Dockerfile for the environment listed above.

Install cuda-bundle-adjustment

First, build and install cuda-bundle-adjustment. We need to build the shared library, so set BUILD_SHARED_LIB to ON. We choose ORB_SLAM2/Thirdparty as install directory, so set CMAKE_INSTALL_PREFIX to {YOUR_ORB_SLAM2_DIR}/Thirdparty/cuda-bundle-adjustment.

$ git clone https://github.com/fixstars/cuda-bundle-adjustment.git
$ cd cuda-bundle-adjustment
$ mkdir build
$ cd build
$ cmake .. -DBUILD_SHARED_LIB=ON -DCMAKE_INSTALL_PREFIX={YOUR_ORB_SLAM2_DIR}/Thirdparty/cuda-bundle-adjustment

Then, build and install.

$ make -j 4
$ make install

Confirm that the library and header files are installed into ORB_SLAM2/Thirdparty/cuda-bundle-adjustment.

Edit CMakeLists.txt

Edit ORB_SLAM2/CMakeLists.txt as follows. Here, we focus on only stereo_kitti sample.

 find_package(Eigen3 3.1.0 REQUIRED)
 find_package(Pangolin REQUIRED)
+find_package(CUDA REQUIRED)
 
 include_directories(
 ${PROJECT_SOURCE_DIR}
 ${PROJECT_SOURCE_DIR}/include
 ${EIGEN3_INCLUDE_DIR}
 ${Pangolin_INCLUDE_DIRS}
+${PROJECT_SOURCE_DIR}/Thirdparty/cuda-bundle-adjustment/include
 )
 
+link_directories(${PROJECT_SOURCE_DIR}/Thirdparty/cuda-bundle-adjustment/lib)

 set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
 
 add_library(${PROJECT_NAME} SHARED
 ${EIGEN3_LIBS}
 ${Pangolin_LIBRARIES}
 ${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
 ${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
+cuda_bundle_adjustment
 )
 
-add_executable(stereo_kitti
+cuda_add_executable(stereo_kitti
 Examples/Stereo/stereo_kitti.cc)
 target_link_libraries(stereo_kitti ${PROJECT_NAME})

Create Optimizer

Let's try to replace g2o::SparseOptimizer with cuba::CudaBundleAdjustment in Optimizer::BundleAdjustment().
Include cuda_bundle_adjustment.h in Optimizer.cc.

 #include "Thirdparty/g2o/g2o/core/robust_kernel_impl.h"
 #include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h"
 #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"
+#include <cuda_bundle_adjustment.h>

Then, edit the part of creating optimizer as follows.

-g2o::SparseOptimizer optimizer;
-g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
-linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
-g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
-g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
-optimizer.setAlgorithm(solver);
-if(pbStopFlag)
-    optimizer.setForceStopFlag(pbStopFlag);

+auto optimizer = cuba::CudaBundleAdjustment::create();

+std::set<cuba::PoseVertex*> verticesP;
+std::set<cuba::LandmarkVertex*> verticesL;
+std::set<cuba::MonoEdge*> edgesMono;
+std::set<cuba::StereoEdge*> edgesStereo;

note:
cuba::CudaBundleAdjustment doesn't take responsibility for deleting pointers to vertices and edges in the graph. We need to delete the pointers after optimization by ourselves. To do so, keep the pointers in extra containers.

In optimization, there is overhead time caused by memory allocations. If optimization is called multiple times in one sequence, we can reuse optimizer object and reduce the overhead from next time.

-auto optimizer = cuba::CudaBundleAdjustment::create();
+static cuba::CudaBundleAdjustment::Ptr optimizer = nullptr;
+if (!optimizer)
+	optimizer = cuba::CudaBundleAdjustment::create();

Create Camera Parameters

In cuba::CudaBundleAdjustment, the camera parameters are associated with each of the pose vertices. We assume the obserbations are done by a single camera and the camera parameters don't change during SLAM. So we create the camera parameters from the first keyframe.

+// Create camera parameters
+cuba::CameraParams camera;
+camera.fx = vpKFs[0]->fx;
+camera.fy = vpKFs[0]->fy;
+camera.cx = vpKFs[0]->cx;
+camera.cy = vpKFs[0]->cy;
+camera.bf = vpKFs[0]->mbf;

Add Vertices

Edit the part of adding pose vertices into the optimizer as follows. Each pose vertex is represented by cuba::PoseVertex and added in the optimizer by addPoseVertex().

-g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
-vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
-vSE3->setId(pKF->mnId);
-vSE3->setFixed(pKF->mnId==0);
-optimizer.addVertex(vSE3);

+const auto pose = Converter::toSE3Quat(pKF->GetPose());
+const auto& q = pose.rotation();
+const auto& t = pose.translation();
+auto v = new cuba::PoseVertex(pKF->mnId, q, t, camera, pKF->mnId == 0);

+optimizer->addPoseVertex(v);
+verticesP.insert(v);

Edit the part of adding landmark vertices into the optimizer as follows. Each landmark vertex is represented by cuba::LandmarkVertex and added in the optimizer by addLandmarkVertex().

-g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
-vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
-const int id = pMP->mnId+maxKFid+1;
-vPoint->setId(id);
-vPoint->setMarginalized(true);
-optimizer.addVertex(vPoint);
 
+const int id = pMP->mnId + maxKFid + 1;
+const auto Xw = Converter::toVector3d(pMP->GetWorldPos());
+auto v = new cuba::LandmarkVertex(id, Xw);

+optimizer->addLandmarkVertex(v);
+verticesL.insert(v);

Use removeLandmarkVertex() when removing.

 if(nEdges==0)
 {
-    optimizer.removeVertex(vPoint);
+    optimizer->removeLandmarkVertex(v);
     vbNotIncludedMP[i]=true;
 }

Add Edges

Edit the part of adding edges with monocular observation as follows.

-Eigen::Matrix<double,2,1> obs;
-obs << kpUn.pt.x, kpUn.pt.y;
-
-g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
-
-e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
-e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
-e->setMeasurement(obs);
-const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
-e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
 
-if(bRobust)
-{
-    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
-    e->setRobustKernel(rk);
-    rk->setDelta(thHuber2D);
-}
-
-e->fx = pKF->fx;
-e->fy = pKF->fy;
-e->cx = pKF->cx;
-e->cy = pKF->cy;
-optimizer.addEdge(e);

+const float u = kpUn.pt.x;
+const float v = kpUn.pt.y;
+const float invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
+auto vertexP = optimizer->poseVertex(pKF->mnId);
+auto vertexL = optimizer->landmarkVertex(id);
+auto e = new cuba::MonoEdge({ u, v }, invSigma2, vertexP, vertexL);

+optimizer->addMonocularEdge(e);
+edgesMono.insert(e);

Edit the part of adding edges with stereo observation as follows.

-Eigen::Matrix<double,3,1> obs;
-const float kp_ur = pKF->mvuRight[mit->second];
-obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
-
-g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
-
-e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
-e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
-e->setMeasurement(obs);
-const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
-Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
-e->setInformation(Info);
 
-if(bRobust)
-{
-    g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
-    e->setRobustKernel(rk);
-    rk->setDelta(thHuber3D);
-}
 
-e->fx = pKF->fx;
-e->fy = pKF->fy;
-e->cx = pKF->cx;
-e->cy = pKF->cy;
-e->bf = pKF->mbf;
-
-optimizer.addEdge(e);

+const float u = kpUn.pt.x;
+const float v = kpUn.pt.y;
+const float ur = pKF->mvuRight[mit->second];
+const float invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
+auto vertexP = optimizer->poseVertex(pKF->mnId);
+auto vertexL = optimizer->landmarkVertex(id);
+auto e = new cuba::StereoEdge({ u, v, ur }, invSigma2, vertexP, vertexL);

+optimizer->addStereoEdge(e);
+edgesStereo.insert(e);

note:
You can apply robust kernels by adding following codes (actually bRobust is always false in the source code).

if(bRobust)
{
    optimizer->setRobustKernels(cuba::RobustKernelType::HUBER, thHuber2D, cuba::EdgeType::MONOCULAR);
    optimizer->setRobustKernels(cuba::RobustKernelType::HUBER, thHuber3D, cuba::EdgeType::STEREO);	
}

Optimize and Recover Optimized Data

Edit the part of optimization.

-optimizer.initializeOptimization();
-optimizer.optimize(nIterations);
+optimizer->initialize();
+optimizer->optimize(nIterations);

Edit the part of recovering optimized data as follows.

//Keyframes
for(size_t i=0; i<vpKFs.size(); i++)
{
    KeyFrame* pKF = vpKFs[i];
    if(pKF->isBad())
        continue;
-   g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
-   g2o::SE3Quat SE3quat = vSE3->estimate();
+   auto v = optimizer->poseVertex(pKF->mnId);
+   const g2o::SE3Quat estimate(v->q, v->t);
    if(nLoopKF==0)
    {
-       pKF->SetPose(Converter::toCvMat(SE3quat));
+       pKF->SetPose(Converter::toCvMat(estimate));
    }
    else
    {
        pKF->mTcwGBA.create(4,4,CV_32F);
-       Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA);
+       Converter::toCvMat(estimate).copyTo(pKF->mTcwGBA);
       pKF->mnBAGlobalForKF = nLoopKF;
    }
}

//Points
for(size_t i=0; i<vpMP.size(); i++)
{
    if(vbNotIncludedMP[i])
        continue;

    if(pMP->isBad())
        continue;
-   g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
+   auto v = optimizer->landmarkVertex(pMP->mnId + maxKFid + 1);

    if(nLoopKF==0)
    {
-       pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
+       pMP->SetWorldPos(Converter::toCvMat(v->Xw));
        pMP->UpdateNormalAndDepth();
    }
    else
    {
        pMP->mPosGBA.create(3,1,CV_32F);
-       Converter::toCvMat(vPoint->estimate()).copyTo(pMP->mPosGBA);
+       Converter::toCvMat(v->Xw).copyTo(pMP->mPosGBA);
        pMP->mnBAGlobalForKF = nLoopKF;
    }
}

Finalize

Delete pointers to vertices and edges. If reusing optimizer, call optimizer->clear() to reset the graph.

+for (auto v : verticesP) delete v;
+for (auto v : verticesL) delete v;
+for (auto e : edgesMono) delete e;
+for (auto e : edgesStereo) delete e;
+
+optimizer->clear();
Clone this wiki locally