v1
This commit is contained in:
Ivan
256
src/FullSystem/PixelSelector.h
Normal file
256
src/FullSystem/PixelSelector.h
Normal file
@@ -0,0 +1,256 @@
|
||||
/**
|
||||
* This file is part of DSO.
|
||||
*
|
||||
* Copyright 2016 Technical University of Munich and Intel.
|
||||
* Developed by Jakob Engel <engelj at in dot tum dot de>,
|
||||
* for more information see <http://vision.in.tum.de/dso>.
|
||||
* If you use this code, please cite the respective publications as
|
||||
* listed on the above website.
|
||||
*
|
||||
* DSO is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* DSO is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with DSO. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include "util/NumType.h"
|
||||
|
||||
|
||||
|
||||
|
||||
namespace dso
|
||||
{
|
||||
|
||||
|
||||
const float minUseGrad_pixsel = 10;
|
||||
|
||||
|
||||
template<int pot>
|
||||
inline int gridMaxSelection(Eigen::Vector3f* grads, bool* map_out, int w, int h, float THFac)
|
||||
{
|
||||
|
||||
memset(map_out, 0, sizeof(bool)*w*h);
|
||||
|
||||
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;
|
||||
for(int dx=0;dx<pot;dx++)
|
||||
for(int dy=0;dy<pot;dy++)
|
||||
{
|
||||
int idx = dx+dy*w;
|
||||
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]);
|
||||
if(agx > bestXX) {bestXX=agx; bestXXID=idx;}
|
||||
|
||||
float agy = fabs((float)g[2]);
|
||||
if(agy > bestYY) {bestYY=agy; bestYYID=idx;}
|
||||
|
||||
float gxpy = fabs((float)(g[1]-g[2]));
|
||||
if(gxpy > bestXY) {bestXY=gxpy; bestXYID=idx;}
|
||||
|
||||
float gxmy = fabs((float)(g[1]+g[2]));
|
||||
if(gxmy > bestYX) {bestYX=gxmy; bestYXID=idx;}
|
||||
}
|
||||
}
|
||||
|
||||
bool* map0 = map_out+x+y*w;
|
||||
|
||||
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;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return numGood;
|
||||
}
|
||||
|
||||
|
||||
inline int gridMaxSelection(Eigen::Vector3f* grads, bool* map_out, int w, int h, int pot, float THFac)
|
||||
{
|
||||
|
||||
memset(map_out, 0, sizeof(bool)*w*h);
|
||||
|
||||
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;
|
||||
for(int dx=0;dx<pot;dx++)
|
||||
for(int dy=0;dy<pot;dy++)
|
||||
{
|
||||
int idx = dx+dy*w;
|
||||
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]);
|
||||
if(agx > bestXX) {bestXX=agx; bestXXID=idx;}
|
||||
|
||||
float agy = fabs((float)g[2]);
|
||||
if(agy > bestYY) {bestYY=agy; bestYYID=idx;}
|
||||
|
||||
float gxpy = fabs((float)(g[1]-g[2]));
|
||||
if(gxpy > bestXY) {bestXY=gxpy; bestXYID=idx;}
|
||||
|
||||
float gxmy = fabs((float)(g[1]+g[2]));
|
||||
if(gxmy > bestYX) {bestYX=gxmy; bestYXID=idx;}
|
||||
}
|
||||
}
|
||||
|
||||
bool* map0 = map_out+x+y*w;
|
||||
|
||||
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;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return numGood;
|
||||
}
|
||||
|
||||
|
||||
inline int makePixelStatus(Eigen::Vector3f* grads, bool* map, int w, int h, float desiredDensity, int recsLeft=5, float THFac = 1)
|
||||
{
|
||||
if(sparsityFactor < 1) sparsityFactor = 1;
|
||||
|
||||
int numGoodPoints;
|
||||
|
||||
|
||||
if(sparsityFactor==1) numGoodPoints = gridMaxSelection<1>(grads, map, w, h, THFac);
|
||||
else if(sparsityFactor==2) numGoodPoints = gridMaxSelection<2>(grads, map, w, h, THFac);
|
||||
else if(sparsityFactor==3) numGoodPoints = gridMaxSelection<3>(grads, map, w, h, THFac);
|
||||
else if(sparsityFactor==4) numGoodPoints = gridMaxSelection<4>(grads, map, w, h, THFac);
|
||||
else if(sparsityFactor==5) numGoodPoints = gridMaxSelection<5>(grads, map, w, h, THFac);
|
||||
else if(sparsityFactor==6) numGoodPoints = gridMaxSelection<6>(grads, map, w, h, THFac);
|
||||
else if(sparsityFactor==7) numGoodPoints = gridMaxSelection<7>(grads, map, w, h, THFac);
|
||||
else if(sparsityFactor==8) numGoodPoints = gridMaxSelection<8>(grads, map, w, h, THFac);
|
||||
else if(sparsityFactor==9) numGoodPoints = gridMaxSelection<9>(grads, map, w, h, THFac);
|
||||
else if(sparsityFactor==10) numGoodPoints = gridMaxSelection<10>(grads, map, w, h, THFac);
|
||||
else if(sparsityFactor==11) numGoodPoints = gridMaxSelection<11>(grads, map, w, h, THFac);
|
||||
else numGoodPoints = gridMaxSelection(grads, map, w, h, sparsityFactor, THFac);
|
||||
|
||||
|
||||
/*
|
||||
* #points is approximately proportional to sparsityFactor^2.
|
||||
*/
|
||||
|
||||
float quotia = numGoodPoints / (float)(desiredDensity);
|
||||
|
||||
int newSparsity = (sparsityFactor * sqrtf(quotia))+0.7f;
|
||||
|
||||
|
||||
if(newSparsity < 1) newSparsity=1;
|
||||
|
||||
|
||||
float oldTHFac = THFac;
|
||||
if(newSparsity==1 && sparsityFactor==1) THFac = 0.5;
|
||||
|
||||
|
||||
if((abs(newSparsity-sparsityFactor) < 1 && THFac==oldTHFac) ||
|
||||
( quotia > 0.8 && 1.0f / quotia > 0.8) ||
|
||||
recsLeft == 0)
|
||||
{
|
||||
|
||||
// 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);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user