hmetric.cc

来自「VC视频对象的跟踪提取原代码（vc视频监控源码）」· CC 代码 · 共 327 行

/***************************************************************
 * C++ source
 *
 * File : 	HMetric.cc
 *
 * Module :	
 *
 * Author : 	A M Baumberg (CoMIR)
 *
 * Creation Date : Tue Feb 21 11:56:43 1995 
 *
 * Comments : 	
 *
 ***************************************************************/


/* include files */

#include "NagMatrix.h"
#include "Profile.h"
#include "SplineWeights.h"
#include "EnvParameter.h"
#include "HMetric.h"

namespace ReadingPeopleTracker
{

// definition and initialisation of static member variables
EnvParameter HMetric::points_only("PROFILE_POINTS", false);

NagMatrix HMetric::H_metric;
NagMatrix HMetric::H_root;
NagMatrix HMetric::H_inv_root;
NagMatrix HMetric::Strain_Matrix;

const unsigned int HMetric::d = 3;

void HMetric::get_G(unsigned int n, NagMatrix &res, unsigned int N)
{
    if (res.get_data() == NULL)
	res.reconstruct(d+1, N);

    res.clear(0);

    for (unsigned int i = n; i <= n + d; i++)
	res.set(i-n, i % N, 1);
}

// 
// B spline matrix ...
//
//  1   4   1   0
// -3   0   3   0
//  3  -6   3   0
// -1   3  -3   1

realno HMetric::B3_data[16] = { 1.0/6.0, -3.0/6.0, 3.0/6.0, -1.0/6.0,
				4.0/6.0, 0, -6.0/6.0, 3.0/6.0, 
				1.0/6.0, 3.0/6.0, 3.0/6.0, -3.0/6.0,
				0,0,0,1.0/6.0 };

realno HMetric::dB3_data[16] = { -3.0/ 6.0, 6.0/6.0, -3.0/6.0, 0.0,
				 0.0, -12.0/6.0, 9.0/ 6.0, 0.0,
				 3.0/6.0, 6.0/6.0, -9.0/6.0, 0.0,
				 0.0, 0.0, 3.0/6.0, 0.0 };

realno HMetric::B2_data[9] = {1.0/2.0, -2.0/2.0, 1.0/2.0, 
			      1.0/2.0, 2.0/2.0, -2.0/2.0,
			      0.0, 0.0, 1.0/2.0};

realno HMetric::S3_data[16] = { 1.0, 1.0/2.0, 1.0/3.0, 1.0/4.0,
				1.0/2.0, 1.0/3.0, 1.0/4.0, 1.0/5.0,
				1.0/3.0, 1.0/4.0, 1.0/5.0, 1.0/6.0,
				1.0/4.0, 1.0/5.0, 1.0/6.0, 1.0/7.0 };


realno HMetric::S2_data[9] = { 1.0, 1.0/2.0, 1.0/3.0,
			       1.0/2.0, 1.0/3.0, 1.0/4.0,
			       1.0/3.0, 1.0/4.0, 1.0/5.0 };



void HMetric::setup_H_metric(unsigned int N2)
{
    if (H_metric.get_data() != NULL) return;
    
    if (points_only.is_on())
    {
	NagMatrix a(N2, N2, &NagMatrix::identity_fn);
	H_inv_root = a;
	NagMatrix b(N2, N2, &NagMatrix::identity_fn);
	H_metric =  b;
	NagMatrix c(N2, N2, &NagMatrix::identity_fn);
	H_root =  c; 
/*    H_inv_root =  NagMatrix(N2, N2, &NagMatrix::identity_fn);
      H_metric =  NagMatrix(N2, N2, &NagMatrix::identity_fn);
      H_root =  NagMatrix(N2, N2, &NagMatrix::identity_fn);  */
	cerror << " HMetric::setup_H_metric: Warning: using points, not splines " << endl;
	return;
    }
    setup_H(N2, H_metric);
    H_metric.msqrt(H_root);
    H_root.invert(H_inv_root);
}

void HMetric::setup_H_metric(NagVector &mean_prf)
{
    get_H(mean_prf, H_metric);
    H_metric.msqrt(H_root);
    H_root.invert(H_inv_root);
}

void HMetric::setup_H(unsigned int N2, NagMatrix &res)
{
    
    NagMatrix B3(4,4, B3_data);
    NagMatrix B2(3,3,B2_data);
    NagMatrix S2(3,3,S2_data);
    NagMatrix S3(4,4, S3_data);
    
    int N = N2 / 2;
    
    res.reconstruct(N2,N2);
    res.clear(0.0);
    Strain_Matrix.reconstruct(N2, N2);
    Strain_Matrix.clear(0.0);
    
    NagMatrix B_t;
    B3.transpose(B_t);
    NagMatrix S_B;
    S3.multiply(B3, S_B);
    NagMatrix BSB;
    B_t.multiply(S_B, BSB);
    
    NagMatrix G_n;
    NagMatrix G_n_t;
    NagMatrix tmp1;
    NagMatrix tmp2;
    NagMatrix tmp3(N,N);
    tmp3.clear(0);
    NagMatrix dtmp3(N,N);
    dtmp3.clear(0);
    
    unsigned int i,j;

    for (i = 0; i < N; i++)
    {
	get_G(i, G_n, N);
	G_n.transpose(G_n_t);
	G_n_t.multiply(BSB, tmp1);
	tmp1.multiply(G_n, tmp2);
	tmp3.add(tmp2, tmp3);  
    }
    
    for (i = 0; i < N; i++)
	for (j = 0; j < N; j++)
	{
	    res.set(2*i,2*j, tmp3.read(i,j));
	    res.set(2*i+1, 2*j+1, tmp3.read(i,j));
	}
    
    B2.reset(); B3.reset();
    S2.reset(); S3.reset();
}



// get the inverse covariance measurement matrix for this profile
// assuming measurement along the normals
// using an arbitrarily large no. of measurements

NagMatrix HMetric::G;
NagMatrix HMetric::G_t;
NagMatrix HMetric::dG;

void HMetric::get_H(NagVector &prf, NagMatrix &H)
{
    int N = Profile::NO_CONTROL_POINTS;
    if (H.get_data() == NULL)
	H.reconstruct(2 * N, 2 * N);
    int no_subdivisions = 8;
    int no_sample_points = N * no_subdivisions;
    if (G.get_data() == NULL)
    {
	
	SplineWeights sweights(no_subdivisions);
	
	G.reconstruct(2 * no_sample_points, 2 * N);
	dG.reconstruct(2 * no_sample_points, 2 * N);
	G.clear(); dG.clear();
	for (int i = 0; i < no_sample_points; i++)
	{
	    int k = (i / no_subdivisions);
	    G.set(2 * i, 2 * ((k-1+N) % N),
		  sweights.w0[i % no_subdivisions]);
	    G.set(2 * i, 2 * ((k+N) % N),
		  sweights.w1[i % no_subdivisions]);
	    G.set(2 * i, 2 * ((k+1+N) % N),
		  sweights.w2[i % no_subdivisions]);
	    G.set(2 * i, 2 * ((k+2+N) % N),
		  sweights.w3[i % no_subdivisions]);
	    
	    G.set(1 + 2 * i, 1 + 2 * ((k-1+N) % N),
		  sweights.w0[i % no_subdivisions]);
	    G.set(1 + 2 * i, 1 + 2 * ((k+N) % N),
		  sweights.w1[i % no_subdivisions]);
	    G.set(1 + 2 * i, 1 + 2 * ((k+1+N) % N),
		  sweights.w2[i % no_subdivisions]);
	    G.set(1 + 2 * i, 1 + 2 * ((k+2+N) % N),
		  sweights.w3[i % no_subdivisions]);
	    
	    
	    dG.set(2 * i, 2 * ((k-1+N) % N),
		   sweights.dw0[i % no_subdivisions]);
	    dG.set(2 * i, 2 * ((k+N) % N),
		   sweights.dw1[i % no_subdivisions]);
	    dG.set(2 * i, 2 * ((k+1+N) % N),
		   sweights.dw2[i % no_subdivisions]);
	    dG.set(2 * i, 2 * ((k+2+N) % N),
		   sweights.dw3[i % no_subdivisions]);
	    
	    dG.set(1 + 2 * i, 1 + 2 * ((k-1+N) % N),
		   sweights.dw0[i % no_subdivisions]);
	    dG.set(1 + 2 * i, 1 + 2 * ((k+N) % N),
		   sweights.dw1[i % no_subdivisions]);
	    dG.set(1 + 2 * i, 1 + 2 * ((k+1+N) % N),
		   sweights.dw2[i % no_subdivisions]);
	    dG.set(1 + 2 * i, 1 + 2 * ((k+2+N) % N),
		   sweights.dw3[i % no_subdivisions]);
	}
	G.transpose(G_t);
    }
    NagVector norms(2 * no_sample_points);
    NagVector sub_v;
    prf.sub_vector(0, 2*N-1, sub_v);
    dG.multiply(sub_v, norms);
    
    NagMatrix inv_R(2 * no_sample_points, 2 * no_sample_points);
    inv_R.clear();
    for (int i = 0; i < no_sample_points; i++)
    {
	Point2 n(norms[2*i], norms[2*i+1]);
	n.normalise();
	NagMatrix tmp(2,2);	// pointwise inverse covariance
	tmp.set(0,0, n.x * n.x);
	tmp.set(0,1, n.x * n.y);
	tmp.set(1,0, n.x * n.y);
	tmp.set(1,1, n.y * n.y);
	tmp.scale(N / ((realno) no_sample_points), tmp);
	inv_R.set_block(2*i, 2*i, tmp);
    }
    
    NagMatrix tmp1;
    G_t.multiply(inv_R, tmp1);
    tmp1.multiply(G, H);
}


void HMetric::setup_split_matrix(unsigned int N2, NagMatrix &Sm)
{
    int M2 = N2 / 2;
    Sm.reconstruct(N2, M2);
    Sm.clear(0.0);

    int M = M2 / 2;
    for (int i = 0; i < M; i++)
    {
	Sm.set(2*(2*i), (2*(i)) % M2, 0.5);
	Sm.set(2*2*i, (2*(i+1)) % M2, 0.5);
	Sm.set(2*(2*i+1), (2*(i)) % M2, 1.0 / 8.0);
	Sm.set(2*(2*i+1), (2*(i+1)) % M2, 6.0 / 8.0);
	Sm.set(2*(2*i+1), (2*(i+2)) % M2, 1.0 / 8.0);
	
	Sm.set(2*(2*i)+1, (2*(i)+1) % M2, 0.5);
	Sm.set(2*(2*i)+1, (2*(i+1)+1) % M2, 0.5);
	Sm.set(2*(2*i+1)+1, (2*(i)+1) % M2, 1.0 / 8.0);
	Sm.set(2*(2*i+1)+1, (2*(i+1)+1) % M2, 6.0 / 8.0);
	Sm.set(2*(2*i+1)+1, (2*(i+2)+1) % M2, 1.0 / 8.0);
	
    }
}

// build the covariance matrix
// associated with an nsp (no spline points) coupling
// with an ncp (no control points) contour representation
// where ncp = 2^k nsp

void HMetric::build_covariance_matrix(NagMatrix &cov, unsigned int ncp, unsigned int nsp)
{
    if ((ncp == nsp) || (nsp == 0))
    {
	NagMatrix tmp;
	setup_H(2 * ncp, tmp);
	tmp.invert(cov);
	return;
    }
    
    int N2 = ncp * 2;
    int M2 = nsp * 2;
    NagMatrix Split;
    NagMatrix split1; NagMatrix split2;
    while (M2 < N2)
    {
	setup_split_matrix(M2 * 2, split1);
	if (Split.get_data() == NULL)
	    Split = split1;
	else
	{
	    split1.multiply(Split, split2);
	    Split = split2;
	}
	M2 *= 2;
    }
    M2 = nsp * 2;
    
    NagMatrix H_m;
    setup_H(M2, H_m);
    NagMatrix H_m_inv;
    H_m.invert(H_m_inv);
    NagMatrix tmp1;
    
    H_m_inv.ortho_transform(Split, cov, false);
    
}

} // namespace ReadingPeopleTracker