第八讲 direct_sparse.cpp

#include

#include

#include

#include

#include

#include

#include

 

#include

#include

#include

#include

 

#include

#include

#include

#include

#include

#include

 

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;

};

 

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

}

 

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

//将一个3d点投影到一个图像平面，误差是光度误差

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

    {}//自定义构造函数，参数为:

    //一个3d点世界坐标系下坐标

    //内参矩阵的4个参数

    //参考图，变换后的图

 

    virtual void computeError()

    {

        const VertexSE3Expmap* v  =static_cast<const VertexSE3Expmap*> ( _vertices[0] );//顶点数组中取出顶点，转换成位姿指针类型

//从世界坐标系下坐标到像素坐标：

        //位姿估计值.map()函数即为乘上位姿T,这里其实为3d点世界坐标乘上相机位姿，计算出当前相机坐标系下的坐标

        Eigen::Vector3d x_local = v->estimate().map ( x_world_ );//根据估计位姿算出的世界坐标

        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

//检查像素是否还在图像中，这里靠近边缘有4像素时就认为已经出了图像，将误差设置为0,此条边的Level设置为１，用于区分

        if ( x-4<0 || ( x+4 ) >image_->cols || ( y-4 ) <0 || ( y+4 ) >image_->rows )

        {

            _error ( 0,0 ) = 0.0;

            this->setLevel ( 1 );

        }

        else

        {//这里误差为标量(光度值的差值)，用估计出来的(u.v)处灰度值(双线性插值)，减去测量值。

            //这里的getPixelValue(u,v)相当于I(u,v)

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

        }

    }

 

    // plus in manifold    //计算线性增量，也就是雅克比矩阵J

    virtual void linearizeOplus( )

    {

        if ( level() == 1 ) //先判断一下此条边对应的像素点是不是出了像素平面，出了就直接设置为０。

        {

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

            return;

        }

//如果没出，正常的点

        //取出位姿估计，求出在此位姿相机坐标系下的空间点坐标，xyz_trans也就是P195页的q

        VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*> ( _vertices[0] );

        Eigen::Vector3d xyz_trans = vtx->estimate().map ( x_world_ );   // q in book

//取出q的x和y值

        double x = xyz_trans[0];

        double y = xyz_trans[1];

//这里将z值取出为倒数和倒数的平方，因为后面构造jacobian_uv_ksai时，涉及到z值都是用的倒数和倒数平方，所以这里就直接取倒数了

        double invz = 1.0/xyz_trans[2];

        double invz_2 = invz*invz;

  //进一步取得像素坐标，用于后面求取像素梯度

        float u = x*fx_*invz + cx_;

        float v = y*fy_*invz + cy_;

//整个雅克比矩阵不多说，分两部分，一部分是像素坐标对变换部分偏导，一部分是像素梯度部分偏导，两部分乘在一起，书上P195页

        // 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;//像素坐标对变换部分偏导

 

        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;//像素梯度部分偏导

 //这里注意一下像素梯度的求法，像素梯度是一个平面二维向量，第一个为u方向，第二个为v方向

        //这里由于各个像素点其实是离散值，其实求的是差分，前一个像素灰度值减后一个像素灰度值，除以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;//式8。16

    }

 

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

    inline float getPixelValue ( float x, float y )//灰度值函数是对图像进行双线性插值得到的。所谓的双线性插值

    {//这里先说一下各个参数的类型：

        //image_为Mat*类型，图像指针，所以调用data时用->符号，

        //data为图像矩阵首地址，支持数组形式访问，data[]就是访问到像素的值了，此处为像素的灰度值，类型为uchar

        //关于step有点复杂，data[]中括号的式子有点复杂，总的意思就是y行乘上每行内存数，定位到行，然后在加上x，定位到像素

        //step具体解释在最后面有一些资料

        //image_->data[int(y)*image_->step + int(x)]这一步读到了x,y处的灰度值，类型为uchar，

        //但是后面由于线性插值，需要定位这个像素的位置，而不是他的灰度值，所以取其地址，赋值给data_ptr，记住它的位置，后面使用

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

        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]

               );

    }

//这里说一下自定义边类型时的成员变量怎么来，不是随便写，而是误差需要哪些变量算出来，就定义哪些。

    //这里需要世界坐标系下的空间点坐标，相机内参，和第二帧图

    //这里说一下这个第二帧图：空间点经RT,经内参投影到第二帧图(image_)上，在这个image_上找像素的灰度值，这个灰度值是估计值

    //而测量值在前一帧上，也就是上面的_measurement，在main()函数中直接赋值给到。

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;//这里设置了单位换算比例，RGBD相机出来的单位是毫米，而三维世界空间点坐标单位是米

    Eigen::Matrix3f K;

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

 

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

 

    cv::Mat prev_color; //这里的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();

            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;

 //int cvRound (double value); 对一个double型的数进行四舍五入，并返回一个整型数

                //寻找关键点位置上，深度图上的深度值

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

                if ( d==0 )

                    continue;

    //深度值找到后，进一步反投影得到空间点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 ) ] );

//最后这里，得到了三维坐标和对应的灰度值，一个测量值就搞定了，被推进measurements数组，

                //最终第一帧所有关键点对应空间中点的坐标和灰度值，被压入measurements数组

                //整个过程第一帧就是世界坐标系下拍到的空间点，通过像素位置和相机内参求得世界坐标系下坐标，通过灰度图求得灰度

                //后面求取光度误差时，其实就只要这两个数据

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

            }

            prev_color = color.clone();//第一帧，赋值给前一张变量prev_color

            continue;

        }

        // 使用直接法计算相机运动  //整个程序中，只在第一帧被写入，其他地方均是调用，所以后续的帧均是以第一帧为参考帧去估计位姿，并不是以前一帧

        //因为这个函数循环执行时，measurements是不变的，只有不断读入的&gray灰度图是变化的。

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

        chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

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

        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 )//这里是随机选20%的关键点在图上显示，rand()生成0-RAND_MAX之间的一个数，如果这个数比RAND_MAX/5大就continue掉，不画了。

            //RAND_MAX/5=RAND_MAX*20%,也就是说每个点有20%的概率被留下，所以最终也即是随机选了20%的点

                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;

 

            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;

}

//这个函数，就是用直接法求相机位姿，

//measurements为包含了很多测量点的数组，测量点有空间坐标和对应灰度值信息，

//Measurement应该就是P193页图8-3的P和I1(p1),这个归到测量值里面，因为P193页式(8.10)来看，测量值-估计值，那么前一张图的光度，就是测量值

//cv::Mat*　gray 为一张灰度图，这张图就是第二帧。但并不是估计值，而是通过位姿估计出来一个像素坐标，用这个坐标在这张图上去查找估计值(也就是坐标对应的光度)。

//相机内参和输出位姿。注意这里位姿表示形式为：Eigen::Isometry3d&

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

{

    // 初始化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::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 ); //这些边(误差)对应的顶点都是ID为0的那一个pose顶点

//这里看出来了，测量值就是第一帧中的灰度值，对应边类型定义中computeError()函数中的_measurement变量，直接取的第一帧空间点的灰度值

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

        //进一步说一下，书上(8.10)是测量值减去估计值，而这里computeError()函数中的_error(0,0)=getPixelValue ( u,v )-_measurement;

        //很明显是估计值减去测量值，所以在雅克比矩阵_jacobianOplusXi中，并没有书上式(8.16)的负号，因为误差定义反过来了。

        edge->setMeasurement ( m.grayscale );

        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;

    optimizer.initializeOptimization();

    optimizer.optimize ( 30 );

    Tcw = pose->estimate();

}