Use cuda bundle adjustment in ORB SLAM2 (old version)

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().

Install cuda-bundle-adjustment

First, build and install cuda-bundle-adjustment. 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 .. -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/DBoW2/lib)
 link_directories(${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib)
+link_directories(${PROJECT_SOURCE_DIR}/Thirdparty/cuda-bundle-adjustment/lib)

 target_link_libraries(${PROJECT_NAME}
 ${OpenCV_LIBS}
 ${EIGEN3_LIBS}
 ${Pangolin_LIBRARIES}
 DBoW2
 g2o
+cuda_bundle_adjustment
+${CUDA_cusparse_LIBRARY}
+${CUDA_cusolver_LIBRARY}
 )

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

+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();

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

limitation:
Robust kernels are currently not supported (actually bRobust is always false in the source code).

Set Camera Parameters

In cuba::CudaBundleAdjustment, the camera parameters are the same in all edges (it is assumed obserbations are done by a single camera). Set the camera parameters at first keyframe to optimizer.

+// Set 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;
+optimizer->setCameraPrams(camera);

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