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函数提取的特征要更加丰富些。
到这里,对第一帧图像的处理就算是完成大半了吧。