generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

a LOT of obsolete stuff has been moved to the legacy module.

Browse Source
This commit is contained in:
Vadim Pisarevsky
2012-03-30 12:19:25 +00:00
parent 7e5726e251
commit beb7fc3c92
42 changed files with 3711 additions and 1960 deletions
Download Patch File Download Diff File Expand all files Collapse all files

63
modules/legacy/src/_featuretree.h Normal file
View File

@@ -0,0 +1,63 @@
/*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.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2009, Xavier Delacour, 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 Intel Corporation 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*/
// 2009-01-09, Xavier Delacour <xavier.delacour@gmail.com>
#ifndef __opencv_featuretree_h__
#define __opencv_featuretree_h__
struct CvFeatureTree {
CvFeatureTree(const CvFeatureTree& x);
CvFeatureTree& operator= (const CvFeatureTree& rhs);
CvFeatureTree() {}
virtual ~CvFeatureTree() {}
virtual void FindFeatures(const CvMat* d, int k, int emax, CvMat* results, CvMat* dist) = 0;
virtual int FindOrthoRange(CvMat* /*bounds_min*/, CvMat* /*bounds_max*/,CvMat* /*results*/) {
return 0;
}
};
#endif // __cv_featuretree_h__
// Local Variables:
// mode:C++
// End:

467
modules/legacy/src/_kdtree.hpp Normal file
View File

@@ -0,0 +1,467 @@
/*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.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2008, Xavier Delacour, 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 Intel Corporation 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*/
// 2008-05-13, Xavier Delacour <xavier.delacour@gmail.com>
#ifndef __cv_kdtree_h__
#define __cv_kdtree_h__
#include "precomp.hpp"
#include <vector>
#include <algorithm>
#include <limits>
#include <iostream>
#include "assert.h"
#include "math.h"
#if _MSC_VER >= 1400
#pragma warning(disable: 4512) // suppress "assignment operator could not be generated"
#endif
// J.S. Beis and D.G. Lowe. Shape indexing using approximate nearest-neighbor search
// in highdimensional spaces. In Proc. IEEE Conf. Comp. Vision Patt. Recog.,
// pages 1000--1006, 1997. http://citeseer.ist.psu.edu/beis97shape.html
#undef __deref
#undef __valuetype
template < class __valuetype, class __deref >
class CvKDTree {
public:
typedef __deref deref_type;
typedef typename __deref::scalar_type scalar_type;
typedef typename __deref::accum_type accum_type;
private:
struct node {
int dim; // split dimension; >=0 for nodes, -1 for leaves
__valuetype value; // if leaf, value of leaf
int left, right; // node indices of left and right branches
scalar_type boundary; // left if deref(value,dim)<=boundary, otherwise right
};
typedef std::vector < node > node_array;
__deref deref; // requires operator() (__valuetype lhs,int dim)
node_array nodes; // node storage
int point_dim; // dimension of points (the k in kd-tree)
int root_node; // index of root node, -1 if empty tree
// for given set of point indices, compute dimension of highest variance
template < class __instype, class __valuector >
int dimension_of_highest_variance(__instype * first, __instype * last,
__valuector ctor) {
assert(last - first > 0);
accum_type maxvar = -std::numeric_limits < accum_type >::max();
int maxj = -1;
for (int j = 0; j < point_dim; ++j) {
accum_type mean = 0;
for (__instype * k = first; k < last; ++k)
mean += deref(ctor(*k), j);
mean /= last - first;
accum_type var = 0;
for (__instype * k = first; k < last; ++k) {
accum_type diff = accum_type(deref(ctor(*k), j)) - mean;
var += diff * diff;
}
var /= last - first;
assert(maxj != -1 || var >= maxvar);
if (var >= maxvar) {
maxvar = var;
maxj = j;
}
}
return maxj;
}
// given point indices and dimension, find index of median; (almost) modifies [first,last)
// such that points_in[first,median]<=point[median], points_in(median,last)>point[median].
// implemented as partial quicksort; expected linear perf.
template < class __instype, class __valuector >
__instype * median_partition(__instype * first, __instype * last,
int dim, __valuector ctor) {
assert(last - first > 0);
__instype *k = first + (last - first) / 2;
median_partition(first, last, k, dim, ctor);
return k;
}
template < class __instype, class __valuector >
struct median_pr {
const __instype & pivot;
int dim;
__deref deref;
__valuector ctor;
median_pr(const __instype & _pivot, int _dim, __deref _deref, __valuector _ctor)
: pivot(_pivot), dim(_dim), deref(_deref), ctor(_ctor) {
}
bool operator() (const __instype & lhs) const {
return deref(ctor(lhs), dim) <= deref(ctor(pivot), dim);
}
};
template < class __instype, class __valuector >
void median_partition(__instype * first, __instype * last,
__instype * k, int dim, __valuector ctor) {
int pivot = (int)((last - first) / 2);
std::swap(first[pivot], last[-1]);
__instype *middle = std::partition(first, last - 1,
median_pr < __instype, __valuector >
(last[-1], dim, deref, ctor));
std::swap(*middle, last[-1]);
if (middle < k)
median_partition(middle + 1, last, k, dim, ctor);
else if (middle > k)
median_partition(first, middle, k, dim, ctor);
}
// insert given points into the tree; return created node
template < class __instype, class __valuector >
int insert(__instype * first, __instype * last, __valuector ctor) {
if (first == last)
return -1;
else {
int dim = dimension_of_highest_variance(first, last, ctor);
__instype *median = median_partition(first, last, dim, ctor);
__instype *split = median;
for (; split != last && deref(ctor(*split), dim) ==
deref(ctor(*median), dim); ++split);
if (split == last) { // leaf
int nexti = -1;
for (--split; split >= first; --split) {
int i = (int)nodes.size();
node & n = *nodes.insert(nodes.end(), node());
n.dim = -1;
n.value = ctor(*split);
n.left = -1;
n.right = nexti;
nexti = i;
}
return nexti;
} else { // node
int i = (int)nodes.size();
// note that recursive insert may invalidate this ref
node & n = *nodes.insert(nodes.end(), node());
n.dim = dim;
n.boundary = deref(ctor(*median), dim);
int left = insert(first, split, ctor);
nodes[i].left = left;
int right = insert(split, last, ctor);
nodes[i].right = right;
return i;
}
}
}
// run to leaf; linear search for p;
// if found, remove paths to empty leaves on unwind
bool remove(int *i, const __valuetype & p) {
if (*i == -1)
return false;
node & n = nodes[*i];
bool r;
if (n.dim >= 0) { // node
if (deref(p, n.dim) <= n.boundary) // left
r = remove(&n.left, p);
else // right
r = remove(&n.right, p);
// if terminal, remove this node
if (n.left == -1 && n.right == -1)
*i = -1;
return r;
} else { // leaf
if (n.value == p) {
*i = n.right;
return true;
} else
return remove(&n.right, p);
}
}
public:
struct identity_ctor {
const __valuetype & operator() (const __valuetype & rhs) const {
return rhs;
}
};
// initialize an empty tree
CvKDTree(__deref _deref = __deref())
: deref(_deref), root_node(-1) {
}
// given points, initialize a balanced tree
CvKDTree(__valuetype * first, __valuetype * last, int _point_dim,
__deref _deref = __deref())
: deref(_deref) {
set_data(first, last, _point_dim, identity_ctor());
}
// given points, initialize a balanced tree
template < class __instype, class __valuector >
CvKDTree(__instype * first, __instype * last, int _point_dim,
__valuector ctor, __deref _deref = __deref())
: deref(_deref) {
set_data(first, last, _point_dim, ctor);
}
void set_deref(__deref _deref) {
deref = _deref;
}
void set_data(__valuetype * first, __valuetype * last, int _point_dim) {
set_data(first, last, _point_dim, identity_ctor());
}
template < class __instype, class __valuector >
void set_data(__instype * first, __instype * last, int _point_dim,
__valuector ctor) {
point_dim = _point_dim;
nodes.clear();
nodes.reserve(last - first);
root_node = insert(first, last, ctor);
}
int dims() const {
return point_dim;
}
// remove the given point
bool remove(const __valuetype & p) {
return remove(&root_node, p);
}
void print() const {
print(root_node);
}
void print(int i, int indent = 0) const {
if (i == -1)
return;
for (int j = 0; j < indent; ++j)
std::cout << " ";
const node & n = nodes[i];
if (n.dim >= 0) {
std::cout << "node " << i << ", left " << nodes[i].left << ", right " <<
nodes[i].right << ", dim " << nodes[i].dim << ", boundary " <<
nodes[i].boundary << std::endl;
print(n.left, indent + 3);
print(n.right, indent + 3);
} else
std::cout << "leaf " << i << ", value = " << nodes[i].value << std::endl;
}
////////////////////////////////////////////////////////////////////////////////////////
// bbf search
public:
struct bbf_nn { // info on found neighbors (approx k nearest)
const __valuetype *p; // nearest neighbor
accum_type dist; // distance from d to query point
bbf_nn(const __valuetype & _p, accum_type _dist)
: p(&_p), dist(_dist) {
}
bool operator<(const bbf_nn & rhs) const {
return dist < rhs.dist;
}
};
typedef std::vector < bbf_nn > bbf_nn_pqueue;
private:
struct bbf_node { // info on branches not taken
int node; // corresponding node
accum_type dist; // minimum distance from bounds to query point
bbf_node(int _node, accum_type _dist)
: node(_node), dist(_dist) {
}
bool operator<(const bbf_node & rhs) const {
return dist > rhs.dist;
}
};
typedef std::vector < bbf_node > bbf_pqueue;
mutable bbf_pqueue tmp_pq;
// called for branches not taken, as bbf walks to leaf;
// construct bbf_node given minimum distance to bounds of alternate branch
void pq_alternate(int alt_n, bbf_pqueue & pq, scalar_type dist) const {
if (alt_n == -1)
return;
// add bbf_node for alternate branch in priority queue
pq.push_back(bbf_node(alt_n, dist));
std::push_heap(pq.begin(), pq.end());
}
// called by bbf to walk to leaf;
// takes one step down the tree towards query point d
template < class __desctype >
int bbf_branch(int i, const __desctype * d, bbf_pqueue & pq) const {
const node & n = nodes[i];
// push bbf_node with bounds of alternate branch, then branch
if (d[n.dim] <= n.boundary) { // left
pq_alternate(n.right, pq, n.boundary - d[n.dim]);
return n.left;
} else { // right
pq_alternate(n.left, pq, d[n.dim] - n.boundary);
return n.right;
}
}
// compute euclidean distance between two points
template < class __desctype >
accum_type distance(const __desctype * d, const __valuetype & p) const {
accum_type dist = 0;
for (int j = 0; j < point_dim; ++j) {
accum_type diff = accum_type(d[j]) - accum_type(deref(p, j));
dist += diff * diff;
} return (accum_type) sqrt(dist);
}
// called per candidate nearest neighbor; constructs new bbf_nn for
// candidate and adds it to priority queue of all candidates; if
// queue len exceeds k, drops the point furthest from query point d.
template < class __desctype >
void bbf_new_nn(bbf_nn_pqueue & nn_pq, int k,
const __desctype * d, const __valuetype & p) const {
bbf_nn nn(p, distance(d, p));
if ((int) nn_pq.size() < k) {
nn_pq.push_back(nn);
std::push_heap(nn_pq.begin(), nn_pq.end());
} else if (nn_pq[0].dist > nn.dist) {
std::pop_heap(nn_pq.begin(), nn_pq.end());
nn_pq.end()[-1] = nn;
std::push_heap(nn_pq.begin(), nn_pq.end());
}
assert(nn_pq.size() < 2 || nn_pq[0].dist >= nn_pq[1].dist);
}
public:
// finds (with high probability) the k nearest neighbors of d,
// searching at most emax leaves/bins.
// ret_nn_pq is an array containing the (at most) k nearest neighbors
// (see bbf_nn structure def above).
template < class __desctype >
int find_nn_bbf(const __desctype * d,
int k, int emax,
bbf_nn_pqueue & ret_nn_pq) const {
assert(k > 0);
ret_nn_pq.clear();
if (root_node == -1)
return 0;
// add root_node to bbf_node priority queue;
// iterate while queue non-empty and emax>0
tmp_pq.clear();
tmp_pq.push_back(bbf_node(root_node, 0));
while (tmp_pq.size() && emax > 0) {
// from node nearest query point d, run to leaf
std::pop_heap(tmp_pq.begin(), tmp_pq.end());
bbf_node bbf(tmp_pq.end()[-1]);
tmp_pq.erase(tmp_pq.end() - 1);
int i;
for (i = bbf.node;
i != -1 && nodes[i].dim >= 0;
i = bbf_branch(i, d, tmp_pq));
if (i != -1) {
// add points in leaf/bin to ret_nn_pq
do {
bbf_new_nn(ret_nn_pq, k, d, nodes[i].value);
} while (-1 != (i = nodes[i].right));
--emax;
}
}
tmp_pq.clear();
return (int)ret_nn_pq.size();
}
////////////////////////////////////////////////////////////////////////////////////////
// orthogonal range search
private:
void find_ortho_range(int i, scalar_type * bounds_min,
scalar_type * bounds_max,
std::vector < __valuetype > &inbounds) const {
if (i == -1)
return;
const node & n = nodes[i];
if (n.dim >= 0) { // node
if (bounds_min[n.dim] <= n.boundary)
find_ortho_range(n.left, bounds_min, bounds_max, inbounds);
if (bounds_max[n.dim] > n.boundary)
find_ortho_range(n.right, bounds_min, bounds_max, inbounds);
} else { // leaf
do {
inbounds.push_back(nodes[i].value);
} while (-1 != (i = nodes[i].right));
}
}
public:
// return all points that lie within the given bounds; inbounds is cleared
int find_ortho_range(scalar_type * bounds_min,
scalar_type * bounds_max,
std::vector < __valuetype > &inbounds) const {
inbounds.clear();
find_ortho_range(root_node, bounds_min, bounds_max, inbounds);
return (int)inbounds.size();
}
};
#endif // __cv_kdtree_h__
// Local Variables:
// mode:C++
// End:

741
modules/legacy/src/bgfg_acmmm2003.cpp Normal file
View File

@@ -0,0 +1,741 @@
/*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.
//
//
// Intel License Agreement
//
// Copyright (C) 2000, Intel Corporation, 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 Intel Corporation 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*/
// This file implements the foreground/background pixel
// discrimination algorithm described in
//
// Foreground Object Detection from Videos Containing Complex Background
// Li, Huan, Gu, Tian 2003 9p
// http://muq.org/~cynbe/bib/foreground-object-detection-from-videos-containing-complex-background.pdf
#include "precomp.hpp"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
//#include <algorithm>
static double* _cv_max_element( double* start, double* end )
{
double* p = start++;
for( ; start != end; ++start) {
if (*p < *start) p = start;
}
return p;
}
static void CV_CDECL icvReleaseFGDStatModel( CvFGDStatModel** model );
static int CV_CDECL icvUpdateFGDStatModel( IplImage* curr_frame,
CvFGDStatModel* model,
double );
// Function cvCreateFGDStatModel initializes foreground detection process
// parameters:
// first_frame - frame from video sequence
// parameters - (optional) if NULL default parameters of the algorithm will be used
// p_model - pointer to CvFGDStatModel structure
CV_IMPL CvBGStatModel*
cvCreateFGDStatModel( IplImage* first_frame, CvFGDStatModelParams* parameters )
{
CvFGDStatModel* p_model = 0;
CV_FUNCNAME( "cvCreateFGDStatModel" );
__BEGIN__;
int i, j, k, pixel_count, buf_size;
CvFGDStatModelParams params;
if( !CV_IS_IMAGE(first_frame) )
CV_ERROR( CV_StsBadArg, "Invalid or NULL first_frame parameter" );
if (first_frame->nChannels != 3)
CV_ERROR( CV_StsBadArg, "first_frame must have 3 color channels" );
// Initialize parameters:
if( parameters == NULL )
{
params.Lc = CV_BGFG_FGD_LC;
params.N1c = CV_BGFG_FGD_N1C;
params.N2c = CV_BGFG_FGD_N2C;
params.Lcc = CV_BGFG_FGD_LCC;
params.N1cc = CV_BGFG_FGD_N1CC;
params.N2cc = CV_BGFG_FGD_N2CC;
params.delta = CV_BGFG_FGD_DELTA;
params.alpha1 = CV_BGFG_FGD_ALPHA_1;
params.alpha2 = CV_BGFG_FGD_ALPHA_2;
params.alpha3 = CV_BGFG_FGD_ALPHA_3;
params.T = CV_BGFG_FGD_T;
params.minArea = CV_BGFG_FGD_MINAREA;
params.is_obj_without_holes = 1;
params.perform_morphing = 1;
}
else
{
params = *parameters;
}
CV_CALL( p_model = (CvFGDStatModel*)cvAlloc( sizeof(*p_model) ));
memset( p_model, 0, sizeof(*p_model) );
p_model->type = CV_BG_MODEL_FGD;
p_model->release = (CvReleaseBGStatModel)icvReleaseFGDStatModel;
p_model->update = (CvUpdateBGStatModel)icvUpdateFGDStatModel;;
p_model->params = params;
// Initialize storage pools:
pixel_count = first_frame->width * first_frame->height;
buf_size = pixel_count*sizeof(p_model->pixel_stat[0]);
CV_CALL( p_model->pixel_stat = (CvBGPixelStat*)cvAlloc(buf_size) );
memset( p_model->pixel_stat, 0, buf_size );
buf_size = pixel_count*params.N2c*sizeof(p_model->pixel_stat[0].ctable[0]);
CV_CALL( p_model->pixel_stat[0].ctable = (CvBGPixelCStatTable*)cvAlloc(buf_size) );
memset( p_model->pixel_stat[0].ctable, 0, buf_size );
buf_size = pixel_count*params.N2cc*sizeof(p_model->pixel_stat[0].cctable[0]);
CV_CALL( p_model->pixel_stat[0].cctable = (CvBGPixelCCStatTable*)cvAlloc(buf_size) );
memset( p_model->pixel_stat[0].cctable, 0, buf_size );
for( i = 0, k = 0; i < first_frame->height; i++ ) {
for( j = 0; j < first_frame->width; j++, k++ )
{
p_model->pixel_stat[k].ctable = p_model->pixel_stat[0].ctable + k*params.N2c;
p_model->pixel_stat[k].cctable = p_model->pixel_stat[0].cctable + k*params.N2cc;
}
}
// Init temporary images:
CV_CALL( p_model->Ftd = cvCreateImage(cvSize(first_frame->width, first_frame->height), IPL_DEPTH_8U, 1));
CV_CALL( p_model->Fbd = cvCreateImage(cvSize(first_frame->width, first_frame->height), IPL_DEPTH_8U, 1));
CV_CALL( p_model->foreground = cvCreateImage(cvSize(first_frame->width, first_frame->height), IPL_DEPTH_8U, 1));
CV_CALL( p_model->background = cvCloneImage(first_frame));
CV_CALL( p_model->prev_frame = cvCloneImage(first_frame));
CV_CALL( p_model->storage = cvCreateMemStorage());
__END__;
if( cvGetErrStatus() < 0 )
{
CvBGStatModel* base_ptr = (CvBGStatModel*)p_model;
if( p_model && p_model->release )
p_model->release( &base_ptr );
else
cvFree( &p_model );
p_model = 0;
}
return (CvBGStatModel*)p_model;
}
static void CV_CDECL
icvReleaseFGDStatModel( CvFGDStatModel** _model )
{
CV_FUNCNAME( "icvReleaseFGDStatModel" );
__BEGIN__;
if( !_model )
CV_ERROR( CV_StsNullPtr, "" );
if( *_model )
{
CvFGDStatModel* model = *_model;
if( model->pixel_stat )
{
cvFree( &model->pixel_stat[0].ctable );
cvFree( &model->pixel_stat[0].cctable );
cvFree( &model->pixel_stat );
}
cvReleaseImage( &model->Ftd );
cvReleaseImage( &model->Fbd );
cvReleaseImage( &model->foreground );
cvReleaseImage( &model->background );
cvReleaseImage( &model->prev_frame );
cvReleaseMemStorage(&model->storage);
cvFree( _model );
}
__END__;
}
// Function cvChangeDetection performs change detection for Foreground detection algorithm
// parameters:
// prev_frame -
// curr_frame -
// change_mask -
CV_IMPL int
cvChangeDetection( IplImage* prev_frame,
IplImage* curr_frame,
IplImage* change_mask )
{
int i, j, b, x, y, thres;
const int PIXELRANGE=256;
if( !prev_frame
|| !curr_frame
|| !change_mask
|| prev_frame->nChannels != 3
|| curr_frame->nChannels != 3
|| change_mask->nChannels != 1
|| prev_frame->depth != IPL_DEPTH_8U
|| curr_frame->depth != IPL_DEPTH_8U
|| change_mask->depth != IPL_DEPTH_8U
|| prev_frame->width != curr_frame->width
|| prev_frame->height != curr_frame->height
|| prev_frame->width != change_mask->width
|| prev_frame->height != change_mask->height
){
return 0;
}
cvZero ( change_mask );
// All operations per colour
for (b=0 ; b<prev_frame->nChannels ; b++) {
// Create histogram:
long HISTOGRAM[PIXELRANGE];
for (i=0 ; i<PIXELRANGE; i++) HISTOGRAM[i]=0;
for (y=0 ; y<curr_frame->height ; y++)
{
uchar* rowStart1 = (uchar*)curr_frame->imageData + y * curr_frame->widthStep + b;
uchar* rowStart2 = (uchar*)prev_frame->imageData + y * prev_frame->widthStep + b;
for (x=0 ; x<curr_frame->width ; x++, rowStart1+=curr_frame->nChannels, rowStart2+=prev_frame->nChannels) {
int diff = abs( int(*rowStart1) - int(*rowStart2) );
HISTOGRAM[diff]++;
}
}
double relativeVariance[PIXELRANGE];
for (i=0 ; i<PIXELRANGE; i++) relativeVariance[i]=0;
for (thres=PIXELRANGE-2; thres>=0 ; thres--)
{
// fprintf(stderr, "Iter %d\n", thres);
double sum=0;
double sqsum=0;
int count=0;
// fprintf(stderr, "Iter %d entering loop\n", thres);
for (j=thres ; j<PIXELRANGE ; j++) {
sum += double(j)*double(HISTOGRAM[j]);
sqsum += double(j*j)*double(HISTOGRAM[j]);
count += HISTOGRAM[j];
}
count = count == 0 ? 1 : count;
// fprintf(stderr, "Iter %d finishing loop\n", thres);
double my = sum / count;
double sigma = sqrt( sqsum/count - my*my);
// fprintf(stderr, "Iter %d sum=%g sqsum=%g count=%d sigma = %g\n", thres, sum, sqsum, count, sigma);
// fprintf(stderr, "Writing to %x\n", &(relativeVariance[thres]));
relativeVariance[thres] = sigma;
// fprintf(stderr, "Iter %d finished\n", thres);
}
// Find maximum:
uchar bestThres = 0;
double* pBestThres = _cv_max_element(relativeVariance, relativeVariance+PIXELRANGE);
bestThres = (uchar)(*pBestThres); if (bestThres <10) bestThres=10;
for (y=0 ; y<prev_frame->height ; y++)
{
uchar* rowStart1 = (uchar*)(curr_frame->imageData) + y * curr_frame->widthStep + b;
uchar* rowStart2 = (uchar*)(prev_frame->imageData) + y * prev_frame->widthStep + b;
uchar* rowStart3 = (uchar*)(change_mask->imageData) + y * change_mask->widthStep;
for (x = 0; x < curr_frame->width; x++, rowStart1+=curr_frame->nChannels,
rowStart2+=prev_frame->nChannels, rowStart3+=change_mask->nChannels) {
// OR between different color channels
int diff = abs( int(*rowStart1) - int(*rowStart2) );
if ( diff > bestThres)
*rowStart3 |=255;
}
}
}
return 1;
}
#define MIN_PV 1E-10
#define V_C(k,l) ctable[k].v[l]
#define PV_C(k) ctable[k].Pv
#define PVB_C(k) ctable[k].Pvb
#define V_CC(k,l) cctable[k].v[l]
#define PV_CC(k) cctable[k].Pv
#define PVB_CC(k) cctable[k].Pvb
// Function cvUpdateFGDStatModel updates statistical model and returns number of foreground regions
// parameters:
// curr_frame - current frame from video sequence
// p_model - pointer to CvFGDStatModel structure
static int CV_CDECL
icvUpdateFGDStatModel( IplImage* curr_frame, CvFGDStatModel* model, double )
{
int mask_step = model->Ftd->widthStep;
CvSeq *first_seq = NULL, *prev_seq = NULL, *seq = NULL;
IplImage* prev_frame = model->prev_frame;
int region_count = 0;
int FG_pixels_count = 0;
int deltaC = cvRound(model->params.delta * 256 / model->params.Lc);
int deltaCC = cvRound(model->params.delta * 256 / model->params.Lcc);
int i, j, k, l;
//clear storages
cvClearMemStorage(model->storage);
cvZero(model->foreground);
// From foreground pixel candidates using image differencing
// with adaptive thresholding. The algorithm is from:
//
// Thresholding for Change Detection
// Paul L. Rosin 1998 6p
// http://www.cis.temple.edu/~latecki/Courses/CIS750-03/Papers/thresh-iccv.pdf
//
cvChangeDetection( prev_frame, curr_frame, model->Ftd );
cvChangeDetection( model->background, curr_frame, model->Fbd );
for( i = 0; i < model->Ftd->height; i++ )
{
for( j = 0; j < model->Ftd->width; j++ )
{
if( ((uchar*)model->Fbd->imageData)[i*mask_step+j] || ((uchar*)model->Ftd->imageData)[i*mask_step+j] )
{
float Pb = 0;
float Pv = 0;
float Pvb = 0;
CvBGPixelStat* stat = model->pixel_stat + i * model->Ftd->width + j;
CvBGPixelCStatTable* ctable = stat->ctable;
CvBGPixelCCStatTable* cctable = stat->cctable;
uchar* curr_data = (uchar*)(curr_frame->imageData) + i*curr_frame->widthStep + j*3;
uchar* prev_data = (uchar*)(prev_frame->imageData) + i*prev_frame->widthStep + j*3;
int val = 0;
// Is it a motion pixel?
if( ((uchar*)model->Ftd->imageData)[i*mask_step+j] )
{
if( !stat->is_trained_dyn_model ) {
val = 1;
} else {
// Compare with stored CCt vectors:
for( k = 0; PV_CC(k) > model->params.alpha2 && k < model->params.N1cc; k++ )
{
if ( abs( V_CC(k,0) - prev_data[0]) <= deltaCC &&
abs( V_CC(k,1) - prev_data[1]) <= deltaCC &&
abs( V_CC(k,2) - prev_data[2]) <= deltaCC &&
abs( V_CC(k,3) - curr_data[0]) <= deltaCC &&
abs( V_CC(k,4) - curr_data[1]) <= deltaCC &&
abs( V_CC(k,5) - curr_data[2]) <= deltaCC)
{
Pv += PV_CC(k);
Pvb += PVB_CC(k);
}
}
Pb = stat->Pbcc;
if( 2 * Pvb * Pb <= Pv ) val = 1;
}
}
else if( stat->is_trained_st_model )
{
// Compare with stored Ct vectors:
for( k = 0; PV_C(k) > model->params.alpha2 && k < model->params.N1c; k++ )
{
if ( abs( V_C(k,0) - curr_data[0]) <= deltaC &&
abs( V_C(k,1) - curr_data[1]) <= deltaC &&
abs( V_C(k,2) - curr_data[2]) <= deltaC )
{
Pv += PV_C(k);
Pvb += PVB_C(k);
}
}
Pb = stat->Pbc;
if( 2 * Pvb * Pb <= Pv ) val = 1;
}
// Update foreground:
((uchar*)model->foreground->imageData)[i*mask_step+j] = (uchar)(val*255);
FG_pixels_count += val;
} // end if( change detection...
} // for j...
} // for i...
//end BG/FG classification
// Foreground segmentation.
// Smooth foreground map:
if( model->params.perform_morphing ){
cvMorphologyEx( model->foreground, model->foreground, 0, 0, CV_MOP_OPEN, model->params.perform_morphing );
cvMorphologyEx( model->foreground, model->foreground, 0, 0, CV_MOP_CLOSE, model->params.perform_morphing );
}
if( model->params.minArea > 0 || model->params.is_obj_without_holes ){
// Discard under-size foreground regions:
//
cvFindContours( model->foreground, model->storage, &first_seq, sizeof(CvContour), CV_RETR_LIST );
for( seq = first_seq; seq; seq = seq->h_next )
{
CvContour* cnt = (CvContour*)seq;
if( cnt->rect.width * cnt->rect.height < model->params.minArea ||
(model->params.is_obj_without_holes && CV_IS_SEQ_HOLE(seq)) )
{
// Delete under-size contour:
prev_seq = seq->h_prev;
if( prev_seq )
{
prev_seq->h_next = seq->h_next;
if( seq->h_next ) seq->h_next->h_prev = prev_seq;
}
else
{
first_seq = seq->h_next;
if( seq->h_next ) seq->h_next->h_prev = NULL;
}
}
else
{
region_count++;
}
}
model->foreground_regions = first_seq;
cvZero(model->foreground);
cvDrawContours(model->foreground, first_seq, CV_RGB(0, 0, 255), CV_RGB(0, 0, 255), 10, -1);
} else {
model->foreground_regions = NULL;
}
// Check ALL BG update condition:
if( ((float)FG_pixels_count/(model->Ftd->width*model->Ftd->height)) > CV_BGFG_FGD_BG_UPDATE_TRESH )
{
for( i = 0; i < model->Ftd->height; i++ )
for( j = 0; j < model->Ftd->width; j++ )
{
CvBGPixelStat* stat = model->pixel_stat + i * model->Ftd->width + j;
stat->is_trained_st_model = stat->is_trained_dyn_model = 1;
}
}
// Update background model:
for( i = 0; i < model->Ftd->height; i++ )
{
for( j = 0; j < model->Ftd->width; j++ )
{
CvBGPixelStat* stat = model->pixel_stat + i * model->Ftd->width + j;
CvBGPixelCStatTable* ctable = stat->ctable;
CvBGPixelCCStatTable* cctable = stat->cctable;
uchar *curr_data = (uchar*)(curr_frame->imageData)+i*curr_frame->widthStep+j*3;
uchar *prev_data = (uchar*)(prev_frame->imageData)+i*prev_frame->widthStep+j*3;
if( ((uchar*)model->Ftd->imageData)[i*mask_step+j] || !stat->is_trained_dyn_model )
{
float alpha = stat->is_trained_dyn_model ? model->params.alpha2 : model->params.alpha3;
float diff = 0;
int dist, min_dist = 2147483647, indx = -1;
//update Pb
stat->Pbcc *= (1.f-alpha);
if( !((uchar*)model->foreground->imageData)[i*mask_step+j] )
{
stat->Pbcc += alpha;
}
// Find best Vi match:
for(k = 0; PV_CC(k) && k < model->params.N2cc; k++ )
{
// Exponential decay of memory
PV_CC(k) *= (1-alpha);
PVB_CC(k) *= (1-alpha);
if( PV_CC(k) < MIN_PV )
{
PV_CC(k) = 0;
PVB_CC(k) = 0;
continue;
}
dist = 0;
for( l = 0; l < 3; l++ )
{
int val = abs( V_CC(k,l) - prev_data[l] );
if( val > deltaCC ) break;
dist += val;
val = abs( V_CC(k,l+3) - curr_data[l] );
if( val > deltaCC) break;
dist += val;
}
if( l == 3 && dist < min_dist )
{
min_dist = dist;
indx = k;
}
}
if( indx < 0 )
{ // Replace N2th elem in the table by new feature:
indx = model->params.N2cc - 1;
PV_CC(indx) = alpha;
PVB_CC(indx) = alpha;
//udate Vt
for( l = 0; l < 3; l++ )
{
V_CC(indx,l) = prev_data[l];
V_CC(indx,l+3) = curr_data[l];
}
}
else
{ // Update:
PV_CC(indx) += alpha;
if( !((uchar*)model->foreground->imageData)[i*mask_step+j] )
{
PVB_CC(indx) += alpha;
}
}
//re-sort CCt table by Pv
for( k = 0; k < indx; k++ )
{
if( PV_CC(k) <= PV_CC(indx) )
{
//shift elements
CvBGPixelCCStatTable tmp1, tmp2 = cctable[indx];
for( l = k; l <= indx; l++ )
{
tmp1 = cctable[l];
cctable[l] = tmp2;
tmp2 = tmp1;
}
break;
}
}
float sum1=0, sum2=0;
//check "once-off" changes
for(k = 0; PV_CC(k) && k < model->params.N1cc; k++ )
{
sum1 += PV_CC(k);
sum2 += PVB_CC(k);
}
if( sum1 > model->params.T ) stat->is_trained_dyn_model = 1;
diff = sum1 - stat->Pbcc * sum2;
// Update stat table:
if( diff > model->params.T )
{
//printf("once off change at motion mode\n");
//new BG features are discovered
for( k = 0; PV_CC(k) && k < model->params.N1cc; k++ )
{
PVB_CC(k) =
(PV_CC(k)-stat->Pbcc*PVB_CC(k))/(1-stat->Pbcc);
}
assert(stat->Pbcc<=1 && stat->Pbcc>=0);
}
}
// Handle "stationary" pixel:
if( !((uchar*)model->Ftd->imageData)[i*mask_step+j] )
{
float alpha = stat->is_trained_st_model ? model->params.alpha2 : model->params.alpha3;
float diff = 0;
int dist, min_dist = 2147483647, indx = -1;
//update Pb
stat->Pbc *= (1.f-alpha);
if( !((uchar*)model->foreground->imageData)[i*mask_step+j] )
{
stat->Pbc += alpha;
}
//find best Vi match
for( k = 0; k < model->params.N2c; k++ )
{
// Exponential decay of memory
PV_C(k) *= (1-alpha);
PVB_C(k) *= (1-alpha);
if( PV_C(k) < MIN_PV )
{
PV_C(k) = 0;
PVB_C(k) = 0;
continue;
}
dist = 0;
for( l = 0; l < 3; l++ )
{
int val = abs( V_C(k,l) - curr_data[l] );
if( val > deltaC ) break;
dist += val;
}
if( l == 3 && dist < min_dist )
{
min_dist = dist;
indx = k;
}
}
if( indx < 0 )
{//N2th elem in the table is replaced by a new features
indx = model->params.N2c - 1;
PV_C(indx) = alpha;
PVB_C(indx) = alpha;
//udate Vt
for( l = 0; l < 3; l++ )
{
V_C(indx,l) = curr_data[l];
}
} else
{//update
PV_C(indx) += alpha;
if( !((uchar*)model->foreground->imageData)[i*mask_step+j] )
{
PVB_C(indx) += alpha;
}
}
//re-sort Ct table by Pv
for( k = 0; k < indx; k++ )
{
if( PV_C(k) <= PV_C(indx) )
{
//shift elements
CvBGPixelCStatTable tmp1, tmp2 = ctable[indx];
for( l = k; l <= indx; l++ )
{
tmp1 = ctable[l];
ctable[l] = tmp2;
tmp2 = tmp1;
}
break;
}
}
// Check "once-off" changes:
float sum1=0, sum2=0;
for( k = 0; PV_C(k) && k < model->params.N1c; k++ )
{
sum1 += PV_C(k);
sum2 += PVB_C(k);
}
diff = sum1 - stat->Pbc * sum2;
if( sum1 > model->params.T ) stat->is_trained_st_model = 1;
// Update stat table:
if( diff > model->params.T )
{
//printf("once off change at stat mode\n");
//new BG features are discovered
for( k = 0; PV_C(k) && k < model->params.N1c; k++ )
{
PVB_C(k) = (PV_C(k)-stat->Pbc*PVB_C(k))/(1-stat->Pbc);
}
stat->Pbc = 1 - stat->Pbc;
}
} // if !(change detection) at pixel (i,j)
// Update the reference BG image:
if( !((uchar*)model->foreground->imageData)[i*mask_step+j])
{
uchar* ptr = ((uchar*)model->background->imageData) + i*model->background->widthStep+j*3;
if( !((uchar*)model->Ftd->imageData)[i*mask_step+j] &&
!((uchar*)model->Fbd->imageData)[i*mask_step+j] )
{
// Apply IIR filter:
for( l = 0; l < 3; l++ )
{
int a = cvRound(ptr[l]*(1 - model->params.alpha1) + model->params.alpha1*curr_data[l]);
ptr[l] = (uchar)a;
//((uchar*)model->background->imageData)[i*model->background->widthStep+j*3+l]*=(1 - model->params.alpha1);
//((uchar*)model->background->imageData)[i*model->background->widthStep+j*3+l] += model->params.alpha1*curr_data[l];
}
}
else
{
// Background change detected:
for( l = 0; l < 3; l++ )
{
//((uchar*)model->background->imageData)[i*model->background->widthStep+j*3+l] = curr_data[l];
ptr[l] = curr_data[l];
}
}
}
} // j
} // i
// Keep previous frame:
cvCopy( curr_frame, model->prev_frame );
return region_count;
}
/* End of file. */

362
modules/legacy/src/bgfg_codebook.cpp Normal file
View File

@@ -0,0 +1,362 @@
/*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.
//
//
// Intel License Agreement
//
// Copyright (C) 2000, Intel Corporation, 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 Intel Corporation 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"
CvBGCodeBookModel* cvCreateBGCodeBookModel()
{
CvBGCodeBookModel* model = (CvBGCodeBookModel*)cvAlloc( sizeof(*model) );
memset( model, 0, sizeof(*model) );
model->cbBounds[0] = model->cbBounds[1] = model->cbBounds[2] = 10;
model->modMin[0] = 3;
model->modMax[0] = 10;
model->modMin[1] = model->modMin[2] = 1;
model->modMax[1] = model->modMax[2] = 1;
model->storage = cvCreateMemStorage();
return model;
}
void cvReleaseBGCodeBookModel( CvBGCodeBookModel** model )
{
if( model && *model )
{
cvReleaseMemStorage( &(*model)->storage );
memset( *model, 0, sizeof(**model) );
cvFree( model );
}
}
static uchar satTab8u[768];
#undef SAT_8U
#define SAT_8U(x) satTab8u[(x) + 255]
static void icvInitSatTab()
{
static int initialized = 0;
if( !initialized )
{
for( int i = 0; i < 768; i++ )
{
int v = i - 255;
satTab8u[i] = (uchar)(v < 0 ? 0 : v > 255 ? 255 : v);
}
initialized = 1;
}
}
void cvBGCodeBookUpdate( CvBGCodeBookModel* model, const CvArr* _image,
CvRect roi, const CvArr* _mask )
{
CV_FUNCNAME( "cvBGCodeBookUpdate" );
__BEGIN__;
CvMat stub, *image = cvGetMat( _image, &stub );
CvMat mstub, *mask = _mask ? cvGetMat( _mask, &mstub ) : 0;
int i, x, y, T;
int nblocks;
uchar cb0, cb1, cb2;
CvBGCodeBookElem* freeList;
CV_ASSERT( model && CV_MAT_TYPE(image->type) == CV_8UC3 &&
(!mask || (CV_IS_MASK_ARR(mask) && CV_ARE_SIZES_EQ(image, mask))) );
if( roi.x == 0 && roi.y == 0 && roi.width == 0 && roi.height == 0 )
{
roi.width = image->cols;
roi.height = image->rows;
}
else
CV_ASSERT( (unsigned)roi.x < (unsigned)image->cols &&
(unsigned)roi.y < (unsigned)image->rows &&
roi.width >= 0 && roi.height >= 0 &&
roi.x + roi.width <= image->cols &&
roi.y + roi.height <= image->rows );
if( image->cols != model->size.width || image->rows != model->size.height )
{
cvClearMemStorage( model->storage );
model->freeList = 0;
cvFree( &model->cbmap );
int bufSz = image->cols*image->rows*sizeof(model->cbmap[0]);
model->cbmap = (CvBGCodeBookElem**)cvAlloc(bufSz);
memset( model->cbmap, 0, bufSz );
model->size = cvSize(image->cols, image->rows);
}
icvInitSatTab();
cb0 = model->cbBounds[0];
cb1 = model->cbBounds[1];
cb2 = model->cbBounds[2];
T = ++model->t;
freeList = model->freeList;
nblocks = (int)((model->storage->block_size - sizeof(CvMemBlock))/sizeof(*freeList));
nblocks = MIN( nblocks, 1024 );
CV_ASSERT( nblocks > 0 );
for( y = 0; y < roi.height; y++ )
{
const uchar* p = image->data.ptr + image->step*(y + roi.y) + roi.x*3;
const uchar* m = mask ? mask->data.ptr + mask->step*(y + roi.y) + roi.x : 0;
CvBGCodeBookElem** cb = model->cbmap + image->cols*(y + roi.y) + roi.x;
for( x = 0; x < roi.width; x++, p += 3, cb++ )
{
CvBGCodeBookElem *e, *found = 0;
uchar p0, p1, p2, l0, l1, l2, h0, h1, h2;
int negRun;
if( m && m[x] == 0 )
continue;
p0 = p[0]; p1 = p[1]; p2 = p[2];
l0 = SAT_8U(p0 - cb0); l1 = SAT_8U(p1 - cb1); l2 = SAT_8U(p2 - cb2);
h0 = SAT_8U(p0 + cb0); h1 = SAT_8U(p1 + cb1); h2 = SAT_8U(p2 + cb2);
for( e = *cb; e != 0; e = e->next )
{
if( e->learnMin[0] <= p0 && p0 <= e->learnMax[0] &&
e->learnMin[1] <= p1 && p1 <= e->learnMax[1] &&
e->learnMin[2] <= p2 && p2 <= e->learnMax[2] )
{
e->tLastUpdate = T;
e->boxMin[0] = MIN(e->boxMin[0], p0);
e->boxMax[0] = MAX(e->boxMax[0], p0);
e->boxMin[1] = MIN(e->boxMin[1], p1);
e->boxMax[1] = MAX(e->boxMax[1], p1);
e->boxMin[2] = MIN(e->boxMin[2], p2);
e->boxMax[2] = MAX(e->boxMax[2], p2);
// no need to use SAT_8U for updated learnMin[i] & learnMax[i] here,
// as the bounding li & hi are already within 0..255.
if( e->learnMin[0] > l0 ) e->learnMin[0]--;
if( e->learnMax[0] < h0 ) e->learnMax[0]++;
if( e->learnMin[1] > l1 ) e->learnMin[1]--;
if( e->learnMax[1] < h1 ) e->learnMax[1]++;
if( e->learnMin[2] > l2 ) e->learnMin[2]--;
if( e->learnMax[2] < h2 ) e->learnMax[2]++;
found = e;
break;
}
negRun = T - e->tLastUpdate;
e->stale = MAX( e->stale, negRun );
}
for( ; e != 0; e = e->next )
{
negRun = T - e->tLastUpdate;
e->stale = MAX( e->stale, negRun );
}
if( !found )
{
if( !freeList )
{
freeList = (CvBGCodeBookElem*)cvMemStorageAlloc(model->storage,
nblocks*sizeof(*freeList));
for( i = 0; i < nblocks-1; i++ )
freeList[i].next = &freeList[i+1];
freeList[nblocks-1].next = 0;
}
e = freeList;
freeList = freeList->next;
e->learnMin[0] = l0; e->learnMax[0] = h0;
e->learnMin[1] = l1; e->learnMax[1] = h1;
e->learnMin[2] = l2; e->learnMax[2] = h2;
e->boxMin[0] = e->boxMax[0] = p0;
e->boxMin[1] = e->boxMax[1] = p1;
e->boxMin[2] = e->boxMax[2] = p2;
e->tLastUpdate = T;
e->stale = 0;
e->next = *cb;
*cb = e;
}
}
}
model->freeList = freeList;
__END__;
}
int cvBGCodeBookDiff( const CvBGCodeBookModel* model, const CvArr* _image,
CvArr* _fgmask, CvRect roi )
{
int maskCount = -1;
CV_FUNCNAME( "cvBGCodeBookDiff" );
__BEGIN__;
CvMat stub, *image = cvGetMat( _image, &stub );
CvMat mstub, *mask = cvGetMat( _fgmask, &mstub );
int x, y;
uchar m0, m1, m2, M0, M1, M2;
CV_ASSERT( model && CV_MAT_TYPE(image->type) == CV_8UC3 &&
image->cols == model->size.width && image->rows == model->size.height &&
CV_IS_MASK_ARR(mask) && CV_ARE_SIZES_EQ(image, mask) );
if( roi.x == 0 && roi.y == 0 && roi.width == 0 && roi.height == 0 )
{
roi.width = image->cols;
roi.height = image->rows;
}
else
CV_ASSERT( (unsigned)roi.x < (unsigned)image->cols &&
(unsigned)roi.y < (unsigned)image->rows &&
roi.width >= 0 && roi.height >= 0 &&
roi.x + roi.width <= image->cols &&
roi.y + roi.height <= image->rows );
m0 = model->modMin[0]; M0 = model->modMax[0];
m1 = model->modMin[1]; M1 = model->modMax[1];
m2 = model->modMin[2]; M2 = model->modMax[2];
maskCount = roi.height*roi.width;
for( y = 0; y < roi.height; y++ )
{
const uchar* p = image->data.ptr + image->step*(y + roi.y) + roi.x*3;
uchar* m = mask->data.ptr + mask->step*(y + roi.y) + roi.x;
CvBGCodeBookElem** cb = model->cbmap + image->cols*(y + roi.y) + roi.x;
for( x = 0; x < roi.width; x++, p += 3, cb++ )
{
CvBGCodeBookElem *e;
uchar p0 = p[0], p1 = p[1], p2 = p[2];
int l0 = p0 + m0, l1 = p1 + m1, l2 = p2 + m2;
int h0 = p0 - M0, h1 = p1 - M1, h2 = p2 - M2;
m[x] = (uchar)255;
for( e = *cb; e != 0; e = e->next )
{
if( e->boxMin[0] <= l0 && h0 <= e->boxMax[0] &&
e->boxMin[1] <= l1 && h1 <= e->boxMax[1] &&
e->boxMin[2] <= l2 && h2 <= e->boxMax[2] )
{
m[x] = 0;
maskCount--;
break;
}
}
}
}
__END__;
return maskCount;
}
void cvBGCodeBookClearStale( CvBGCodeBookModel* model, int staleThresh,
CvRect roi, const CvArr* _mask )
{
CV_FUNCNAME( "cvBGCodeBookClearStale" );
__BEGIN__;
CvMat mstub, *mask = _mask ? cvGetMat( _mask, &mstub ) : 0;
int x, y, T;
CvBGCodeBookElem* freeList;
CV_ASSERT( model && (!mask || (CV_IS_MASK_ARR(mask) &&
mask->cols == model->size.width && mask->rows == model->size.height)) );
if( roi.x == 0 && roi.y == 0 && roi.width == 0 && roi.height == 0 )
{
roi.width = model->size.width;
roi.height = model->size.height;
}
else
CV_ASSERT( (unsigned)roi.x < (unsigned)mask->cols &&
(unsigned)roi.y < (unsigned)mask->rows &&
roi.width >= 0 && roi.height >= 0 &&
roi.x + roi.width <= mask->cols &&
roi.y + roi.height <= mask->rows );
icvInitSatTab();
freeList = model->freeList;
T = model->t;
for( y = 0; y < roi.height; y++ )
{
const uchar* m = mask ? mask->data.ptr + mask->step*(y + roi.y) + roi.x : 0;
CvBGCodeBookElem** cb = model->cbmap + model->size.width*(y + roi.y) + roi.x;
for( x = 0; x < roi.width; x++, cb++ )
{
CvBGCodeBookElem *e, first, *prev = &first;
if( m && m[x] == 0 )
continue;
for( first.next = e = *cb; e != 0; e = prev->next )
{
if( e->stale > staleThresh )
{
prev->next = e->next;
e->next = freeList;
freeList = e;
}
else
{
e->stale = 0;
e->tLastUpdate = T;
prev = e;
}
}
*cb = first.next;
}
}
model->freeList = freeList;
__END__;
}
/* End of file. */

138
modules/legacy/src/bgfg_common.cpp Normal file
View File

@@ -0,0 +1,138 @@
/*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.
//
//
// Intel License Agreement
//
// Copyright (C) 2000, Intel Corporation, 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 Intel Corporation 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"
void cvReleaseBGStatModel( CvBGStatModel** bg_model )
{
if( bg_model && *bg_model && (*bg_model)->release )
(*bg_model)->release( bg_model );
}
int cvUpdateBGStatModel( IplImage* current_frame,
CvBGStatModel* bg_model,
double learningRate )
{
return bg_model && bg_model->update ? bg_model->update( current_frame, bg_model, learningRate ) : 0;
}
// Function cvRefineForegroundMaskBySegm preforms FG post-processing based on segmentation
// (all pixels of the segment will be classified as FG if majority of pixels of the region are FG).
// parameters:
// segments - pointer to result of segmentation (for example MeanShiftSegmentation)
// bg_model - pointer to CvBGStatModel structure
CV_IMPL void cvRefineForegroundMaskBySegm( CvSeq* segments, CvBGStatModel* bg_model )
{
IplImage* tmp_image = cvCreateImage(cvSize(bg_model->foreground->width,bg_model->foreground->height),
IPL_DEPTH_8U, 1);
for( ; segments; segments = ((CvSeq*)segments)->h_next )
{
CvSeq seq = *segments;
seq.v_next = seq.h_next = NULL;
cvZero(tmp_image);
cvDrawContours( tmp_image, &seq, CV_RGB(0, 0, 255), CV_RGB(0, 0, 255), 10, -1);
int num1 = cvCountNonZero(tmp_image);
cvAnd(tmp_image, bg_model->foreground, tmp_image);
int num2 = cvCountNonZero(tmp_image);
if( num2 > num1*0.5 )
cvDrawContours( bg_model->foreground, &seq, CV_RGB(0, 0, 255), CV_RGB(0, 0, 255), 10, -1);
else
cvDrawContours( bg_model->foreground, &seq, CV_RGB(0, 0, 0), CV_RGB(0, 0, 0), 10, -1);
}
cvReleaseImage(&tmp_image);
}
CV_IMPL CvSeq*
cvSegmentFGMask( CvArr* _mask, int poly1Hull0, float perimScale,
CvMemStorage* storage, CvPoint offset )
{
CvMat mstub, *mask = cvGetMat( _mask, &mstub );
CvMemStorage* tempStorage = storage ? storage : cvCreateMemStorage();
CvSeq *contours, *c;
int nContours = 0;
CvContourScanner scanner;
// clean up raw mask
cvMorphologyEx( mask, mask, 0, 0, CV_MOP_OPEN, 1 );
cvMorphologyEx( mask, mask, 0, 0, CV_MOP_CLOSE, 1 );
// find contours around only bigger regions
scanner = cvStartFindContours( mask, tempStorage,
sizeof(CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, offset );
while( (c = cvFindNextContour( scanner )) != 0 )
{
double len = cvContourPerimeter( c );
double q = (mask->rows + mask->cols)/perimScale; // calculate perimeter len threshold
if( len < q ) //Get rid of blob if it's perimeter is too small
cvSubstituteContour( scanner, 0 );
else //Smooth it's edges if it's large enough
{
CvSeq* newC;
if( poly1Hull0 ) //Polygonal approximation of the segmentation
newC = cvApproxPoly( c, sizeof(CvContour), tempStorage, CV_POLY_APPROX_DP, 2, 0 );
else //Convex Hull of the segmentation
newC = cvConvexHull2( c, tempStorage, CV_CLOCKWISE, 1 );
cvSubstituteContour( scanner, newC );
nContours++;
}
}
contours = cvEndFindContours( &scanner );
// paint the found regions back into the image
cvZero( mask );
for( c=contours; c != 0; c = c->h_next )
cvDrawContours( mask, c, cvScalarAll(255), cvScalarAll(0), -1, CV_FILLED, 8,
cvPoint(-offset.x,-offset.y));
if( tempStorage != storage )
{
cvReleaseMemStorage( &tempStorage );
contours = 0;
}
return contours;
}
/* End of file. */

1376
modules/legacy/src/bgfg_gaussmix.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

64
modules/legacy/src/featuretree.cpp Normal file
View File

@@ -0,0 +1,64 @@
//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.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, 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 Intel Corporation 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 "_featuretree.h"
void cvReleaseFeatureTree(CvFeatureTree* tr)
{
delete tr;
}
// desc is m x d set of candidate points.
// results is m x k set of row indices of matching points.
// dist is m x k distance to matching points.
void cvFindFeatures(CvFeatureTree* tr, const CvMat* desc,
CvMat* results, CvMat* dist, int k, int emax)
{
tr->FindFeatures(desc, k, emax, results, dist);
}
int cvFindFeaturesBoxed(CvFeatureTree* tr,
CvMat* bounds_min, CvMat* bounds_max,
CvMat* results)
{
return tr->FindOrthoRange(bounds_min, bounds_max, results);
}

241
modules/legacy/src/kdtree.cpp Normal file
View File

@@ -0,0 +1,241 @@
/*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.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2008, Xavier Delacour, 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 Intel Corporation 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*/
// 2008-05-13, Xavier Delacour <xavier.delacour@gmail.com>
#include "precomp.hpp"
#if !defined _MSC_VER || defined __ICL || _MSC_VER >= 1300
#include "_kdtree.hpp"
#include "_featuretree.h"
#if _MSC_VER >= 1400
#pragma warning(disable:4996) // suppress "function call with parameters may be unsafe" in std::copy
#endif
class CvKDTreeWrap : public CvFeatureTree {
template <class __scalartype, int __cvtype>
struct deref {
typedef __scalartype scalar_type;
typedef double accum_type;
CvMat* mat;
deref(CvMat* _mat) : mat(_mat) {
assert(CV_ELEM_SIZE1(__cvtype) == sizeof(__scalartype));
}
scalar_type operator() (int i, int j) const {
return *((scalar_type*)(mat->data.ptr + i * mat->step) + j);
}
};
#define dispatch_cvtype(mat, c) \
switch (CV_MAT_DEPTH((mat)->type)) { \
case CV_32F: \
{ typedef CvKDTree<int, deref<float, CV_32F> > tree_type; c; break; } \
case CV_64F: \
{ typedef CvKDTree<int, deref<double, CV_64F> > tree_type; c; break; } \
default: assert(0); \
}
CvMat* mat;
void* data;
template <class __treetype>
void find_nn(const CvMat* d, int k, int emax, CvMat* results, CvMat* dist) {
__treetype* tr = (__treetype*) data;
const uchar* dptr = d->data.ptr;
uchar* resultsptr = results->data.ptr;
uchar* distptr = dist->data.ptr;
typename __treetype::bbf_nn_pqueue nn;
assert(d->cols == tr->dims());
assert(results->rows == d->rows);
assert(results->rows == dist->rows);
assert(results->cols == k);
assert(dist->cols == k);
for (int j = 0; j < d->rows; ++j) {
const typename __treetype::scalar_type* dj =
(const typename __treetype::scalar_type*) dptr;
int* resultsj = (int*) resultsptr;
double* distj = (double*) distptr;
tr->find_nn_bbf(dj, k, emax, nn);
assert((int)nn.size() <= k);
for (unsigned int j = 0; j < nn.size(); ++j) {
*resultsj++ = *nn[j].p;
*distj++ = nn[j].dist;
}
std::fill(resultsj, resultsj + k - nn.size(), -1);
std::fill(distj, distj + k - nn.size(), 0);
dptr += d->step;
resultsptr += results->step;
distptr += dist->step;
}
}
template <class __treetype>
int find_ortho_range(CvMat* bounds_min, CvMat* bounds_max,
CvMat* results) {
int rn = results->rows * results->cols;
std::vector<int> inbounds;
dispatch_cvtype(mat, ((__treetype*)data)->
find_ortho_range((typename __treetype::scalar_type*)bounds_min->data.ptr,
(typename __treetype::scalar_type*)bounds_max->data.ptr,
inbounds));
std::copy(inbounds.begin(),
inbounds.begin() + std::min((int)inbounds.size(), rn),
(int*) results->data.ptr);
return (int)inbounds.size();
}
CvKDTreeWrap(const CvKDTreeWrap& x);
CvKDTreeWrap& operator= (const CvKDTreeWrap& rhs);
public:
CvKDTreeWrap(CvMat* _mat) : mat(_mat) {
// * a flag parameter should tell us whether
// * (a) user ensures *mat outlives *this and is unchanged,
// * (b) we take reference and user ensures mat is unchanged,
// * (c) we copy data, (d) we own and release data.
std::vector<int> tmp(mat->rows);
for (unsigned int j = 0; j < tmp.size(); ++j)
tmp[j] = j;
dispatch_cvtype(mat, data = new tree_type
(&tmp[0], &tmp[0] + tmp.size(), mat->cols,
tree_type::deref_type(mat)));
}
~CvKDTreeWrap() {
dispatch_cvtype(mat, delete (tree_type*) data);
}
int dims() {
int d = 0;
dispatch_cvtype(mat, d = ((tree_type*) data)->dims());
return d;
}
int type() {
return mat->type;
}
void FindFeatures(const CvMat* desc, int k, int emax, CvMat* results, CvMat* dist) {
cv::Ptr<CvMat> tmp_desc;
if (desc->cols != dims())
CV_Error(CV_StsUnmatchedSizes, "desc columns be equal feature dimensions");
if (results->rows != desc->rows && results->cols != k)
CV_Error(CV_StsUnmatchedSizes, "results and desc must be same height");
if (dist->rows != desc->rows && dist->cols != k)
CV_Error(CV_StsUnmatchedSizes, "dist and desc must be same height");
if (CV_MAT_TYPE(results->type) != CV_32SC1)
CV_Error(CV_StsUnsupportedFormat, "results must be CV_32SC1");
if (CV_MAT_TYPE(dist->type) != CV_64FC1)
CV_Error(CV_StsUnsupportedFormat, "dist must be CV_64FC1");
if (CV_MAT_TYPE(type()) != CV_MAT_TYPE(desc->type)) {
tmp_desc = cvCreateMat(desc->rows, desc->cols, type());
cvConvert(desc, tmp_desc);
desc = tmp_desc;
}
assert(CV_MAT_TYPE(desc->type) == CV_MAT_TYPE(mat->type));
assert(CV_MAT_TYPE(dist->type) == CV_64FC1);
assert(CV_MAT_TYPE(results->type) == CV_32SC1);
dispatch_cvtype(mat, find_nn<tree_type>
(desc, k, emax, results, dist));
}
int FindOrthoRange(CvMat* bounds_min, CvMat* bounds_max,
CvMat* results) {
bool free_bounds = false;
int count = -1;
if (bounds_min->cols * bounds_min->rows != dims() ||
bounds_max->cols * bounds_max->rows != dims())
CV_Error(CV_StsUnmatchedSizes, "bounds_{min,max} must 1 x dims or dims x 1");
if (CV_MAT_TYPE(bounds_min->type) != CV_MAT_TYPE(bounds_max->type))
CV_Error(CV_StsUnmatchedFormats, "bounds_{min,max} must have same type");
if (CV_MAT_TYPE(results->type) != CV_32SC1)
CV_Error(CV_StsUnsupportedFormat, "results must be CV_32SC1");
if (CV_MAT_TYPE(bounds_min->type) != CV_MAT_TYPE(type())) {
free_bounds = true;
CvMat* old_bounds_min = bounds_min;
bounds_min = cvCreateMat(bounds_min->rows, bounds_min->cols, type());
cvConvert(old_bounds_min, bounds_min);
CvMat* old_bounds_max = bounds_max;
bounds_max = cvCreateMat(bounds_max->rows, bounds_max->cols, type());
cvConvert(old_bounds_max, bounds_max);
}
assert(CV_MAT_TYPE(bounds_min->type) == CV_MAT_TYPE(mat->type));
assert(CV_MAT_TYPE(bounds_min->type) == CV_MAT_TYPE(bounds_max->type));
assert(bounds_min->rows * bounds_min->cols == dims());
assert(bounds_max->rows * bounds_max->cols == dims());
dispatch_cvtype(mat, count = find_ortho_range<tree_type>
(bounds_min, bounds_max,results));
if (free_bounds) {
cvReleaseMat(&bounds_min);
cvReleaseMat(&bounds_max);
}
return count;
}
};
CvFeatureTree* cvCreateKDTree(CvMat* desc) {
if (CV_MAT_TYPE(desc->type) != CV_32FC1 &&
CV_MAT_TYPE(desc->type) != CV_64FC1)
CV_Error(CV_StsUnsupportedFormat, "descriptors must be either CV_32FC1 or CV_64FC1");
return new CvKDTreeWrap(desc);
}
#endif

454
modules/legacy/src/lsh.cpp Normal file
View File

@@ -0,0 +1,454 @@
/*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.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2009, Xavier Delacour, 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 Intel Corporation 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*/
// 2009-01-12, Xavier Delacour <xavier.delacour@gmail.com>
// * hash perf could be improved
// * in particular, implement integer only (converted fixed from float input)
// * number of hash functions could be reduced (andoni paper)
// * redundant distance computations could be suppressed
// * rework CvLSHOperations interface-- move some of the loops into it to
// * allow efficient async storage
// Datar, M., Immorlica, N., Indyk, P., and Mirrokni, V. S. 2004. Locality-sensitive hashing
// scheme based on p-stable distributions. In Proceedings of the Twentieth Annual Symposium on
// Computational Geometry (Brooklyn, New York, USA, June 08 - 11, 2004). SCG '04. ACM, New York,
// NY, 253-262. DOI= http://doi.acm.org/10.1145/997817.997857
#include "precomp.hpp"
#include <math.h>
#include <vector>
#include <algorithm>
#include <limits>
template <class T>
class memory_hash_ops : public CvLSHOperations {
int d;
std::vector<T> data;
std::vector<int> free_data;
struct node {
int i, h2, next;
};
std::vector<node> nodes;
std::vector<int> free_nodes;
std::vector<int> bins;
public:
memory_hash_ops(int _d, int n) : d(_d) {
bins.resize(n, -1);
}
virtual int vector_add(const void* _p) {
const T* p = (const T*)_p;
int i;
if (free_data.empty()) {
i = (int)data.size();
data.insert(data.end(), d, 0);
} else {
i = free_data.end()[-1];
free_data.pop_back();
}
std::copy(p, p + d, data.begin() + i);
return i / d;
}
virtual void vector_remove(int i) {
free_data.push_back(i * d);
}
virtual const void* vector_lookup(int i) {
return &data[i * d];
}
virtual void vector_reserve(int n) {
data.reserve(n * d);
}
virtual unsigned int vector_count() {
return (unsigned)(data.size() / d - free_data.size());
}
virtual void hash_insert(lsh_hash h, int /*l*/, int i) {
int ii;
if (free_nodes.empty()) {
ii = (int)nodes.size();
nodes.push_back(node());
} else {
ii = free_nodes.end()[-1];
free_nodes.pop_back();
}
node& n = nodes[ii];
int h1 = (int)(h.h1 % bins.size());
n.i = i;
n.h2 = h.h2;
n.next = bins[h1];
bins[h1] = ii;
}
virtual void hash_remove(lsh_hash h, int /*l*/, int i) {
int h1 = (int)(h.h1 % bins.size());
for (int ii = bins[h1], iin, iip = -1; ii != -1; iip = ii, ii = iin) {
iin = nodes[ii].next;
if (nodes[ii].h2 == h.h2 && nodes[ii].i == i) {
free_nodes.push_back(ii);
if (iip == -1)
bins[h1] = iin;
else
nodes[iip].next = iin;
}
}
}
virtual int hash_lookup(lsh_hash h, int /*l*/, int* ret_i, int ret_i_max) {
int h1 = (int)(h.h1 % bins.size());
int k = 0;
for (int ii = bins[h1]; ii != -1 && k < ret_i_max; ii = nodes[ii].next)
if (nodes[ii].h2 == h.h2)
ret_i[k++] = nodes[ii].i;
return k;
}
};
template <class T,int cvtype>
class pstable_l2_func {
CvMat *a, *b, *r1, *r2;
int d, k;
double r;
pstable_l2_func(const pstable_l2_func& x);
pstable_l2_func& operator= (const pstable_l2_func& rhs);
public:
typedef T scalar_type;
typedef T accum_type;
pstable_l2_func(int _d, int _k, double _r, CvRNG& rng)
: d(_d), k(_k), r(_r) {
assert(sizeof(T) == CV_ELEM_SIZE1(cvtype));
a = cvCreateMat(k, d, cvtype);
b = cvCreateMat(k, 1, cvtype);
r1 = cvCreateMat(k, 1, CV_32SC1);
r2 = cvCreateMat(k, 1, CV_32SC1);
cvRandArr(&rng, a, CV_RAND_NORMAL, cvScalar(0), cvScalar(1));
cvRandArr(&rng, b, CV_RAND_UNI, cvScalar(0), cvScalar(r));
cvRandArr(&rng, r1, CV_RAND_UNI,
cvScalar(std::numeric_limits<int>::min()),
cvScalar(std::numeric_limits<int>::max()));
cvRandArr(&rng, r2, CV_RAND_UNI,
cvScalar(std::numeric_limits<int>::min()),
cvScalar(std::numeric_limits<int>::max()));
}
~pstable_l2_func() {
cvReleaseMat(&a);
cvReleaseMat(&b);
cvReleaseMat(&r1);
cvReleaseMat(&r2);
}
// * factor all L functions into this (reduces number of matrices to 4 total;
// * simpler syntax in lsh_table). give parameter l here that tells us which
// * row to use etc.
lsh_hash operator() (const T* x) const {
const T* aj = (const T*)a->data.ptr;
const T* bj = (const T*)b->data.ptr;
lsh_hash h;
h.h1 = h.h2 = 0;
for (int j = 0; j < k; ++j) {
accum_type s = 0;
for (int jj = 0; jj < d; ++jj)
s += aj[jj] * x[jj];
s += *bj;
s = accum_type(s/r);
int si = int(s);
h.h1 += r1->data.i[j] * si;
h.h2 += r2->data.i[j] * si;
aj += d;
bj++;
}
return h;
}
accum_type distance(const T* p, const T* q) const {
accum_type s = 0;
for (int j = 0; j < d; ++j) {
accum_type d1 = p[j] - q[j];
s += d1 * d1;
}
return s;
}
};
template <class H>
class lsh_table {
public:
typedef typename H::scalar_type scalar_type;
typedef typename H::accum_type accum_type;
private:
std::vector<H*> g;
CvLSHOperations* ops;
int d, L, k;
double r;
static accum_type comp_dist(const std::pair<int,accum_type>& x,
const std::pair<int,accum_type>& y) {
return x.second < y.second;
}
lsh_table(const lsh_table& x);
lsh_table& operator= (const lsh_table& rhs);
public:
lsh_table(CvLSHOperations* _ops, int _d, int Lval, int _k, double _r, CvRNG& rng)
: ops(_ops), d(_d), L(Lval), k(_k), r(_r) {
g.resize(L);
for (int j = 0; j < L; ++j)
g[j] = new H(d, k, r, rng);
}
~lsh_table() {
for (int j = 0; j < L; ++j)
delete g[j];
delete ops;
}
int dims() const {
return d;
}
unsigned int size() const {
return ops->vector_count();
}
void add(const scalar_type* data, int n, int* ret_indices = 0) {
for (int j=0;j<n;++j) {
const scalar_type* x = data+j*d;
int i = ops->vector_add(x);
if (ret_indices)
ret_indices[j] = i;
for (int l = 0; l < L; ++l) {
lsh_hash h = (*g[l])(x);
ops->hash_insert(h, l, i);
}
}
}
void remove(const int* indices, int n) {
for (int j = 0; j < n; ++j) {
int i = indices[n];
const scalar_type* x = (const scalar_type*)ops->vector_lookup(i);
for (int l = 0; l < L; ++l) {
lsh_hash h = (*g[l])(x);
ops->hash_remove(h, l, i);
}
ops->vector_remove(i);
}
}
void query(const scalar_type* q, int k0, int emax, double* dist, int* results) {
cv::AutoBuffer<int> tmp(emax);
typedef std::pair<int, accum_type> dr_type; // * swap int and accum_type here, for naming consistency
cv::AutoBuffer<dr_type> dr(k0);
int k1 = 0;
// * handle k0 >= emax, in which case don't track max distance
for (int l = 0; l < L && emax > 0; ++l) {
lsh_hash h = (*g[l])(q);
int m = ops->hash_lookup(h, l, tmp, emax);
for (int j = 0; j < m && emax > 0; ++j, --emax) {
int i = tmp[j];
const scalar_type* p = (const scalar_type*)ops->vector_lookup(i);
accum_type pd = (*g[l]).distance(p, q);
if (k1 < k0) {
dr[k1++] = std::make_pair(i, pd);
std::push_heap(&dr[0], &dr[k1], comp_dist);
} else if (pd < dr[0].second) {
std::pop_heap(&dr[0], &dr[k0], comp_dist);
dr[k0 - 1] = std::make_pair(i, pd);
std::push_heap(&dr[0], &dr[k0], comp_dist);
}
}
}
for (int j = 0; j < k1; ++j)
dist[j] = dr[j].second, results[j] = dr[j].first;
std::fill(dist + k1, dist + k0, 0);
std::fill(results + k1, results + k0, -1);
}
void query(const scalar_type* data, int n, int k0, int emax, double* dist, int* results) {
for (int j = 0; j < n; ++j) {
query(data, k0, emax, dist, results);
data += d; // * this may not agree with step for some scalar_types
dist += k0;
results += k0;
}
}
};
typedef lsh_table<pstable_l2_func<float, CV_32FC1> > lsh_pstable_l2_32f;
typedef lsh_table<pstable_l2_func<double, CV_64FC1> > lsh_pstable_l2_64f;
struct CvLSH {
int type;
union {
lsh_pstable_l2_32f* lsh_32f;
lsh_pstable_l2_64f* lsh_64f;
} u;
};
CvLSH* cvCreateLSH(CvLSHOperations* ops, int d, int L, int k, int type, double r, int64 seed) {
CvLSH* lsh = 0;
CvRNG rng = cvRNG(seed);
if (type != CV_32FC1 && type != CV_64FC1)
CV_Error(CV_StsUnsupportedFormat, "vectors must be either CV_32FC1 or CV_64FC1");
lsh = new CvLSH;
lsh->type = type;
switch (type) {
case CV_32FC1: lsh->u.lsh_32f = new lsh_pstable_l2_32f(ops, d, L, k, r, rng); break;
case CV_64FC1: lsh->u.lsh_64f = new lsh_pstable_l2_64f(ops, d, L, k, r, rng); break;
}
return lsh;
}
CvLSH* cvCreateMemoryLSH(int d, int n, int L, int k, int type, double r, int64 seed) {
CvLSHOperations* ops = 0;
switch (type) {
case CV_32FC1: ops = new memory_hash_ops<float>(d,n); break;
case CV_64FC1: ops = new memory_hash_ops<double>(d,n); break;
}
return cvCreateLSH(ops, d, L, k, type, r, seed);
}
void cvReleaseLSH(CvLSH** lsh) {
switch ((*lsh)->type) {
case CV_32FC1: delete (*lsh)->u.lsh_32f; break;
case CV_64FC1: delete (*lsh)->u.lsh_64f; break;
default: assert(0);
}
delete *lsh;
*lsh = 0;
}
unsigned int LSHSize(CvLSH* lsh) {
switch (lsh->type) {
case CV_32FC1: return lsh->u.lsh_32f->size(); break;
case CV_64FC1: return lsh->u.lsh_64f->size(); break;
default: assert(0);
}
return 0;
}
void cvLSHAdd(CvLSH* lsh, const CvMat* data, CvMat* indices) {
int dims, n;
int* ret_indices = 0;
switch (lsh->type) {
case CV_32FC1: dims = lsh->u.lsh_32f->dims(); break;
case CV_64FC1: dims = lsh->u.lsh_64f->dims(); break;
default: assert(0); return;
}
n = data->rows;
if (dims != data->cols)
CV_Error(CV_StsBadSize, "data must be n x d, where d is what was used to construct LSH");
if (CV_MAT_TYPE(data->type) != lsh->type)
CV_Error(CV_StsUnsupportedFormat, "type of data and constructed LSH must agree");
if (indices) {
if (CV_MAT_TYPE(indices->type) != CV_32SC1)
CV_Error(CV_StsUnsupportedFormat, "indices must be CV_32SC1");
if (indices->rows * indices->cols != n)
CV_Error(CV_StsBadSize, "indices must be n x 1 or 1 x n for n x d data");
ret_indices = indices->data.i;
}
switch (lsh->type) {
case CV_32FC1: lsh->u.lsh_32f->add(data->data.fl, n, ret_indices); break;
case CV_64FC1: lsh->u.lsh_64f->add(data->data.db, n, ret_indices); break;
default: assert(0); return;
}
}
void cvLSHRemove(CvLSH* lsh, const CvMat* indices) {
int n;
if (CV_MAT_TYPE(indices->type) != CV_32SC1)
CV_Error(CV_StsUnsupportedFormat, "indices must be CV_32SC1");
n = indices->rows * indices->cols;
switch (lsh->type) {
case CV_32FC1: lsh->u.lsh_32f->remove(indices->data.i, n); break;
case CV_64FC1: lsh->u.lsh_64f->remove(indices->data.i, n); break;
default: assert(0); return;
}
}
void cvLSHQuery(CvLSH* lsh, const CvMat* data, CvMat* indices, CvMat* dist, int k, int emax) {
int dims;
switch (lsh->type) {
case CV_32FC1: dims = lsh->u.lsh_32f->dims(); break;
case CV_64FC1: dims = lsh->u.lsh_64f->dims(); break;
default: assert(0); return;
}
if (k<1)
CV_Error(CV_StsOutOfRange, "k must be positive");
if (CV_MAT_TYPE(data->type) != lsh->type)
CV_Error(CV_StsUnsupportedFormat, "type of data and constructed LSH must agree");
if (dims != data->cols)
CV_Error(CV_StsBadSize, "data must be n x d, where d is what was used to construct LSH");
if (dist->rows != data->rows || dist->cols != k)
CV_Error(CV_StsBadSize, "dist must be n x k for n x d data");
if (dist->rows != indices->rows || dist->cols != indices->cols)
CV_Error(CV_StsBadSize, "dist and indices must be same size");
if (CV_MAT_TYPE(dist->type) != CV_64FC1)
CV_Error(CV_StsUnsupportedFormat, "dist must be CV_64FC1");
if (CV_MAT_TYPE(indices->type) != CV_32SC1)
CV_Error(CV_StsUnsupportedFormat, "indices must be CV_32SC1");
switch (lsh->type) {
case CV_32FC1: lsh->u.lsh_32f->query(data->data.fl, data->rows,
k, emax, dist->data.db, indices->data.i); break;
case CV_64FC1: lsh->u.lsh_64f->query(data->data.db, data->rows,
k, emax, dist->data.db, indices->data.i); break;
default: assert(0); return;
}
}

303
modules/legacy/src/optflowbm.cpp Normal file
View File

@@ -0,0 +1,303 @@
/*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.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, 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 Intel Corporation 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"
static inline int cmpBlocks(const uchar* A, const uchar* B, int Bstep, CvSize blockSize )
{
int x, s = 0;
for( ; blockSize.height--; A += blockSize.width, B += Bstep )
{
for( x = 0; x <= blockSize.width - 4; x += 4 )
s += std::abs(A[x] - B[x]) + std::abs(A[x+1] - B[x+1]) +
std::abs(A[x+2] - B[x+2]) + std::abs(A[x+3] - B[x+3]);
for( ; x < blockSize.width; x++ )
s += std::abs(A[x] - B[x]);
}
return s;
}
CV_IMPL void
cvCalcOpticalFlowBM( const void* srcarrA, const void* srcarrB,
CvSize blockSize, CvSize shiftSize,
CvSize maxRange, int usePrevious,
void* velarrx, void* velarry )
{
CvMat stubA, *srcA = cvGetMat( srcarrA, &stubA );
CvMat stubB, *srcB = cvGetMat( srcarrB, &stubB );
CvMat stubx, *velx = cvGetMat( velarrx, &stubx );
CvMat stuby, *vely = cvGetMat( velarry, &stuby );
if( !CV_ARE_TYPES_EQ( srcA, srcB ))
CV_Error( CV_StsUnmatchedFormats, "Source images have different formats" );
if( !CV_ARE_TYPES_EQ( velx, vely ))
CV_Error( CV_StsUnmatchedFormats, "Destination images have different formats" );
CvSize velSize =
{
(srcA->width - blockSize.width + shiftSize.width)/shiftSize.width,
(srcA->height - blockSize.height + shiftSize.height)/shiftSize.height
};
if( !CV_ARE_SIZES_EQ( srcA, srcB ) ||
!CV_ARE_SIZES_EQ( velx, vely ) ||
velx->width != velSize.width ||
vely->height != velSize.height )
CV_Error( CV_StsUnmatchedSizes, "" );
if( CV_MAT_TYPE( srcA->type ) != CV_8UC1 ||
CV_MAT_TYPE( velx->type ) != CV_32FC1 )
CV_Error( CV_StsUnsupportedFormat, "Source images must have 8uC1 type and "
"destination images must have 32fC1 type" );
if( srcA->step != srcB->step || velx->step != vely->step )
CV_Error( CV_BadStep, "two source or two destination images have different steps" );
const int SMALL_DIFF=2;
const int BIG_DIFF=128;
// scanning scheme coordinates
cv::vector<CvPoint> _ss((2 * maxRange.width + 1) * (2 * maxRange.height + 1));
CvPoint* ss = &_ss[0];
int ss_count = 0;
int blWidth = blockSize.width, blHeight = blockSize.height;
int blSize = blWidth*blHeight;
int acceptLevel = blSize * SMALL_DIFF;
int escapeLevel = blSize * BIG_DIFF;
int i, j;
cv::vector<uchar> _blockA(cvAlign(blSize + 16, 16));
uchar* blockA = (uchar*)cvAlignPtr(&_blockA[0], 16);
// Calculate scanning scheme
int min_count = MIN( maxRange.width, maxRange.height );
// use spiral search pattern
//
// 9 10 11 12
// 8 1 2 13
// 7 * 3 14
// 6 5 4 15
//... 20 19 18 17
//
for( i = 0; i < min_count; i++ )
{
// four cycles along sides
int x = -i-1, y = x;
// upper side
for( j = -i; j <= i + 1; j++, ss_count++ )
{
ss[ss_count].x = ++x;
ss[ss_count].y = y;
}
// right side
for( j = -i; j <= i + 1; j++, ss_count++ )
{
ss[ss_count].x = x;
ss[ss_count].y = ++y;
}
// bottom side
for( j = -i; j <= i + 1; j++, ss_count++ )
{
ss[ss_count].x = --x;
ss[ss_count].y = y;
}
// left side
for( j = -i; j <= i + 1; j++, ss_count++ )
{
ss[ss_count].x = x;
ss[ss_count].y = --y;
}
}
// the rest part
if( maxRange.width < maxRange.height )
{
int xleft = -min_count;
// cycle by neighbor rings
for( i = min_count; i < maxRange.height; i++ )
{
// two cycles by x
int y = -(i + 1);
int x = xleft;
// upper side
for( j = -maxRange.width; j <= maxRange.width; j++, ss_count++, x++ )
{
ss[ss_count].x = x;
ss[ss_count].y = y;
}
x = xleft;
y = -y;
// bottom side
for( j = -maxRange.width; j <= maxRange.width; j++, ss_count++, x++ )
{
ss[ss_count].x = x;
ss[ss_count].y = y;
}
}
}
else if( maxRange.width > maxRange.height )
{
int yupper = -min_count;
// cycle by neighbor rings
for( i = min_count; i < maxRange.width; i++ )
{
// two cycles by y
int x = -(i + 1);
int y = yupper;
// left side
for( j = -maxRange.height; j <= maxRange.height; j++, ss_count++, y++ )
{
ss[ss_count].x = x;
ss[ss_count].y = y;
}
y = yupper;
x = -x;
// right side
for( j = -maxRange.height; j <= maxRange.height; j++, ss_count++, y++ )
{
ss[ss_count].x = x;
ss[ss_count].y = y;
}
}
}
int maxX = srcB->cols - blockSize.width, maxY = srcB->rows - blockSize.height;
const uchar* Adata = srcA->data.ptr;
const uchar* Bdata = srcB->data.ptr;
int Astep = srcA->step, Bstep = srcB->step;
// compute the flow
for( i = 0; i < velx->rows; i++ )
{
float* vx = (float*)(velx->data.ptr + velx->step*i);
float* vy = (float*)(vely->data.ptr + vely->step*i);
for( j = 0; j < velx->cols; j++ )
{
int X1 = j*shiftSize.width, Y1 = i*shiftSize.height, X2, Y2;
int offX = 0, offY = 0;
if( usePrevious )
{
offX = cvRound(vx[j]);
offY = cvRound(vy[j]);
}
int k;
for( k = 0; k < blHeight; k++ )
memcpy( blockA + k*blWidth, Adata + Astep*(Y1 + k) + X1, blWidth );
X2 = X1 + offX;
Y2 = Y1 + offY;
int dist = INT_MAX;
if( 0 <= X2 && X2 <= maxX && 0 <= Y2 && Y2 <= maxY )
dist = cmpBlocks( blockA, Bdata + Bstep*Y2 + X2, Bstep, blockSize );
int countMin = 1;
int sumx = offX, sumy = offY;
if( dist > acceptLevel )
{
// do brute-force search
for( k = 0; k < ss_count; k++ )
{
int dx = offX + ss[k].x;
int dy = offY + ss[k].y;
X2 = X1 + dx;
Y2 = Y1 + dy;
if( !(0 <= X2 && X2 <= maxX && 0 <= Y2 && Y2 <= maxY) )
continue;
int tmpDist = cmpBlocks( blockA, Bdata + Bstep*Y2 + X2, Bstep, blockSize );
if( tmpDist < acceptLevel )
{
sumx = dx; sumy = dy;
countMin = 1;
break;
}
if( tmpDist < dist )
{
dist = tmpDist;
sumx = dx; sumy = dy;
countMin = 1;
}
else if( tmpDist == dist )
{
sumx += dx; sumy += dy;
countMin++;
}
}
if( dist > escapeLevel )
{
sumx = offX;
sumy = offY;
countMin = 1;
}
}
vx[j] = (float)sumx/countMin;
vy[j] = (float)sumy/countMin;
}
}
}
/* End of file. */

524
modules/legacy/src/optflowhs.cpp Normal file
View File

@@ -0,0 +1,524 @@
/*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.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, 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 Intel Corporation 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"
#define CONV( A, B, C) ( (float)( A + (B<<1) + C ) )
typedef struct
{
float xx;
float xy;
float yy;
float xt;
float yt;
float alpha; /* alpha = 1 / ( 1/lambda + xx + yy ) */
}
icvDerProductEx;
/*F///////////////////////////////////////////////////////////////////////////////////////
// Name: icvCalcOpticalFlowHS_8u32fR (Horn & Schunck method )
// Purpose: calculate Optical flow for 2 images using Horn & Schunck algorithm
// Context:
// Parameters:
// imgA - pointer to first frame ROI
// imgB - pointer to second frame ROI
// imgStep - width of single row of source images in bytes
// imgSize - size of the source image ROI
// usePrevious - use previous (input) velocity field.
// velocityX - pointer to horizontal and
// velocityY - vertical components of optical flow ROI
// velStep - width of single row of velocity frames in bytes
// lambda - Lagrangian multiplier
// criteria - criteria of termination processmaximum number of iterations
//
// Returns: CV_OK - all ok
// CV_OUTOFMEM_ERR - insufficient memory for function work
// CV_NULLPTR_ERR - if one of input pointers is NULL
// CV_BADSIZE_ERR - wrong input sizes interrelation
//
// Notes: 1.Optical flow to be computed for every pixel in ROI
// 2.For calculating spatial derivatives we use 3x3 Sobel operator.
// 3.We use the following border mode.
// The last row or column is replicated for the border
// ( IPL_BORDER_REPLICATE in IPL ).
//
//
//F*/
static CvStatus CV_STDCALL
icvCalcOpticalFlowHS_8u32fR( uchar* imgA,
uchar* imgB,
int imgStep,
CvSize imgSize,
int usePrevious,
float* velocityX,
float* velocityY,
int velStep,
float lambda,
CvTermCriteria criteria )
{
/* Loops indexes */
int i, j, k, address;
/* Buffers for Sobel calculations */
float *MemX[2];
float *MemY[2];
float ConvX, ConvY;
float GradX, GradY, GradT;
int imageWidth = imgSize.width;
int imageHeight = imgSize.height;
int ConvLine;
int LastLine;
int BufferSize;
float Ilambda = 1 / lambda;
int iter = 0;
int Stop;
/* buffers derivatives product */
icvDerProductEx *II;
float *VelBufX[2];
float *VelBufY[2];
/* variables for storing number of first pixel of image line */
int Line1;
int Line2;
int Line3;
int pixNumber;
/* auxiliary */
int NoMem = 0;
/* Checking bad arguments */
if( imgA == NULL )
return CV_NULLPTR_ERR;
if( imgB == NULL )
return CV_NULLPTR_ERR;
if( imgSize.width <= 0 )
return CV_BADSIZE_ERR;
if( imgSize.height <= 0 )
return CV_BADSIZE_ERR;
if( imgSize.width > imgStep )
return CV_BADSIZE_ERR;
if( (velStep & 3) != 0 )
return CV_BADSIZE_ERR;
velStep /= 4;
/****************************************************************************************/
/* Allocating memory for all buffers */
/****************************************************************************************/
for( k = 0; k < 2; k++ )
{
MemX[k] = (float *) cvAlloc( (imgSize.height) * sizeof( float ));
if( MemX[k] == NULL )
NoMem = 1;
MemY[k] = (float *) cvAlloc( (imgSize.width) * sizeof( float ));
if( MemY[k] == NULL )
NoMem = 1;
VelBufX[k] = (float *) cvAlloc( imageWidth * sizeof( float ));
if( VelBufX[k] == NULL )
NoMem = 1;
VelBufY[k] = (float *) cvAlloc( imageWidth * sizeof( float ));
if( VelBufY[k] == NULL )
NoMem = 1;
}
BufferSize = imageHeight * imageWidth;
II = (icvDerProductEx *) cvAlloc( BufferSize * sizeof( icvDerProductEx ));
if( II == NULL )
NoMem = 1;
if( NoMem )
{
for( k = 0; k < 2; k++ )
{
if( MemX[k] )
cvFree( &MemX[k] );
if( MemY[k] )
cvFree( &MemY[k] );
if( VelBufX[k] )
cvFree( &VelBufX[k] );
if( VelBufY[k] )
cvFree( &VelBufY[k] );
}
if( II )
cvFree( &II );
return CV_OUTOFMEM_ERR;
}
/****************************************************************************************\
* Calculate first line of memX and memY *
\****************************************************************************************/
MemY[0][0] = MemY[1][0] = CONV( imgA[0], imgA[0], imgA[1] );
MemX[0][0] = MemX[1][0] = CONV( imgA[0], imgA[0], imgA[imgStep] );
for( j = 1; j < imageWidth - 1; j++ )
{
MemY[0][j] = MemY[1][j] = CONV( imgA[j - 1], imgA[j], imgA[j + 1] );
}
pixNumber = imgStep;
for( i = 1; i < imageHeight - 1; i++ )
{
MemX[0][i] = MemX[1][i] = CONV( imgA[pixNumber - imgStep],
imgA[pixNumber], imgA[pixNumber + imgStep] );
pixNumber += imgStep;
}
MemY[0][imageWidth - 1] =
MemY[1][imageWidth - 1] = CONV( imgA[imageWidth - 2],
imgA[imageWidth - 1], imgA[imageWidth - 1] );
MemX[0][imageHeight - 1] =
MemX[1][imageHeight - 1] = CONV( imgA[pixNumber - imgStep],
imgA[pixNumber], imgA[pixNumber] );
/****************************************************************************************\
* begin scan image, calc derivatives *
\****************************************************************************************/
ConvLine = 0;
Line2 = -imgStep;
address = 0;
LastLine = imgStep * (imageHeight - 1);
while( ConvLine < imageHeight )
{
/*Here we calculate derivatives for line of image */
int memYline = (ConvLine + 1) & 1;
Line2 += imgStep;
Line1 = Line2 - ((Line2 == 0) ? 0 : imgStep);
Line3 = Line2 + ((Line2 == LastLine) ? 0 : imgStep);
/* Process first pixel */
ConvX = CONV( imgA[Line1 + 1], imgA[Line2 + 1], imgA[Line3 + 1] );
ConvY = CONV( imgA[Line3], imgA[Line3], imgA[Line3 + 1] );
GradY = (ConvY - MemY[memYline][0]) * 0.125f;
GradX = (ConvX - MemX[1][ConvLine]) * 0.125f;
MemY[memYline][0] = ConvY;
MemX[1][ConvLine] = ConvX;
GradT = (float) (imgB[Line2] - imgA[Line2]);
II[address].xx = GradX * GradX;
II[address].xy = GradX * GradY;
II[address].yy = GradY * GradY;
II[address].xt = GradX * GradT;
II[address].yt = GradY * GradT;
II[address].alpha = 1 / (Ilambda + II[address].xx + II[address].yy);
address++;
/* Process middle of line */
for( j = 1; j < imageWidth - 1; j++ )
{
ConvX = CONV( imgA[Line1 + j + 1], imgA[Line2 + j + 1], imgA[Line3 + j + 1] );
ConvY = CONV( imgA[Line3 + j - 1], imgA[Line3 + j], imgA[Line3 + j + 1] );
GradY = (ConvY - MemY[memYline][j]) * 0.125f;
GradX = (ConvX - MemX[(j - 1) & 1][ConvLine]) * 0.125f;
MemY[memYline][j] = ConvY;
MemX[(j - 1) & 1][ConvLine] = ConvX;
GradT = (float) (imgB[Line2 + j] - imgA[Line2 + j]);
II[address].xx = GradX * GradX;
II[address].xy = GradX * GradY;
II[address].yy = GradY * GradY;
II[address].xt = GradX * GradT;
II[address].yt = GradY * GradT;
II[address].alpha = 1 / (Ilambda + II[address].xx + II[address].yy);
address++;
}
/* Process last pixel of line */
ConvX = CONV( imgA[Line1 + imageWidth - 1], imgA[Line2 + imageWidth - 1],
imgA[Line3 + imageWidth - 1] );
ConvY = CONV( imgA[Line3 + imageWidth - 2], imgA[Line3 + imageWidth - 1],
imgA[Line3 + imageWidth - 1] );
GradY = (ConvY - MemY[memYline][imageWidth - 1]) * 0.125f;
GradX = (ConvX - MemX[(imageWidth - 2) & 1][ConvLine]) * 0.125f;
MemY[memYline][imageWidth - 1] = ConvY;
GradT = (float) (imgB[Line2 + imageWidth - 1] - imgA[Line2 + imageWidth - 1]);
II[address].xx = GradX * GradX;
II[address].xy = GradX * GradY;
II[address].yy = GradY * GradY;
II[address].xt = GradX * GradT;
II[address].yt = GradY * GradT;
II[address].alpha = 1 / (Ilambda + II[address].xx + II[address].yy);
address++;
ConvLine++;
}
/****************************************************************************************\
* Prepare initial approximation *
\****************************************************************************************/
if( !usePrevious )
{
float *vx = velocityX;
float *vy = velocityY;
for( i = 0; i < imageHeight; i++ )
{
memset( vx, 0, imageWidth * sizeof( float ));
memset( vy, 0, imageWidth * sizeof( float ));
vx += velStep;
vy += velStep;
}
}
/****************************************************************************************\
* Perform iterations *
\****************************************************************************************/
iter = 0;
Stop = 0;
LastLine = velStep * (imageHeight - 1);
while( !Stop )
{
float Eps = 0;
address = 0;
iter++;
/****************************************************************************************\
* begin scan velocity and update it *
\****************************************************************************************/
Line2 = -velStep;
for( i = 0; i < imageHeight; i++ )
{
/* Here average velocity */
float averageX;
float averageY;
float tmp;
Line2 += velStep;
Line1 = Line2 - ((Line2 == 0) ? 0 : velStep);
Line3 = Line2 + ((Line2 == LastLine) ? 0 : velStep);
/* Process first pixel */
averageX = (velocityX[Line2] +
velocityX[Line2 + 1] + velocityX[Line1] + velocityX[Line3]) / 4;
averageY = (velocityY[Line2] +
velocityY[Line2 + 1] + velocityY[Line1] + velocityY[Line3]) / 4;
VelBufX[i & 1][0] = averageX -
(II[address].xx * averageX +
II[address].xy * averageY + II[address].xt) * II[address].alpha;
VelBufY[i & 1][0] = averageY -
(II[address].xy * averageX +
II[address].yy * averageY + II[address].yt) * II[address].alpha;
/* update Epsilon */
if( criteria.type & CV_TERMCRIT_EPS )
{
tmp = (float)fabs(velocityX[Line2] - VelBufX[i & 1][0]);
Eps = MAX( tmp, Eps );
tmp = (float)fabs(velocityY[Line2] - VelBufY[i & 1][0]);
Eps = MAX( tmp, Eps );
}
address++;
/* Process middle of line */
for( j = 1; j < imageWidth - 1; j++ )
{
averageX = (velocityX[Line2 + j - 1] +
velocityX[Line2 + j + 1] +
velocityX[Line1 + j] + velocityX[Line3 + j]) / 4;
averageY = (velocityY[Line2 + j - 1] +
velocityY[Line2 + j + 1] +
velocityY[Line1 + j] + velocityY[Line3 + j]) / 4;
VelBufX[i & 1][j] = averageX -
(II[address].xx * averageX +
II[address].xy * averageY + II[address].xt) * II[address].alpha;
VelBufY[i & 1][j] = averageY -
(II[address].xy * averageX +
II[address].yy * averageY + II[address].yt) * II[address].alpha;
/* update Epsilon */
if( criteria.type & CV_TERMCRIT_EPS )
{
tmp = (float)fabs(velocityX[Line2 + j] - VelBufX[i & 1][j]);
Eps = MAX( tmp, Eps );
tmp = (float)fabs(velocityY[Line2 + j] - VelBufY[i & 1][j]);
Eps = MAX( tmp, Eps );
}
address++;
}
/* Process last pixel of line */
averageX = (velocityX[Line2 + imageWidth - 2] +
velocityX[Line2 + imageWidth - 1] +
velocityX[Line1 + imageWidth - 1] +
velocityX[Line3 + imageWidth - 1]) / 4;
averageY = (velocityY[Line2 + imageWidth - 2] +
velocityY[Line2 + imageWidth - 1] +
velocityY[Line1 + imageWidth - 1] +
velocityY[Line3 + imageWidth - 1]) / 4;
VelBufX[i & 1][imageWidth - 1] = averageX -
(II[address].xx * averageX +
II[address].xy * averageY + II[address].xt) * II[address].alpha;
VelBufY[i & 1][imageWidth - 1] = averageY -
(II[address].xy * averageX +
II[address].yy * averageY + II[address].yt) * II[address].alpha;
/* update Epsilon */
if( criteria.type & CV_TERMCRIT_EPS )
{
tmp = (float)fabs(velocityX[Line2 + imageWidth - 1] -
VelBufX[i & 1][imageWidth - 1]);
Eps = MAX( tmp, Eps );
tmp = (float)fabs(velocityY[Line2 + imageWidth - 1] -
VelBufY[i & 1][imageWidth - 1]);
Eps = MAX( tmp, Eps );
}
address++;
/* store new velocity from old buffer to velocity frame */
if( i > 0 )
{
memcpy( &velocityX[Line1], VelBufX[(i - 1) & 1], imageWidth * sizeof( float ));
memcpy( &velocityY[Line1], VelBufY[(i - 1) & 1], imageWidth * sizeof( float ));
}
} /*for */
/* store new velocity from old buffer to velocity frame */
memcpy( &velocityX[imageWidth * (imageHeight - 1)],
VelBufX[(imageHeight - 1) & 1], imageWidth * sizeof( float ));
memcpy( &velocityY[imageWidth * (imageHeight - 1)],
VelBufY[(imageHeight - 1) & 1], imageWidth * sizeof( float ));
if( (criteria.type & CV_TERMCRIT_ITER) && (iter == criteria.max_iter) )
Stop = 1;
if( (criteria.type & CV_TERMCRIT_EPS) && (Eps < criteria.epsilon) )
Stop = 1;
}
/* Free memory */
for( k = 0; k < 2; k++ )
{
cvFree( &MemX[k] );
cvFree( &MemY[k] );
cvFree( &VelBufX[k] );
cvFree( &VelBufY[k] );
}
cvFree( &II );
return CV_OK;
} /*icvCalcOpticalFlowHS_8u32fR*/
/*F///////////////////////////////////////////////////////////////////////////////////////
// Name: cvCalcOpticalFlowHS
// Purpose: Optical flow implementation
// Context:
// Parameters:
// srcA, srcB - source image
// velx, vely - destination image
// Returns:
//
// Notes:
//F*/
CV_IMPL void
cvCalcOpticalFlowHS( const void* srcarrA, const void* srcarrB, int usePrevious,
void* velarrx, void* velarry,
double lambda, CvTermCriteria criteria )
{
CvMat stubA, *srcA = cvGetMat( srcarrA, &stubA );
CvMat stubB, *srcB = cvGetMat( srcarrB, &stubB );
CvMat stubx, *velx = cvGetMat( velarrx, &stubx );
CvMat stuby, *vely = cvGetMat( velarry, &stuby );
if( !CV_ARE_TYPES_EQ( srcA, srcB ))
CV_Error( CV_StsUnmatchedFormats, "Source images have different formats" );
if( !CV_ARE_TYPES_EQ( velx, vely ))
CV_Error( CV_StsUnmatchedFormats, "Destination images have different formats" );
if( !CV_ARE_SIZES_EQ( srcA, srcB ) ||
!CV_ARE_SIZES_EQ( velx, vely ) ||
!CV_ARE_SIZES_EQ( srcA, velx ))
CV_Error( CV_StsUnmatchedSizes, "" );
if( CV_MAT_TYPE( srcA->type ) != CV_8UC1 ||
CV_MAT_TYPE( velx->type ) != CV_32FC1 )
CV_Error( CV_StsUnsupportedFormat, "Source images must have 8uC1 type and "
"destination images must have 32fC1 type" );
if( srcA->step != srcB->step || velx->step != vely->step )
CV_Error( CV_BadStep, "source and destination images have different step" );
IPPI_CALL( icvCalcOpticalFlowHS_8u32fR( (uchar*)srcA->data.ptr, (uchar*)srcB->data.ptr,
srcA->step, cvGetMatSize( srcA ), usePrevious,
velx->data.fl, vely->data.fl,
velx->step, (float)lambda, criteria ));
}
/* End of file. */

599
modules/legacy/src/optflowlk.cpp Normal file
View File

@@ -0,0 +1,599 @@
/*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.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, 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 Intel Corporation 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"
typedef struct
{
float xx;
float xy;
float yy;
float xt;
float yt;
}
icvDerProduct;
#define CONV( A, B, C) ((float)( A + (B<<1) + C ))
/*F///////////////////////////////////////////////////////////////////////////////////////
// Name: icvCalcOpticalFlowLK_8u32fR ( Lucas & Kanade method )
// Purpose: calculate Optical flow for 2 images using Lucas & Kanade algorithm
// Context:
// Parameters:
// imgA, // pointer to first frame ROI
// imgB, // pointer to second frame ROI
// imgStep, // width of single row of source images in bytes
// imgSize, // size of the source image ROI
// winSize, // size of the averaging window used for grouping
// velocityX, // pointer to horizontal and
// velocityY, // vertical components of optical flow ROI
// velStep // width of single row of velocity frames in bytes
//
// Returns: CV_OK - all ok
// CV_OUTOFMEM_ERR - insufficient memory for function work
// CV_NULLPTR_ERR - if one of input pointers is NULL
// CV_BADSIZE_ERR - wrong input sizes interrelation
//
// Notes: 1.Optical flow to be computed for every pixel in ROI
// 2.For calculating spatial derivatives we use 3x3 Sobel operator.
// 3.We use the following border mode.
// The last row or column is replicated for the border
// ( IPL_BORDER_REPLICATE in IPL ).
//
//
//F*/
static CvStatus CV_STDCALL
icvCalcOpticalFlowLK_8u32fR( uchar * imgA,
uchar * imgB,
int imgStep,
CvSize imgSize,
CvSize winSize,
float *velocityX,
float *velocityY, int velStep )
{
/* Loops indexes */
int i, j, k;
/* Gaussian separable kernels */
float GaussX[16];
float GaussY[16];
float *KerX;
float *KerY;
/* Buffers for Sobel calculations */
float *MemX[2];
float *MemY[2];
float ConvX, ConvY;
float GradX, GradY, GradT;
int winWidth = winSize.width;
int winHeight = winSize.height;
int imageWidth = imgSize.width;
int imageHeight = imgSize.height;
int HorRadius = (winWidth - 1) >> 1;
int VerRadius = (winHeight - 1) >> 1;
int PixelLine;
int ConvLine;
int BufferAddress;
int BufferHeight = 0;
int BufferWidth;
int BufferSize;
/* buffers derivatives product */
icvDerProduct *II;
/* buffers for gaussian horisontal convolution */
icvDerProduct *WII;
/* variables for storing number of first pixel of image line */
int Line1;
int Line2;
int Line3;
/* we must have 2*2 linear system coeffs
| A1B2 B1 | {u} {C1} {0}
| | { } + { } = { }
| A2 A1B2 | {v} {C2} {0}
*/
float A1B2, A2, B1, C1, C2;
int pixNumber;
/* auxiliary */
int NoMem = 0;
velStep /= sizeof(velocityX[0]);
/* Checking bad arguments */
if( imgA == NULL )
return CV_NULLPTR_ERR;
if( imgB == NULL )
return CV_NULLPTR_ERR;
if( imageHeight < winHeight )
return CV_BADSIZE_ERR;
if( imageWidth < winWidth )
return CV_BADSIZE_ERR;
if( winHeight >= 16 )
return CV_BADSIZE_ERR;
if( winWidth >= 16 )
return CV_BADSIZE_ERR;
if( !(winHeight & 1) )
return CV_BADSIZE_ERR;
if( !(winWidth & 1) )
return CV_BADSIZE_ERR;
BufferHeight = winHeight;
BufferWidth = imageWidth;
/****************************************************************************************/
/* Computing Gaussian coeffs */
/****************************************************************************************/
GaussX[0] = 1;
GaussY[0] = 1;
for( i = 1; i < winWidth; i++ )
{
GaussX[i] = 1;
for( j = i - 1; j > 0; j-- )
{
GaussX[j] += GaussX[j - 1];
}
}
for( i = 1; i < winHeight; i++ )
{
GaussY[i] = 1;
for( j = i - 1; j > 0; j-- )
{
GaussY[j] += GaussY[j - 1];
}
}
KerX = &GaussX[HorRadius];
KerY = &GaussY[VerRadius];
/****************************************************************************************/
/* Allocating memory for all buffers */
/****************************************************************************************/
for( k = 0; k < 2; k++ )
{
MemX[k] = (float *) cvAlloc( (imgSize.height) * sizeof( float ));
if( MemX[k] == NULL )
NoMem = 1;
MemY[k] = (float *) cvAlloc( (imgSize.width) * sizeof( float ));
if( MemY[k] == NULL )
NoMem = 1;
}
BufferSize = BufferHeight * BufferWidth;
II = (icvDerProduct *) cvAlloc( BufferSize * sizeof( icvDerProduct ));
WII = (icvDerProduct *) cvAlloc( BufferSize * sizeof( icvDerProduct ));
if( (II == NULL) || (WII == NULL) )
NoMem = 1;
if( NoMem )
{
for( k = 0; k < 2; k++ )
{
if( MemX[k] )
cvFree( &MemX[k] );
if( MemY[k] )
cvFree( &MemY[k] );
}
if( II )
cvFree( &II );
if( WII )
cvFree( &WII );
return CV_OUTOFMEM_ERR;
}
/****************************************************************************************/
/* Calculate first line of memX and memY */
/****************************************************************************************/
MemY[0][0] = MemY[1][0] = CONV( imgA[0], imgA[0], imgA[1] );
MemX[0][0] = MemX[1][0] = CONV( imgA[0], imgA[0], imgA[imgStep] );
for( j = 1; j < imageWidth - 1; j++ )
{
MemY[0][j] = MemY[1][j] = CONV( imgA[j - 1], imgA[j], imgA[j + 1] );
}
pixNumber = imgStep;
for( i = 1; i < imageHeight - 1; i++ )
{
MemX[0][i] = MemX[1][i] = CONV( imgA[pixNumber - imgStep],
imgA[pixNumber], imgA[pixNumber + imgStep] );
pixNumber += imgStep;
}
MemY[0][imageWidth - 1] =
MemY[1][imageWidth - 1] = CONV( imgA[imageWidth - 2],
imgA[imageWidth - 1], imgA[imageWidth - 1] );
MemX[0][imageHeight - 1] =
MemX[1][imageHeight - 1] = CONV( imgA[pixNumber - imgStep],
imgA[pixNumber], imgA[pixNumber] );
/****************************************************************************************/
/* begin scan image, calc derivatives and solve system */
/****************************************************************************************/
PixelLine = -VerRadius;
ConvLine = 0;
BufferAddress = -BufferWidth;
while( PixelLine < imageHeight )
{
if( ConvLine < imageHeight )
{
/*Here we calculate derivatives for line of image */
int address;
i = ConvLine;
int L1 = i - 1;
int L2 = i;
int L3 = i + 1;
int memYline = L3 & 1;
if( L1 < 0 )
L1 = 0;
if( L3 >= imageHeight )
L3 = imageHeight - 1;
BufferAddress += BufferWidth;
BufferAddress -= ((BufferAddress >= BufferSize) ? 0xffffffff : 0) & BufferSize;
address = BufferAddress;
Line1 = L1 * imgStep;
Line2 = L2 * imgStep;
Line3 = L3 * imgStep;
/* Process first pixel */
ConvX = CONV( imgA[Line1 + 1], imgA[Line2 + 1], imgA[Line3 + 1] );
ConvY = CONV( imgA[Line3], imgA[Line3], imgA[Line3 + 1] );
GradY = ConvY - MemY[memYline][0];
GradX = ConvX - MemX[1][L2];
MemY[memYline][0] = ConvY;
MemX[1][L2] = ConvX;
GradT = (float) (imgB[Line2] - imgA[Line2]);
II[address].xx = GradX * GradX;
II[address].xy = GradX * GradY;
II[address].yy = GradY * GradY;
II[address].xt = GradX * GradT;
II[address].yt = GradY * GradT;
address++;
/* Process middle of line */
for( j = 1; j < imageWidth - 1; j++ )
{
ConvX = CONV( imgA[Line1 + j + 1], imgA[Line2 + j + 1], imgA[Line3 + j + 1] );
ConvY = CONV( imgA[Line3 + j - 1], imgA[Line3 + j], imgA[Line3 + j + 1] );
GradY = ConvY - MemY[memYline][j];
GradX = ConvX - MemX[(j - 1) & 1][L2];
MemY[memYline][j] = ConvY;
MemX[(j - 1) & 1][L2] = ConvX;
GradT = (float) (imgB[Line2 + j] - imgA[Line2 + j]);
II[address].xx = GradX * GradX;
II[address].xy = GradX * GradY;
II[address].yy = GradY * GradY;
II[address].xt = GradX * GradT;
II[address].yt = GradY * GradT;
address++;
}
/* Process last pixel of line */
ConvX = CONV( imgA[Line1 + imageWidth - 1], imgA[Line2 + imageWidth - 1],
imgA[Line3 + imageWidth - 1] );
ConvY = CONV( imgA[Line3 + imageWidth - 2], imgA[Line3 + imageWidth - 1],
imgA[Line3 + imageWidth - 1] );
GradY = ConvY - MemY[memYline][imageWidth - 1];
GradX = ConvX - MemX[(imageWidth - 2) & 1][L2];
MemY[memYline][imageWidth - 1] = ConvY;
GradT = (float) (imgB[Line2 + imageWidth - 1] - imgA[Line2 + imageWidth - 1]);
II[address].xx = GradX * GradX;
II[address].xy = GradX * GradY;
II[address].yy = GradY * GradY;
II[address].xt = GradX * GradT;
II[address].yt = GradY * GradT;
address++;
/* End of derivatives for line */
/****************************************************************************************/
/* ---------Calculating horizontal convolution of processed line----------------------- */
/****************************************************************************************/
address -= BufferWidth;
/* process first HorRadius pixels */
for( j = 0; j < HorRadius; j++ )
{
int jj;
WII[address].xx = 0;
WII[address].xy = 0;
WII[address].yy = 0;
WII[address].xt = 0;
WII[address].yt = 0;
for( jj = -j; jj <= HorRadius; jj++ )
{
float Ker = KerX[jj];
WII[address].xx += II[address + jj].xx * Ker;
WII[address].xy += II[address + jj].xy * Ker;
WII[address].yy += II[address + jj].yy * Ker;
WII[address].xt += II[address + jj].xt * Ker;
WII[address].yt += II[address + jj].yt * Ker;
}
address++;
}
/* process inner part of line */
for( j = HorRadius; j < imageWidth - HorRadius; j++ )
{
int jj;
float Ker0 = KerX[0];
WII[address].xx = 0;
WII[address].xy = 0;
WII[address].yy = 0;
WII[address].xt = 0;
WII[address].yt = 0;
for( jj = 1; jj <= HorRadius; jj++ )
{
float Ker = KerX[jj];
WII[address].xx += (II[address - jj].xx + II[address + jj].xx) * Ker;
WII[address].xy += (II[address - jj].xy + II[address + jj].xy) * Ker;
WII[address].yy += (II[address - jj].yy + II[address + jj].yy) * Ker;
WII[address].xt += (II[address - jj].xt + II[address + jj].xt) * Ker;
WII[address].yt += (II[address - jj].yt + II[address + jj].yt) * Ker;
}
WII[address].xx += II[address].xx * Ker0;
WII[address].xy += II[address].xy * Ker0;
WII[address].yy += II[address].yy * Ker0;
WII[address].xt += II[address].xt * Ker0;
WII[address].yt += II[address].yt * Ker0;
address++;
}
/* process right side */
for( j = imageWidth - HorRadius; j < imageWidth; j++ )
{
int jj;
WII[address].xx = 0;
WII[address].xy = 0;
WII[address].yy = 0;
WII[address].xt = 0;
WII[address].yt = 0;
for( jj = -HorRadius; jj < imageWidth - j; jj++ )
{
float Ker = KerX[jj];
WII[address].xx += II[address + jj].xx * Ker;
WII[address].xy += II[address + jj].xy * Ker;
WII[address].yy += II[address + jj].yy * Ker;
WII[address].xt += II[address + jj].xt * Ker;
WII[address].yt += II[address + jj].yt * Ker;
}
address++;
}
}
/****************************************************************************************/
/* Calculating velocity line */
/****************************************************************************************/
if( PixelLine >= 0 )
{
int USpace;
int BSpace;
int address;
if( PixelLine < VerRadius )
USpace = PixelLine;
else
USpace = VerRadius;
if( PixelLine >= imageHeight - VerRadius )
BSpace = imageHeight - PixelLine - 1;
else
BSpace = VerRadius;
address = ((PixelLine - USpace) % BufferHeight) * BufferWidth;
for( j = 0; j < imageWidth; j++ )
{
int addr = address;
A1B2 = 0;
A2 = 0;
B1 = 0;
C1 = 0;
C2 = 0;
for( i = -USpace; i <= BSpace; i++ )
{
A2 += WII[addr + j].xx * KerY[i];
A1B2 += WII[addr + j].xy * KerY[i];
B1 += WII[addr + j].yy * KerY[i];
C2 += WII[addr + j].xt * KerY[i];
C1 += WII[addr + j].yt * KerY[i];
addr += BufferWidth;
addr -= ((addr >= BufferSize) ? 0xffffffff : 0) & BufferSize;
}
/****************************************************************************************\
* Solve Linear System *
\****************************************************************************************/
{
float delta = (A1B2 * A1B2 - A2 * B1);
if( delta )
{
/* system is not singular - solving by Kramer method */
float deltaX;
float deltaY;
float Idelta = 8 / delta;
deltaX = -(C1 * A1B2 - C2 * B1);
deltaY = -(A1B2 * C2 - A2 * C1);
velocityX[j] = deltaX * Idelta;
velocityY[j] = deltaY * Idelta;
}
else
{
/* singular system - find optical flow in gradient direction */
float Norm = (A1B2 + A2) * (A1B2 + A2) + (B1 + A1B2) * (B1 + A1B2);
if( Norm )
{
float IGradNorm = 8 / Norm;
float temp = -(C1 + C2) * IGradNorm;
velocityX[j] = (A1B2 + A2) * temp;
velocityY[j] = (B1 + A1B2) * temp;
}
else
{
velocityX[j] = 0;
velocityY[j] = 0;
}
}
}
/****************************************************************************************\
* End of Solving Linear System *
\****************************************************************************************/
} /*for */
velocityX += velStep;
velocityY += velStep;
} /*for */
PixelLine++;
ConvLine++;
}
/* Free memory */
for( k = 0; k < 2; k++ )
{
cvFree( &MemX[k] );
cvFree( &MemY[k] );
}
cvFree( &II );
cvFree( &WII );
return CV_OK;
} /*icvCalcOpticalFlowLK_8u32fR*/
/*F///////////////////////////////////////////////////////////////////////////////////////
// Name: cvCalcOpticalFlowLK
// Purpose: Optical flow implementation
// Context:
// Parameters:
// srcA, srcB - source image
// velx, vely - destination image
// Returns:
//
// Notes:
//F*/
CV_IMPL void
cvCalcOpticalFlowLK( const void* srcarrA, const void* srcarrB, CvSize winSize,
void* velarrx, void* velarry )
{
CvMat stubA, *srcA = cvGetMat( srcarrA, &stubA );
CvMat stubB, *srcB = cvGetMat( srcarrB, &stubB );
CvMat stubx, *velx = cvGetMat( velarrx, &stubx );
CvMat stuby, *vely = cvGetMat( velarry, &stuby );
if( !CV_ARE_TYPES_EQ( srcA, srcB ))
CV_Error( CV_StsUnmatchedFormats, "Source images have different formats" );
if( !CV_ARE_TYPES_EQ( velx, vely ))
CV_Error( CV_StsUnmatchedFormats, "Destination images have different formats" );
if( !CV_ARE_SIZES_EQ( srcA, srcB ) ||
!CV_ARE_SIZES_EQ( velx, vely ) ||
!CV_ARE_SIZES_EQ( srcA, velx ))
CV_Error( CV_StsUnmatchedSizes, "" );
if( CV_MAT_TYPE( srcA->type ) != CV_8UC1 ||
CV_MAT_TYPE( velx->type ) != CV_32FC1 )
CV_Error( CV_StsUnsupportedFormat, "Source images must have 8uC1 type and "
"destination images must have 32fC1 type" );
if( srcA->step != srcB->step || velx->step != vely->step )
CV_Error( CV_BadStep, "source and destination images have different step" );
IPPI_CALL( icvCalcOpticalFlowLK_8u32fR( (uchar*)srcA->data.ptr, (uchar*)srcB->data.ptr,
srcA->step, cvGetMatSize( srcA ), winSize,
velx->data.fl, vely->data.fl, velx->step ));
}
/* End of file. */

1880
modules/legacy/src/pyrsegmentation.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

498
modules/legacy/src/spilltree.cpp Normal file
View File

@@ -0,0 +1,498 @@
/* Original code has been submitted by Liu Liu.
----------------------------------------------------------------------------------
* Spill-Tree for Approximate KNN Search
* Author: Liu Liu
* mailto: liuliu.1987+opencv@gmail.com
* Refer to Paper:
* An Investigation of Practical Approximate Nearest Neighbor Algorithms
* cvMergeSpillTree TBD
*
* Redistribution and use in source and binary forms, with or
* without modification, are permitted provided that the following
* conditions are met:
* Redistributions of source code must retain the above
* copyright notice, this list of conditions and the following
* disclaimer.
* Redistributions 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 Contributor 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 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.
*/
#include "precomp.hpp"
#include "_featuretree.h"
struct CvSpillTreeNode
{
bool leaf; // is leaf or not (leaf is the point that have no more child)
bool spill; // is not a non-overlapping point (defeatist search)
CvSpillTreeNode* lc; // left child (<)
CvSpillTreeNode* rc; // right child (>)
int cc; // child count
CvMat* u; // projection vector
CvMat* center; // center
int i; // original index
double r; // radius of remaining feature point
double ub; // upper bound
double lb; // lower bound
double mp; // mean point
double p; // projection value
};
struct CvSpillTree
{
CvSpillTreeNode* root;
CvMat** refmat; // leaf ref matrix
int total; // total leaves
int naive; // under this value, we perform naive search
int type; // mat type
double rho; // under this value, it is a spill tree
double tau; // the overlapping buffer ratio
};
struct CvResult
{
int index;
double distance;
};
// find the farthest node in the "list" from "node"
static inline CvSpillTreeNode*
icvFarthestNode( CvSpillTreeNode* node,
CvSpillTreeNode* list,
int total )
{
double farthest = -1.;
CvSpillTreeNode* result = NULL;
for ( int i = 0; i < total; i++ )
{
double norm = cvNorm( node->center, list->center );
if ( norm > farthest )
{
farthest = norm;
result = list;
}
list = list->rc;
}
return result;
}
// clone a new tree node
static inline CvSpillTreeNode*
icvCloneSpillTreeNode( CvSpillTreeNode* node )
{
CvSpillTreeNode* result = (CvSpillTreeNode*)cvAlloc( sizeof(CvSpillTreeNode) );
memcpy( result, node, sizeof(CvSpillTreeNode) );
return result;
}
// append the link-list of a tree node
static inline void
icvAppendSpillTreeNode( CvSpillTreeNode* node,
CvSpillTreeNode* append )
{
if ( node->lc == NULL )
{
node->lc = node->rc = append;
node->lc->lc = node->rc->rc = NULL;
} else {
append->lc = node->rc;
append->rc = NULL;
node->rc->rc = append;
node->rc = append;
}
node->cc++;
}
#define _dispatch_mat_ptr(x, step) (CV_MAT_DEPTH((x)->type) == CV_32F ? (void*)((x)->data.fl+(step)) : (CV_MAT_DEPTH((x)->type) == CV_64F ? (void*)((x)->data.db+(step)) : (void*)(0)))
static void
icvDFSInitSpillTreeNode( const CvSpillTree* tr,
const int d,
CvSpillTreeNode* node )
{
if ( node->cc <= tr->naive )
{
// already get to a leaf, terminate the recursion.
node->leaf = true;
node->spill = false;
return;
}
// random select a node, then find a farthest node from this one, then find a farthest from that one...
// to approximate the farthest node-pair
static CvRNG rng_state = cvRNG(0xdeadbeef);
int rn = cvRandInt( &rng_state ) % node->cc;
CvSpillTreeNode* lnode = NULL;
CvSpillTreeNode* rnode = node->lc;
for ( int i = 0; i < rn; i++ )
rnode = rnode->rc;
lnode = icvFarthestNode( rnode, node->lc, node->cc );
rnode = icvFarthestNode( lnode, node->lc, node->cc );
// u is the projection vector
node->u = cvCreateMat( 1, d, tr->type );
cvSub( lnode->center, rnode->center, node->u );
cvNormalize( node->u, node->u );
// find the center of node in hyperspace
node->center = cvCreateMat( 1, d, tr->type );
cvZero( node->center );
CvSpillTreeNode* it = node->lc;
for ( int i = 0; i < node->cc; i++ )
{
cvAdd( it->center, node->center, node->center );
it = it->rc;
}
cvConvertScale( node->center, node->center, 1./node->cc );
// project every node to "u", and find the mean point "mp"
it = node->lc;
node->r = -1.;
node->mp = 0;
for ( int i = 0; i < node->cc; i++ )
{
node->mp += ( it->p = cvDotProduct( it->center, node->u ) );
double norm = cvNorm( node->center, it->center );
if ( norm > node->r )
node->r = norm;
it = it->rc;
}
node->mp = node->mp / node->cc;
// overlapping buffer and upper bound, lower bound
double ob = (lnode->p-rnode->p)*tr->tau*.5;
node->ub = node->mp+ob;
node->lb = node->mp-ob;
int sl = 0, l = 0;
int sr = 0, r = 0;
it = node->lc;
for ( int i = 0; i < node->cc; i++ )
{
if ( it->p <= node->ub )
sl++;
if ( it->p >= node->lb )
sr++;
if ( it->p < node->mp )
l++;
else
r++;
it = it->rc;
}
// precision problem, return the node as it is.
if (( l == 0 )||( r == 0 ))
{
cvReleaseMat( &(node->u) );
cvReleaseMat( &(node->center) );
node->leaf = true;
node->spill = false;
return;
}
CvSpillTreeNode* lc = (CvSpillTreeNode*)cvAlloc( sizeof(CvSpillTreeNode) );
memset(lc, 0, sizeof(CvSpillTreeNode));
CvSpillTreeNode* rc = (CvSpillTreeNode*)cvAlloc( sizeof(CvSpillTreeNode) );
memset(rc, 0, sizeof(CvSpillTreeNode));
lc->lc = lc->rc = rc->lc = rc->rc = NULL;
lc->cc = rc->cc = 0;
int undo = cvRound(node->cc*tr->rho);
if (( sl >= undo )||( sr >= undo ))
{
// it is not a spill point (defeatist search disabled)
it = node->lc;
for ( int i = 0; i < node->cc; i++ )
{
CvSpillTreeNode* next = it->rc;
if ( it->p < node->mp )
icvAppendSpillTreeNode( lc, it );
else
icvAppendSpillTreeNode( rc, it );
it = next;
}
node->spill = false;
} else {
// a spill point
it = node->lc;
for ( int i = 0; i < node->cc; i++ )
{
CvSpillTreeNode* next = it->rc;
if ( it->p < node->lb )
icvAppendSpillTreeNode( lc, it );
else if ( it->p > node->ub )
icvAppendSpillTreeNode( rc, it );
else {
CvSpillTreeNode* cit = icvCloneSpillTreeNode( it );
icvAppendSpillTreeNode( lc, it );
icvAppendSpillTreeNode( rc, cit );
}
it = next;
}
node->spill = true;
}
node->lc = lc;
node->rc = rc;
// recursion process
icvDFSInitSpillTreeNode( tr, d, node->lc );
icvDFSInitSpillTreeNode( tr, d, node->rc );
}
static CvSpillTree*
icvCreateSpillTree( const CvMat* raw_data,
const int naive,
const double rho,
const double tau )
{
int n = raw_data->rows;
int d = raw_data->cols;
CvSpillTree* tr = (CvSpillTree*)cvAlloc( sizeof(CvSpillTree) );
tr->root = (CvSpillTreeNode*)cvAlloc( sizeof(CvSpillTreeNode) );
memset(tr->root, 0, sizeof(CvSpillTreeNode));
tr->refmat = (CvMat**)cvAlloc( sizeof(CvMat*)*n );
tr->total = n;
tr->naive = naive;
tr->rho = rho;
tr->tau = tau;
tr->type = raw_data->type;
// tie a link-list to the root node
tr->root->lc = (CvSpillTreeNode*)cvAlloc( sizeof(CvSpillTreeNode) );
memset(tr->root->lc, 0, sizeof(CvSpillTreeNode));
tr->root->lc->center = cvCreateMatHeader( 1, d, tr->type );
cvSetData( tr->root->lc->center, _dispatch_mat_ptr(raw_data, 0), raw_data->step );
tr->refmat[0] = tr->root->lc->center;
tr->root->lc->lc = NULL;
tr->root->lc->leaf = true;
tr->root->lc->i = 0;
CvSpillTreeNode* node = tr->root->lc;
for ( int i = 1; i < n; i++ )
{
CvSpillTreeNode* newnode = (CvSpillTreeNode*)cvAlloc( sizeof(CvSpillTreeNode) );
memset(newnode, 0, sizeof(CvSpillTreeNode));
newnode->center = cvCreateMatHeader( 1, d, tr->type );
cvSetData( newnode->center, _dispatch_mat_ptr(raw_data, i*d), raw_data->step );
tr->refmat[i] = newnode->center;
newnode->lc = node;
newnode->i = i;
newnode->leaf = true;
newnode->rc = NULL;
node->rc = newnode;
node = newnode;
}
tr->root->rc = node;
tr->root->cc = n;
icvDFSInitSpillTreeNode( tr, d, tr->root );
return tr;
}
static void
icvSpillTreeNodeHeapify( CvResult * heap,
int i,
const int k )
{
if ( heap[i].index == -1 )
return;
int l, r, largest = i;
CvResult inp;
do {
i = largest;
r = (i+1)<<1;
l = r-1;
if (( l < k )&&( heap[l].index == -1 ))
largest = l;
else if (( r < k )&&( heap[r].index == -1 ))
largest = r;
else {
if (( l < k )&&( heap[l].distance > heap[i].distance ))
largest = l;
if (( r < k )&&( heap[r].distance > heap[largest].distance ))
largest = r;
}
if ( largest != i )
CV_SWAP( heap[largest], heap[i], inp );
} while ( largest != i );
}
static void
icvSpillTreeDFSearch( CvSpillTree* tr,
CvSpillTreeNode* node,
CvResult* heap,
int* es,
const CvMat* desc,
const int k,
const int emax,
bool * cache)
{
if ((emax > 0)&&( *es >= emax ))
return;
double dist, p=0;
double distance;
while ( node->spill )
{
// defeatist search
if ( !node->leaf )
p = cvDotProduct( node->u, desc );
if ( p < node->lb && node->lc->cc >= k ) // check the number of children larger than k otherwise you'll skip over better neighbor
node = node->lc;
else if ( p > node->ub && node->rc->cc >= k )
node = node->rc;
else
break;
if ( NULL == node )
return;
}
if ( node->leaf )
{
// a leaf, naive search
CvSpillTreeNode* it = node->lc;
for ( int i = 0; i < node->cc; i++ )
{
if ( !cache[it->i] )
{
distance = cvNorm( it->center, desc );
cache[it->i] = true;
if (( heap[0].index == -1)||( distance < heap[0].distance ))
{
CvResult current_result;
current_result.index = it->i;
current_result.distance = distance;
heap[0] = current_result;
icvSpillTreeNodeHeapify( heap, 0, k );
(*es)++;
}
}
it = it->rc;
}
return;
}
dist = cvNorm( node->center, desc );
// impossible case, skip
if (( heap[0].index != -1 )&&( dist-node->r > heap[0].distance ))
return;
p = cvDotProduct( node->u, desc );
// guided dfs
if ( p < node->mp )
{
icvSpillTreeDFSearch( tr, node->lc, heap, es, desc, k, emax, cache );
icvSpillTreeDFSearch( tr, node->rc, heap, es, desc, k, emax, cache );
} else {
icvSpillTreeDFSearch( tr, node->rc, heap, es, desc, k, emax, cache );
icvSpillTreeDFSearch( tr, node->lc, heap, es, desc, k, emax, cache );
}
}
static void
icvFindSpillTreeFeatures( CvSpillTree* tr,
const CvMat* desc,
CvMat* results,
CvMat* dist,
const int k,
const int emax )
{
assert( desc->type == tr->type );
CvResult* heap = (CvResult*)cvAlloc( k*sizeof(heap[0]) );
bool* cache = (bool*)cvAlloc( sizeof(bool)*tr->total );
for ( int j = 0; j < desc->rows; j++ )
{
CvMat _desc = cvMat( 1, desc->cols, desc->type, _dispatch_mat_ptr(desc, j*desc->cols) );
for ( int i = 0; i < k; i++ ) {
CvResult current;
current.index=-1;
current.distance=-1;
heap[i] = current;
}
memset( cache, 0, sizeof(bool)*tr->total );
int es = 0;
icvSpillTreeDFSearch( tr, tr->root, heap, &es, &_desc, k, emax, cache );
CvResult inp;
for ( int i = k-1; i > 0; i-- )
{
CV_SWAP( heap[i], heap[0], inp );
icvSpillTreeNodeHeapify( heap, 0, i );
}
int* rs = results->data.i+j*results->cols;
double* dt = dist->data.db+j*dist->cols;
for ( int i = 0; i < k; i++, rs++, dt++ )
if ( heap[i].index != -1 )
{
*rs = heap[i].index;
*dt = heap[i].distance;
} else
*rs = -1;
}
cvFree( &heap );
cvFree( &cache );
}
static void
icvDFSReleaseSpillTreeNode( CvSpillTreeNode* node )
{
if ( node->leaf )
{
CvSpillTreeNode* it = node->lc;
for ( int i = 0; i < node->cc; i++ )
{
CvSpillTreeNode* s = it;
it = it->rc;
cvFree( &s );
}
} else {
cvReleaseMat( &node->u );
cvReleaseMat( &node->center );
icvDFSReleaseSpillTreeNode( node->lc );
icvDFSReleaseSpillTreeNode( node->rc );
}
cvFree( &node );
}
static void
icvReleaseSpillTree( CvSpillTree** tr )
{
for ( int i = 0; i < (*tr)->total; i++ )
cvReleaseMat( &((*tr)->refmat[i]) );
cvFree( &((*tr)->refmat) );
icvDFSReleaseSpillTreeNode( (*tr)->root );
cvFree( tr );
}
class CvSpillTreeWrap : public CvFeatureTree {
CvSpillTree* tr;
public:
CvSpillTreeWrap(const CvMat* raw_data,
const int naive,
const double rho,
const double tau) {
tr = icvCreateSpillTree(raw_data, naive, rho, tau);
}
~CvSpillTreeWrap() {
icvReleaseSpillTree(&tr);
}
void FindFeatures(const CvMat* desc, int k, int emax, CvMat* results, CvMat* dist) {
icvFindSpillTreeFeatures(tr, desc, results, dist, k, emax);
}
};
CvFeatureTree* cvCreateSpillTree( const CvMat* raw_data,
const int naive,
const double rho,
const double tau ) {
return new CvSpillTreeWrap(raw_data, naive, rho, tau);
}

950
modules/legacy/src/stereogc.cpp Normal file
View File

@@ -0,0 +1,950 @@
//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.
//
//
// Intel License Agreement
// For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, 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 Intel Corporation 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"
using namespace std;
#undef INFINITY
#define INFINITY 10000
#define OCCLUSION_PENALTY 10000
#define OCCLUSION_PENALTY2 1000
#define DENOMINATOR 16
#undef OCCLUDED
#define OCCLUDED CV_STEREO_GC_OCCLUDED
#define CUTOFF 1000
#define IS_BLOCKED(d1, d2) ((d1) > (d2))
typedef struct GCVtx
{
GCVtx *next;
int parent;
int first;
int ts;
int dist;
short weight;
uchar t;
}
GCVtx;
typedef struct GCEdge
{
GCVtx* dst;
int next;
int weight;
}
GCEdge;
typedef struct CvStereoGCState2
{
int Ithreshold, interactionRadius;
int lambda, lambda1, lambda2, K;
int dataCostFuncTab[CUTOFF+1];
int smoothnessR[CUTOFF*2+1];
int smoothnessGrayDiff[512];
GCVtx** orphans;
int maxOrphans;
}
CvStereoGCState2;
// truncTab[x+255] = MAX(x-255,0)
static uchar icvTruncTab[512];
// cutoffSqrTab[x] = MIN(x*x, CUTOFF)
static int icvCutoffSqrTab[256];
static void icvInitStereoConstTabs()
{
static volatile int initialized = 0;
if( !initialized )
{
int i;
for( i = 0; i < 512; i++ )
icvTruncTab[i] = (uchar)MIN(MAX(i-255,0),255);
for( i = 0; i < 256; i++ )
icvCutoffSqrTab[i] = MIN(i*i, CUTOFF);
initialized = 1;
}
}
static void icvInitStereoTabs( CvStereoGCState2* state2 )
{
int i, K = state2->K;
for( i = 0; i <= CUTOFF; i++ )
state2->dataCostFuncTab[i] = MIN(i*DENOMINATOR - K, 0);
for( i = 0; i < CUTOFF*2 + 1; i++ )
state2->smoothnessR[i] = MIN(abs(i-CUTOFF), state2->interactionRadius);
for( i = 0; i < 512; i++ )
{
int diff = abs(i - 255);
state2->smoothnessGrayDiff[i] = diff < state2->Ithreshold ? state2->lambda1 : state2->lambda2;
}
}
static int icvGCResizeOrphansBuf( GCVtx**& orphans, int norphans )
{
int i, newNOrphans = MAX(norphans*3/2, 256);
GCVtx** newOrphans = (GCVtx**)cvAlloc( newNOrphans*sizeof(orphans[0]) );
for( i = 0; i < norphans; i++ )
newOrphans[i] = orphans[i];
cvFree( &orphans );
orphans = newOrphans;
return newNOrphans;
}
static int64 icvGCMaxFlow( GCVtx* vtx, int nvtx, GCEdge* edges, GCVtx**& _orphans, int& _maxOrphans )
{
const int TERMINAL = -1, ORPHAN = -2;
GCVtx stub, *nilNode = &stub, *first = nilNode, *last = nilNode;
int i, k;
int curr_ts = 0;
int64 flow = 0;
int norphans = 0, maxOrphans = _maxOrphans;
GCVtx** orphans = _orphans;
stub.next = nilNode;
// initialize the active queue and the graph vertices
for( i = 0; i < nvtx; i++ )
{
GCVtx* v = vtx + i;
v->ts = 0;
if( v->weight != 0 )
{
last = last->next = v;
v->dist = 1;
v->parent = TERMINAL;
v->t = v->weight < 0;
}
else
v->parent = 0;
}
first = first->next;
last->next = nilNode;
nilNode->next = 0;
// run the search-path -> augment-graph -> restore-trees loop
for(;;)
{
GCVtx* v, *u;
int e0 = -1, ei = 0, ej = 0, min_weight, weight;
uchar vt;
// grow S & T search trees, find an edge connecting them
while( first != nilNode )
{
v = first;
if( v->parent )
{
vt = v->t;
for( ei = v->first; ei != 0; ei = edges[ei].next )
{
if( edges[ei^vt].weight == 0 )
continue;
u = edges[ei].dst;
if( !u->parent )
{
u->t = vt;
u->parent = ei ^ 1;
u->ts = v->ts;
u->dist = v->dist + 1;
if( !u->next )
{
u->next = nilNode;
last = last->next = u;
}
continue;
}
if( u->t != vt )
{
e0 = ei ^ vt;
break;
}
if( u->dist > v->dist+1 && u->ts <= v->ts )
{
// reassign the parent
u->parent = ei ^ 1;
u->ts = v->ts;
u->dist = v->dist + 1;
}
}
if( e0 > 0 )
break;
}
// exclude the vertex from the active list
first = first->next;
v->next = 0;
}
if( e0 <= 0 )
break;
// find the minimum edge weight along the path
min_weight = edges[e0].weight;
assert( min_weight > 0 );
// k = 1: source tree, k = 0: destination tree
for( k = 1; k >= 0; k-- )
{
for( v = edges[e0^k].dst;; v = edges[ei].dst )
{
if( (ei = v->parent) < 0 )
break;
weight = edges[ei^k].weight;
min_weight = MIN(min_weight, weight);
assert( min_weight > 0 );
}
weight = abs(v->weight);
min_weight = MIN(min_weight, weight);
assert( min_weight > 0 );
}
// modify weights of the edges along the path and collect orphans
edges[e0].weight -= min_weight;
edges[e0^1].weight += min_weight;
flow += min_weight;
// k = 1: source tree, k = 0: destination tree
for( k = 1; k >= 0; k-- )
{
for( v = edges[e0^k].dst;; v = edges[ei].dst )
{
if( (ei = v->parent) < 0 )
break;
edges[ei^(k^1)].weight += min_weight;
if( (edges[ei^k].weight -= min_weight) == 0 )
{
if( norphans >= maxOrphans )
maxOrphans = icvGCResizeOrphansBuf( orphans, norphans );
orphans[norphans++] = v;
v->parent = ORPHAN;
}
}
v->weight = (short)(v->weight + min_weight*(1-k*2));
if( v->weight == 0 )
{
if( norphans >= maxOrphans )
maxOrphans = icvGCResizeOrphansBuf( orphans, norphans );
orphans[norphans++] = v;
v->parent = ORPHAN;
}
}
// restore the search trees by finding new parents for the orphans
curr_ts++;
while( norphans > 0 )
{
GCVtx* v = orphans[--norphans];
int d, min_dist = INT_MAX;
e0 = 0;
vt = v->t;
for( ei = v->first; ei != 0; ei = edges[ei].next )
{
if( edges[ei^(vt^1)].weight == 0 )
continue;
u = edges[ei].dst;
if( u->t != vt || u->parent == 0 )
continue;
// compute the distance to the tree root
for( d = 0;; )
{
if( u->ts == curr_ts )
{
d += u->dist;
break;
}
ej = u->parent;
d++;
if( ej < 0 )
{
if( ej == ORPHAN )
d = INT_MAX-1;
else
{
u->ts = curr_ts;
u->dist = 1;
}
break;
}
u = edges[ej].dst;
}
// update the distance
if( ++d < INT_MAX )
{
if( d < min_dist )
{
min_dist = d;
e0 = ei;
}
for( u = edges[ei].dst; u->ts != curr_ts; u = edges[u->parent].dst )
{
u->ts = curr_ts;
u->dist = --d;
}
}
}
if( (v->parent = e0) > 0 )
{
v->ts = curr_ts;
v->dist = min_dist;
continue;
}
/* no parent is found */
v->ts = 0;
for( ei = v->first; ei != 0; ei = edges[ei].next )
{
u = edges[ei].dst;
ej = u->parent;
if( u->t != vt || !ej )
continue;
if( edges[ei^(vt^1)].weight && !u->next )
{
u->next = nilNode;
last = last->next = u;
}
if( ej > 0 && edges[ej].dst == v )
{
if( norphans >= maxOrphans )
maxOrphans = icvGCResizeOrphansBuf( orphans, norphans );
orphans[norphans++] = u;
u->parent = ORPHAN;
}
}
}
}
_orphans = orphans;
_maxOrphans = maxOrphans;
return flow;
}
CvStereoGCState* cvCreateStereoGCState( int numberOfDisparities, int maxIters )
{
CvStereoGCState* state = 0;
state = (CvStereoGCState*)cvAlloc( sizeof(*state) );
memset( state, 0, sizeof(*state) );
state->minDisparity = 0;
state->numberOfDisparities = numberOfDisparities;
state->maxIters = maxIters <= 0 ? 3 : maxIters;
state->Ithreshold = 5;
state->interactionRadius = 1;
state->K = state->lambda = state->lambda1 = state->lambda2 = -1.f;
state->occlusionCost = OCCLUSION_PENALTY;
return state;
}
void cvReleaseStereoGCState( CvStereoGCState** _state )
{
CvStereoGCState* state;
if( !_state && !*_state )
return;
state = *_state;
cvReleaseMat( &state->left );
cvReleaseMat( &state->right );
cvReleaseMat( &state->ptrLeft );
cvReleaseMat( &state->ptrRight );
cvReleaseMat( &state->vtxBuf );
cvReleaseMat( &state->edgeBuf );
cvFree( _state );
}
// ||I(x) - J(x')|| =
// min(CUTOFF,
// min(
// max(
// max(minJ(x') - I(x), 0),
// max(I(x) - maxJ(x'), 0)),
// max(
// max(minI(x) - J(x'), 0),
// max(J(x') - maxI(x), 0)))**2) ==
// min(CUTOFF,
// min(
// max(minJ(x') - I(x), 0) +
// max(I(x) - maxJ(x'), 0),
//
// max(minI(x) - J(x'), 0) +
// max(J(x') - maxI(x), 0)))**2)
// where (I, minI, maxI) and
// (J, minJ, maxJ) are stored as interleaved 3-channel images.
// minI, maxI are computed from I,
// minJ, maxJ are computed from J - see icvInitGraySubPix.
static inline int icvDataCostFuncGraySubpix( const uchar* a, const uchar* b )
{
int va = a[0], vb = b[0];
int da = icvTruncTab[b[1] - va + 255] + icvTruncTab[va - b[2] + 255];
int db = icvTruncTab[a[1] - vb + 255] + icvTruncTab[vb - a[2] + 255];
return icvCutoffSqrTab[MIN(da,db)];
}
static inline int icvSmoothnessCostFunc( int da, int db, int maxR, const int* stabR, int scale )
{
return da == db ? 0 : (da == OCCLUDED || db == OCCLUDED ? maxR : stabR[da - db])*scale;
}
static void icvInitGraySubpix( const CvMat* left, const CvMat* right,
CvMat* left3, CvMat* right3 )
{
int k, x, y, rows = left->rows, cols = left->cols;
for( k = 0; k < 2; k++ )
{
const CvMat* src = k == 0 ? left : right;
CvMat* dst = k == 0 ? left3 : right3;
int sstep = src->step;
for( y = 0; y < rows; y++ )
{
const uchar* sptr = src->data.ptr + sstep*y;
const uchar* sptr_prev = y > 0 ? sptr - sstep : sptr;
const uchar* sptr_next = y < rows-1 ? sptr + sstep : sptr;
uchar* dptr = dst->data.ptr + dst->step*y;
int v_prev = sptr[0];
for( x = 0; x < cols; x++, dptr += 3 )
{
int v = sptr[x], v1, minv = v, maxv = v;
v1 = (v + v_prev)/2;
minv = MIN(minv, v1); maxv = MAX(maxv, v1);
v1 = (v + sptr_prev[x])/2;
minv = MIN(minv, v1); maxv = MAX(maxv, v1);
v1 = (v + sptr_next[x])/2;
minv = MIN(minv, v1); maxv = MAX(maxv, v1);
if( x < cols-1 )
{
v1 = (v + sptr[x+1])/2;
minv = MIN(minv, v1); maxv = MAX(maxv, v1);
}
v_prev = v;
dptr[0] = (uchar)v;
dptr[1] = (uchar)minv;
dptr[2] = (uchar)maxv;
}
}
}
}
// Optimal K is computed as avg_x(k-th-smallest_d(||I(x)-J(x+d)||)),
// where k = number_of_disparities*0.25.
static float
icvComputeK( CvStereoGCState* state )
{
int x, y, x1, d, i, j, rows = state->left->rows, cols = state->left->cols, n = 0;
int mind = state->minDisparity, nd = state->numberOfDisparities, maxd = mind + nd;
int k = MIN(MAX((nd + 2)/4, 3), nd), delta, t, sum = 0;
std::vector<int> _arr(k+1);
int *arr = &_arr[0];
for( y = 0; y < rows; y++ )
{
const uchar* lptr = state->left->data.ptr + state->left->step*y;
const uchar* rptr = state->right->data.ptr + state->right->step*y;
for( x = 0; x < cols; x++ )
{
for( d = maxd-1, i = 0; d >= mind; d-- )
{
x1 = x - d;
if( (unsigned)x1 >= (unsigned)cols )
continue;
delta = icvDataCostFuncGraySubpix( lptr + x*3, rptr + x1*3 );
if( i < k )
arr[i++] = delta;
else
for( i = 0; i < k; i++ )
if( delta < arr[i] )
CV_SWAP( arr[i], delta, t );
}
delta = arr[0];
for( j = 1; j < i; j++ )
delta = MAX(delta, arr[j]);
sum += delta;
n++;
}
}
return (float)sum/n;
}
static int64 icvComputeEnergy( const CvStereoGCState* state, const CvStereoGCState2* state2,
bool allOccluded )
{
int x, y, rows = state->left->rows, cols = state->left->cols;
int64 E = 0;
const int* dtab = state2->dataCostFuncTab;
int maxR = state2->interactionRadius;
const int* stabR = state2->smoothnessR + CUTOFF;
const int* stabI = state2->smoothnessGrayDiff + 255;
const uchar* left = state->left->data.ptr;
const uchar* right = state->right->data.ptr;
short* dleft = state->dispLeft->data.s;
short* dright = state->dispRight->data.s;
int step = state->left->step;
int dstep = (int)(state->dispLeft->step/sizeof(short));
assert( state->left->step == state->right->step &&
state->dispLeft->step == state->dispRight->step );
if( allOccluded )
return (int64)OCCLUSION_PENALTY*rows*cols*2;
for( y = 0; y < rows; y++, left += step, right += step, dleft += dstep, dright += dstep )
{
for( x = 0; x < cols; x++ )
{
int d = dleft[x], x1, d1;
if( d == OCCLUDED )
E += OCCLUSION_PENALTY;
else
{
x1 = x + d;
if( (unsigned)x1 >= (unsigned)cols )
continue;
d1 = dright[x1];
if( d == -d1 )
E += dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )];
}
if( x < cols-1 )
{
d1 = dleft[x+1];
E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+3]] );
}
if( y < rows-1 )
{
d1 = dleft[x+dstep];
E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[left[x*3] - left[x*3+step]] );
}
d = dright[x];
if( d == OCCLUDED )
E += OCCLUSION_PENALTY;
if( x < cols-1 )
{
d1 = dright[x+1];
E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+3]] );
}
if( y < rows-1 )
{
d1 = dright[x+dstep];
E += icvSmoothnessCostFunc(d, d1, maxR, stabR, stabI[right[x*3] - right[x*3+step]] );
}
assert( E >= 0 );
}
}
return E;
}
static inline void icvAddEdge( GCVtx *x, GCVtx* y, GCEdge* edgeBuf, int nedges, int w, int rw )
{
GCEdge *xy = edgeBuf + nedges, *yx = xy + 1;
assert( x != 0 && y != 0 );
xy->dst = y;
xy->next = x->first;
xy->weight = (short)w;
x->first = nedges;
yx->dst = x;
yx->next = y->first;
yx->weight = (short)rw;
y->first = nedges+1;
}
static inline int icvAddTWeights( GCVtx* vtx, int sourceWeight, int sinkWeight )
{
int w = vtx->weight;
if( w > 0 )
sourceWeight += w;
else
sinkWeight -= w;
vtx->weight = (short)(sourceWeight - sinkWeight);
return MIN(sourceWeight, sinkWeight);
}
static inline int icvAddTerm( GCVtx* x, GCVtx* y, int A, int B, int C, int D,
GCEdge* edgeBuf, int& nedges )
{
int dE = 0, w;
assert(B - A + C - D >= 0);
if( B < A )
{
dE += icvAddTWeights(x, D, B);
dE += icvAddTWeights(y, 0, A - B);
if( (w = B - A + C - D) != 0 )
{
icvAddEdge( x, y, edgeBuf, nedges, 0, w );
nedges += 2;
}
}
else if( C < D )
{
dE += icvAddTWeights(x, D, A + D - C);
dE += icvAddTWeights(y, 0, C - D);
if( (w = B - A + C - D) != 0 )
{
icvAddEdge( x, y, edgeBuf, nedges, w, 0 );
nedges += 2;
}
}
else
{
dE += icvAddTWeights(x, D, A);
if( B != A || C != D )
{
icvAddEdge( x, y, edgeBuf, nedges, B - A, C - D );
nedges += 2;
}
}
return dE;
}
static int64 icvAlphaExpand( int64 Eprev, int alpha, CvStereoGCState* state, CvStereoGCState2* state2 )
{
GCVtx *var, *var1;
int64 E = 0;
int delta, E00=0, E0a=0, Ea0=0, Eaa=0;
int k, a, d, d1, x, y, x1, y1, rows = state->left->rows, cols = state->left->cols;
int nvtx = 0, nedges = 2;
GCVtx* vbuf = (GCVtx*)state->vtxBuf->data.ptr;
GCEdge* ebuf = (GCEdge*)state->edgeBuf->data.ptr;
int maxR = state2->interactionRadius;
const int* dtab = state2->dataCostFuncTab;
const int* stabR = state2->smoothnessR + CUTOFF;
const int* stabI = state2->smoothnessGrayDiff + 255;
const uchar* left0 = state->left->data.ptr;
const uchar* right0 = state->right->data.ptr;
short* dleft0 = state->dispLeft->data.s;
short* dright0 = state->dispRight->data.s;
GCVtx** pleft0 = (GCVtx**)state->ptrLeft->data.ptr;
GCVtx** pright0 = (GCVtx**)state->ptrRight->data.ptr;
int step = state->left->step;
int dstep = (int)(state->dispLeft->step/sizeof(short));
int pstep = (int)(state->ptrLeft->step/sizeof(GCVtx*));
int aa[] = { alpha, -alpha };
//double t = (double)cvGetTickCount();
assert( state->left->step == state->right->step &&
state->dispLeft->step == state->dispRight->step &&
state->ptrLeft->step == state->ptrRight->step );
for( k = 0; k < 2; k++ )
{
ebuf[k].dst = 0;
ebuf[k].next = 0;
ebuf[k].weight = 0;
}
for( y = 0; y < rows; y++ )
{
const uchar* left = left0 + step*y;
const uchar* right = right0 + step*y;
const short* dleft = dleft0 + dstep*y;
const short* dright = dright0 + dstep*y;
GCVtx** pleft = pleft0 + pstep*y;
GCVtx** pright = pright0 + pstep*y;
const uchar* lr[] = { left, right };
const short* dlr[] = { dleft, dright };
GCVtx** plr[] = { pleft, pright };
for( k = 0; k < 2; k++ )
{
a = aa[k];
for( y1 = y+(y>0); y1 <= y+(y<rows-1); y1++ )
{
const short* disp = (k == 0 ? dleft0 : dright0) + y1*dstep;
GCVtx** ptr = (k == 0 ? pleft0 : pright0) + y1*pstep;
for( x = 0; x < cols; x++ )
{
GCVtx* v = ptr[x] = &vbuf[nvtx++];
v->first = 0;
v->weight = disp[x] == (short)(OCCLUDED ? -OCCLUSION_PENALTY2 : 0);
}
}
}
for( x = 0; x < cols; x++ )
{
d = dleft[x];
x1 = x + d;
var = pleft[x];
// (left + x, right + x + d)
if( d != alpha && d != OCCLUDED && (unsigned)x1 < (unsigned)cols )
{
var1 = pright[x1];
d1 = dright[x1];
if( d == -d1 )
{
assert( var1 != 0 );
delta = IS_BLOCKED(alpha, d) ? INFINITY : 0;
//add inter edge
E += icvAddTerm( var, var1,
dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )],
delta, delta, 0, ebuf, nedges );
}
else if( IS_BLOCKED(alpha, d) )
E += icvAddTerm( var, var1, 0, INFINITY, 0, 0, ebuf, nedges );
}
// (left + x, right + x + alpha)
x1 = x + alpha;
if( (unsigned)x1 < (unsigned)cols )
{
var1 = pright[x1];
d1 = dright[x1];
E0a = IS_BLOCKED(d, alpha) ? INFINITY : 0;
Ea0 = IS_BLOCKED(-d1, alpha) ? INFINITY : 0;
Eaa = dtab[icvDataCostFuncGraySubpix( left + x*3, right + x1*3 )];
E += icvAddTerm( var, var1, 0, E0a, Ea0, Eaa, ebuf, nedges );
}
// smoothness
for( k = 0; k < 2; k++ )
{
GCVtx** p = plr[k];
const short* disp = dlr[k];
const uchar* img = lr[k] + x*3;
int scale;
var = p[x];
d = disp[x];
a = aa[k];
if( x < cols - 1 )
{
var1 = p[x+1];
d1 = disp[x+1];
scale = stabI[img[0] - img[3]];
E0a = icvSmoothnessCostFunc( d, a, maxR, stabR, scale );
Ea0 = icvSmoothnessCostFunc( a, d1, maxR, stabR, scale );
E00 = icvSmoothnessCostFunc( d, d1, maxR, stabR, scale );
E += icvAddTerm( var, var1, E00, E0a, Ea0, 0, ebuf, nedges );
}
if( y < rows - 1 )
{
var1 = p[x+pstep];
d1 = disp[x+dstep];
scale = stabI[img[0] - img[step]];
E0a = icvSmoothnessCostFunc( d, a, maxR, stabR, scale );
Ea0 = icvSmoothnessCostFunc( a, d1, maxR, stabR, scale );
E00 = icvSmoothnessCostFunc( d, d1, maxR, stabR, scale );
E += icvAddTerm( var, var1, E00, E0a, Ea0, 0, ebuf, nedges );
}
}
// visibility term
if( d != OCCLUDED && IS_BLOCKED(alpha, -d))
{
x1 = x + d;
if( (unsigned)x1 < (unsigned)cols )
{
if( d != -dleft[x1] )
{
var1 = pleft[x1];
E += icvAddTerm( var, var1, 0, INFINITY, 0, 0, ebuf, nedges );
}
}
}
}
}
//t = (double)cvGetTickCount() - t;
ebuf[0].weight = ebuf[1].weight = 0;
E += icvGCMaxFlow( vbuf, nvtx, ebuf, state2->orphans, state2->maxOrphans );
if( E < Eprev )
{
for( y = 0; y < rows; y++ )
{
short* dleft = dleft0 + dstep*y;
short* dright = dright0 + dstep*y;
GCVtx** pleft = pleft0 + pstep*y;
GCVtx** pright = pright0 + pstep*y;
for( x = 0; x < cols; x++ )
{
GCVtx* var = pleft[x];
if( var && var->parent && var->t )
dleft[x] = (short)alpha;
var = pright[x];
if( var && var->parent && var->t )
dright[x] = (short)-alpha;
}
}
}
return MIN(E, Eprev);
}
CV_IMPL void cvFindStereoCorrespondenceGC( const CvArr* _left, const CvArr* _right,
CvArr* _dispLeft, CvArr* _dispRight, CvStereoGCState* state, int useDisparityGuess )
{
CvStereoGCState2 state2;
state2.orphans = 0;
state2.maxOrphans = 0;
CvMat lstub, *left = cvGetMat( _left, &lstub );
CvMat rstub, *right = cvGetMat( _right, &rstub );
CvMat dlstub, *dispLeft = cvGetMat( _dispLeft, &dlstub );
CvMat drstub, *dispRight = cvGetMat( _dispRight, &drstub );
CvSize size;
int iter, i, nZeroExpansions = 0;
CvRNG rng = cvRNG(-1);
int64 E;
CV_Assert( state != 0 );
CV_Assert( CV_ARE_SIZES_EQ(left, right) && CV_ARE_TYPES_EQ(left, right) &&
CV_MAT_TYPE(left->type) == CV_8UC1 );
CV_Assert( !dispLeft ||
(CV_ARE_SIZES_EQ(dispLeft, left) && CV_MAT_CN(dispLeft->type) == 1) );
CV_Assert( !dispRight ||
(CV_ARE_SIZES_EQ(dispRight, left) && CV_MAT_CN(dispRight->type) == 1) );
size = cvGetSize(left);
if( !state->left || state->left->width != size.width || state->left->height != size.height )
{
int pcn = (int)(sizeof(GCVtx*)/sizeof(int));
int vcn = (int)(sizeof(GCVtx)/sizeof(int));
int ecn = (int)(sizeof(GCEdge)/sizeof(int));
cvReleaseMat( &state->left );
cvReleaseMat( &state->right );
cvReleaseMat( &state->ptrLeft );
cvReleaseMat( &state->ptrRight );
cvReleaseMat( &state->dispLeft );
cvReleaseMat( &state->dispRight );
state->left = cvCreateMat( size.height, size.width, CV_8UC3 );
state->right = cvCreateMat( size.height, size.width, CV_8UC3 );
state->dispLeft = cvCreateMat( size.height, size.width, CV_16SC1 );
state->dispRight = cvCreateMat( size.height, size.width, CV_16SC1 );
state->ptrLeft = cvCreateMat( size.height, size.width, CV_32SC(pcn) );
state->ptrRight = cvCreateMat( size.height, size.width, CV_32SC(pcn) );
state->vtxBuf = cvCreateMat( 1, size.height*size.width*2, CV_32SC(vcn) );
state->edgeBuf = cvCreateMat( 1, size.height*size.width*12 + 16, CV_32SC(ecn) );
}
if( !useDisparityGuess )
{
cvSet( state->dispLeft, cvScalarAll(OCCLUDED));
cvSet( state->dispRight, cvScalarAll(OCCLUDED));
}
else
{
CV_Assert( dispLeft && dispRight );
cvConvert( dispLeft, state->dispLeft );
cvConvert( dispRight, state->dispRight );
}
state2.Ithreshold = state->Ithreshold;
state2.interactionRadius = state->interactionRadius;
state2.lambda = cvRound(state->lambda*DENOMINATOR);
state2.lambda1 = cvRound(state->lambda1*DENOMINATOR);
state2.lambda2 = cvRound(state->lambda2*DENOMINATOR);
state2.K = cvRound(state->K*DENOMINATOR);
icvInitStereoConstTabs();
icvInitGraySubpix( left, right, state->left, state->right );
std::vector<int> disp(state->numberOfDisparities);
CvMat _disp = cvMat( 1, (int)disp.size(), CV_32S, &disp[0] );
cvRange( &_disp, state->minDisparity, state->minDisparity + state->numberOfDisparities );
cvRandShuffle( &_disp, &rng );
if( state2.lambda < 0 && (state2.K < 0 || state2.lambda1 < 0 || state2.lambda2 < 0) )
{
float L = icvComputeK(state)*0.2f;
state2.lambda = cvRound(L*DENOMINATOR);
}
if( state2.K < 0 )
state2.K = state2.lambda*5;
if( state2.lambda1 < 0 )
state2.lambda1 = state2.lambda*3;
if( state2.lambda2 < 0 )
state2.lambda2 = state2.lambda;
icvInitStereoTabs( &state2 );
E = icvComputeEnergy( state, &state2, !useDisparityGuess );
for( iter = 0; iter < state->maxIters; iter++ )
{
for( i = 0; i < state->numberOfDisparities; i++ )
{
int alpha = disp[i];
int64 Enew = icvAlphaExpand( E, -alpha, state, &state2 );
if( Enew < E )
{
nZeroExpansions = 0;
E = Enew;
}
else if( ++nZeroExpansions >= state->numberOfDisparities )
break;
}
}
if( dispLeft )
cvConvert( state->dispLeft, dispLeft );
if( dispRight )
cvConvert( state->dispRight, dispRight );
cvFree( &state2.orphans );
}

703
modules/legacy/src/subdiv2.cpp
View File

@@ -41,6 +41,709 @@
#include "precomp.hpp"
CV_IMPL CvSubdiv2D *
cvCreateSubdiv2D( int subdiv_type, int header_size,
int vtx_size, int quadedge_size, CvMemStorage * storage )
{
if( !storage )
CV_Error( CV_StsNullPtr, "" );
if( header_size < (int)sizeof( CvSubdiv2D ) ||
quadedge_size < (int)sizeof( CvQuadEdge2D ) ||
vtx_size < (int)sizeof( CvSubdiv2DPoint ))
CV_Error( CV_StsBadSize, "" );
return (CvSubdiv2D *)cvCreateGraph( subdiv_type, header_size,
vtx_size, quadedge_size, storage );
}
/****************************************************************************************\
* Quad Edge algebra *
\****************************************************************************************/
static CvSubdiv2DEdge
cvSubdiv2DMakeEdge( CvSubdiv2D * subdiv )
{
if( !subdiv )
CV_Error( CV_StsNullPtr, "" );
CvQuadEdge2D* edge = (CvQuadEdge2D*)cvSetNew( (CvSet*)subdiv->edges );
memset( edge->pt, 0, sizeof( edge->pt ));
CvSubdiv2DEdge edgehandle = (CvSubdiv2DEdge) edge;
edge->next[0] = edgehandle;
edge->next[1] = edgehandle + 3;
edge->next[2] = edgehandle + 2;
edge->next[3] = edgehandle + 1;
subdiv->quad_edges++;
return edgehandle;
}
static CvSubdiv2DPoint *
cvSubdiv2DAddPoint( CvSubdiv2D * subdiv, CvPoint2D32f pt, int is_virtual )
{
CvSubdiv2DPoint* subdiv_point = (CvSubdiv2DPoint*)cvSetNew( (CvSet*)subdiv );
if( subdiv_point )
{
memset( subdiv_point, 0, subdiv->elem_size );
subdiv_point->pt = pt;
subdiv_point->first = 0;
subdiv_point->flags |= is_virtual ? CV_SUBDIV2D_VIRTUAL_POINT_FLAG : 0;
subdiv_point->id = -1;
}
return subdiv_point;
}
static void
cvSubdiv2DSplice( CvSubdiv2DEdge edgeA, CvSubdiv2DEdge edgeB )
{
CvSubdiv2DEdge *a_next = &CV_SUBDIV2D_NEXT_EDGE( edgeA );
CvSubdiv2DEdge *b_next = &CV_SUBDIV2D_NEXT_EDGE( edgeB );
CvSubdiv2DEdge a_rot = cvSubdiv2DRotateEdge( *a_next, 1 );
CvSubdiv2DEdge b_rot = cvSubdiv2DRotateEdge( *b_next, 1 );
CvSubdiv2DEdge *a_rot_next = &CV_SUBDIV2D_NEXT_EDGE( a_rot );
CvSubdiv2DEdge *b_rot_next = &CV_SUBDIV2D_NEXT_EDGE( b_rot );
CvSubdiv2DEdge t;
CV_SWAP( *a_next, *b_next, t );
CV_SWAP( *a_rot_next, *b_rot_next, t );
}
static void
cvSubdiv2DSetEdgePoints( CvSubdiv2DEdge edge,
CvSubdiv2DPoint * org_pt, CvSubdiv2DPoint * dst_pt )
{
CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (edge & ~3);
if( !quadedge )
CV_Error( CV_StsNullPtr, "" );
quadedge->pt[edge & 3] = org_pt;
quadedge->pt[(edge + 2) & 3] = dst_pt;
}
static void
cvSubdiv2DDeleteEdge( CvSubdiv2D * subdiv, CvSubdiv2DEdge edge )
{
CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (edge & ~3);
if( !subdiv || !quadedge )
CV_Error( CV_StsNullPtr, "" );
cvSubdiv2DSplice( edge, cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_ORG ));
CvSubdiv2DEdge sym_edge = cvSubdiv2DSymEdge( edge );
cvSubdiv2DSplice( sym_edge, cvSubdiv2DGetEdge( sym_edge, CV_PREV_AROUND_ORG ));
cvSetRemoveByPtr( (CvSet*)(subdiv->edges), quadedge );
subdiv->quad_edges--;
}
static CvSubdiv2DEdge
cvSubdiv2DConnectEdges( CvSubdiv2D * subdiv, CvSubdiv2DEdge edgeA, CvSubdiv2DEdge edgeB )
{
if( !subdiv )
CV_Error( CV_StsNullPtr, "" );
CvSubdiv2DEdge new_edge = cvSubdiv2DMakeEdge( subdiv );
cvSubdiv2DSplice( new_edge, cvSubdiv2DGetEdge( edgeA, CV_NEXT_AROUND_LEFT ));
cvSubdiv2DSplice( cvSubdiv2DSymEdge( new_edge ), edgeB );
CvSubdiv2DPoint* dstA = cvSubdiv2DEdgeDst( edgeA );
CvSubdiv2DPoint* orgB = cvSubdiv2DEdgeOrg( edgeB );
cvSubdiv2DSetEdgePoints( new_edge, dstA, orgB );
return new_edge;
}
static void
cvSubdiv2DSwapEdges( CvSubdiv2DEdge edge )
{
CvSubdiv2DEdge sym_edge = cvSubdiv2DSymEdge( edge );
CvSubdiv2DEdge a = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_ORG );
CvSubdiv2DEdge b = cvSubdiv2DGetEdge( sym_edge, CV_PREV_AROUND_ORG );
CvSubdiv2DPoint *dstB, *dstA;
cvSubdiv2DSplice( edge, a );
cvSubdiv2DSplice( sym_edge, b );
dstA = cvSubdiv2DEdgeDst( a );
dstB = cvSubdiv2DEdgeDst( b );
cvSubdiv2DSetEdgePoints( edge, dstA, dstB );
cvSubdiv2DSplice( edge, cvSubdiv2DGetEdge( a, CV_NEXT_AROUND_LEFT ));
cvSubdiv2DSplice( sym_edge, cvSubdiv2DGetEdge( b, CV_NEXT_AROUND_LEFT ));
}
static int
icvIsRightOf( CvPoint2D32f& pt, CvSubdiv2DEdge edge )
{
CvSubdiv2DPoint *org = cvSubdiv2DEdgeOrg(edge), *dst = cvSubdiv2DEdgeDst(edge);
double cw_area = cvTriangleArea( pt, dst->pt, org->pt );
return (cw_area > 0) - (cw_area < 0);
}
CV_IMPL CvSubdiv2DPointLocation
cvSubdiv2DLocate( CvSubdiv2D * subdiv, CvPoint2D32f pt,
CvSubdiv2DEdge * _edge, CvSubdiv2DPoint ** _point )
{
CvSubdiv2DPoint *point = 0;
int right_of_curr = 0;
if( !subdiv )
CV_Error( CV_StsNullPtr, "" );
if( !CV_IS_SUBDIV2D(subdiv) )
CV_Error( CV_StsBadFlag, "" );
int i, max_edges = subdiv->quad_edges * 4;
CvSubdiv2DEdge edge = subdiv->recent_edge;
if( max_edges == 0 )
CV_Error( CV_StsBadSize, "" );
CV_Assert(edge != 0);
if( pt.x < subdiv->topleft.x || pt.y < subdiv->topleft.y ||
pt.x >= subdiv->bottomright.x || pt.y >= subdiv->bottomright.y )
CV_Error( CV_StsOutOfRange, "" );
CvSubdiv2DPointLocation location = CV_PTLOC_ERROR;
right_of_curr = icvIsRightOf( pt, edge );
if( right_of_curr > 0 )
{
edge = cvSubdiv2DSymEdge( edge );
right_of_curr = -right_of_curr;
}
for( i = 0; i < max_edges; i++ )
{
CvSubdiv2DEdge onext_edge = cvSubdiv2DNextEdge( edge );
CvSubdiv2DEdge dprev_edge = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_DST );
int right_of_onext = icvIsRightOf( pt, onext_edge );
int right_of_dprev = icvIsRightOf( pt, dprev_edge );
if( right_of_dprev > 0 )
{
if( right_of_onext > 0 || (right_of_onext == 0 && right_of_curr == 0) )
{
location = CV_PTLOC_INSIDE;
goto exit;
}
else
{
right_of_curr = right_of_onext;
edge = onext_edge;
}
}
else
{
if( right_of_onext > 0 )
{
if( right_of_dprev == 0 && right_of_curr == 0 )
{
location = CV_PTLOC_INSIDE;
goto exit;
}
else
{
right_of_curr = right_of_dprev;
edge = dprev_edge;
}
}
else if( right_of_curr == 0 &&
icvIsRightOf( cvSubdiv2DEdgeDst( onext_edge )->pt, edge ) >= 0 )
{
edge = cvSubdiv2DSymEdge( edge );
}
else
{
right_of_curr = right_of_onext;
edge = onext_edge;
}
}
}
exit:
subdiv->recent_edge = edge;
if( location == CV_PTLOC_INSIDE )
{
double t1, t2, t3;
CvPoint2D32f org_pt = cvSubdiv2DEdgeOrg( edge )->pt;
CvPoint2D32f dst_pt = cvSubdiv2DEdgeDst( edge )->pt;
t1 = fabs( pt.x - org_pt.x );
t1 += fabs( pt.y - org_pt.y );
t2 = fabs( pt.x - dst_pt.x );
t2 += fabs( pt.y - dst_pt.y );
t3 = fabs( org_pt.x - dst_pt.x );
t3 += fabs( org_pt.y - dst_pt.y );
if( t1 < FLT_EPSILON )
{
location = CV_PTLOC_VERTEX;
point = cvSubdiv2DEdgeOrg( edge );
edge = 0;
}
else if( t2 < FLT_EPSILON )
{
location = CV_PTLOC_VERTEX;
point = cvSubdiv2DEdgeDst( edge );
edge = 0;
}
else if( (t1 < t3 || t2 < t3) &&
fabs( cvTriangleArea( pt, org_pt, dst_pt )) < FLT_EPSILON )
{
location = CV_PTLOC_ON_EDGE;
point = 0;
}
}
if( location == CV_PTLOC_ERROR )
{
edge = 0;
point = 0;
}
if( _edge )
*_edge = edge;
if( _point )
*_point = point;
return location;
}
CV_INLINE int
icvIsPtInCircle3( CvPoint2D32f pt, CvPoint2D32f a, CvPoint2D32f b, CvPoint2D32f c )
{
const double eps = FLT_EPSILON*0.125;
double val = ((double)a.x * a.x + (double)a.y * a.y) * cvTriangleArea( b, c, pt );
val -= ((double)b.x * b.x + (double)b.y * b.y) * cvTriangleArea( a, c, pt );
val += ((double)c.x * c.x + (double)c.y * c.y) * cvTriangleArea( a, b, pt );
val -= ((double)pt.x * pt.x + (double)pt.y * pt.y) * cvTriangleArea( a, b, c );
return val > eps ? 1 : val < -eps ? -1 : 0;
}
CV_IMPL CvSubdiv2DPoint *
cvSubdivDelaunay2DInsert( CvSubdiv2D * subdiv, CvPoint2D32f pt )
{
CvSubdiv2DPointLocation location = CV_PTLOC_ERROR;
CvSubdiv2DPoint *curr_point = 0, *first_point = 0;
CvSubdiv2DEdge curr_edge = 0, deleted_edge = 0, base_edge = 0;
int i, max_edges;
if( !subdiv )
CV_Error( CV_StsNullPtr, "" );
if( !CV_IS_SUBDIV2D(subdiv) )
CV_Error( CV_StsBadFlag, "" );
location = cvSubdiv2DLocate( subdiv, pt, &curr_edge, &curr_point );
switch (location)
{
case CV_PTLOC_ERROR:
CV_Error( CV_StsBadSize, "" );
case CV_PTLOC_OUTSIDE_RECT:
CV_Error( CV_StsOutOfRange, "" );
case CV_PTLOC_VERTEX:
break;
case CV_PTLOC_ON_EDGE:
deleted_edge = curr_edge;
subdiv->recent_edge = curr_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG );
cvSubdiv2DDeleteEdge( subdiv, deleted_edge );
/* no break */
case CV_PTLOC_INSIDE:
assert( curr_edge != 0 );
subdiv->is_geometry_valid = 0;
curr_point = cvSubdiv2DAddPoint( subdiv, pt, 0 );
base_edge = cvSubdiv2DMakeEdge( subdiv );
first_point = cvSubdiv2DEdgeOrg( curr_edge );
cvSubdiv2DSetEdgePoints( base_edge, first_point, curr_point );
cvSubdiv2DSplice( base_edge, curr_edge );
do
{
base_edge = cvSubdiv2DConnectEdges( subdiv, curr_edge,
cvSubdiv2DSymEdge( base_edge ));
curr_edge = cvSubdiv2DGetEdge( base_edge, CV_PREV_AROUND_ORG );
}
while( cvSubdiv2DEdgeDst( curr_edge ) != first_point );
curr_edge = cvSubdiv2DGetEdge( base_edge, CV_PREV_AROUND_ORG );
max_edges = subdiv->quad_edges * 4;
for( i = 0; i < max_edges; i++ )
{
CvSubdiv2DPoint *temp_dst = 0, *curr_org = 0, *curr_dst = 0;
CvSubdiv2DEdge temp_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG );
temp_dst = cvSubdiv2DEdgeDst( temp_edge );
curr_org = cvSubdiv2DEdgeOrg( curr_edge );
curr_dst = cvSubdiv2DEdgeDst( curr_edge );
if( icvIsRightOf( temp_dst->pt, curr_edge ) > 0 &&
icvIsPtInCircle3( curr_org->pt, temp_dst->pt,
curr_dst->pt, curr_point->pt ) < 0 )
{
cvSubdiv2DSwapEdges( curr_edge );
curr_edge = cvSubdiv2DGetEdge( curr_edge, CV_PREV_AROUND_ORG );
}
else if( curr_org == first_point )
{
break;
}
else
{
curr_edge = cvSubdiv2DGetEdge( cvSubdiv2DNextEdge( curr_edge ),
CV_PREV_AROUND_LEFT );
}
}
break;
default:
CV_Error_(CV_StsError, ("cvSubdiv2DLocate returned invalid location = %d", location) );
}
return curr_point;
}
CV_IMPL void
cvInitSubdivDelaunay2D( CvSubdiv2D * subdiv, CvRect rect )
{
float big_coord = 3.f * MAX( rect.width, rect.height );
CvPoint2D32f ppA, ppB, ppC;
CvSubdiv2DPoint *pA, *pB, *pC;
CvSubdiv2DEdge edge_AB, edge_BC, edge_CA;
float rx = (float) rect.x;
float ry = (float) rect.y;
if( !subdiv )
CV_Error( CV_StsNullPtr, "" );
cvClearSet( (CvSet *) (subdiv->edges) );
cvClearSet( (CvSet *) subdiv );
subdiv->quad_edges = 0;
subdiv->recent_edge = 0;
subdiv->is_geometry_valid = 0;
subdiv->topleft = cvPoint2D32f( rx, ry );
subdiv->bottomright = cvPoint2D32f( rx + rect.width, ry + rect.height );
ppA = cvPoint2D32f( rx + big_coord, ry );
ppB = cvPoint2D32f( rx, ry + big_coord );
ppC = cvPoint2D32f( rx - big_coord, ry - big_coord );
pA = cvSubdiv2DAddPoint( subdiv, ppA, 0 );
pB = cvSubdiv2DAddPoint( subdiv, ppB, 0 );
pC = cvSubdiv2DAddPoint( subdiv, ppC, 0 );
edge_AB = cvSubdiv2DMakeEdge( subdiv );
edge_BC = cvSubdiv2DMakeEdge( subdiv );
edge_CA = cvSubdiv2DMakeEdge( subdiv );
cvSubdiv2DSetEdgePoints( edge_AB, pA, pB );
cvSubdiv2DSetEdgePoints( edge_BC, pB, pC );
cvSubdiv2DSetEdgePoints( edge_CA, pC, pA );
cvSubdiv2DSplice( edge_AB, cvSubdiv2DSymEdge( edge_CA ));
cvSubdiv2DSplice( edge_BC, cvSubdiv2DSymEdge( edge_AB ));
cvSubdiv2DSplice( edge_CA, cvSubdiv2DSymEdge( edge_BC ));
subdiv->recent_edge = edge_AB;
}
CV_IMPL void
cvClearSubdivVoronoi2D( CvSubdiv2D * subdiv )
{
int elem_size;
int i, total;
CvSeqReader reader;
if( !subdiv )
CV_Error( CV_StsNullPtr, "" );
/* clear pointers to voronoi points */
total = subdiv->edges->total;
elem_size = subdiv->edges->elem_size;
cvStartReadSeq( (CvSeq *) (subdiv->edges), &reader, 0 );
for( i = 0; i < total; i++ )
{
CvQuadEdge2D *quadedge = (CvQuadEdge2D *) reader.ptr;
quadedge->pt[1] = quadedge->pt[3] = 0;
CV_NEXT_SEQ_ELEM( elem_size, reader );
}
/* remove voronoi points */
total = subdiv->total;
elem_size = subdiv->elem_size;
cvStartReadSeq( (CvSeq *) subdiv, &reader, 0 );
for( i = 0; i < total; i++ )
{
CvSubdiv2DPoint *pt = (CvSubdiv2DPoint *) reader.ptr;
/* check for virtual point. it is also check that the point exists */
if( pt->flags & CV_SUBDIV2D_VIRTUAL_POINT_FLAG )
{
cvSetRemoveByPtr( (CvSet*)subdiv, pt );
}
CV_NEXT_SEQ_ELEM( elem_size, reader );
}
subdiv->is_geometry_valid = 0;
}
static void
icvCreateCenterNormalLine( CvSubdiv2DEdge edge, double *_a, double *_b, double *_c )
{
CvPoint2D32f org = cvSubdiv2DEdgeOrg( edge )->pt;
CvPoint2D32f dst = cvSubdiv2DEdgeDst( edge )->pt;
double a = dst.x - org.x;
double b = dst.y - org.y;
double c = -(a * (dst.x + org.x) + b * (dst.y + org.y));
*_a = a + a;
*_b = b + b;
*_c = c;
}
static void
icvIntersectLines3( double *a0, double *b0, double *c0,
double *a1, double *b1, double *c1, CvPoint2D32f * point )
{
double det = a0[0] * b1[0] - a1[0] * b0[0];
if( det != 0 )
{
det = 1. / det;
point->x = (float) ((b0[0] * c1[0] - b1[0] * c0[0]) * det);
point->y = (float) ((a1[0] * c0[0] - a0[0] * c1[0]) * det);
}
else
{
point->x = point->y = FLT_MAX;
}
}
CV_IMPL void
cvCalcSubdivVoronoi2D( CvSubdiv2D * subdiv )
{
CvSeqReader reader;
int i, total, elem_size;
if( !subdiv )
CV_Error( CV_StsNullPtr, "" );
/* check if it is already calculated */
if( subdiv->is_geometry_valid )
return;
total = subdiv->edges->total;
elem_size = subdiv->edges->elem_size;
cvClearSubdivVoronoi2D( subdiv );
cvStartReadSeq( (CvSeq *) (subdiv->edges), &reader, 0 );
if( total <= 3 )
return;
/* skip first three edges (bounding triangle) */
for( i = 0; i < 3; i++ )
CV_NEXT_SEQ_ELEM( elem_size, reader );
/* loop through all quad-edges */
for( ; i < total; i++ )
{
CvQuadEdge2D *quadedge = (CvQuadEdge2D *) (reader.ptr);
if( CV_IS_SET_ELEM( quadedge ))
{
CvSubdiv2DEdge edge0 = (CvSubdiv2DEdge) quadedge, edge1, edge2;
double a0, b0, c0, a1, b1, c1;
CvPoint2D32f virt_point;
CvSubdiv2DPoint *voronoi_point;
if( !quadedge->pt[3] )
{
edge1 = cvSubdiv2DGetEdge( edge0, CV_NEXT_AROUND_LEFT );
edge2 = cvSubdiv2DGetEdge( edge1, CV_NEXT_AROUND_LEFT );
icvCreateCenterNormalLine( edge0, &a0, &b0, &c0 );
icvCreateCenterNormalLine( edge1, &a1, &b1, &c1 );
icvIntersectLines3( &a0, &b0, &c0, &a1, &b1, &c1, &virt_point );
if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
fabs( virt_point.y ) < FLT_MAX * 0.5 )
{
voronoi_point = cvSubdiv2DAddPoint( subdiv, virt_point, 1 );
quadedge->pt[3] =
((CvQuadEdge2D *) (edge1 & ~3))->pt[3 - (edge1 & 2)] =
((CvQuadEdge2D *) (edge2 & ~3))->pt[3 - (edge2 & 2)] = voronoi_point;
}
}
if( !quadedge->pt[1] )
{
edge1 = cvSubdiv2DGetEdge( edge0, CV_NEXT_AROUND_RIGHT );
edge2 = cvSubdiv2DGetEdge( edge1, CV_NEXT_AROUND_RIGHT );
icvCreateCenterNormalLine( edge0, &a0, &b0, &c0 );
icvCreateCenterNormalLine( edge1, &a1, &b1, &c1 );
icvIntersectLines3( &a0, &b0, &c0, &a1, &b1, &c1, &virt_point );
if( fabs( virt_point.x ) < FLT_MAX * 0.5 &&
fabs( virt_point.y ) < FLT_MAX * 0.5 )
{
voronoi_point = cvSubdiv2DAddPoint( subdiv, virt_point, 1 );
quadedge->pt[1] =
((CvQuadEdge2D *) (edge1 & ~3))->pt[1 + (edge1 & 2)] =
((CvQuadEdge2D *) (edge2 & ~3))->pt[1 + (edge2 & 2)] = voronoi_point;
}
}
}
CV_NEXT_SEQ_ELEM( elem_size, reader );
}
subdiv->is_geometry_valid = 1;
}
static int
icvIsRightOf2( const CvPoint2D32f& pt, const CvPoint2D32f& org, const CvPoint2D32f& diff )
{
double cw_area = ((double)org.x - pt.x)*diff.y - ((double)org.y - pt.y)*diff.x;
return (cw_area > 0) - (cw_area < 0);
}
CV_IMPL CvSubdiv2DPoint*
cvFindNearestPoint2D( CvSubdiv2D* subdiv, CvPoint2D32f pt )
{
CvSubdiv2DPoint* point = 0;
CvPoint2D32f start;
CvPoint2D32f diff;
CvSubdiv2DPointLocation loc;
CvSubdiv2DEdge edge;
int i;
if( !subdiv )
CV_Error( CV_StsNullPtr, "" );
if( !CV_IS_SUBDIV2D( subdiv ))
CV_Error( CV_StsNullPtr, "" );
if( subdiv->edges->active_count <= 3 )
return 0;
if( !subdiv->is_geometry_valid )
cvCalcSubdivVoronoi2D( subdiv );
loc = cvSubdiv2DLocate( subdiv, pt, &edge, &point );
switch( loc )
{
case CV_PTLOC_ON_EDGE:
case CV_PTLOC_INSIDE:
break;
default:
return point;
}
point = 0;
start = cvSubdiv2DEdgeOrg( edge )->pt;
diff.x = pt.x - start.x;
diff.y = pt.y - start.y;
edge = cvSubdiv2DRotateEdge( edge, 1 );
for( i = 0; i < subdiv->total; i++ )
{
CvPoint2D32f t;
for(;;)
{
assert( cvSubdiv2DEdgeDst( edge ));
t = cvSubdiv2DEdgeDst( edge )->pt;
if( icvIsRightOf2( t, start, diff ) >= 0 )
break;
edge = cvSubdiv2DGetEdge( edge, CV_NEXT_AROUND_LEFT );
}
for(;;)
{
assert( cvSubdiv2DEdgeOrg( edge ));
t = cvSubdiv2DEdgeOrg( edge )->pt;
if( icvIsRightOf2( t, start, diff ) < 0 )
break;
edge = cvSubdiv2DGetEdge( edge, CV_PREV_AROUND_LEFT );
}
{
CvPoint2D32f tempDiff = cvSubdiv2DEdgeDst( edge )->pt;
t = cvSubdiv2DEdgeOrg( edge )->pt;
tempDiff.x -= t.x;
tempDiff.y -= t.y;
if( icvIsRightOf2( pt, t, tempDiff ) >= 0 )
{
point = cvSubdiv2DEdgeOrg( cvSubdiv2DRotateEdge( edge, 3 ));
break;
}
}
edge = cvSubdiv2DSymEdge( edge );
}
return point;
}
CV_IMPL int
icvSubdiv2DCheck( CvSubdiv2D* subdiv )
{