From d8aa162545076763367631bd3594cc518fa4e3f0 Mon Sep 17 00:00:00 2001 From: Vladislav Samsonov Date: Thu, 9 Jun 2016 01:55:31 +0300 Subject: [PATCH] Added support for prior (covariance matrix) --- .../include/opencv2/optflow/pcaflow.hpp | 137 +++--------------- modules/optflow/src/learn_prior.py | 123 ++++++++++++++++ modules/optflow/src/pcaflow.cpp | 103 +++++++++++-- 3 files changed, 236 insertions(+), 127 deletions(-) create mode 100644 modules/optflow/src/learn_prior.py diff --git a/modules/optflow/include/opencv2/optflow/pcaflow.hpp b/modules/optflow/include/opencv2/optflow/pcaflow.hpp index 8f6dda290..2d9495489 100644 --- a/modules/optflow/include/opencv2/optflow/pcaflow.hpp +++ b/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 &features, const std::vector &predictedFeatures, const Size size ); + + void getSystem( OutputArray A1Out, OutputArray A2Out, OutputArray b1Out, OutputArray b2Out, + const std::vector &features, const std::vector &predictedFeatures, + const Size size ); }; CV_EXPORTS_W Ptr createOptFlow_PCAFlow(); diff --git a/modules/optflow/src/learn_prior.py b/modules/optflow/src/learn_prior.py new file mode 100644 index 000000000..100cfe67c --- /dev/null +++ b/modules/optflow/src/learn_prior.py @@ -0,0 +1,123 @@ +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 diff --git a/modules/optflow/src/pcaflow.cpp b/modules/optflow/src/pcaflow.cpp index bb9f5e575..64aa39c6f 100644 --- a/modules/optflow/src/pcaflow.cpp +++ b/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 &features, const std::vector &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( 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( i ) = flow.x; + b2.at( i ) = flow.y; + } + + memcpy( A2.ptr(), A1.ptr(), features.size() * basisSize.area() * sizeof( float ) ); + prior->fillConstraints( A1.ptr( features.size(), 0 ), A2.ptr( features.size(), 0 ), + b1.ptr( features.size(), 0 ), b2.ptr( features.size(), 0 ) ); +} + static void applyCLAHE( Mat &img ) { Ptr 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; - 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 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 ); + solveLSQR( A, b1, w1, dampingFactor * size.area() ); + solveLSQR( A, b2, w2, dampingFactor * size.area() ); + } + 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 createOptFlow_PCAFlow() { return makePtr(); } + +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(), n * m * sizeof( float ), 1, f ) == 1 ); + CV_Assert( fread( L2.ptr(), n * m * sizeof( float ), 1, f ) == 1 ); + CV_Assert( fread( c1.ptr(), n * sizeof( float ), 1, f ) == 1 ); + CV_Assert( fread( c2.ptr(), n * sizeof( float ), 1, f ) == 1 ); + + fclose( f ); +} + +void Prior::fillConstraints( float *A1, float *A2, float *b1, float *b2 ) const +{ + memcpy( A1, L1.ptr(), L1.size().area() * sizeof( float ) ); + memcpy( A2, L2.ptr(), L2.size().area() * sizeof( float ) ); + memcpy( b1, c1.ptr(), c1.size().area() * sizeof( float ) ); + memcpy( b2, c2.ptr(), c2.size().area() * sizeof( float ) ); +} +} } }