/*M/////////////////////////////////////////////////////////////////////////////////////// // // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. // // By downloading, copying, installing or using the software you agree to this license. // If you do not agree to this license, do not download, install, // copy or use the software. // // // License Agreement // For Open Source Computer Vision Library // // Copyright (C) 2000-2008, Intel Corporation, all rights reserved. // Copyright (C) 2009, Willow Garage Inc., all rights reserved. // Third party copyrights are property of their respective owners. // // Redistribution and use in source and binary forms, with or without modification, // are permitted provided that the following conditions are met: // // * Redistribution's of source code must retain the above copyright notice, // this list of conditions and the following disclaimer. // // * Redistribution's in binary form must reproduce the above copyright notice, // this list of conditions and the following disclaimer in the documentation // and/or other materials provided with the distribution. // // * The name of the copyright holders may not be used to endorse or promote products // derived from this software without specific prior written permission. // // This software is provided by the copyright holders and contributors "as is" and // any express or implied warranties, including, but not limited to, the implied // warranties of merchantability and fitness for a particular purpose are disclaimed. // In no event shall the Intel Corporation or contributors be liable for any direct, // indirect, incidental, special, exemplary, or consequential damages // (including, but not limited to, procurement of substitute goods or services; // loss of use, data, or profits; or business interruption) however caused // and on any theory of liability, whether in contract, strict liability, // or tort (including negligence or otherwise) arising in any way out of // the use of this software, even if advised of the possibility of such damage. // //M*/ #include "precomp.hpp" #include "opencv2/ximgproc/edge_filter.hpp" /* Disable "from double to float" and "from size_t to int" warnings. * Fixing these would make the code look ugly by introducing explicit cast all around. * Here these warning are pointless anyway. */ #ifdef _MSC_VER #pragma warning( disable : 4305 4244 4267 4838 ) #endif #ifdef __clang__ #pragma clang diagnostic ignored "-Wshorten-64-to-32" #endif namespace cv { namespace optflow { namespace { #ifndef M_SQRT2 const float M_SQRT2 = 1.41421356237309504880; #endif template <typename T> inline int mathSign( T val ) { return ( T( 0 ) < val ) - ( val < T( 0 ) ); } /* Stable symmetric Householder reflection that gives c and s such that * [ c s ][a] = [d], * [ s -c ][b] [0] * * Output: * c -- cosine(theta), where theta is the implicit angle of rotation * (counter-clockwise) in a plane-rotation * s -- sine(theta) * r -- two-norm of [a; b] */ inline void symOrtho( double a, double b, double &c, double &s, double &r ) { if ( b == 0 ) { c = mathSign( a ); s = 0; r = std::abs( a ); } else if ( a == 0 ) { c = 0; s = mathSign( b ); r = std::abs( b ); } else if ( std::abs( b ) > std::abs( a ) ) { const double tau = a / b; s = mathSign( b ) / std::sqrt( 1 + tau * tau ); c = s * tau; r = b / s; } else { const double tau = b / a; c = mathSign( a ) / std::sqrt( 1 + tau * tau ); s = c * tau; r = a / c; } } /* Iterative LSQR algorithm for solving least squares problems. * * [1] Paige, C. C. and M. A. Saunders, * LSQR: An Algorithm for Sparse Linear Equations And Sparse Least Squares * ACM Trans. Math. Soft., Vol.8, 1982, pp. 43-71. * * Solves the following problem: * argmin_x ||Ax - b|| + damp||x|| * * Output: * x -- approximate solution */ void solveLSQR( const Mat &A, const Mat &b, OutputArray xOut, const double damp = 0.0, const unsigned iter_lim = 10 ) { const int n = A.size().width; CV_Assert( A.size().height == b.size().height ); CV_Assert( A.type() == CV_32F ); CV_Assert( b.type() == CV_32F ); xOut.create( n, 1, CV_32F ); Mat v( n, 1, CV_32F, 0.0f ); Mat u = b; Mat x = xOut.getMat(); x = Mat::zeros( x.size(), x.type() ); double alfa = 0; double beta = cv::norm( u, NORM_L2 ); Mat w( n, 1, CV_32F, 0.0f ); const Mat AT = A.t(); if ( beta > 0 ) { u *= 1 / beta; v = AT * u; alfa = cv::norm( v, NORM_L2 ); } if ( alfa > 0 ) { v *= 1 / alfa; w = v.clone(); } double rhobar = alfa; double phibar = beta; if ( alfa * beta == 0 ) return; for ( unsigned itn = 0; itn < iter_lim; ++itn ) { u *= -alfa; u += A * v; beta = cv::norm( u, NORM_L2 ); if ( beta > 0 ) { u *= 1 / beta; v *= -beta; v += AT * u; alfa = cv::norm( v, NORM_L2 ); if ( alfa > 0 ) v *= 1 / alfa; } double rhobar1 = sqrt( rhobar * rhobar + damp * damp ); double cs1 = rhobar / rhobar1; phibar = cs1 * phibar; double cs, sn, rho; symOrtho( rhobar1, beta, cs, sn, rho ); double theta = sn * alfa; rhobar = -cs * alfa; double phi = cs * phibar; phibar = sn * phibar; double t1 = phi / rho; double t2 = -theta / rho; x += t1 * w; w *= t2; w += v; } } inline void _cpu_fillDCTSampledPoints( float *row, const Point2f &p, const Size &basisSize, const Size &size ) { for ( int n1 = 0; n1 < basisSize.width; ++n1 ) for ( int n2 = 0; n2 < basisSize.height; ++n2 ) row[n1 * basisSize.height + n2] = cosf( ( n1 * CV_PI / size.width ) * ( p.x + 0.5 ) ) * cosf( ( n2 * CV_PI / size.height ) * ( p.y + 0.5 ) ); } ocl::ProgramSource _ocl_fillDCTSampledPointsSource( "__kernel void fillDCTSampledPoints(__global const uchar* features, int fstep, int foff, __global " "uchar* A, int Astep, int Aoff, int fs, int bsw, int bsh, int sw, int sh) {" "const int i = get_global_id(0);" "const int n1 = get_global_id(1);" "const int n2 = get_global_id(2);" "if (i >= fs || n1 >= bsw || n2 >= bsh) return;" "__global const float2* f = (__global const float2*)(features + (fstep * i + foff));" "__global float* a = (__global float*)(A + (Astep * i + Aoff + (n1 * bsh + n2) * sizeof(float)));" "const float2 p = f[0];" "const float pi = 3.14159265358979323846;" "a[0] = cos((n1 * pi / sw) * (p.x + 0.5)) * cos((n2 * pi / sh) * (p.y + 0.5));" "}" ); void applyCLAHE( UMat &img, float claheClip ) { Ptr<CLAHE> clahe = createCLAHE(); clahe->setClipLimit( claheClip ); clahe->apply( img, img ); } void reduceToFlow( const Mat &w1, const Mat &w2, Mat &flow, const Size &basisSize ) { const Size size = flow.size(); Mat flowX( size, CV_32F, 0.0f ); Mat flowY( size, CV_32F, 0.0f ); const float mult = sqrt( static_cast<float>(size.area()) ) * 0.5; for ( int i = 0; i < basisSize.width; ++i ) for ( int j = 0; j < basisSize.height; ++j ) { flowX.at<float>( j, i ) = w1.at<float>( i * basisSize.height + j ) * mult; flowY.at<float>( j, i ) = w2.at<float>( i * basisSize.height + j ) * mult; } for ( int i = 0; i < basisSize.height; ++i ) { flowX.at<float>( i, 0 ) *= M_SQRT2; flowY.at<float>( i, 0 ) *= M_SQRT2; } for ( int i = 0; i < basisSize.width; ++i ) { flowX.at<float>( 0, i ) *= M_SQRT2; flowY.at<float>( 0, i ) *= M_SQRT2; } dct( flowX, flowX, DCT_INVERSE ); dct( flowY, flowY, DCT_INVERSE ); for ( int i = 0; i < size.height; ++i ) for ( int j = 0; j < size.width; ++j ) flow.at<Point2f>( i, j ) = Point2f( flowX.at<float>( i, j ), flowY.at<float>( i, j ) ); } } void OpticalFlowPCAFlow::findSparseFeatures( UMat &from, UMat &to, std::vector<Point2f> &features, std::vector<Point2f> &predictedFeatures ) const { Size size = from.size(); const unsigned maxFeatures = size.area() * sparseRate; goodFeaturesToTrack( from, features, maxFeatures * retainedCornersFraction, 0.005, 3 ); // Add points along the grid if not enough features if ( maxFeatures > features.size() ) { const unsigned missingPoints = maxFeatures - features.size(); const unsigned blockSize = sqrt( (float)size.area() / missingPoints ); for ( int x = blockSize / 2; x < size.width; x += blockSize ) for ( int y = blockSize / 2; y < size.height; y += blockSize ) features.push_back( Point2f( x, y ) ); } std::vector<uchar> predictedStatus; std::vector<float> predictedError; calcOpticalFlowPyrLK( from, to, features, predictedFeatures, predictedStatus, predictedError ); size_t j = 0; for ( size_t i = 0; i < features.size(); ++i ) { if ( predictedStatus[i] ) { features[j] = features[i]; predictedFeatures[j] = predictedFeatures[i]; ++j; } } features.resize( j ); predictedFeatures.resize( j ); } void OpticalFlowPCAFlow::removeOcclusions( UMat &from, UMat &to, std::vector<Point2f> &features, std::vector<Point2f> &predictedFeatures ) const { std::vector<uchar> predictedStatus; std::vector<float> predictedError; std::vector<Point2f> backwardFeatures; calcOpticalFlowPyrLK( to, from, predictedFeatures, backwardFeatures, predictedStatus, predictedError ); size_t j = 0; const float threshold = occlusionsThreshold * sqrt( static_cast<float>(from.size().area()) ); for ( size_t i = 0; i < predictedFeatures.size(); ++i ) { if ( predictedStatus[i] ) { Point2f flowDiff = features[i] - backwardFeatures[i]; if ( flowDiff.dot( flowDiff ) <= threshold ) { features[j] = features[i]; predictedFeatures[j] = predictedFeatures[i]; ++j; } } } features.resize( j ); predictedFeatures.resize( j ); } void OpticalFlowPCAFlow::getSystem( OutputArray AOut, OutputArray b1Out, OutputArray b2Out, const std::vector<Point2f> &features, const std::vector<Point2f> &predictedFeatures, const Size size ) { AOut.create( features.size(), basisSize.area(), CV_32F ); b1Out.create( features.size(), 1, CV_32F ); b2Out.create( features.size(), 1, CV_32F ); if ( useOpenCL ) { UMat A = AOut.getUMat(); Mat b1 = b1Out.getMat(); Mat b2 = b2Out.getMat(); ocl::Kernel kernel( "fillDCTSampledPoints", _ocl_fillDCTSampledPointsSource ); CV_Assert(basisSize.width > 0 && basisSize.height > 0); size_t globSize[] = {features.size(), (size_t)basisSize.width, (size_t)basisSize.height}; kernel .args( cv::ocl::KernelArg::ReadOnlyNoSize( Mat( features ).getUMat( ACCESS_READ ) ), cv::ocl::KernelArg::WriteOnlyNoSize( A ), (int)features.size(), (int)basisSize.width, (int)basisSize.height, (int)size.width, (int)size.height ) .run( 3, globSize, 0, true ); for ( size_t i = 0; i < features.size(); ++i ) { const Point2f flow = predictedFeatures[i] - features[i]; b1.at<float>( i ) = flow.x; b2.at<float>( i ) = flow.y; } } else { Mat A = AOut.getMat(); Mat b1 = b1Out.getMat(); Mat b2 = b2Out.getMat(); for ( size_t i = 0; i < features.size(); ++i ) { _cpu_fillDCTSampledPoints( A.ptr<float>( i ), features[i], basisSize, size ); const Point2f flow = predictedFeatures[i] - features[i]; b1.at<float>( i ) = flow.x; b2.at<float>( i ) = flow.y; } } } 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 ); if ( useOpenCL ) { UMat A = A1Out.getUMat(); Mat b1 = b1Out.getMat(); Mat b2 = b2Out.getMat(); ocl::Kernel kernel( "fillDCTSampledPoints", _ocl_fillDCTSampledPointsSource ); CV_Assert(basisSize.width > 0 && basisSize.height > 0); size_t globSize[] = {features.size(), (size_t)basisSize.width, (size_t)basisSize.height}; kernel .args( cv::ocl::KernelArg::ReadOnlyNoSize( Mat( features ).getUMat( ACCESS_READ ) ), cv::ocl::KernelArg::WriteOnlyNoSize( A ), (int)features.size(), (int)basisSize.width, (int)basisSize.height, (int)size.width, (int)size.height ) .run( 3, globSize, 0, true ); for ( size_t i = 0; i < features.size(); ++i ) { const Point2f flow = predictedFeatures[i] - features[i]; b1.at<float>( i ) = flow.x; b2.at<float>( i ) = flow.y; } } else { Mat A1 = A1Out.getMat(); Mat b1 = b1Out.getMat(); Mat b2 = b2Out.getMat(); for ( size_t i = 0; i < features.size(); ++i ) { _cpu_fillDCTSampledPoints( A1.ptr<float>( i ), features[i], basisSize, size ); const Point2f flow = predictedFeatures[i] - features[i]; b1.at<float>( i ) = flow.x; b2.at<float>( i ) = flow.y; } } Mat A1 = A1Out.getMat(); Mat A2 = A2Out.getMat(); Mat b1 = b1Out.getMat(); Mat b2 = b2Out.getMat(); 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 ) ); } void OpticalFlowPCAFlow::calc( InputArray I0, InputArray I1, InputOutputArray flowOut ) { const Size size = I0.size(); CV_Assert( size == I1.size() ); UMat from, to; if ( I0.channels() == 3 ) { cvtColor( I0, from, COLOR_BGR2GRAY ); from.convertTo( from, CV_8U ); } else { I0.getMat().convertTo( from, CV_8U ); } if ( I1.channels() == 3 ) { cvtColor( I1, to, COLOR_BGR2GRAY ); to.convertTo( to, CV_8U ); } else { I1.getMat().convertTo( to, CV_8U ); } CV_Assert( from.channels() == 1 ); CV_Assert( to.channels() == 1 ); const Mat fromOrig = from.getMat( ACCESS_READ ).clone(); useOpenCL = flowOut.isUMat() && ocl::useOpenCL(); applyCLAHE( from, claheClip ); applyCLAHE( to, claheClip ); std::vector<Point2f> features, predictedFeatures; findSparseFeatures( from, to, features, predictedFeatures ); removeOcclusions( from, to, features, predictedFeatures ); flowOut.create( size, CV_32FC2 ); Mat flow = flowOut.getMat(); Mat w1, w2; if ( prior.get() ) { 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 ); } OpticalFlowPCAFlow::OpticalFlowPCAFlow( Ptr<const PCAPrior> _prior, const Size _basisSize, float _sparseRate, float _retainedCornersFraction, float _occlusionsThreshold, float _dampingFactor, float _claheClip ) : prior( _prior ), basisSize( _basisSize ), sparseRate( _sparseRate ), retainedCornersFraction( _retainedCornersFraction ), occlusionsThreshold( _occlusionsThreshold ), dampingFactor( _dampingFactor ), claheClip( _claheClip ), useOpenCL( false ) { CV_Assert( sparseRate > 0 && sparseRate <= 0.1 ); CV_Assert( retainedCornersFraction >= 0 && retainedCornersFraction <= 1.0 ); CV_Assert( occlusionsThreshold > 0 ); } void OpticalFlowPCAFlow::collectGarbage() {} Ptr<DenseOpticalFlow> createOptFlow_PCAFlow() { return makePtr<OpticalFlowPCAFlow>(); } PCAPrior::PCAPrior( const char *pathToPrior ) { FILE *f = fopen( pathToPrior, "rb" ); 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 PCAPrior::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 ) ); } } }