void PyramidDenoiser::DenoiseLevelLaplacian(int level)
{
if (level < (int)(mpPyramid->GetNumberOfScales() - 1))
{
PyramidLevel<double>* pBandSet;
if (level != -1)
{
pBandSet = mpPyramid->GetRecursiveScale(level);
}
else
{
pBandSet = mpPyramid->GetResidualScale();
}
for (int orientationIndex = 0; orientationIndex < pBandSet->GetNumberOfOrientations(); orientationIndex++)
{
ArrayGrid<double>* pBand = pBandSet->GetOrientedBand( orientationIndex);
for (int y = 0; y < pBand->GetHeight(); y++)
{
for (int x = 0; x < pBand->GetWidth(); x++)
{
double sigmaSignal = EstimateSigmaSignal( pBand, x, y, mSigmaNoise, mWindowSize );
double w1 = pBand->GetValue(x,y);
double shrinkageFactor = ComputeLaplacianShrinkagefactor(w1, sigmaSignal, mSigmaNoise);
mpPyramid->GetRecursiveScale( level)->GetOrientedBand( orientationIndex)->SetValue(x,y, shrinkageFactor * w1);
}
}
}
}
else
{
std::cerr << "PyramidDenoiser::DenoiseLevelLaplacian: Cannot denoise level " << level << " since top level is " << mpPyramid->GetNumberOfScales() << std::endl << std::flush;
}
}
void
StereoDisparity::getXyz(OdometryFrame * odom_frame)
{
int num_levels = odom_frame->getNumLevels();
for(int level_num=0; level_num < num_levels; ++level_num) {
PyramidLevel* level = odom_frame->getLevel(level_num);
int num_kp = level->getNumKeypoints();
//std::cout << level_num << " | number of keypoints: " << num_kp << "\n";
for(int kp_ind=0; kp_ind < num_kp; ++kp_ind) {
KeypointData* kpdata(level->getKeypointData(kp_ind));
int u = (int)(kpdata->rect_base_uv(0)+0.5);
int v = (int)(kpdata->rect_base_uv(1)+0.5);
kpdata->disparity = _disparity_data[(int) v*_width + (int) u];
if (kpdata->disparity == MIN_DISPARITY){ // disp ==0 if no disparity available given
kpdata->disparity = NAN;
kpdata->has_depth = false;
kpdata->xyzw = Eigen::Vector4d(NAN, NAN, NAN, NAN);
kpdata->xyz = Eigen::Vector3d(NAN, NAN, NAN);
} else {
kpdata->has_depth = true;
kpdata->xyz = getXyzValues(u, v, kpdata->disparity);
kpdata->xyzw.head<3>() = kpdata->xyz;
kpdata->xyzw.w() = 1;
}
}
}
}
void
DepthImage::getXyz(OdometryFrame * frame)
{
int num_levels = frame->getNumLevels();
for(int level_num=0; level_num < num_levels; ++level_num) {
PyramidLevel* level = frame->getLevel(level_num);
int num_kp = level->getNumKeypoints();
for(int kp_ind=0; kp_ind < num_kp; ++kp_ind) {
KeypointData* kpdata(level->getKeypointData(kp_ind));
int u = (int)(kpdata->rect_base_uv(0)+0.5);
int v = (int)(kpdata->rect_base_uv(1)+0.5);
kpdata->disparity = NAN;
float z = _depth_data[rgbToDepthIndex(u, v)];
if(isnan(z)) {
kpdata->has_depth = false;
kpdata->xyzw = Eigen::Vector4d(NAN, NAN, NAN, NAN);
kpdata->xyz = Eigen::Vector3d(NAN, NAN, NAN);
} else {
kpdata->has_depth = true;
kpdata->xyz = z * _rays->col(v * _rgb_width + u);
kpdata->xyzw.head<3>() = kpdata->xyz;
kpdata->xyzw.w() = 1;
}
}
}
}
bool PyramidKeyPointDetector::MergeOrientations1( int scale )
{
ArrayGrid<double>* pEnergyMap = 0;
ArrayGrid<double>* pUpscaledEnergyMap = 0;
PyramidLevel< std::complex< double > >* pPyrLevel = 0;
if (scale >= 0)
{
pPyrLevel = mpPyramid->GetRecursiveScale( scale );
}
else
{
pPyrLevel = mpPyramid->GetResidualScale( );
}
int width = pPyrLevel->GetWidth();
int height = pPyrLevel->GetHeight();
bool needsInitialisation = true;
double initialValue = 0.0;
pEnergyMap = new ArrayGrid<double>( width, height, needsInitialisation, initialValue );
int nrOrientations = mpPyramid->GetNumberOfOrientations();
for (int y = 0; y < height; y++)
{
for (int x = 0; x < width; x++)
{
double tmpValue = 0.0;
for (int i = 0; i < nrOrientations-1; i++)
{
for (int j = i+1; j < nrOrientations; j++)
{
if ( ( abs( i - j ) > 1 ) && ( abs( i - j%(nrOrientations-1) ) > 0) )
{
tmpValue += ( abs( pPyrLevel->GetOrientedBand( i )->GetValue(x, y) )
* abs( pPyrLevel->GetOrientedBand( j )->GetValue(x, y) )
);
}
}
}
pEnergyMap->SetValue( x, y, tmpValue);
}
}
if (scale > 0)
{
pUpscaledEnergyMap = GaussConvolve::UpsampleGaussianInterpolated( pEnergyMap, MathUtils::ComputeIntegerPower(2, scale ) );
delete pEnergyMap;
}
else
{
pUpscaledEnergyMap = pEnergyMap;
}
mvEnergyMaps.push_back( pUpscaledEnergyMap );
return true;
}
shared_ptr<FeatureVector> ImagePyramidFeatureExtractor::extract(int x, int y, int size) {
PyramidLevel* level = getLevel(size);
if (level == NULL)
return shared_ptr<FeatureVector>();
int scaledX = level->getScaled(x);
int scaledY = level->getScaled(y);
Patch patch(scaledX, scaledY);
unordered_set<Patch, Patch::hash>::iterator pit = level->getPatches().find(patch);
if (pit == level->getPatches().end()) { // patch does not exist, feature vector has to be created
const Mat& image = level->getScaledImage();
int patchBeginX = scaledX - featureSize.width / 2; // inclusive
int patchBeginY = scaledY - featureSize.height / 2; // inclusive
int patchEndX = patchBeginX + featureSize.width; // exclusive
int patchEndY = patchBeginY + featureSize.height; // exclusive
if (patchBeginX < 0 || patchEndX > image.cols
|| patchBeginY < 0 || patchEndY > image.rows)
return shared_ptr<FeatureVector>();
shared_ptr<FeatureVector> featureVector = extract(
Mat(image, Rect(patchBeginX, patchBeginY, featureSize.width, featureSize.height)));
patch.setFeatureVector(featureVector);
level->getPatches().insert(patch);
return featureVector;
} else { // patch and feature vector exist already
return pit->getFeatureVector();
}
}
void PyramidDenoiser::DenoiseLevelBivariate(int level)
{
if (level < (int)(mpPyramid->GetNumberOfScales() - 1))
{
PyramidLevel<double>* pBandSetCurrent;
PyramidLevel<double>* pBandSetParent;
if (level != -1)
{
pBandSetCurrent = mpPyramid->GetRecursiveScale(level);
pBandSetParent = mpPyramid->GetRecursiveScale(level+1);
}
else
{
pBandSetCurrent = mpPyramid->GetResidualScale();
pBandSetParent = mpPyramid->GetRecursiveScale(0);
}
double avgVariance = 0.0;
double scaleNoiseVariance = mSigmaNoise / GetScaledNoiseVarianceFactor( level );
double scaleParentChildFactor = GetScaledParentChildFactor( level);
for (int orientationIndex = 0; orientationIndex < pBandSetCurrent->GetNumberOfOrientations(); orientationIndex++)
{
ArrayGrid<double>* pBandCurrent = pBandSetCurrent->GetOrientedBand( orientationIndex);
ArrayGrid<double>* pBandParent = pBandSetParent->GetOrientedBand( orientationIndex);
double meanVal = GridStatistics<double>::GetGridMean( pBandCurrent );
double varianceVal = GridStatistics<double>::GetGridVariance( pBandCurrent, meanVal );
#ifdef DEBUG
cout << "Denoising level " << level << " and orientationIndex " << orientationIndex
<< ", variance is " << varianceVal << " and scaled noise variance = " << scaleNoiseVariance << endl << flush;
#endif
avgVariance += varianceVal;
for (int y = 0; y < pBandCurrent->GetHeight(); y++)
{
for (int x = 0; x < pBandCurrent->GetWidth(); x++)
{
double sigmaSignal = EstimateSigmaSignal( pBandCurrent, x, y, mSigmaNoise, mWindowSize );
double w1 = pBandCurrent->GetValue(x,y);
double w2 = pBandParent->GetValue(x/2,y/2) / scaleParentChildFactor;
double shrinkageFactor = ComputeBivariateShrinkagefactor(w1, w2, sigmaSignal, scaleNoiseVariance );
pBandCurrent->SetValue(x,y, shrinkageFactor * w1);
}
}
}
#ifdef DEBUG
cout << "Average variance at scale level = " << (avgVariance) / (double)(pBandSetCurrent->GetNumberOfOrientations())
<< endl << flush;
#endif
}
else
{
std::cerr << "PyramidDenoiser::DenoiseLevelBivariate: Cannot denoise level " << level << " since top level is " << mpPyramid->GetNumberOfScales() << std::endl << std::flush;
}
}
