DSO学习笔记二 makeMaps函数
目录
- 一、概述
- 二、代码测试
-
- 测试结果
- 问题
-
- 更新：问题
一、概述
接着上一次的学习。DSO在完成输入图像、计算金字塔等图像处理操作后，会进入初始化流程。初始化对于DSO而言非常重要，选择不同的初始化图像，最后DSO的效果也会有差别。
在探索过程中，我遇到名为makeMaps的函数，虽然这个函数的设计非常巧妙，但是还是有很多地方让我很迷惑。
二、代码测试
这里先把测试makeMaps的代码贴上。主要代码引用自JakobEngel/dso，代码注释主要参考alalagong/DSO。
#include (y,x)=Vec3b(255,255,255);break;
// case 1:map_out_show.at(y,x)=Vec3b(255,0,0);break;
// case 2:map_out_show.at(y,x)=Vec3b(0,255,0);break;
// case 4:map_out_show.at(y,x)=Vec3b(0,0,255);break;
// }
}
imshow("Selector Image", img_select_result);
// imshow("map_out Image", map_out_show);
int num=0;
for(int y=3; y<h-3;y++)
for(int x=3;x<w-3;x++)
{
int pi=x+y*w;
int val;
if(map_out[pi] == 1)
val= 0;
else if(map_out[pi] == 2)
val= 1;
else if(map_out[pi] == 4)
val= 2;
else
continue;
for(int i=-3;i<=3;i++)
{
img_select_result.at<Vec3b>(y+i,x+3)[val] = 255;
img_select_result.at<Vec3b>(y+i,x-3)[val] = 255;
img_select_result.at<Vec3b>(y+i,x+2)[val] = 255;
img_select_result.at<Vec3b>(y+i,x-2)[val] = 255;
img_select_result.at<Vec3b>(y-3,x+i)[val] = 255;
img_select_result.at<Vec3b>(y+3,x+i)[val] = 255;
img_select_result.at<Vec3b>(y-2,x+i)[val] = 255;
img_select_result.at<Vec3b>(y+2,x+i)[val] = 255;
}
num++;
}
cout <<"num = "<< num <<endl;
imshow("Selector Pixels", img_select_result);
}
// */
return numHaveSub;
}
//int makePixelStatus(Eigen::Vector3f* grads, bool* map, int w, int h, float desiredDensity, int recsLeft=5, float THFac = 1)
//{
//}
int main(int argc, char *argv[])
{
Mat img,img_f;
float* color;
int pyrLevelsUsed;
float* img_pyr[PYR_LEVELS];
//step1:读入图像
// /*
if(argc<=3)
img = imread(argv[1], IMREAD_GRAYSCALE);
else
{
cout << "error, input './img_test path/image.png path/camera.txt' " <<endl;
return -1;
}
imshow("raw image", img);
wG[0]=img.cols;hG[0]=img.rows;
int w=wG[0];//0层图像宽度
int h=hG[0];
color = new float[w*h];
// 数据类型转化 unsigned char 转化为 float
img.convertTo(img_f, CV_32F);
memcpy(color, img_f.data, w*h*sizeof(float));//void memcpy(void * _Dst, const void *_Src, size_t _Size)；
// */
//step2:初始化金字塔
// /*
//2.1 确定图像金字塔层数
int wlvl=w;
int hlvl=h;
pyrLevelsUsed=1;
while(wlvl%2==0 && hlvl%2==0 && wlvl*hlvl > 5000 && pyrLevelsUsed < PYR_LEVELS)
{
wlvl /=2;
hlvl /=2;
pyrLevelsUsed++;
}
printf("using pyramid levels 0 to %d. coarsest resolution: %d x %d!\n",
pyrLevelsUsed-1, wlvl, hlvl);
if(wlvl>100 && hlvl > 100)
{
printf("\n\n===============WARNING!===================\n "
"using not enough pyramid levels.\n"
"Consider scaling to a resolution that is a multiple of a power of 2.\n");
}
if(pyrLevelsUsed < 3)
{
printf("\n\n===============WARNING!===================\n "
"I need higher resolution.\n"
"I will probably segfault.\n");
}
//2.2 初始化
for (int level = 1; level < pyrLevelsUsed; ++ level)
{
wG[level] = w >> level;//位右移,等效除以2
hG[level] = h >> level;
}
for(int i=0;i<pyrLevelsUsed;i++)
{
dIp[i] = new Eigen::Vector3f[wG[i]*hG[i]];//初始化各层图像导数，new结果是一段3*1向量数组的地址
absSquaredGrad[i] = new float[wG[i]*hG[i]];//xy方向梯度平方和
img_pyr[i] = new float[wG[i]*hG[i]];
}
dI = dIp[0];//图像导数指针，dI指向第0层图像梯度
// */
//step3:生成金字塔
// /*
for(int i=0;i<w*h;i++)
{
dI[i][0] = color[i];//图像按照辐射度数组存储，第0层第一通道存储原图
img_pyr[0][i] = color[i];
}
//3.1 生成图像金字塔
for(int lvl=0; lvl<pyrLevelsUsed; lvl++)
{
int wl = wG[lvl], hl = hG[lvl];//当前层图像尺寸
Eigen::Vector3f* dI_l = dIp[lvl];//当前层图像梯度数组地址
float* dabs_l = absSquaredGrad[lvl];//当前层图像梯度平方和数组地址
if(lvl>0)//第0层不执行
{
int lvlm1 = lvl-1;//上一层ID
int wlm1 = wG[lvlm1];//上一层列数
Eigen::Vector3f* dI_lm = dIp[lvlm1];//上一层梯度数组地址
//像素4合1，生成金字塔
for(int y=0;y<hl;y++)
for(int x=0;x<wl;x++)
{
//(x，y)位置的第一个通道存入灰度值，由上一层4合1得到
dI_l[x + y*wl][0] = 0.25f * (dI_lm[2*x + 2*y*wlm1][0] +
dI_lm[2*x+1 + 2*y*wlm1][0] +
dI_lm[2*x + 2*y*wlm1+wlm1][0] +
dI_lm[2*x+1 + 2*y*wlm1+wlm1][0]);
img_pyr[lvl][x + y*wl] = dI_l[x + y*wl][0];//
}
}
//3.2 对当前层图像求梯度
for(int idx=wl;idx < wl*(hl-1);idx++)//从第2行第1列开始
{
float dx = 0.5f*(dI_l[idx+1][0] - dI_l[idx-1][0]);//x方向梯度(宽度)
float dy = 0.5f*(dI_l[idx+wl][0] - dI_l[idx-wl][0]);
if(!std::isfinite(dx)) dx=0;//当参数不是NaN、Infinity、-Infinity 时，返回true
if(!std::isfinite(dy)) dy=0;
dI_l[idx][1] = dx;//第2通道存储x方向梯度
dI_l[idx][2] = dy;//第3通道存储y方向梯度
dabs_l[idx] = dx*dx+dy*dy;//absSquaredGrad存储梯度平方和
}
}
//3.3 显示金字塔
for(int i=0; i<pyrLevelsUsed; i++)
{
Mat img_pyr_show(hG[i], wG[i], CV_32F);
memcpy(img_pyr_show.data, img_pyr[i], wG[i]*hG[i]*sizeof(float));//
img_pyr_show.convertTo(img_pyr_show, CV_8UC1);
string name = to_string(i);
imshow("P"+name, img_pyr_show);
}
cout << "pyrLevelsUsed = " << pyrLevelsUsed << endl;
// */
//step4:为每层计算内参K
// /*
double fx[PYR_LEVELS], fy[PYR_LEVELS], cx[PYR_LEVELS], cy[PYR_LEVELS];
double k1, k2, p1, p2;
Eigen::Matrix<double,3,3> K[PYR_LEVELS];
ifstream readFile(argv[2]);//打开相机内参数据文件
if (!readFile.is_open())
{
cout << "未成功打开内参文件" << endl;
return -1;
}
readFile >> fx[0] >> fy[0] >> cx[0] >> cy[0]
>> k1 >> k2 >> p1 >> p2;
K[0] << fx[0], 0.0, cx[0], 0.0, fy[0], cy[0], 0.0, 0.0, 1.0;
for (int level = 1; level < pyrLevelsUsed; ++ level)
{
fx[level] = fx[level-1] * 0.5;
fy[level] = fy[level-1] * 0.5;
cx[level] = (cx[0] + 0.5) / ((int)1<<level) - 0.5;
cy[level] = (cy[0] + 0.5) / ((int)1<<level) - 0.5;//左移，相当于乘以2的幂
K[level] << fx[level], 0.0, cx[level], 0.0, fy[level], cy[level], 0.0, 0.0, 1.0;
cout << K[level] << endl;
}
readFile.close(); //关闭文件
// */
//step5:提取特征像素
// /*
float* statusMap = new float[wG[0]*hG[0]];
bool* statusMapB = new bool[wG[0]*hG[0]];
float densities[] = {
0.03,0.05,0.15,0.5,1};//不同层点密度
gradHist = new int[100*(1+wG[0]/32)*(1+hG[0]/32)];//gradHist可能只是为了开辟足够大的空间
ths = new float[(wG[0]/32)*(hG[0]/32)+100];
thsSmoothed = new float[(wG[0]/32)*(hG[0]/32)+100];
//5.1 产生随机数
randomPattern = new unsigned char[wG[0]*hG[0]];
std::srand(3141592); // want to be deterministic.rand()和srand()要一起使用，其中srand()用来初始化随机数种子,rand()用来产生随机数。
for(int i=0;i<wG[0]*hG[0];i++) randomPattern[i] = rand() & 0xFF;
//5.2 遍历金字塔
for(int lvl=0; lvl<pyrLevelsUsed; lvl++)
{
int npts;
// //如果是第0层
if(lvl == 0)
npts = makeMaps(statusMap,densities[lvl]*w*h,1,true,2);//
// else//封印中
// npts = makePixelStatus(firstFrame->dIp[lvl], statusMapB, w[lvl], h[lvl], densities[lvl]*w[0]*h[0]);
}
delete []color;
delete []randomPattern;
delete []gradHist;
delete []ths;
delete []thsSmoothed;
delete []statusMap;
delete []statusMapB;
for(int i=0;i<pyrLevelsUsed;i++)
{
delete []dIp[i];
delete []absSquaredGrad[i];
delete []img_pyr[i];
}
waitKey(0);
return 0;
}
测试结果
问题
1、直方图归一化我直接用的总数，所以画出的图显得有点大。
2、最大的问题还是特征像素选取上。可以看到为了满足需要的选点个数，第0层的选点（蓝色点）大部分都集中到图像下部，数量占总数的90%。最开始我以为是测试程序出错，但是对DSO项目运行过程中，也观测到这种不合理。（是只有我吗，还是已经有过解释了？）不过DSO项目还是可以正常运行，可能影响不大。
笔记先记录到这了，最后祝大家新年快乐，牛年大吉。
更新：问题
经过各种调试，终于发现问题所在。原来程序每个block的阈值都是96，这个问题再往上看，才发现自己在中位数选值后忘记break，失之毫厘谬以千里啊。
这里贴上修改之后的结果。虽然已经得到和原DSO项目一样的效果，但是图像下部集中了大量的像素（苦笑）。
继续修改，我发现DSO项目程序里有这样一句代码
if(xf<4 || xf>=w-5 || yf<4 || yf>h-4) continue;
刚开始我的理解是选点忽略最外的一圈像素。可是在我继续深究图像下部的像素集中问题时，我发现并不是这样。
xf>>5 和 yf>>5 的值是分别等于w32和h32的，而梯度阈值的序号是从0开始，最大到w32-1，这些多余的像素位阈值为0，必定被选中，所以底下有一条错误选点带。于是修改上面这句代码为
if(xf<4 || xf>=w-17 || yf<4 || yf>h-17) continue;
可以得到更好的结果，见下图。
