【SLAM】视觉SLAM十四讲-设计前端v0.3

说在前面

• 书籍参考：《视觉SLAM十四讲》高翔著
• 书籍代码地址：Github
• 数据集：TUM
• 环境搭建：【SLAM】在WSL中搭建环境(Linux子系统)
• 前篇：【SLAM】视觉SLAM十四讲-设计前端v0.2
• 其他说明：学习记录

代码理解

• g2o优化

  • 前置知识点
    《视觉SLAM十四讲》第四讲、第六讲、7.7.3、7.8

  • Bundle Adjustment
    

    重投影误差示意图-《视觉SLAM十四讲》

    重投影误差：点$p_2$是空间中3D点P在该帧中的投影，而$\widehat{p_2}$为通过相机姿态计算出来的点P的投影点；点$p_2、\widehat{p_2}$之间的误差即为重投影误差。
    问题提出：对于n个这样的点P，我们希望得到一个最佳的相机姿态，使得这n个点的平均重投影误差最小。
    解决方法：最小二乘问题的解决思路。(详细推导见书)
    对于：
    $${\xi }^{\ast }=*{argmin}_{\xi }\frac{1}{2}\sum _{i=1}^n\parallel u_i-\frac{1}{s_i}Kexp({\xi }^{˄})P_i\parallel$$
    使用优化方法前，需要求每个误差项(旋转、平移)关于优化变量的导数，也就是线性化：
    $$e(x+\Delta x)=e(x)+J\Delta x$$
    经过一系列推导，得到$J$如下
    $$\frac{\partial e}{\partial \delta \xi }=-\left[\begin{array}{cccccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}& -\frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& f_x+\frac{f_x{X}^{\prime 2}}{Z^{\prime 2}}& -\frac{f_x{Y}^{\prime }}{Z^{\prime }}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_x{Y}^{\prime }}{Z^{\prime 2}}& -f_y-\frac{f_y{Y}^{\prime 2}}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}& \frac{f_y{X}^{\prime }}{Z^{\prime }}\end{array}\right]$$
    注：平移在前旋转在后。
    然后我们还需优化特征点的空间位置。经过推导：
    $$\frac{\partial e}{\partial P}=-\left[\begin{array}{ccc}\frac{f_x}{Z^{\prime }}& 0& -\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0& \frac{f_y}{Z^{\prime }}& -\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]R$$
    其中$f_x、f_y$为相机内参，$P^{\prime }(X^{\prime },Y^{\prime },X^{\prime })$为相机坐标系中P的坐标。

• Code

  • g2otype类
    可以看到实现的正是上述的2x6的雅可比矩阵；

    void EdgeProjectXYZ2UVPoseOnly::linearizeOplus()
    {
        g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );
        g2o::SE3Quat T ( pose->estimate() );
        Vector3d xyz_trans = T.map ( point_ );
        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];
        double z_2 = z*z;
    
        _jacobianOplusXi ( 0,0 ) =  x*y/z_2 *camera_->fx_;
        _jacobianOplusXi ( 0,1 ) = - ( 1+ ( x*x/z_2 ) ) *camera_->fx_;
        _jacobianOplusXi ( 0,2 ) = y/z * camera_->fx_;
        _jacobianOplusXi ( 0,3 ) = -1./z * camera_->fx_;
        _jacobianOplusXi ( 0,4 ) = 0;
        _jacobianOplusXi ( 0,5 ) = x/z_2 * camera_->fx_;
    
        _jacobianOplusXi ( 1,0 ) = ( 1+y*y/z_2 ) *camera_->fy_;
        _jacobianOplusXi ( 1,1 ) = -x*y/z_2 *camera_->fy_;
        _jacobianOplusXi ( 1,2 ) = -x/z *camera_->fy_;
        _jacobianOplusXi ( 1,3 ) = 0;
        _jacobianOplusXi ( 1,4 ) = -1./z *camera_->fy_;
        _jacobianOplusXi ( 1,5 ) = y/z_2 *camera_->fy_;
    }
    
    

  • 相对于前一版本，主要是在使用PnP求解的时候添加了Bundle Adjustment

    1. 节点：第二个相机的位姿节点 $\xi \in se(3)$，以及所有特征点的空间位置 $P\in R3$
    2. 边：每个 3D 点在第二个相机中的投影，以观测方程来描述：
       $$zj=h(\xi ,Pj)$$

    void VisualOdometry::poseEstimationPnP()
    {
        // construct the 3d 2d observations
        vector<cv::Point3f> pts3d;
        vector<cv::Point2f> pts2d;
        
        for ( cv::DMatch m:feature_matches_ )
        {
            pts3d.push_back( pts_3d_ref_[m.queryIdx] );
            pts2d.push_back( keypoints_curr_[m.trainIdx].pt );
        }
        
        Mat K = ( cv::Mat_<double>(3,3)<<
            ref_->camera_->fx_, 0, ref_->camera_->cx_,
            0, ref_->camera_->fy_, ref_->camera_->cy_,
            0,0,1
        );
        Mat rvec, tvec, inliers;//inliers存储PnP使用到的点对数据的序号(pts3d\pts2d)
        cv::solvePnPRansac( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers );
        num_inliers_ = inliers.rows;
        cout<<"pnp inliers: "<<num_inliers_<<endl;
        T_c_r_estimated_ = SE3(
            SO3(rvec.at<double>(0,0), rvec.at<double>(1,0), rvec.at<double>(2,0)), 
            Vector3d( tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0))
        );
        
        // using bundle adjustment to optimize the pose 
        // 初始化优化器
        typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block;
        Block::LinearSolverType* linearSolver = 
        		new g2o::LinearSolverDense<Block::PoseMatrixType>();
        Block* solver_ptr = new Block( linearSolver );//这里需要改一下，见注意
        g2o::OptimizationAlgorithmLevenberg* solver = 
        		new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );//这里需要改一下，见注意
        g2o::SparseOptimizer optimizer;
        optimizer.setAlgorithm ( solver );
        
        g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
        pose->setId ( 0 );
        pose->setEstimate ( g2o::SE3Quat (
            T_c_r_estimated_.rotation_matrix(), 
            T_c_r_estimated_.translation()
        ) );
        optimizer.addVertex ( pose );
    
        // edges
        // 添加点对数据，即边数据
        for ( int i=0; i<inliers.rows; i++ )
        {
            int index = inliers.at<int>(i,0);
            // 3D -> 2D projection
            EdgeProjectXYZ2UVPoseOnly* edge = new EdgeProjectXYZ2UVPoseOnly();
            edge->setId(i);
            edge->setVertex(0, pose);
            edge->camera_ = curr_->camera_.get();
            edge->point_ = Vector3d( pts3d[index].x, pts3d[index].y, pts3d[index].z );
            edge->setMeasurement( Vector2d(pts2d[index].x, pts2d[index].y) );
            edge->setInformation( Eigen::Matrix2d::Identity() );
            optimizer.addEdge( edge );
        }
        // 开始进行优化
        optimizer.initializeOptimization();
        optimizer.optimize(10);
        
        T_c_r_estimated_ = SE3 (
            pose->estimate().rotation(),
            pose->estimate().translation()
        );
    }
    

    注意：将原代码中的下述：

     Block* solver_ptr = new Block( linearSolver );
     g2o::OptimizationAlgorithmLevenberg* solver = 
     	new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    

    改成：

    Block* solver_ptr =
    	new Block( std::unique_ptr<Block::LinearSolverType>(linearSolver) );
    g2o::OptimizationAlgorithmLevenberg* solver = 
     	new g2o::OptimizationAlgorithmLevenberg ( std::unique_ptr<Block>(solver_ptr) );
    

运行结果

• 我，，，，哭了，这个错误真的是，，，

  double free or corruption (out)
  Aborted (core dumped)
  

• 最后，还真有人找到解决方式了
  哭了/(ㄒoㄒ)/~~

• 结果