From 746ccac0e7f1260a65b3b36c4822402ac443ad2a Mon Sep 17 00:00:00 2001
From: Guillaume Pasero <guillaume.pasero@c-s.fr>
Date: Thu, 15 Mar 2012 18:37:47 +0100
Subject: [PATCH] ENH: in PixelWiseBlockMatching : add basic subp-pixel
interpolation
---
.../otbPixelWiseBlockMatching.cxx | 18 ++
.../otbPixelWiseBlockMatchingImageFilter.h | 32 ++-
.../otbPixelWiseBlockMatchingImageFilter.txx | 240 ++++++++++++++++++
3 files changed, 283 insertions(+), 7 deletions(-)
diff --git a/Applications/DisparityMap/otbPixelWiseBlockMatching.cxx b/Applications/DisparityMap/otbPixelWiseBlockMatching.cxx
index 3a9f47c965..3328caa8cd 100644
--- a/Applications/DisparityMap/otbPixelWiseBlockMatching.cxx
+++ b/Applications/DisparityMap/otbPixelWiseBlockMatching.cxx
@@ -187,6 +187,9 @@ private:
AddParameter(ParameterType_Int,"bm.maxvd","Maximum vertical disparity");
SetParameterDescription("bm.maxvd","Maximum vertical disparity to explore (can be negative)");
+ AddParameter(ParameterType_Empty,"bm.subpixel","Do sub-pixel interpolation");
+ SetParameterDescription("bm.subpixel", "Estimate disparities with sub-pixel precision");
+
AddParameter(ParameterType_Choice, "bm.initdisp", "Initial disparities");
AddChoice("bm.initdisp.none", "None");
SetParameterDescription("bm.initdisp.none", "No initial disparity used");
@@ -379,6 +382,11 @@ private:
m_SSDBlockMatcher->SetMaximumHorizontalDisparity(maxhdisp);
m_SSDBlockMatcher->SetMinimumVerticalDisparity(minvdisp);
m_SSDBlockMatcher->SetMaximumVerticalDisparity(maxvdisp);
+ if (IsParameterEnabled("bm.subpixel"))
+ {
+ m_SSDBlockMatcher->DoSubPixelInterpolationOn();
+ }
+
AddProcess(m_SSDBlockMatcher,"SSD block matching");
if(masking)
{
@@ -418,6 +426,11 @@ private:
m_NCCBlockMatcher->SetMinimumVerticalDisparity(minvdisp);
m_NCCBlockMatcher->SetMaximumVerticalDisparity(maxvdisp);
m_NCCBlockMatcher->MinimizeOff();
+ if (IsParameterEnabled("bm.subpixel"))
+ {
+ m_NCCBlockMatcher->DoSubPixelInterpolationOn();
+ }
+
AddProcess(m_NCCBlockMatcher,"NCC block matching");
if(masking)
@@ -458,6 +471,11 @@ private:
m_LPBlockMatcher->SetMaximumHorizontalDisparity(maxhdisp);
m_LPBlockMatcher->SetMinimumVerticalDisparity(minvdisp);
m_LPBlockMatcher->SetMaximumVerticalDisparity(maxvdisp);
+ if (IsParameterEnabled("bm.subpixel"))
+ {
+ m_LPBlockMatcher->DoSubPixelInterpolationOn();
+ }
+
AddProcess(m_LPBlockMatcher,"Lp block matching");
if(masking)
diff --git a/Code/DisparityMap/otbPixelWiseBlockMatchingImageFilter.h b/Code/DisparityMap/otbPixelWiseBlockMatchingImageFilter.h
index 01ef8f1bb8..e12a245d1b 100644
--- a/Code/DisparityMap/otbPixelWiseBlockMatchingImageFilter.h
+++ b/Code/DisparityMap/otbPixelWiseBlockMatchingImageFilter.h
@@ -21,6 +21,8 @@
#include "itkImageToImageFilter.h"
#include "itkConstNeighborhoodIterator.h"
+#include "itkImageRegionConstIterator.h"
+#include "itkImageRegionIterator.h"
#include "otbImage.h"
namespace otb
@@ -41,11 +43,11 @@ template <class TInputImage, class TOutputMetricImage>
ITK_EXPORT class SSDBlockMatching
{
public:
- typedef itk::ConstNeighborhoodIterator<TInputImage> ConstNeigghborhoodIteratorType;
+ typedef itk::ConstNeighborhoodIterator<TInputImage> ConstNeighborhoodIteratorType;
typedef typename TOutputMetricImage::ValueType MetricValueType;
// Implement the SSD operator
- inline MetricValueType operator()(ConstNeigghborhoodIteratorType & a, ConstNeigghborhoodIteratorType & b) const
+ inline MetricValueType operator()(ConstNeighborhoodIteratorType & a, ConstNeighborhoodIteratorType & b) const
{
MetricValueType ssd = 0;
@@ -71,11 +73,11 @@ template <class TInputImage, class TOutputMetricImage>
ITK_EXPORT class NCCBlockMatching
{
public:
- typedef itk::ConstNeighborhoodIterator<TInputImage> ConstNeigghborhoodIteratorType;
+ typedef itk::ConstNeighborhoodIterator<TInputImage> ConstNeighborhoodIteratorType;
typedef typename TOutputMetricImage::ValueType MetricValueType;
// Implement the NCC operator
- inline MetricValueType operator()(ConstNeigghborhoodIteratorType & a, ConstNeigghborhoodIteratorType & b) const
+ inline MetricValueType operator()(ConstNeighborhoodIteratorType & a, ConstNeighborhoodIteratorType & b) const
{
MetricValueType meanA(0),meanB(0), sigmaA(0), sigmaB(0), cov(0), ncc(0);
@@ -129,10 +131,10 @@ template <class TInputImage, class TOutputMetricImage>
ITK_EXPORT class LPBlockMatching
{
public:
- typedef itk::ConstNeighborhoodIterator<TInputImage> ConstNeigghborhoodIteratorType;
+ typedef itk::ConstNeighborhoodIterator<TInputImage> ConstNeighborhoodIteratorType;
typedef typename TOutputMetricImage::ValueType MetricValueType;
- LPBlockMatching(): m_P(0)
+ LPBlockMatching(): m_P(1)
{
}
@@ -149,7 +151,7 @@ public:
}
// Implement the Lp metric
- inline MetricValueType operator()(ConstNeigghborhoodIteratorType & a, ConstNeigghborhoodIteratorType & b) const
+ inline MetricValueType operator()(ConstNeighborhoodIteratorType & a, ConstNeighborhoodIteratorType & b) const
{
MetricValueType score(0);
@@ -244,6 +246,10 @@ public:
typedef typename InputImageType::SizeType SizeType;
typedef typename InputImageType::IndexType IndexType;
typedef typename InputImageType::RegionType RegionType;
+
+ typedef typename TOutputMetricImage::ValueType MetricValueType;
+
+ typedef itk::ConstNeighborhoodIterator<TInputImage> ConstNeighborhoodIteratorType;
/** Set left input */
void SetLeftInput( const TInputImage * image);
@@ -339,6 +345,10 @@ public:
/** Get the initial disparity fields */
const TOutputDisparityImage * GetHorizontalDisparityInput() const;
const TOutputDisparityImage * GetVerticalDisparityInput() const;
+
+ itkSetMacro(DoSubPixelInterpolation, bool);
+ itkGetConstReferenceMacro(DoSubPixelInterpolation,bool);
+ itkBooleanMacro(DoSubPixelInterpolation);
protected:
/** Constructor */
@@ -360,6 +370,11 @@ private:
PixelWiseBlockMatchingImageFilter(const Self&); //purposely not implemented
void operator=(const Self&); //purposely not implemeFnted
+ bool InterpolateSubPixelPosition(ConstNeighborhoodIteratorType & leftPatch,
+ itk::ImageRegionIterator<OutputDisparityImageType> & hDispIt,
+ itk::ImageRegionIterator<OutputDisparityImageType> & vDispIt,
+ itk::ImageRegionIterator<OutputMetricImageType> & metricIt);
+
/** The radius of the blocks */
SizeType m_Radius;
@@ -389,6 +404,9 @@ private:
/** Initial vertical disparity (0 by default, used if an exploration radius is set) */
int m_InitVerticalDisparity;
+
+ /** flag to perform sub-pixel interpolation */
+ bool m_DoSubPixelInterpolation;
};
} // end namespace otb
diff --git a/Code/DisparityMap/otbPixelWiseBlockMatchingImageFilter.txx b/Code/DisparityMap/otbPixelWiseBlockMatchingImageFilter.txx
index e6762a991e..3d215380c3 100644
--- a/Code/DisparityMap/otbPixelWiseBlockMatchingImageFilter.txx
+++ b/Code/DisparityMap/otbPixelWiseBlockMatchingImageFilter.txx
@@ -60,6 +60,8 @@ TOutputDisparityImage,TMaskImage,TBlockMatchingFunctor>
// Default exploration radius : 0 (not used)
m_ExplorationRadius.Fill(0);
+
+ m_DoSubPixelInterpolation = false;
}
@@ -675,7 +677,245 @@ TOutputDisparityImage,TMaskImage,TBlockMatchingFunctor>
}
}
+
+ // subpixel interpolation case
+ if (m_DoSubPixelInterpolation)
+ {
+ itk::ConstNeighborhoodIterator<TInputImage> leftIt(m_Radius,inLeftPtr,outputRegionForThread);
+ itk::ImageRegionIterator<TOutputDisparityImage> outHDispIt(outHDispPtr,outputRegionForThread);
+ itk::ImageRegionIterator<TOutputDisparityImage> outVDispIt(outVDispPtr,outputRegionForThread);
+ itk::ImageRegionIterator<TOutputMetricImage> outMetricIt(outMetricPtr,outputRegionForThread);
+
+ itk::ConstantBoundaryCondition<TInputImage> nbc1;
+ leftIt.OverrideBoundaryCondition(&nbc1);
+
+ leftIt.GoToBegin();
+ outHDispIt.GoToBegin();
+ outVDispIt.GoToBegin();
+ outMetricIt.GoToBegin();
+
+ while (!leftIt.IsAtEnd()
+ || !outHDispIt.IsAtEnd()
+ || !outVDispIt.IsAtEnd()
+ || !outMetricIt.IsAtEnd())
+ {
+ bool isInterpolated = InterpolateSubPixelPosition(leftIt,outHDispIt,outVDispIt,outMetricIt);
+
+ ++leftIt;
+ ++outHDispIt;
+ ++outVDispIt;
+ ++outMetricIt;
+ }
+
+ }
+
+}
+
+template <class TInputImage, class TOutputMetricImage,
+class TOutputDisparityImage, class TMaskImage, class TBlockMatchingFunctor>
+bool
+PixelWiseBlockMatchingImageFilter<TInputImage,TOutputMetricImage,
+TOutputDisparityImage,TMaskImage,TBlockMatchingFunctor>
+::InterpolateSubPixelPosition(ConstNeighborhoodIteratorType & leftPatch,
+ itk::ImageRegionIterator<OutputDisparityImageType> & hDispIt,
+ itk::ImageRegionIterator<OutputDisparityImageType> & vDispIt,
+ itk::ImageRegionIterator<OutputMetricImageType> & metricIt)
+{
+ IndexType curLeftPos = leftPatch.GetIndex();
+ SizeType curRadius = leftPatch.GetRadius();
+
+ const TInputImage * inRightPtr = this->GetRightInput();
+ RegionType rightLargestRegion = inRightPtr->GetLargestPossibleRegion();
+
+ float hDisp_f = static_cast<float>(hDispIt.Get());
+ float vDisp_f = static_cast<float>(vDispIt.Get());
+
+ int hDisp_i = static_cast<int>(vcl_floor(hDisp_f + 0.5));
+ int vDisp_i = static_cast<int>(vcl_floor(vDisp_f + 0.5));
+
+ IndexType curRightPos(curLeftPos);
+ curRightPos[0]+=hDisp_i;
+ curRightPos[1]+=vDisp_i;
+
+ // check if the current right position is inside the right image
+ if (!rightLargestRegion.IsInside(curRightPos))
+ {
+ return false;
+ }
+
+ // compute metric around current right position
+ bool horizontalInterpolation = false;
+ bool verticalInterpolation = false;
+
+ // metrics for neighbors positions, sorted like this :
+ // [up,up-right,right,down-right,down,down-left,left,up-left]
+ double neighborsMetric[8] = {0,0,0,0,0,0,0,0};
+ int firstNeighbor = 0;
+ int stepNeighbor = 4;
+
+ RegionType smallRightRegion;
+ smallRightRegion.SetIndex(0,curRightPos[0]-1);
+ smallRightRegion.SetIndex(1,curRightPos[1]-1);
+ smallRightRegion.SetSize(0,3);
+ smallRightRegion.SetSize(1,3);
+
+ itk::ConstNeighborhoodIterator<TInputImage> rightIt(curRadius,inRightPtr,smallRightRegion);
+
+ if (m_MinimumVerticalDisparity < vDisp_i && vDisp_i < m_MaximumVerticalDisparity)
+ {
+ IndexType upIndex(curRightPos);
+ upIndex[1]+= (-1);
+ IndexType downIndex(curRightPos);
+ downIndex[1]+= 1;
+ if ( rightLargestRegion.IsInside(upIndex) && rightLargestRegion.IsInside(downIndex) )
+ {
+ verticalInterpolation = true;
+
+ rightIt.SetLocation(upIndex);
+ neighborsMetric[0] = m_Functor(leftPatch,rightIt);
+
+ rightIt.SetLocation(downIndex);
+ neighborsMetric[4] = m_Functor(leftPatch,rightIt);
+
+ }
+ }
+
+ if (m_MinimumHorizontalDisparity < hDisp_i && hDisp_i < m_MaximumHorizontalDisparity)
+ {
+ IndexType leftIndex(curRightPos);
+ leftIndex[0]+= (-1);
+ IndexType rightIndex(curRightPos);
+ rightIndex[0]+= 1;
+ if ( rightLargestRegion.IsInside(leftIndex) && rightLargestRegion.IsInside(rightIndex) )
+ {
+ horizontalInterpolation = true;
+ firstNeighbor = 2;
+
+ rightIt.SetLocation(rightIndex);
+ neighborsMetric[2] = m_Functor(leftPatch,rightIt);
+
+ rightIt.SetLocation(leftIndex);
+ neighborsMetric[6] = m_Functor(leftPatch,rightIt);
+
+ }
+ }
+
+ if ( !verticalInterpolation && !horizontalInterpolation)
+ {
+ return false;
+ }
+
+ // if both vertical and horizontal interpolation, compute metrics on corners
+ if (verticalInterpolation && horizontalInterpolation)
+ {
+ firstNeighbor = 0;
+ stepNeighbor = 1;
+
+ IndexType uprightIndex(curRightPos);
+ uprightIndex[0]+= 1;
+ uprightIndex[1]+= (-1);
+ rightIt.SetLocation(uprightIndex);
+ neighborsMetric[1] = m_Functor(leftPatch,rightIt);
+
+ IndexType downrightIndex(curRightPos);
+ downrightIndex[0]+= 1;
+ downrightIndex[1]+= 1;
+ rightIt.SetLocation(downrightIndex);
+ neighborsMetric[3] = m_Functor(leftPatch,rightIt);
+
+ IndexType downleftIndex(curRightPos);
+ downleftIndex[0]+= (-1);
+ downleftIndex[1]+= 1;
+ rightIt.SetLocation(downleftIndex);
+ neighborsMetric[5] = m_Functor(leftPatch,rightIt);
+
+ IndexType upleftIndex(curRightPos);
+ upleftIndex[0]+= (-1);
+ upleftIndex[1]+= (-1);
+ rightIt.SetLocation(upleftIndex);
+ neighborsMetric[7] = m_Functor(leftPatch,rightIt);
+ }
+
+ // check if the position is a minimum (or maximum if !m_Minimize)
+ double curMetric = metricIt.Get();
+ for (unsigned int i=firstNeighbor; i<8 ; i+=stepNeighbor)
+ {
+ if (m_Minimize)
+ {
+ if (curMetric > neighborsMetric[i])
+ {
+ return false;
+ }
+ }
+ else
+ {
+ if (curMetric < neighborsMetric[i])
+ {
+ return false;
+ }
+ }
+ }
+
+
+ // Interpolate position : parabola fit
+ if (verticalInterpolation && !horizontalInterpolation)
+ {
+ //vertical only
+ double deltaV = 0.5 - (1.0 / (1.0 + (neighborsMetric[0]-curMetric) / (neighborsMetric[4]-curMetric)));
+ if (deltaV > (-0.5) && deltaV < 0.5)
+ {
+ vDispIt.Set( static_cast<double>(vDisp_i) + deltaV);
+ }
+ else
+ {
+ return false;
+ }
+ }
+ else if (!verticalInterpolation && horizontalInterpolation)
+ {
+ // horizontal only
+ double deltaH = 0.5 - (1.0 / (1.0 + (neighborsMetric[6]-curMetric) / (neighborsMetric[2]-curMetric)));
+ if (deltaH > (-0.5) && deltaH < 0.5)
+ {
+ hDispIt.Set( static_cast<double>(hDisp_i) + deltaH);
+ }
+ else
+ {
+ return false;
+ }
+ }
+ else
+ {
+ //both horizontal and vertical
+ double dx = 0.5 * (neighborsMetric[2] - neighborsMetric[6]);
+ double dy = 0.5 * (neighborsMetric[4] - neighborsMetric[0]);
+ double dxx = neighborsMetric[2] + neighborsMetric[6] - 2.0 * curMetric;
+ double dyy = neighborsMetric[4] + neighborsMetric[0] - 2.0 * curMetric;
+ double dxy = 0.25 * (neighborsMetric[3] + neighborsMetric[7] - neighborsMetric[1] - neighborsMetric[5]);
+ double det = dxx*dyy - dxy*dxy;
+ if (vcl_abs(det) < (1e-10))
+ {
+ return false;
+ }
+ else
+ {
+ double deltaH = (-dx * dyy + dy * dxy )/det;
+ double deltaV = ( dx * dxy - dy * dxx )/det;
+ if (deltaH > (-1.0) && deltaH < 1.0 && deltaV > (-1.0) && deltaV < 1.0)
+ {
+ hDispIt.Set( static_cast<double>(hDisp_i) + deltaH);
+ vDispIt.Set( static_cast<double>(vDisp_i) + deltaV);
+ }
+ else
+ {
+ return false;
+ }
+ }
+ }
+
+ return true;
}
+
} // End namespace otb
#endif
--
GitLab