diff --git a/Code/DisparityMap/otbDisparityMapToDEMFilter.h b/Code/DisparityMap/otbDisparityMapToDEMFilter.h
index e90afed2626d027d03b80b413ee6cb94f7cd4c90..dda0995d1ce1d6ff53261cf62d885d8cb15718e8 100644
--- a/Code/DisparityMap/otbDisparityMapToDEMFilter.h
+++ b/Code/DisparityMap/otbDisparityMapToDEMFilter.h
@@ -20,10 +20,12 @@
#include "itkImageToImageFilter.h"
#include "otbGenericRSTransform.h"
+#include "itkImageRegionSplitter.h"
// #include "itkConstNeighborhoodIterator.h"
// #include "itkImageRegionConstIterator.h"
// #include "itkImageRegionIterator.h"
-// #include "otbImage.h"
+#include "otbVectorImage.h"
+#include "otbImage.h"
namespace otb
{
@@ -33,7 +35,8 @@ namespace otb
*
* This filter uses an input disparity map (horizontal and vertical) to produce a DEM with a regular sampling
* in the chosen map projection system. The elevation is computed from the triangulation of the "left-right" pairs
- * of pixels matched. When several elevations are possible on a DEM cell, the highest is kept.
+ * of pixels matched. When several elevations are possible on a DEM cell, the highest is kept. Note : disparity maps
+ * and DEM are expected to be of scalar image type.
*
* The inputs are:
* - the horizontal and vertical disparity maps (SetHorizontalDisparityMapInput, SetVerticalDisparityMapInput)
@@ -55,7 +58,7 @@ namespace otb
*
*/
template <class TDisparityImage, class TInputImage, class TOutputDEMImage = TDisparityImage,
- class TEpipolarGridImage = TDisparityImage, class TMaskImage = otb::Image<unsigned char> >
+ class TEpipolarGridImage = otb::VectorImage<float,2> , class TMaskImage = otb::Image<unsigned char> >
class ITK_EXPORT DisparityMapToDEMFilter :
public itk::ImageToImageFilter<TDisparityImage,TOutputDEMImage>
{
@@ -81,6 +84,9 @@ public:
typedef TMaskImage MaskImageType;
typedef typename DEMImageType::RegionType RegionType;
+ typedef typename DEMImageType::PixelType DEMPixelType;
+
+ typedef itk::ImageRegionSplitter<2> SplitterType;
// 3D RS transform
// TODO: Allow to tune precision (i.e. double or float)
@@ -167,14 +173,17 @@ protected:
/** Threaded generate data */
virtual void ThreadedGenerateData(const RegionType & outputRegionForThread, int threadId);
+ /** After threaded generate data : sum up temporary DEMs */
+ virtual void AfterThreadedGenerateData();
+
private:
DisparityMapToDEMFilter(const Self&); //purposely not implemented
void operator=(const Self&); //purposely not implemented
- /** Minimum elevation of the DEM in meters (over the chosen reference : DEM, geoid, average)*/
+ /** Minimum elevation of the DEM in meters */
double m_ElevationMin;
- /** Maximum elevation of the DEM in meters (over the chosen reference : DEM, geoid, average)*/
+ /** Maximum elevation of the DEM in meters */
double m_ElevationMax;
/** DEM grid step (in meters) */
@@ -188,6 +197,15 @@ private:
/** Average Elevation */
double m_AverageElevation;
+
+ /** Region splitter for input disparity maps */
+ SplitterType::Pointer m_InputSplitter;
+
+ /** Number of splits used for input multithreading */
+ unsigned int m_UsedInputSplits;
+
+ /** Temporary DEMs for mutlithreading */
+ std::vector<typename DEMImageType::Pointer> m_TempDEMRegions;
};
} // end namespace otb
diff --git a/Code/DisparityMap/otbDisparityMapToDEMFilter.txx b/Code/DisparityMap/otbDisparityMapToDEMFilter.txx
index b0cc31feae02e40827ef218a77a31cd2a2768b02..cd2cf031cf5c06a88e5c33c151c93b98d340af0b 100644
--- a/Code/DisparityMap/otbDisparityMapToDEMFilter.txx
+++ b/Code/DisparityMap/otbDisparityMapToDEMFilter.txx
@@ -19,8 +19,8 @@
#define __otbDisparityMapToDEMFilter_txx
#include "otbDisparityMapToDEMFilter.h"
-#include "itkImageConstIteratorWithIndex.h"
-// #include "itkImageRegionIterator.h"
+#include "itkImageRegionConstIteratorWithIndex.h"
+#include "itkImageRegionIterator.h"
// #include "itkProgressReporter.h"
// #include "itkConstantBoundaryCondition.h"
@@ -45,9 +45,12 @@ DisparityMapToDEMFilter<TDisparityImage,TInputImage,TOutputDEMImage,TEpipolarGri
m_ElevationMax = 100.0;
m_DEMGridStep = 10.0;
- m_AverageElevation = 0;
+ m_AverageElevation = 0.0;
m_DEMDirectory = "";
m_GeoidFile = "";
+
+ m_InputSplitter = SplitterType::New();
+ m_UsedInputSplits = 1;
}
template <class TDisparityImage, class TInputImage, class TOutputDEMImage,
@@ -254,10 +257,10 @@ DisparityMapToDEMFilter<TDisparityImage,TInputImage,TOutputDEMImage,TEpipolarGri
const TInputImage * rightImgPtr = this->GetRightInput();
TOutputDEMImage * outputPtr = this->GetDEMOutput();
- // Setup the DEM handler if needed
- typename DEMHandler::Pointer demHandler = DEMHandler::New();
+ // // Setup the DEM handler if needed
+ //typename DEMHandler::Pointer demHandler = DEMHandler::New();
- bool useDEM = false;
+ //bool useDEM = false;
// Set-up a transform to use the DEMHandler
typedef otb::GenericRSTransform<> RSTransform2DType;
@@ -265,30 +268,34 @@ DisparityMapToDEMFilter<TDisparityImage,TInputImage,TOutputDEMImage,TEpipolarGri
leftToGroundTransform->SetInputKeywordList(leftImgPtr->GetImageKeywordlist());
if(m_DEMDirectory!="")
{
- demHandler->OpenDEMDirectory(m_DEMDirectory);
+ //demHandler->OpenDEMDirectory(m_DEMDirectory);
leftToGroundTransform->SetDEMDirectory(m_DEMDirectory);
- useDEM = true;
+ //useDEM = true;
}
if(m_GeoidFile!="")
{
leftToGroundTransform->SetGeoidFile(m_GeoidFile);
- demHandler->OpenGeoidFile(m_GeoidFile);
+ //demHandler->OpenGeoidFile(m_GeoidFile);
}
+ leftToGroundTransform->SetAverageElevation(m_AverageElevation);
+
leftToGroundTransform->InstanciateTransform();
RSTransform2DType::Pointer rightToGroundTransform = RSTransform2DType::New();
rightToGroundTransform->SetInputKeywordList(rightImgPtr->GetImageKeywordlist());
if(m_DEMDirectory!="")
{
- demHandler->OpenDEMDirectory(m_DEMDirectory);
+ //demHandler->OpenDEMDirectory(m_DEMDirectory);
rightToGroundTransform->SetDEMDirectory(m_DEMDirectory);
- useDEM = true;
+ //useDEM = true;
}
if(m_GeoidFile!="")
{
rightToGroundTransform->SetGeoidFile(m_GeoidFile);
- demHandler->OpenGeoidFile(m_GeoidFile);
+ //demHandler->OpenGeoidFile(m_GeoidFile);
}
+ rightToGroundTransform->SetAverageElevation(m_AverageElevation);
+
rightToGroundTransform->InstanciateTransform();
// left image
@@ -609,7 +616,33 @@ void
DisparityMapToDEMFilter<TDisparityImage,TInputImage,TOutputDEMImage,TEpipolarGridImage,TMaskImage>
::BeforeThreadedGenerateData()
{
- // TODO
+ const TDisparityImage * horizDisp = this->GetHorizontalDisparityMapInput();
+
+ const TOutputDEMImage * outputDEM = this->GetDEMOutput();
+
+ m_UsedInputSplits = m_InputSplitter->GetNumberOfSplits(horizDisp->GetRequestedRegion(), this->GetNumberOfThreads());
+
+ if (m_UsedInputSplits > 0 && m_UsedInputSplits <= this->GetNumberOfThreads())
+ {
+ m_TempDEMRegions.clear();
+
+ for (unsigned int i=0;i<m_UsedInputSplits;i++)
+ {
+ typename DEMImageType::Pointer tmpImg = TOutputDEMImage::New();
+ tmpImg->SetNumberOfComponentsPerPixel(1);
+ tmpImg->SetRegions(outputDEM->GetRequestedRegion());
+ tmpImg->Allocate();
+
+ typename DEMImageType::PixelType minElevation = static_cast<typename DEMImageType::PixelType>(m_ElevationMin);
+ tmpImg->FillBuffer(minElevation);
+
+ m_TempDEMRegions.push_back(tmpImg);
+ }
+ }
+ else
+ {
+ itkExceptionMacro(<<"Wrong number of splits for input multithreading : "<<m_UsedInputSplits);
+ }
}
template <class TDisparityImage, class TInputImage, class TOutputDEMImage,
@@ -624,22 +657,31 @@ DisparityMapToDEMFilter<TDisparityImage,TInputImage,TOutputDEMImage,TEpipolarGri
const TInputImage * leftSensor = this->GetLeftInput();
const TInputImage * rightSensor = this->GetRightInput();
+ const TMaskImage * disparityMask = this->GetDisparityMaskInput();
+
+ const TOutputDEMImage * outputDEM = this->GetDEMOutput();
+
RSTransformType::Pointer leftToGroundTransform = RSTransformType::New();
RSTransformType::Pointer rightToGroundTransform = RSTransformType::New();
leftToGroundTransform->SetInputKeywordList(leftSensor->GetImageKeywordlist());
rightToGroundTransform->SetInputKeywordList(rightSensor->GetImageKeywordlist());
+ typename DEMHandler::Pointer demHandler = DEMHandler::New();
+
if(m_DEMDirectory!="")
{
leftToGroundTransform->SetDEMDirectory(m_DEMDirectory);
rightToGroundTransform->SetDEMDirectory(m_DEMDirectory);
+ demHandler->OpenDEMDirectory(m_DEMDirectory);
}
if(m_GeoidFile!="")
{
leftToGroundTransform->SetGeoidFile(m_GeoidFile);
rightToGroundTransform->SetGeoidFile(m_GeoidFile);
+ demHandler->OpenGeoidFile(m_GeoidFile);
}
+ demHandler->SetDefaultHeightAboveEllipsoid(m_AverageElevation);
leftToGroundTransform->InstanciateTransform();
rightToGroundTransform->InstanciateTransform();
@@ -647,10 +689,21 @@ DisparityMapToDEMFilter<TDisparityImage,TInputImage,TOutputDEMImage,TEpipolarGri
const TEpipolarGridImage * leftGrid = this->GetLeftEpipolarGridInput();
const TEpipolarGridImage * rightGrid = this->GetRightEpipolarGridInput();
- typename TEpipolarGridImage::RegionType gridRegion = leftGrid->GetLargestPossibleRegion();
+ typename TEpipolarGridImage::RegionType gridRegion = leftGrid->GetLargestPossibleRegion();
+
+ TOutputDEMImage * tmpDEM = NULL;
+ typename TOutputDEMImage::RegionType outputRequestedRegion = outputDEM->GetRequestedRegion();
- // TODO : replace by sub region for thread
- typename TDisparityImage::RegionType disparityRegion = horizDisp->GetRequestedRegion();
+ typename TDisparityImage::RegionType disparityRegion;
+ if (threadId < m_UsedInputSplits)
+ {
+ disparityRegion = m_InputSplitter->GetSplit(threadId,m_UsedInputSplits,horizDisp->GetRequestedRegion());
+ tmpDEM = m_TempDEMRegions[threadId];
+ }
+ else
+ {
+ return;
+ }
itk::ImageRegionConstIteratorWithIndex<DisparityMapType> horizIt(horizDisp,disparityRegion);
itk::ImageRegionConstIteratorWithIndex<DisparityMapType> vertiIt(vertiDisp,disparityRegion);
@@ -658,6 +711,22 @@ DisparityMapToDEMFilter<TDisparityImage,TInputImage,TOutputDEMImage,TEpipolarGri
horizIt.GoToBegin();
vertiIt.GoToBegin();
+ bool useMask = false;
+ if (disparityMask)
+ {
+ useMask = true;
+ }
+ else
+ {
+ disparityMask = MaskImageType::New();
+ }
+
+ itk::ImageRegionConstIterator<MaskImageType> maskIt(disparityMask,disparityRegion);
+ if (useMask)
+ {
+ maskIt.GoToBegin();
+ }
+
typename TDisparityImage::PointType epiPoint;
itk::ContinuousIndex<double,2> gridIndexConti;
double subPixIndex[2];
@@ -676,7 +745,17 @@ DisparityMapToDEMFilter<TDisparityImage,TInputImage,TOutputDEMImage,TEpipolarGri
while (!horizIt.IsAtEnd() && !vertiIt.IsAtEnd())
{
- // TODO : if mask, check value and skip iteration if mask value is not valid
+ // check mask value if any
+ if (useMask)
+ {
+ if (!(maskIt.Get() > 0))
+ {
+ ++horizIt;
+ ++vertiIt;
+ ++maskIt;
+ continue;
+ }
+ }
// compute left ray
horizDisp->TransformIndexToPhysicalPoint(horizIt.GetIndex(),epiPoint);
@@ -719,6 +798,10 @@ DisparityMapToDEMFilter<TDisparityImage,TInputImage,TOutputDEMImage,TEpipolarGri
leftGroundHmax = leftToGroundTransform->TransformPoint(sensorPoint);
// compute right ray
+ itk::ContinuousIndex<double,2> rightIndexEstimate;
+ rightIndexEstimate[0] = static_cast<double>((horizIt.GetIndex())[0]) + static_cast<double>(horizIt.Get());
+ rightIndexEstimate[1] = static_cast<double>((horizIt.GetIndex())[1]) + static_cast<double>(vertiIt.Get());
+ horizDisp->TransformContinuousIndexToPhysicalPoint(rightIndexEstimate,epiPoint);
rightGrid->TransformPhysicalPointToContinuousIndex(epiPoint,gridIndexConti);
ulIndex[0] = static_cast<int>(vcl_floor(gridIndexConti[0]));
@@ -757,19 +840,112 @@ DisparityMapToDEMFilter<TDisparityImage,TInputImage,TOutputDEMImage,TEpipolarGri
sensorPoint[2] = m_ElevationMax;
rightGroundHmax = rightToGroundTransform->TransformPoint(sensorPoint);
- // Compute ray intersection
+ // Compute ray intersection (mid-point method), TODO : implement non-iterative method from Hartley & Sturm
+ double a = (leftGroundHmax[0] - leftGroundHmin[0]) * (leftGroundHmax[0] - leftGroundHmin[0]) +
+ (leftGroundHmax[1] - leftGroundHmin[1]) * (leftGroundHmax[1] - leftGroundHmin[1]) +
+ (leftGroundHmax[2] - leftGroundHmin[2]) * (leftGroundHmax[2] - leftGroundHmin[2]);
+ double b = (rightGroundHmax[0] - rightGroundHmin[0]) * (rightGroundHmax[0] - rightGroundHmin[0]) +
+ (rightGroundHmax[1] - rightGroundHmin[1]) * (rightGroundHmax[1] - rightGroundHmin[1]) +
+ (rightGroundHmax[2] - rightGroundHmin[2]) * (rightGroundHmax[2] - rightGroundHmin[2]);
+ double c = -(leftGroundHmax[0] - leftGroundHmin[0]) * (rightGroundHmax[0] - rightGroundHmin[0])
+ -(leftGroundHmax[1] - leftGroundHmin[1]) * (rightGroundHmax[1] - rightGroundHmin[1])
+ -(leftGroundHmax[2] - leftGroundHmin[2]) * (rightGroundHmax[2] - rightGroundHmin[2]);
+ double g = (leftGroundHmax[0] - leftGroundHmin[0]) * (rightGroundHmin[0] - leftGroundHmin[0]) +
+ (leftGroundHmax[1] - leftGroundHmin[1]) * (rightGroundHmin[1] - leftGroundHmin[1]) +
+ (leftGroundHmax[2] - leftGroundHmin[2]) * (rightGroundHmin[2] - leftGroundHmin[2]);
+ double h = -(rightGroundHmax[0] - rightGroundHmin[0]) * (rightGroundHmin[0] - leftGroundHmin[0])
+ -(rightGroundHmax[1] - rightGroundHmin[1]) * (rightGroundHmin[1] - leftGroundHmin[1])
+ -(rightGroundHmax[2] - rightGroundHmin[2]) * (rightGroundHmin[2] - leftGroundHmin[2]);
+
+ double rLeft = (b * g - c * h) / (a * b - c * c);
+ double rRight = (a * h - c * g) / (a * b - c * c);
+
+ TDPointType leftFoot;
+ leftFoot.SetToBarycentricCombination(leftGroundHmax,leftGroundHmin,rLeft );
- // Optimise position ?
+ TDPointType rightFoot;
+ rightFoot.SetToBarycentricCombination(rightGroundHmax,rightGroundHmin,rRight);
+
+ TDPointType midPoint3D;
+ midPoint3D.SetToMidPoint(leftFoot,rightFoot);
// Is point inside DEM area ?
+ typename DEMImageType::PointType midPoint2D;
+ midPoint2D[0] = midPoint3D[0];
+ midPoint2D[1] = midPoint3D[1];
+ itk::ContinuousIndex<double,2> midIndex;
+ outputDEM->TransformPhysicalPointToContinuousIndex(midPoint2D,midIndex);
+ typename DEMImageType::IndexType cellIndex;
+ cellIndex[0] = static_cast<int>(vcl_floor(midIndex[0] + 0.5));
+ cellIndex[1] = static_cast<int>(vcl_floor(midIndex[1] + 0.5));
- // Add point to its corresponding cell (keep maximum)
+ if (outputRequestedRegion.IsInside(cellIndex))
+ {
+ // Estimate local reference elevation (average, DEM or geoid)
+ double localElevation = demHandler->GetHeightAboveEllipsoid(midPoint2D);
+
+ // Add point to its corresponding cell (keep maximum)
+ DEMPixelType cellHeight = static_cast<DEMPixelType>(midPoint3D[2] + localElevation);
+ if (cellHeight > tmpDEM->GetPixel(cellIndex) && cellHeight < static_cast<DEMPixelType>(m_ElevationMax))
+ {
+ tmpDEM->SetPixel(cellIndex,cellHeight);
+ }
+ }
++horizIt;
++vertiIt;
+
+ if (useMask) ++maskIt;
+
+ }
+}
+
+template <class TDisparityImage, class TInputImage, class TOutputDEMImage,
+class TEpipolarGridImage, class TMaskImage>
+void
+DisparityMapToDEMFilter<TDisparityImage,TInputImage,TOutputDEMImage,TEpipolarGridImage,TMaskImage>
+::AfterThreadedGenerateData()
+{
+ TOutputDEMImage * outputDEM = this->GetDEMOutput();
+
+ if (m_TempDEMRegions.size() < 1)
+ {
+ outputDEM->FillBuffer(m_ElevationMin);
+ return;
+ }
+
+ itk::ImageRegionIterator<DEMImageType> outputDEMIt(outputDEM,outputDEM->GetRequestedRegion());
+ itk::ImageRegionIterator<DEMImageType> firstDEMIt(m_TempDEMRegions[0],outputDEM->GetRequestedRegion());
+
+ outputDEMIt.GoToBegin();
+ firstDEMIt.GoToBegin();
+
+ // Copy first DEM
+ while (!outputDEMIt.IsAtEnd() && !firstDEMIt.IsAtEnd())
+ {
+ outputDEMIt.Set(firstDEMIt.Get());
+ ++outputDEMIt;
+ ++firstDEMIt;
}
- // TODO
+ // Check DEMs from other threads and keep the maximum elevation
+ for (unsigned int i=1;i<m_TempDEMRegions.size();i++)
+ {
+ itk::ImageRegionIterator<DEMImageType> tmpDEMIt(m_TempDEMRegions[i],outputDEM->GetRequestedRegion());
+
+ outputDEMIt.GoToBegin();
+ tmpDEMIt.GoToBegin();
+
+ while (!outputDEMIt.IsAtEnd() && !tmpDEMIt.IsAtEnd())
+ {
+ if (tmpDEMIt.Get() > outputDEMIt.Get())
+ {
+ outputDEMIt.Set(tmpDEMIt.Get());
+ }
+ ++outputDEMIt;
+ ++tmpDEMIt;
+ }
+ }
}
}