欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

视觉SLAM14讲:视觉里程计2

程序员文章站 2022-04-17 16:37:49
...

视觉SLAM14讲:视觉里程计2

8.5.1 稀疏直接法

运行结果

视觉SLAM14讲:视觉里程计2

代码及注释

#include <iostream> //输入输出
#include <fstream> //文件流
#include <list>
#include <vector>
#include <chrono> //计时
#include <ctime>
#include <climits>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp> //图像处理
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp> //二维特征

#include <g2o/core/base_unary_edge.h> //一元边
#include <g2o/core/block_solver.h> //矩阵块处理器,矩阵分解
#include <g2o/core/optimization_algorithm_levenberg.h> //LM数字优化算法
#include <g2o/solvers/dense/linear_solver_dense.h> //线性方程求解其
#include <g2o/core/robust_kernel.h> //核
#include <g2o/types/sba/types_six_dof_expmap.h> //顶点类型,6*度位姿

using namespace std;
using namespace g2o;

/********************************************
 * 本节演示了RGBD上的稀疏直接法 
 ********************************************/

// 一次测量的值,包括一个世界坐标系下三维点与一个灰度值
struct Measurement
{
    Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {}
    Eigen::Vector3d pos_world;
    float grayscale;
};

// 2D坐标 由 相机内参数 和深度信息 得到 像极坐标系下的三维坐标
// 相机归一化 平面下坐标 x =  K逆* px  y =  K逆* py  相机坐标系下 归一化平面上的点
//相机内参数 K=
//   [fx 0 cx
//     0 fy cy
//     0 0  1]
// x= (px -cx)/fx
// y=(py-cy)/fy
// z = 1
// 而深度值 为 dd= d/scale   mm 转成m
// 则相机坐标系下的坐标为  xx = x*dd  yy = y * dd  zz = z * dd


//  相机坐标系下 像素坐标(x, y)转化成 三维坐标 (x, y, z)
//  坐标 xx = zz* ( x-cx ) /fx;
//  坐标 yy = zz* ( y-cy ) /fy;
inline Eigen::Vector3d project2Dto3D ( int x, int y, int d, float fx, float fy, float cx, float cy, float scale )
{
    float zz = float ( d ) /scale; // 深度 mm 转成m
    float xx = zz* ( x-cx ) /fx;
    float yy = zz* ( y-cy ) /fy;
    return Eigen::Vector3d ( xx, yy, zz );
}

//  相机坐标系下 三维坐标 (x, y, z)转化成 像素坐标(u, v)
//  像素坐标 x = xx/zz * fx  + cx
//  像素坐标 y = yy/zz * fy  + cy
inline Eigen::Vector2d project3Dto2D ( float x, float y, float z, float fx, float fy, float cx, float cy )
{
    float u = fx*x/z+cx;
    float v = fy*y/z+cy;
    return Eigen::Vector2d ( u,v );
}

// 直接法估计位姿
// 输入:测量值(空间点的灰度),新的灰度图,相机内参; 输出:相机位姿
// 返回:true为成功,false失败
bool poseEstimationDirect ( const vector<Measurement>& measurements, cv::Mat* gray, Eigen::Matrix3f& intrinsics, Eigen::Isometry3d& Tcw );


// project a 3d point into an image plane, the error is photometric error
// an unary edge with one vertex SE3Expmap (the pose of camera)
// 参数: 测量值维度(灰度值)  数据类型   连接顶点类型
// 基础一元边  连接相机位姿 顶点
class EdgeSE3ProjectDirect: public BaseUnaryEdge< 1, double, VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeSE3ProjectDirect() {}

    EdgeSE3ProjectDirect ( Eigen::Vector3d point, float fx, float fy, float cx, float cy, cv::Mat* image )
        : x_world_ ( point ), fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), image_ ( image )
    {}
    //计算误差  覆写计算误差的 虚函数
    virtual void computeError()
    {
        // //类型强转  相机位姿
        const VertexSE3Expmap* v  =static_cast<const VertexSE3Expmap*> ( _vertices[0] );
        Eigen::Vector3d x_local = v->estimate().map ( x_world_ ); //第二帧下 相机坐标系下的三维坐标x_local[0],x_local[1],x_local[2]
        // 相机坐标系下 三维坐标 (x, y, z)转化成 像素坐标(u, v)
        float x = x_local[0]*fx_/x_local[2] + cx_;
        float y = x_local[1]*fy_/x_local[2] + cy_;
        // check x,y is in the image
        if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )
        {
            _error ( 0,0 ) = 0.0;
            this->setLevel ( 1 ); // 不考虑该边
        }
        else
        {
            // 经过在灰度图中插值获取得的像素值减去测量值
            // _measurement是g2o中的定义
            // 这里 误差为e =   I(p2) - I2(p1)   原来e =  I(p1) - I2(p2)  所以 雅克比 相差一个 负号
            // 根据 像素坐标  和 灰度图得到 灰度值  - 测量值
            // 上面计算得到的xy为浮点数,需要得到 整数形式 的 坐标值 对应图像的亮度值 需要进行插值运算
            _error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;
        }
    }

    // plus in manifold
    virtual void linearizeOplus( )
    {
        if ( level() == 1 )
        {
            _jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();
            return;
        }
        VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );
        // 在第二张图片上的关键点的空间点坐标xyz_trans
        Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );   // q in book

        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double invz = 1.0/xyz_trans[2];
        double invz_2 = invz*invz;

        float u = x*fx_*invz + cx_;
        float v = y*fy_*invz + cy_;

        // jacobian from se3 to u,v
        // NOTE that in g2o the Lie algebra is (\omega, \epsilon), where \omega is so(3) and \epsilon the translation
        Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;
        // 这里的计算和课本不一样
        // 旋转在前 平移在后   g2o   u对∇f的偏导数  像素坐标 对 变换矩阵李代数增量 的导数
        // J1=  [ fx *X*Y/Z^2        -fx *(1 + X^2/Z^2)   fx*Y/Z  -fx/Z   0      fx * X/Z^2
        //       fy *(1 + Y^2/Z^2)  -fy * X*Y/Z^2        -fy*X/Z   0    -fy/Z   fy* Y/Z^2     ]
        // 上面 误差为e =   I(p2) - I2(p1)   原来e =  I(p1) - I2(p2)  所以 雅克比 相差一个 负号

        jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
        jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
        jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
        jacobian_uv_ksai ( 0,3 ) = invz *fx_;
        jacobian_uv_ksai ( 0,4 ) = 0;
        jacobian_uv_ksai ( 0,5 ) = -x*invz_2 *fx_;

        jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
        jacobian_uv_ksai ( 1,1 ) = x*y*invz_2 *fy_;
        jacobian_uv_ksai ( 1,2 ) = x*invz *fy_;
        jacobian_uv_ksai ( 1,3 ) = 0;
        jacobian_uv_ksai ( 1,4 ) = invz *fy_;
        jacobian_uv_ksai ( 1,5 ) = -y*invz_2 *fy_;

        Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;


        // I2对u偏导   J2   图像灰度梯度可以得到(x方向梯度 y方向梯度 离散求解 坐标前后灰度值作差/2)
        //这里由于各个像素点其实是离散值,其实求的是差分,前一个像素灰度值减后一个像素灰度值,除以2,即认为是这个方向上的梯度

        jacobian_pixel_uv ( 0,0 ) = ( getPixelValue ( u+1,v )-getPixelValue ( u-1,v ) ) /2;
        jacobian_pixel_uv ( 0,1 ) = ( getPixelValue ( u,v+1 )-getPixelValue ( u,v-1 ) ) /2;

        _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
    }

    // dummy read and write functions because we don't care...
    virtual bool read ( std::istream& in ) {}
    virtual bool write ( std::ostream& out ) const {}

protected:
    // get a gray scale value from reference image (bilinear interpolated)
    // 双线性插值
    // a+bx+cy+dxy.
    // a=(1-xx)*(1-yy),b=xx*(1-yy),c=yy(1-xx),d=xx*yy.其中xx=x-deltax,y=y-deltay.
    inline float getPixelValue ( float x, float y )
    {
        // 灰度值数据访问形式
        // deltax为floor(x),deltay=floor(y).1,x,y,xy分别为data[0],data[1],data[image_->step],data[image_->step+1]
        // image_为Mat*类型,图像指针,所以调用data时用->符号,


        //image_->data[int(y)*image_->step + int(x)]这一步读到了x,y处的灰度值,类型为uchar,
        //但是后面由于线性插值,需要定位这个像素的位置,而不是他的灰度值,所以取其地址,赋值给data,记住它的位置,后面使用


        uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];

        //由于x,y这里有可能带小数,但是像素位置肯定是整数,所以,问题来了,(1.2, 4.5)像素坐标处的灰度值为多少呢?OK,线性插值!
        //说一下floor(),std中的cmath函数。向下取整,返回不大于x的整数。例floor(4.9)=4
        //xx和yy,就是取到小数部分。例:x=4.9的话,xx=x-floor(x)就为0.9。y同理
        float xx = x - floor ( x );
        float yy = y - floor ( y );
        //公式f(i+u,j+v)=(1-u)(1-v)f(i,j)+u(1-v)f(i+1,j)+(1-u)vf(i,j+1)+uvf(i+1,j+1)
        //这里的u为xx,v为yy.
        //data为图像矩阵首地址,支持数组形式访问,data[]就是访问到像素的值了,此处为像素的灰度值,类型为uchar
        //关于step有点复杂,data[]中括号的式子有点复杂,总的意思就是y行乘上每行内存数,定位到行,然后在加上x,定位到像素
        //f(i,j)是取像素坐标的灰度值,那么f(i,j)=data[0],f(i+1,j)=data[1],f(i,j+1)=data[ image_->step ],f(i+1,j+1)=data[image_->step+1]

        return float (
                   ( 1-xx ) * ( 1-yy ) * data[0] +
                   xx* ( 1-yy ) * data[1] +
                   ( 1-xx ) *yy*data[ image_->step ] +
                   xx*yy*data[image_->step+1]
               );
    }
public:
    Eigen::Vector3d x_world_;   // 3D point in world frame
    float cx_=0, cy_=0, fx_=0, fy_=0; // Camera intrinsics
    cv::Mat* image_=nullptr;    // reference image
};

int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"usage: useLK path_to_dataset"<<endl;
        return 1;
    }
    srand ( ( unsigned int ) time ( 0 ) );
    string path_to_dataset = argv[1];
    string associate_file = path_to_dataset + "/associate.txt";

    ifstream fin ( associate_file );

    string rgb_file, depth_file, time_rgb, time_depth;
    cv::Mat color, depth, gray;
    vector<Measurement> measurements;
    // 相机内参
    float cx = 325.5;
    float cy = 253.5;
    float fx = 518.0;
    float fy = 519.0;
    float depth_scale = 1000.0; // mm  变成 m 的换算单位
    Eigen::Matrix3f K;
    K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;

    //相机位姿 [R t] 的齐次表示 4*4
    Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();

    cv::Mat prev_color;
    // 我们以第一个图像为参考,对后续图像和参考图像做直接法
    // 在结果中看到的是,第一幅图固定,然后找其与其他图像的特征点匹配关系
    for ( int index=0; index<10; index++ )
    {
        cout<<"*********** loop "<<index<<" ************"<<endl;
        fin>>time_rgb>>rgb_file>>time_depth>>depth_file;
        color = cv::imread ( path_to_dataset+"/"+rgb_file );
        depth = cv::imread ( path_to_dataset+"/"+depth_file, -1 );
        if ( color.data==nullptr || depth.data==nullptr )
            continue;
        // 将彩色图转换为灰度图
        cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );
        if ( index ==0 )
        {
            // 对第一帧提取FAST特征点
            vector<cv::KeyPoint> keypoints;
            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();
            // 检测color这幅图像里面的特征点,并返回特征点值
            detector->detect ( color, keypoints );
            for ( auto kp:keypoints )
            {
                // 去掉邻近边缘处的点
                if ( kp.pt.x < 20 || kp.pt.y < 20 || ( kp.pt.x+20 ) >color.cols || ( kp.pt.y+20 ) >color.rows )
                    continue;
                ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];
                if ( d==0 )
                    continue;
                // 2D像素坐标(u,v)转换成相机坐标系下的 三维点 3D
                Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );
                // 特征点对应的灰度值,这里的gray是上面转换后的灰度图,注意坐标值为整数,需要用cvRound进行取整
                float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );
                // 储存的测量值为三维点和对应的灰度值
                measurements.push_back ( Measurement ( p3d, grayscale ) );
            }
            prev_color = color.clone();
            continue;
        }
        // 使用直接法计算相机运动
        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
        poseEstimationDirect ( measurements, &gray, K, Tcw );  // 用g2o进行优化,采用LM算法
        chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
        chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
        cout<<"direct method costs time: "<<time_used.count() <<" seconds."<<endl;
        cout<<"Tcw="<<Tcw.matrix() <<endl;

        // plot the feature points
        cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );
        prev_color.copyTo ( img_show ( cv::Rect ( 0,0,color.cols, color.rows ) ) );
        color.copyTo ( img_show ( cv::Rect ( 0,color.rows,color.cols, color.rows ) ) );
        // 每个测量值都可以构造一个边。所以构造一个循环
        for ( Measurement m:measurements )
        {
            if ( rand() > RAND_MAX/5 )
                continue;
            // 测量值保存的三维点
            Eigen::Vector3d p = m.pos_world;
            // 转换为上一帧的2d像素坐标
            Eigen::Vector2d pixel_prev = project3Dto2D ( p ( 0,0 ), p ( 1,0 ), p ( 2,0 ), fx, fy, cx, cy );
            // 将第一帧变换到第二帧图像坐标系下
            Eigen::Vector3d p2 = Tcw*m.pos_world;
            // 转换为当前帧的2d像素坐标
            Eigen::Vector2d pixel_now = project3Dto2D ( p2 ( 0,0 ), p2 ( 1,0 ), p2 ( 2,0 ), fx, fy, cx, cy );
            if ( pixel_now(0,0)<0 || pixel_now(0,0)>=color.cols || pixel_now(1,0)<0 || pixel_now(1,0)>=color.rows )
                continue;
			// 随机选择绘制的颜色
            float b = 255*float ( rand() ) /RAND_MAX;
            float g = 255*float ( rand() ) /RAND_MAX;
            float r = 255*float ( rand() ) /RAND_MAX;
            // 将上一帧的特征圈出
            cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 8, cv::Scalar ( b,g,r ), 2 );
            // 将当前帧的特征圈出
            cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 8, cv::Scalar ( b,g,r ), 2 );
            // 用线连起来
            cv::line ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), cv::Scalar ( b,g,r ), 1 );
        }
        cv::imshow ( "result", img_show );
        cv::waitKey ( 0 );

    }
    return 0;
}

bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{
    // old version, 初始化g2o
//    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock;  // 求解的向量是6*1的
//    DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
//    DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
//    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N
//    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
//    g2o::SparseOptimizer optimizer;
//    optimizer.setAlgorithm ( solver );
//    optimizer.setVerbose( true );

    // 新版本g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,1> > DirectBlock;  // 求解的向量是6*1的
    // 第1步:创建一个线性求解器LinearSolver
    DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense<DirectBlock::PoseMatrixType>();
    // 第2步:创建BlockSolver。并用上面定义的线性求解器初始化
    DirectBlock* solver_ptr = new DirectBlock (  std::unique_ptr<DirectBlock::LinearSolverType>(linearSolver) );
    // 第3步:创建总求解器solver。并从GN, LM, DogLeg 中选一个,再用上述块求解器BlockSolver初始化
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::unique_ptr<DirectBlock>(solver_ptr) );
    // 第4步:创建稀疏优化器
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );
    optimizer.setVerbose( true );

    // 添加顶点
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    // 旋转矩阵和平移向量
    pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );
    pose->setId ( 0 );
    // 加入顶点
    optimizer.addVertex ( pose );

    // 添加边
    int id=1;
    for ( Measurement m: measurements )
    {
        // 三维坐标
        // 相机内参,灰度图
        EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect (
            m.pos_world,
            K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray
        );
        edge->setVertex ( 0, pose ); // 顶点
        edge->setMeasurement ( m.grayscale ); // 将真实灰度值设置为测量值
        edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() ); // 误差权重的信息矩阵
        edge->setId ( id++ );
        optimizer.addEdge ( edge );
    }
    cout<<"edges in graph: "<<optimizer.edges().size() <<endl;
    optimizer.initializeOptimization(); // 优化初始值
    optimizer.optimize ( 30 ); // 最大优化次数
    Tcw = pose->estimate(); // 输出变换矩阵
}

参考:《视觉SLAM十四讲》高翔