DSO学习笔记三 makePixelStatus函数

一、概述

DSO对金字塔其他层提取特征过程在makePixelStatus函数中，其实已经提取过第1层和第2层的特征，这个函数绝对是重复工作吧。(理不直气还壮)

二、代码测试

代码太长，这里就只贴上makePixelStatus函数的测试代码。
主要代码引用自JakobEngel/dso，代码注释主要参考alalagong/DSO。

int makePixelStatus(Eigen::Vector3f* grads, bool* map, int w, int h, float desiredDensity, int recsLeft=5, float THFac = 1)
{
    int numGoodPoints;
    int pot = sparsityFactor;//初始为5
//    numGoodPoints = gridMaxSelection(grads, map, w, h, THFac);
    memset(map, 0, sizeof(bool)*w*h);//填0
    int numGood = 0;//选点个数
    for(int y=1;y<h-pot;y+=pot)
    {
        for(int x=1;x<w-pot;x+=pot)
        {
            int bestXXID = -1;
            int bestYYID = -1;
            int bestXYID = -1;
            int bestYXID = -1;
            float bestXX=0, bestYY=0, bestXY=0, bestYX=0;
            Eigen::Vector3f* grads0 = grads+x+y*w;//第lvl层的坐标为(x,y)的pot的左上角指针
            //遍历pot里所有像素
            for(int dx=0;dx<pot;dx++)
                for(int dy=0;dy<pot;dy++)
                {
                    int idx = dx+dy*w;//当前像素相对pot左上角的坐标位置
                    Eigen::Vector3f g=grads0[idx];
                    float sqgd = g.tail<2>().squaredNorm();//梯度平方和
                    float TH = THFac*minUseGrad_pixsel * (0.75f);
                    if(sqgd > TH*TH)//如果大于阈值，保存最大的像素位置
                    {
                        float agx = fabs((float)g[1]);//dx
                        if(agx > bestXX) {bestXX=agx; bestXXID=idx;}

                        float agy = fabs((float)g[2]);//dy
                        if(agy > bestYY) {bestYY=agy; bestYYID=idx;}

                        float gxpy = fabs((float)(g[1]-g[2]));//dx-dy
                        if(gxpy > bestXY) {bestXY=gxpy; bestXYID=idx;}

                        float gxmy = fabs((float)(g[1]+g[2]));//dx+dy
                        if(gxmy > bestYX) {bestYX=gxmy; bestYXID=idx;}
                    }
                }
            bool* map0 = map+x+y*w;//保存对应pot的指针
            //对应的最大梯度位置设置为true
            if(bestXXID>=0)
            {
                if(!map0[bestXXID])
                    numGood++;
                map0[bestXXID] = true;
            }
            if(bestYYID>=0)
            {
                if(!map0[bestYYID])
                    numGood++;
                map0[bestYYID] = true;
            }
            if(bestXYID>=0)
            {
                if(!map0[bestXYID])
                    numGood++;
                map0[bestXYID] = true;
            }
            if(bestYXID>=0)
            {
                if(!map0[bestYXID])
                    numGood++;
                map0[bestYXID] = true;
            }
        }
    }
    numGoodPoints = numGood;
    float quotia = numGoodPoints / (float)(desiredDensity);//已有像素/需要提取的像素数
    int newSparsity = (sparsityFactor * sqrtf(quotia))+0.7f;//pot尺寸修改
    if(newSparsity < 1) newSparsity=1;
    float oldTHFac = THFac;
    if(newSparsity==1 && sparsityFactor==1) THFac = 0.5;//pot=1后数量不够，则减小阈值

    if((abs(newSparsity-sparsityFactor) < 1 && THFac==oldTHFac) ||
            ( quotia > 0.8 &&  1.0f / quotia > 0.8) ||
            recsLeft == 0)
    {
        // 画出选择结果
    //    /*
        bool plot =true;
        if(plot)//
        {
            Mat img_select_result = Mat::zeros(h, w, CV_8UC3);
            for(int y=0; y<h;y++)
                for(int x=0;x<w;x++)
                {
                    float c = grads[x+y*w][0]*0.7;
                    if(c>255) c=255;
                    img_select_result.at<Vec3b>(y,x) = Vec3b(c,c,c);
                }
            imshow("I " + to_string(L), img_select_result);
            for(int y=0; y<h;y++)
                for(int x;x<w;x++)
                {
                    int pi=x+y*w;
                    if(map[pi] == true)
                    {
                        img_select_result.at<Vec3b>(y,x)[0] = 255;
                    }
                    else
                        continue;
                }
    //        cout <<"numGoodPoints = "<< numGoodPoints <
            imshow("lv " + to_string(L), img_select_result);
        }
        L++;
//		printf(" \n");
        //all good
        sparsityFactor = newSparsity;//更新尺寸
        return numGoodPoints;//返回提取的特征个数
    }
    else
    {
//		printf(" -> re-evaluate! \n");
        // re-evaluate.
        sparsityFactor = newSparsity;
        return makePixelStatus(grads, map, w,h, desiredDensity, recsLeft-1, THFac);
    }
}

三、测试结果

和makeMaps函数相比（右图的绿点和红点），makePixelStatus函数提取的特征要更加丰富些。
到这里，对第一帧图像的处理就算是完成大半了吧。