八、直接法(semidense与sparse)_RGBD

semidense

#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>

#include <g2o/solvers/dense/linear_solver_dense.h>

#include <g2o/core/robust_kernel.h>

#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace std;

using namespace g2o;

1. 坐标转换project2Dto3D和project3Dto2D

// 一次测量的值，包括一个世界坐标系下三维点与一个灰度值

struct Measurement

{

    Measurement ( Eigen::Vector3d p, float g ) : pos_world ( p ), grayscale ( g ) {} //初始化构造函数

    Eigen::Vector3d pos_world;

    float grayscale;

};

// 2d像素坐标到3D坐标，RGBD照片d单位毫米，空间点zz单位米，scale单位换算

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;

    float xx = zz* ( x-cx ) /fx;

    float yy = zz* ( y-cy ) /fy;

    return Eigen::Vector3d ( xx, yy, zz );

}

//3D到2d像素坐标RGB照片

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

}

2、直接法估计相机运动poseEstimationDirect（使用g2o）

// 输入：测量值（空间点的灰度），新的灰度图，相机内参； 输出：相机位姿

// 返回：true为成功，false失败

bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )

{

   // 1. 初始化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::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M

    g2o::SparseOptimizer optimizer;

    optimizer.setAlgorithm ( solver );

    optimizer.setVerbose( true );

// 2.添加顶点相机位姿李代数pose

    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();

    pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );

    pose->setId ( 0 );

    optimizer.addVertex ( pose );

 // 3. 添加边，光度误差，一帧图上很多像素代表很多一元边

    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 );//这些边（误差），对应的顶点都是ID为0那一个pose顶点

//整个过程测量值只有第一帧的灰度值，后面的每一帧根据位姿找出像素点，再找到灰度值，都是估计值

        edge->setMeasurement ( m.grayscale );

//信息矩阵设置为单位阵，表征每个边的权重都一样

        edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );

        edge->setId ( id++ );//依次增加，给边设置一个ID

        optimizer.addEdge ( edge );

    }

    cout<<"edges in graph: "<<optimizer.edges().size() <<endl;

// 4. 开始优化

    optimizer.initializeOptimization();

    optimizer.optimize ( 30 );

    Tcw = pose->estimate();

}

3.直接法位姿估计的边，构造图优化 EdgeSE3ProjectDirect

//自定义的光度误差 边EdgeSE3ProjectDirect，顶点为g2o中李代数位姿节点VertexSE3Expmap

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 )

    {}

//光度误差计算，3D点投影到图像平面

    virtual void computeError()

    {

//v是位姿pose,     x_local是3D估计值

        const VertexSE3Expmap* v  =static_cast<const VertexSE3Expmap*> ( _vertices[0] );//一元边

        Eigen::Vector3d x_local = v->estimate().map ( x_world_ );

//3D到2D像素点转换（u,v）

        float x = x_local[0]*fx_/x_local[2] + cx_; //x_local为估计值Vector 3d类型的（x,y,z）

        float y = x_local[1]*fy_/x_local[2] + cy_;

 //检查x y是否出界，距离图像四条边4个像素大小内为有效区域

        if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )//rows行；cols列

        {

            _error ( 0,0 ) = 0.0;

            this->setLevel ( 1 );//! sets the level of the edge

        }

        else

        {

//经过在灰度图中插值获取得的灰度值getPixelValue（x,y）减去测量值灰度值

            _error ( 0,0 ) = getPixelValue ( x,y ) - _measurement;

        }

    }

//  计算线性增量，雅可比矩阵J

    virtual void linearizeOplus( )

    {

//先判断是否出界，重置

        if ( level() == 1 )

        {

            _jacobianOplusXi = Eigen::Matrix<double, 1, 6>::Zero();

            return;

        }

// 位姿估计，得到空间坐标系3D坐标

        VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );//相机pose顶点

        Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );   // q in book（x,y,z）

//3D转换为2D像素坐标

        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（注意g2o的jacobian矩阵前3列与后3列相对于书上公式是对调的）

        Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;

//公式后三列

        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;

        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;//整体最终jacobian矩阵

    }

    // 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)getPixelValue函数通过双线性差值获得浮点坐标对应插值后的像素值

    inline float getPixelValue ( float x, float y )

    {

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

        float xx = x - floor ( x );

        float yy = y - floor ( y );

        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:

//变量声明，3D点、内参、灰度图像指针image

    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;

//Measurement类存储世界坐标点（以第一帧为参考的FAST关键点）和对应的灰度图像（由color->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;//RGBD相机单位毫米，3D点单位米

    Eigen::Matrix3f K;

    K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;

// 位姿，变换矩阵T

    Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();

    cv::Mat prev_color;

    // 我们以第一个图像为参考，对后续图像和参考图像做直接法

  //每一副图像都会与第一帧图像做直接法计算第一帧到当前帧的R,t。

// 参考帧一直是第一帧，而不是循环流动当前帧的上一帧为参考帧

  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 );//-1 按原图像的方式存储 detph 16位存储

        if ( color.data==nullptr || depth.data==nullptr )

            continue;

//转换后的灰度图为g2o优化需要的边提供灰度值

//将颜色图3通道 转换为灰度图单通道 8位无符号 对应边类的双线性插值计算放法以单通道计算的

        cv::cvtColor ( color, gray, cv::COLOR_BGR2GRAY );//转化为灰度图

        if ( index ==0 )

        {

            // select the pixels with high gradiants

            for ( int x=10; x<gray.cols-10; x++ )

                for ( int y=10; y<gray.rows-10; y++ )

                {

                    Eigen::Vector2d delta (//计算像素坐标系下 两个方向的变化量

                        gray.ptr<uchar>(y)[x+1] - gray.ptr<uchar>(y)[x-1], //opencv图像y行x列

                        gray.ptr<uchar>(y+1)[x] - gray.ptr<uchar>(y-1)[x]

                    );

                    if ( delta.norm() < 50 )  //自定义阈值，梯度大小

                        continue;

                    ushort d = depth.ptr<ushort> (y)[x];  //16位深度图

                    if ( d==0 )

                        continue;

                    Eigen::Vector3d p3d = project2Dto3D ( x, y, d, fx, fy, cx, cy, depth_scale );

                    float grayscale = float ( gray.ptr<uchar> (y) [x] );

                    measurements.push_back ( Measurement ( p3d, grayscale ) );

                }

            prev_color = color.clone();

            cout<<"add total "<<measurements.size()<<" measurements."<<endl;

            continue;

        }

        // 使用直接法计算相机运动

        poseEstimationDirect ( measurements, &gray, K, Tcw );

        cout<<"Tcw="<<Tcw.matrix() <<endl;   //Isometry3d转为matrix输出

        // plot the feature points

//两张对比图拼接显示

        cv::Mat img_show ( color.rows*2, color.cols, CV_8UC3 );

//  cv::Rect()  行，列到行，列的矩形区域

        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;

//上一帧空间点3D坐标

            Eigen::Vector3d p = m.pos_world;

//上一帧坐标系下的图像像素坐标

            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;

//当前帧坐标系下的图像像素坐标

            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 = 0;

            float g = 250;

            float r = 0;

//上图

            img_show.ptr<uchar>( pixel_prev(1,0) )[int(pixel_prev(0,0))*3] = b;

            img_show.ptr<uchar>( pixel_prev(1,0) )[int(pixel_prev(0,0))*3+1] = g;

            img_show.ptr<uchar>( pixel_prev(1,0) )[int(pixel_prev(0,0))*3+2] = r;

//下图    

            img_show.ptr<uchar>( pixel_now(1,0)+color.rows )[int(pixel_now(0,0))*3] = b;

            img_show.ptr<uchar>( pixel_now(1,0)+color.rows )[int(pixel_now(0,0))*3+1] = g;

            img_show.ptr<uchar>( pixel_now(1,0)+color.rows )[int(pixel_now(0,0))*3+2] = r;

/****************************************/

 //在img_show包含两帧图像上 以关键点为圆心画圆 半径为4个像素 颜色为bgr随机组合  2表示外轮廓线宽度为2 如果为负数则表示填充圆

            cv::circle ( img_show, cv::Point2d ( pixel_prev ( 0,0 ), pixel_prev ( 1,0 ) ), 4, cv::Scalar ( b,g,r ), 2 );//画圆

            cv::circle ( img_show, cv::Point2d ( pixel_now ( 0,0 ), pixel_now ( 1,0 ) +color.rows ), 4, 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 ( 0,0,250 ), 1 );

     

  cv::imshow ( "result", img_show );

        cv::waitKey ( 0 );

    }

    return 0;

}

/-----------------------------------------------------------------------------------------------------------------------------------/

sparse直接法

只有main函数略有不同

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;

//Measurement类存储世界坐标点（以第一帧为参考的FAST关键点）和对应的灰度图像（由color->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;//RGBD相机单位毫米，3D点单位米

    Eigen::Matrix3f K;

    K<<fx,0.f,cx,0.f,fy,cy,0.f,0.f,1.0f;

// 位姿，变换矩阵T

    Eigen::Isometry3d Tcw = Eigen::Isometry3d::Identity();

    cv::Mat prev_color；

// 我们以第一个图像为参考，对后续图像和参考图像做直接法

//每一副图像都会与第一帧图像做直接法计算第一帧到当前帧的R,t。

// 参考帧一直是第一帧，而不是循环流动当前帧的上一帧为参考帧

    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特征点

//遍历第一帧特征点数组，筛选，把3D坐标和灰度值放到measurement中

            vector<cv::KeyPoint> keypoints;

            cv::Ptr<cv::FastFeatureDetector> detector = cv::FastFeatureDetector::create();

            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;

//depth.ptr<ushort>( kp.pt.y)获取行指针  cvRound(kp.pt,y) 表示返回跟参数值最接近的整数值 因为像素量化后是整数，而kp.pt.y存储方式是float，所以强制转换一下即可

                ushort d = depth.ptr<ushort> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ];

                if ( d==0 )

                    continue;

// 找到深度值d后，2D点投影到3D世界坐标系

                Eigen::Vector3d p3d = project2Dto3D ( kp.pt.x, kp.pt.y, d, fx, fy, cx, cy, depth_scale );

// 灰度图上关键点灰度值

                float grayscale = float ( gray.ptr<uchar> ( cvRound ( kp.pt.y ) ) [ cvRound ( kp.pt.x ) ] );

//得到三维3D坐标和对应的灰度值，作为测量值

                measurements.push_back ( Measurement ( p3d, grayscale ) );

            }

            prev_color = color.clone();

            continue;

        }

        // 使用直接法计算相机运动

//measurements是不变的，之后不断读入的fray灰度图变化

        poseEstimationDirect ( measurements, &gray, K, Tcw );

       

        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;

            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;

            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;

//随机获取bgr颜色 在cv::circle中 为关键点用不同的颜色圆来画出

            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;

}