opticalFlow.h

Go to the documentation of this file.

#ifndef _DLRCOMPUTERVISION_OPTICALFLOW_H_
#define _DLRCOMPUTERVISION_OPTICALFLOW_H_

#include <dlrComputerVision/image.h>
#include <dlrNumeric/array2D.h>
#include <dlrNumeric/index2D.h>
#include <dlrNumeric/vector2D.h>

namespace dlr {

  namespace computerVision {
    
    template <ImageFormat Format>
    class OpticalFlow {
    public:
      OpticalFlow(const Image<Format>& image0,
                  const Image<Format>& image1,
                  double sigma = 2.5);


      OpticalFlow(const Image<Format>& image0,
                  const Image<Format>& image1,
                  const Array2D<Float64>& dIdx,
                  const Array2D<Float64>& dIdy);
      
      virtual
      ~OpticalFlow() {};

      Vector2D
      getFlow(const Index2D& upperLeftCorner,
              const Index2D& lowerRightCorner,
              bool& valid);

      void
      ignoreArea(const Array2D<bool>& flags);

    private:

      void
      setImages(const Image<Format>& image0,
                const Image<Format>& image1,
                double sigma = 2.5);
      

      void
      setImages(const Image<Format>& image0,
                const Image<Format>& image1,
                const Array2D<Float64>& dIdx,
                const Array2D<Float64>& dIdy);
      
      
      Array2D<Float64> m_dIdx2;
      Array2D<Float64> m_dIdy2;
      Array2D<Float64> m_dIdxdIdy;
      Array2D<Float64> m_dIdxdIdt;
      Array2D<Float64> m_dIdydIdt;
    };

  
  } // namespace computerVision

} // namespace dlr

/* ========== Definitions of inline and template functions follow ========== */


#include <dlrCommon/types.h>
#include <dlrComputerVision/filter.h>
#include <dlrComputerVision/kernel.h>
#include <dlrComputerVision/kernels.h>
#include <dlrComputerVision/utilities.h>
#include <dlrNumeric/utilities.h>

namespace dlr {

  namespace computerVision {
    
    namespace privateCode {

      const Kernel<double>&
      getGradientXKernel()
      {
        static Kernel<double> gradientXKernel(
          Array1D<double>("[-0.5, 0, 0.5]"), Array1D<double>("[1.0]"));
        return gradientXKernel;
      }
      

      const Kernel<double>&
      getGradientYKernel()
      {
        static Kernel<double> gradientYKernel(
          Array1D<double>("[1.0]"), Array1D<double>("[-0.5, 0, 0.5]"));
        return gradientYKernel;
      }
    
    } // namespace privateCode


    template <ImageFormat Format>
    OpticalFlow<Format>::
    OpticalFlow(const Image<Format>& image0,
                const Image<Format>& image1,
                double sigma)
      : m_dIdx2(),
        m_dIdy2(),
        m_dIdxdIdy(),
        m_dIdxdIdt(),
        m_dIdydIdt()
    {
      this->setImages(image0, image1, sigma);
    }


    
    template <ImageFormat Format>
    OpticalFlow<Format>::
    OpticalFlow(const Image<Format>& image0,
                const Image<Format>& image1,
                const Array2D<Float64>& dIdx,
                const Array2D<Float64>& dIdy)
      : m_dIdx2(),
        m_dIdy2(),
        m_dIdxdIdy(),
        m_dIdxdIdt(),
        m_dIdydIdt()
    {
      this->setImages(image0, image1, dIdx, dIdy);
    }


    
    template <ImageFormat Format>
    Vector2D
    OpticalFlow<Format>::
    getFlow(const Index2D& upperLeftCorner,
            const Index2D& lowerRightCorner,
            bool& valid)
    {
      Float64 rdIdx2 = sum(m_dIdx2, upperLeftCorner, lowerRightCorner);
      Float64 rdIdy2 = sum(m_dIdy2, upperLeftCorner, lowerRightCorner);
      Float64 rdIdxdIdy = sum(m_dIdxdIdy, upperLeftCorner, lowerRightCorner);
      Float64 rdIdxdIdt = sum(m_dIdxdIdt, upperLeftCorner, lowerRightCorner);
      Float64 rdIdydIdt = sum(m_dIdydIdt, upperLeftCorner, lowerRightCorner);

      // Check for singularity.
      double determinant = rdIdx2 * rdIdy2 - rdIdxdIdy * rdIdxdIdy;
      // Warning(xxx): Hardcoded tolerance.
      if(approximatelyEqual(determinant, 0.0, 1.0e-12)) {
        valid = false;
        return Vector2D(0.0, 0.0);
      }

      // Do cofactor-based inversion by hand here.
      Float64 inverseFPrimeSq00 = rdIdy2 / determinant;
      Float64 inverseFPrimeSq01 = -rdIdxdIdy / determinant;
      // Float64 inverseFPrimeSq10 = inverseFPrimeSq01;
      Float64 inverseFPrimeSq11 = rdIdx2 / determinant;

      // Compute flow.
      Float64 vx =
        rdIdxdIdt * inverseFPrimeSq00 + rdIdydIdt * inverseFPrimeSq01;
      Float64 vy =
        rdIdxdIdt * inverseFPrimeSq01 + rdIdydIdt * inverseFPrimeSq11;

      valid = true;
      return Vector2D(-vx, -vy);
    }


    template <ImageFormat Format>
    void
    OpticalFlow<Format>::
    ignoreArea(const Array2D<bool>& flags)
    {
      if(flags.rows() != m_dIdx2.rows()
         || flags.columns() != m_dIdx2.columns()) {
        DLR_THROW(ValueException, "OpticalFlow::setImages()",
                  "Argument flags must have the same size as "
                  "the input images passed to constructor or setImages().");
      }
      for(size_t index0 = 0; index0 < flags.size(); ++index0) {
        if(flags[index0]) {
          m_dIdx2[index0] = 0.0;
          m_dIdy2[index0] = 0.0;
          m_dIdxdIdy[index0] = 0.0;
          m_dIdxdIdt[index0] = 0.0;
          m_dIdydIdt[index0] = 0.0;
        }
      }
    }
    
    
    template <ImageFormat Format>
    void
    OpticalFlow<Format>::
    setImages(const Image<Format>& image0,
              const Image<Format>& image1,
              double sigma)
    {
      if(image0.rows() != image1.rows()
         || image0.columns() != image1.columns()) {
        DLR_THROW(ValueException, "OpticalFlow::setImages()",
                  "Arguments image0 and image1 must have the same size.");
      }
      if(ImageFormatTraits<Format>::getNumberOfComponents() != 1) {
        DLR_THROW(NotImplementedException, "OpticalFlow::setImages()",
                  "Only single-component image formats are currently "
                  "supported.");
      }

      Array2D<Float64> dIdx;
      Array2D<Float64> dIdy;
      size_t kernelSize = static_cast<size_t>(6.0 * sigma + 0.5);
      if(sigma > 0.0 && kernelSize != 0) {
        // Make sure we have an odd kernel size.
        if(kernelSize % 2 == 0) {
          kernelSize += 1;
        }
        
        // Low pass filter the image to smooth out spacial gradients.
        Kernel<Float64> gaussian = getGaussianKernel<Float64>(
          kernelSize, kernelSize, sigma, sigma);
        Image<GRAY_FLOAT64> filteredImage0 =
          filter2D<GRAY_FLOAT64, GRAY_FLOAT64>(
            gaussian, image0, static_cast<Float64>(0.0),
            DLR_CONVOLVE_PAD_RESULT);

        // Differentiate.
        dIdx = filter2D<GRAY_FLOAT64, GRAY_FLOAT64>(
          privateCode::getGradientXKernel(), filteredImage0,
          static_cast<Float64>(0.0), DLR_CONVOLVE_PAD_RESULT);
        dIdy = filter2D<GRAY_FLOAT64, GRAY_FLOAT64>(
          privateCode::getGradientYKernel(), filteredImage0,
          static_cast<Float64>(0.0), DLR_CONVOLVE_PAD_RESULT);

      } else {
        // Low pass filtering disabled by user.  Go straight to
        // differentiation.
        dIdx = filter2D<GRAY_FLOAT64, GRAY_FLOAT64>(
          privateCode::getGradientXKernel(), image0,
          static_cast<Float64>(0.0), DLR_CONVOLVE_PAD_RESULT);
        dIdy = filter2D<GRAY_FLOAT64, GRAY_FLOAT64>(
          privateCode::getGradientYKernel(), image0,
          static_cast<Float64>(0.0), DLR_CONVOLVE_PAD_RESULT);
      }

      // Now that we have spacial gradients, dispatch to parent
      // version of setImages().
      this->setImages(image0, image1, dIdx, dIdy);
    }


    template <ImageFormat Format>
    void
    OpticalFlow<Format>::
    setImages(const Image<Format>& image0,
              const Image<Format>& image1,
              const Array2D<Float64>& dIdx,
              const Array2D<Float64>& dIdy)
    {
      if(image0.rows() != image1.rows()
         || image0.columns() != image1.columns()) {
        DLR_THROW(ValueException, "OpticalFlow::setImages()",
                  "Arguments image0 and image1 must have the same size.");
      }
      if(image0.rows() != dIdx.rows()
         || image0.columns() != dIdx.columns()) {
        DLR_THROW(ValueException, "OpticalFlow::setImages()",
                  "Arguments image0 and dIdx must have the same size.");
      }
      if(image0.rows() != dIdy.rows()
         || image0.columns() != dIdy.columns()) {
        DLR_THROW(ValueException, "OpticalFlow::setImages()",
                  "Arguments image0 and dIdy must have the same size.");
      }
      if(ImageFormatTraits<Format>::getNumberOfComponents() != 1) {
        DLR_THROW(NotImplementedException, "OpticalFlow::setImages()",
                  "Only single-component image formats are currently "
                  "supported.");
      }

      Image<GRAY_FLOAT64> image0Float =
        convertColorspace<GRAY_FLOAT64>(image0);
      Image<GRAY_FLOAT64> image1Float =
        convertColorspace<GRAY_FLOAT64>(image1);
      Array2D<Float64> dIdt = image1Float - image0Float;

      m_dIdx2 = dIdx * dIdx;
      m_dIdy2 = dIdy * dIdy;
      m_dIdxdIdy = dIdx * dIdy;
      m_dIdxdIdt = dIdx * dIdt;
      m_dIdydIdt = dIdy * dIdt;
    }
    
  
  } // namespace computerVision
  
} // namespace dlr

#endif /* #ifndef _DLRCOMPUTERVISION_OPTICALFLOW_H_ */