Added support for prior (covariance matrix)

modules/optflow/include/opencv2/optflow/pcaflow.hpp

@@ -47,127 +47,32 @@ namespace cv
 {
 namespace optflow
 {
-/*
-class PCAFlowBasis
+namespace pcaflow
 {
-public:
-  Size size;
-
-  PCAFlowBasis( Size basisSize = Size( 0, 0 ) ) : size( basisSize ) {}
-
-  virtual ~PCAFlowBasis(){};
-
-  virtual int getNumberOfComponents() const = 0;
 
-  virtual void getBasisAtPoint( const Point2f &p, const Size &maxSize, float *outX, float *outY ) const = 0;
-
-  virtual Point2f reduceAtPoint( const Point2f &p, const Size &maxSize, const float *w1, const float *w2 ) const = 0;
-};*/
-
-/*
- * Orthogonal basis from Discrete Cosine Transform.
- * Can be used without any learning or assumptions about flow structure for general purpose.
- * Gives low quality estimation.
- */
-/*class PCAFlowGeneralBasis : public PCAFlowBasis
-{
-public:
-  PCAFlowGeneralBasis( Size basisSize = Size( 18, 14 ) ) : PCAFlowBasis( basisSize ) {}
-
-  int getNumberOfComponents() const { return size.area(); }
-
-  void getBasisAtPoint( const Point2f &p, const Size &maxSize, float *outX, float *outY ) const
-  {
-    for ( int n1 = 0; n1 < size.width; ++n1 )
-      for ( int n2 = 0; n2 < size.height; ++n2 )
-        outX[n1 * size.height + n2] =
-          cosf( ( n1 * M_PI / maxSize.width ) * ( p.x + 0.5 ) ) * cosf( ( n2 * M_PI / maxSize.height ) * ( p.y + 0.5 )
-);
-    memcpy( outY, outX, getNumberOfComponents() * sizeof( *outY ) );
-  }
-
-  Point2f reduceAtPoint( const Point2f &p, const Size &maxSize, const float *w1, const float *w2 ) const
-  {
-    Point2f res( 0, 0 );
-    for ( int n1 = 0; n1 < size.width; ++n1 )
-      for ( int n2 = 0; n2 < size.height; ++n2 )
-      {
-        const float c =
-          cosf( ( n1 * M_PI / maxSize.width ) * ( p.x + 0.5 ) ) * cosf( ( n2 * M_PI / maxSize.height ) * ( p.y + 0.5 )
-);
-        res.x += c * w1[n1 * size.height + n2];
-        res.y += c * w2[n1 * size.height + n2];
-      }
-    return res;
-  }
-};*/
-/*
-class PCAFlowLearnedBasis : public PCAFlowBasis
+class Prior
 {
 private:
-  float *basisData;
-  unsigned numberOfComponents;
+  Mat L1;
+  Mat L2;
+  Mat c1;
+  Mat c2;
 
 public:
-  PCAFlowLearnedBasis( const char *filename )
-  {
-    basisData = 0;
-    FILE *f = fopen( filename, "r" );
-    CV_Assert( f );
-
-    numberOfComponents = 0;
-    CV_Assert( fread( &numberOfComponents, sizeof( numberOfComponents ), 1, f ) == 1 );
-    CV_Assert( fread( &size.height, sizeof( size.height ), 1, f ) == 1 );
-    CV_Assert( fread( &size.width, sizeof( size.width ), 1, f ) == 1 );
-    CV_Assert( ( numberOfComponents > 0 ) && ( numberOfComponents % 2 == 0 ) );
-
-    basisData = new float[size.width * size.height * numberOfComponents];
-    CV_Assert( fread( basisData, size.width * size.height * sizeof( *basisData ), numberOfComponents, f ) ==
-               numberOfComponents );
-    fclose( f );
-
-    numberOfComponents /= 2;
-  }
-
-  ~PCAFlowLearnedBasis()
-  {
-    if ( basisData )
-      delete[] basisData;
-  }
-
-  int getNumberOfComponents() const { return numberOfComponents; }
-
-  void getBasisAtPoint( const Point2f &p, const Size &maxSize, float *outX, float *outY ) const
-  {
-    const size_t chunk = size.width * size.height;
-    size_t offset = size_t( p.y * float(size.height) / maxSize.height ) * size.width + size_t( p.x * float(size.width) /
-maxSize.width );
-    for ( unsigned i = 0; i < numberOfComponents; ++i )
-      outX[i] = basisData[i * chunk + offset];
-    offset += numberOfComponents * chunk;
-    for ( unsigned i = 0; i < numberOfComponents; ++i )
-      outY[i] = basisData[i * chunk + offset];
-  }
-
-  Point2f reduceAtPoint( const Point2f &p, const Size &maxSize, const float *w1, const float *w2 ) const
-  {
-    Point2f res( 0, 0 );
-    const size_t chunk = size.width * size.height;
-    const size_t offset = size_t( p.y * float(size.height) / maxSize.height ) * size.width + size_t( p.x *
-float(size.width) / maxSize.width );
-    for ( unsigned i = 0; i < numberOfComponents; ++i )
-    {
-      const float c = basisData[i * chunk + offset];
-      res.x += c * w1[i];
-      res.y += c * w2[i];
-    }
-    return res;
-  }
-};*/
+  Prior( const char *pathToPrior );
+
+  int getPadding() const { return L1.size().height; }
+
+  int getBasisSize() const { return L1.size().width; }
+
+  void fillConstraints( float *A1, float *A2, float *b1, float *b2 ) const;
+};
+}
 
 class OpticalFlowPCAFlow : public DenseOpticalFlow
 {
 protected:
+  const pcaflow::Prior *prior;
   const Size basisSize;
   const float sparseRate;              // (0 .. 0.1)
   const float retainedCornersFraction; // [0 .. 1]
@@ -175,9 +80,9 @@ protected:
   const float dampingFactor;
 
 public:
-  OpticalFlowPCAFlow( const Size _basisSize = Size( 18, 14 ), float _sparseRate = 0.02,
-                      float _retainedCornersFraction = 0.7, float _occlusionsThreshold = 0.0003,
-                      float _dampingFactor = 0.00002 );
+  OpticalFlowPCAFlow( const pcaflow::Prior *_prior = 0, const Size _basisSize = Size( 18, 14 ),
+                      float _sparseRate = 0.02, float _retainedCornersFraction = 0.7,
+                      float _occlusionsThreshold = 0.0003, float _dampingFactor = 0.00002 );
 
   void calc( InputArray I0, InputArray I1, InputOutputArray flow );
   void collectGarbage();
@@ -191,6 +96,10 @@ private:
 
   void getSystem( OutputArray AOut, OutputArray b1Out, OutputArray b2Out, const std::vector<Point2f> &features,
                   const std::vector<Point2f> &predictedFeatures, const Size size );
+
+  void getSystem( OutputArray A1Out, OutputArray A2Out, OutputArray b1Out, OutputArray b2Out,
+                  const std::vector<Point2f> &features, const std::vector<Point2f> &predictedFeatures,
+                  const Size size );
 };
 
 CV_EXPORTS_W Ptr<DenseOpticalFlow> createOptFlow_PCAFlow();

modules/optflow/src/learn_prior.py

+import os, sys
+import numpy as np
+import cv2
+import struct
+from math import sqrt
+
+basis_size = (14, 18)
+lambd = 0.2
+
+def find_flo(pp):
+	f = []
+	for p in pp:
+		if os.path.isfile(p):
+			f.append(p)
+		else:
+			for root, subdirs, files in os.walk(p):
+				f += map(lambda x: os.path.join(root, x), filter(lambda x: x.split('.')[-1] == 'flo', files))
+	return list(set(f))
+
+def load_flo(flo):
+    with open(flo) as f:
+            magic = np.fromfile(f, np.float32, count=1)
+            if 202021.25 != magic:
+                print('Magic number incorrect. Invalid .flo file')
+            else:
+                w = np.fromfile(f, np.int32, count=1)
+                h = np.fromfile(f, np.int32, count=1)
+                print('Reading %dx%d flo file %s' % (w, h, flo))
+                data = np.fromfile(f, np.float32, count=2*w*h)
+                # Reshape data into 3D array (columns, rows, bands)
+                flow = np.resize(data, (h, w, 2))
+                return flow[:,:,0], flow[:,:,1]
+
+def get_w(m):
+	s = m.shape
+	w = cv2.dct(m)
+	w *= 2.0 / sqrt(s[0] * s[1])
+	#w[0,0] *= 0.5
+	w[:,0] *= sqrt(0.5)
+	w[0,:] *= sqrt(0.5)
+	w = w[0:basis_size[0],0:basis_size[1]].transpose().flatten()
+	return w
+
+w1 = []
+w2 = []
+
+for flo in find_flo(sys.argv[1:]):
+	x,y = load_flo(flo)
+	w1.append(get_w(x))
+	w2.append(get_w(y))
+
+w1mean = sum(w1) / len(w1)
+w2mean = sum(w2) / len(w2)
+
+for i in xrange(len(w1)):
+	w1[i] -= w1mean
+for i in xrange(len(w2)):
+	w2[i] -= w2mean
+
+Q1 = sum([w1[i].reshape(-1,1).dot(w1[i].reshape(1,-1)) for i in xrange(len(w1))]) / len(w1)
+Q2 = sum([w2[i].reshape(-1,1).dot(w2[i].reshape(1,-1)) for i in xrange(len(w2))]) / len(w2)
+Q1 = np.matrix(Q1)
+Q2 = np.matrix(Q2)
+
+if len(w1) > 1:
+	while True:
+		try:
+			L1 = np.linalg.cholesky(Q1)
+			break
+		except np.linalg.linalg.LinAlgError:
+			mev = min(np.linalg.eig(Q1)[0]).real
+			assert(mev < 0)
+			print('Q1', mev)
+			if -mev < 1e-6:
+				mev = -1e-6
+			Q1 += (-mev*1.000001) * np.identity(Q1.shape[0])
+
+	while True:
+		try:
+			L2 = np.linalg.cholesky(Q2)
+			break
+		except np.linalg.linalg.LinAlgError:
+			mev = min(np.linalg.eig(Q2)[0]).real
+			assert(mev < 0)
+			print('Q2', mev)
+			if -mev < 1e-6:
+				mev = -1e-6
+			Q2 += (-mev*1.000001) * np.identity(Q2.shape[0])
+else:
+	L1 = np.identity(Q1.shape[0])
+	L2 = np.identity(Q2.shape[0])
+
+L1 = np.linalg.inv(L1) * lambd
+L2 = np.linalg.inv(L2) * lambd
+
+assert(L1.shape == L2.shape)
+assert(L1.shape[0] == L1.shape[1])
+
+f = open('cov.dat', 'wb')
+
+f.write(struct.pack('I', L1.shape[0]))
+f.write(struct.pack('I', L1.shape[1]))
+
+for i in xrange(L1.shape[0]):
+	for j in xrange(L1.shape[1]):
+		f.write(struct.pack('f', L1[i,j])) 
+
+for i in xrange(L2.shape[0]):
+	for j in xrange(L2.shape[1]):
+		f.write(struct.pack('f', L2[i,j]))
+
+b1 = L1.dot(w1mean.reshape(-1,1))
+b2 = L2.dot(w2mean.reshape(-1,1))
+
+assert(L1.shape[0] == b1.shape[0])
+
+for i in xrange(b1.shape[0]):
+	f.write(struct.pack('f', b1[i,0]))
+
+for i in xrange(b2.shape[0]):
+	f.write(struct.pack('f', b2[i,0]))
+
+f.close()
\ No newline at end of file

modules/optflow/src/pcaflow.cpp

@@ -40,18 +40,20 @@
  //
  //M*/
 
-#include "precomp.hpp"
 #include "opencv2/ximgproc/edge_filter.hpp"
+#include "precomp.hpp"
 
 namespace cv
 {
 namespace optflow
 {
 
-OpticalFlowPCAFlow::OpticalFlowPCAFlow( const Size _basisSize, float _sparseRate, float _retainedCornersFraction,
-                                        float _occlusionsThreshold, float _dampingFactor )
-    : basisSize( _basisSize ), sparseRate( _sparseRate ), retainedCornersFraction( _retainedCornersFraction ),
-      occlusionsThreshold( _occlusionsThreshold ), dampingFactor( _dampingFactor )
+OpticalFlowPCAFlow::OpticalFlowPCAFlow( const pcaflow::Prior *_prior, const Size _basisSize, float _sparseRate,
+                                        float _retainedCornersFraction, float _occlusionsThreshold,
+                                        float _dampingFactor )
+    : prior( _prior ), basisSize( _basisSize ), sparseRate( _sparseRate ),
+      retainedCornersFraction( _retainedCornersFraction ), occlusionsThreshold( _occlusionsThreshold ),
+      dampingFactor( _dampingFactor )
 {
   CV_Assert( sparseRate > 0 && sparseRate <= 0.1 );
   CV_Assert( retainedCornersFraction >= 0 && retainedCornersFraction <= 1.0 );
@@ -256,6 +258,38 @@ void OpticalFlowPCAFlow::getSystem( OutputArray AOut, OutputArray b1Out, OutputA
   }
 }
 
+void OpticalFlowPCAFlow::getSystem( OutputArray A1Out, OutputArray A2Out, OutputArray b1Out, OutputArray b2Out,
+                                    const std::vector<Point2f> &features, const std::vector<Point2f> &predictedFeatures,
+                                    const Size size )
+{
+  CV_Assert( prior->getBasisSize() == basisSize.area() );
+
+  A1Out.create( features.size() + prior->getPadding(), basisSize.area(), CV_32F );
+  A2Out.create( features.size() + prior->getPadding(), basisSize.area(), CV_32F );
+  b1Out.create( features.size() + prior->getPadding(), 1, CV_32F );
+  b2Out.create( features.size() + prior->getPadding(), 1, CV_32F );
+  Mat A1 = A1Out.getMat();
+  Mat A2 = A2Out.getMat();
+  Mat b1 = b1Out.getMat();
+  Mat b2 = b2Out.getMat();
+  for ( size_t i = 0; i < features.size(); ++i )
+  {
+    const Point2f &p = features[i];
+    float *row = A1.ptr<float>( i );
+    for ( int n1 = 0; n1 < basisSize.width; ++n1 )
+      for ( int n2 = 0; n2 < basisSize.height; ++n2 )
+        row[n1 * basisSize.height + n2] =
+          cosf( ( n1 * M_PI / size.width ) * ( p.x + 0.5 ) ) * cosf( ( n2 * M_PI / size.height ) * ( p.y + 0.5 ) );
+    const Point2f flow = predictedFeatures[i] - features[i];
+    b1.at<float>( i ) = flow.x;
+    b2.at<float>( i ) = flow.y;
+  }
+
+  memcpy( A2.ptr<float>(), A1.ptr<float>(), features.size() * basisSize.area() * sizeof( float ) );
+  prior->fillConstraints( A1.ptr<float>( features.size(), 0 ), A2.ptr<float>( features.size(), 0 ),
+                          b1.ptr<float>( features.size(), 0 ), b2.ptr<float>( features.size(), 0 ) );
+}
+
 static void applyCLAHE( Mat &img )
 {
   Ptr<CLAHE> clahe = createCLAHE();
@@ -335,20 +369,63 @@ void OpticalFlowPCAFlow::calc( InputArray I0, InputArray I1, InputOutputArray fl
   flowOut.create( size, CV_32FC2 );
   Mat flow = flowOut.getMat();
 
-  Mat A, b1, b2, w1, w2;
+  Mat w1, w2;
+  if ( prior )
+  {
+    Mat A1, A2, b1, b2;
+    getSystem( A1, A2, b1, b2, features, predictedFeatures, size );
+    solveLSQR( A1, b1, w1, dampingFactor * size.area() );
+    solveLSQR( A2, b2, w2, dampingFactor * size.area() );
+  }
+  else
+  {
+    Mat A, b1, b2;
     getSystem( A, b1, b2, features, predictedFeatures, size );
-  // solve( A1, b1, w1, DECOMP_CHOLESKY | DECOMP_NORMAL );
-  // solve( A2, b2, w2, DECOMP_CHOLESKY | DECOMP_NORMAL );
     solveLSQR( A, b1, w1, dampingFactor * size.area() );
     solveLSQR( A, b2, w2, dampingFactor * size.area() );
-  Mat flowSmall( (size / 8) * 2, CV_32FC2 );
+  }
+  Mat flowSmall( ( size / 8 ) * 2, CV_32FC2 );
   reduceToFlow( w1, w2, flowSmall, basisSize );
   resize( flowSmall, flow, size, 0, 0, INTER_LINEAR );
-  ximgproc::fastGlobalSmootherFilter(fromOrig, flow, flow, 500, 2);
+  ximgproc::fastGlobalSmootherFilter( fromOrig, flow, flow, 500, 2 );
 }
 
 void OpticalFlowPCAFlow::collectGarbage() {}
 
 Ptr<DenseOpticalFlow> createOptFlow_PCAFlow() { return makePtr<OpticalFlowPCAFlow>(); }
+
+namespace pcaflow
+{
+
+Prior::Prior( const char *pathToPrior )
+{
+  FILE *f = fopen( pathToPrior, "r" );
+  CV_Assert( f );
+
+  unsigned n = 0, m = 0;
+  CV_Assert( fread( &n, sizeof( n ), 1, f ) == 1 );
+  CV_Assert( fread( &m, sizeof( m ), 1, f ) == 1 );
+
+  L1.create( n, m, CV_32F );
+  L2.create( n, m, CV_32F );
+  c1.create( n, 1, CV_32F );
+  c2.create( n, 1, CV_32F );
+
+  CV_Assert( fread( L1.ptr<float>(), n * m * sizeof( float ), 1, f ) == 1 );
+  CV_Assert( fread( L2.ptr<float>(), n * m * sizeof( float ), 1, f ) == 1 );
+  CV_Assert( fread( c1.ptr<float>(), n * sizeof( float ), 1, f ) == 1 );
+  CV_Assert( fread( c2.ptr<float>(), n * sizeof( float ), 1, f ) == 1 );
+
+  fclose( f );
+}
+
+void Prior::fillConstraints( float *A1, float *A2, float *b1, float *b2 ) const
+{
+  memcpy( A1, L1.ptr<float>(), L1.size().area() * sizeof( float ) );
+  memcpy( A2, L2.ptr<float>(), L2.size().area() * sizeof( float ) );
+  memcpy( b1, c1.ptr<float>(), c1.size().area() * sizeof( float ) );
+  memcpy( b2, c2.ptr<float>(), c2.size().area() * sizeof( float ) );
+}
+}
 }
 }