void SpectClust::calcNormalizedCutValues(const Matrix &D, const std::vector<std::pair<double,int> > &indices,
std::vector<double> &cutVals) {
cutVals.resize(indices.size());
std::fill(cutVals.begin(), cutVals.end(), DBL_MAX);
double assocA = 0;
double assocB = D.Sum();
double currentNCut = 0;
double lastNCut = 0;
int nCol = D.Ncols();
for(int cut = 0; cut < indices.size() -1; cut++) {
double rowSum = 0;
for(int colIx = 0; colIx < nCol; colIx++) {
rowSum += D.element(indices[cut].second, colIx);
}
assocA += rowSum;
assocB -= rowSum;
double cutChange = 0;
for(int colIx = 0; colIx < cut; colIx++) {
cutChange -= D.element(indices[colIx].second, indices[cut].second);
}
for(int colIx = cut+1; colIx < nCol; colIx++) {
cutChange += D.element(indices[cut].second, indices[colIx].second);
}
currentNCut = lastNCut + cutChange;
if(assocA == 0 || assocB == 0 || currentNCut == 0) {
cutVals[cut] = DBL_MAX;
}
else {
double nVal = (currentNCut / assocA) + (currentNCut / assocB);
cutVals[cut+1] = nVal; // +1 is to be compatible with existing calcNormalizedCut() conventions
}
lastNCut = currentNCut;
}
}
double SpectClust::calcNormalizedCut(const Matrix &D,
const std::vector<std::pair<double,int> > &indices,
int cut) {
double assocA = 0, assocB = 0, cutAB = 0;
// count up weight of A nodes connected to B nodes.
for(int aIx = 0; aIx < cut; aIx++) {
int Aindex = indices[aIx].second;
for(int colIx = cut; colIx < indices.size(); colIx++) {
int Bindex = indices[colIx].second;
if(Aindex == Bindex) {
Err::errAbort("How can " +ToStr(Aindex) + " be in both the a and b index?");
}
cutAB += D.element(Aindex,Bindex);
}
}
// Count up connectivity of A to entire graph.
for(int aIx = 0; aIx < cut; aIx++) {
int Aindex = indices[aIx].second;
for(int colIx = 0; colIx < D.Ncols(); colIx++) {
assocA += D.element(Aindex, colIx);
}
}
// Count up connectivity of B to entire graph.
for(int bIx = cut; bIx < D.Nrows(); bIx++) {
int Bindex = indices[bIx].second;
for(int colIx = 0; colIx < D.Ncols(); colIx++) {
assocB += D.element(Bindex, colIx);
}
}
double nCut = DBL_MAX;
if(assocA != 0 && cutAB != 0 && assocB != 0)
nCut = (cutAB/assocA + cutAB/assocB);
return nCut;
}
/**
* Compute a distance metric between two columns of a
* matrix. <b>Note that the indexes are *1* based (not 0) as that is
* Newmat's convention</b>. Note that dist(M,i,j) must equal dist(M,j,i);
*
* In this case the distnance metric is the exponated 1 - cosine of the angle between the
* two vectors, this is also sometimes known as the uncentered correlation coefficient
* and is similar to correlation except that it requires the length of the vectors to
* be the same for cos(x,y) to be 1.
*
* If x and y are vectors then:
* \f$ ExpAngleDist(\vec{x},\vec{y}) = e^{-1 * (1 - cos(\theta))/2\sigma^2} \f$
* Where:
* \f$ cos(\theta) = \frac{ \vec{x}^t \cdot \vec{y}}{\|\vec{x}\|\|\vec{y}\|} \f$
* Where the numerator is the dot product of <b>x</b> and <b>y</b>.
* @param M - Matrix whose columns represent individual items to be clustered.
* @param col1Ix - Column index to be compared (1 based).
* @param col2Ix - Column index to be compared (1 based).
*
* @return - "Distance" or "dissimilarity" metric between two columns of matrix.
*/
double ExpAngleMetric::dist(const Matrix &M, int col1Ix, int col2Ix) const {
double dist = 0;
int nRow = M.Nrows();
for(int rowIx = 0; rowIx < nRow; rowIx++) {
dist += M.element(rowIx,col1Ix-1) * M.element(rowIx, col2Ix -1);
}
dist /= (m_Norms[col1Ix-1] * m_Norms[col2Ix-1]);
dist = exp(-1 * ((1 - dist)/(2*m_Sigma*m_Sigma)));
return dist;
}
/** Mutilply each element individually and return matrix. */
Matrix elementMultiplication(Matrix &x, Matrix &y) {
assert(x.Nrows() == y.Nrows());
assert(x.Ncols() == y.Ncols());
Matrix m(x.Nrows(), x.Ncols());
for(unsigned int i = 0; i < x.Nrows(); i++) {
for(unsigned int j = 0; j < x.Ncols(); j++) {
m.element(i,j) = x.element(i,j) * y.element(i,j);
}
}
return m;
}
void genetic::ReadIntegral_nevpt(ifstream& fdump, Matrix& K, int nact)
{
// save file pointer
ifstream::pos_type fp = fdump.tellg();
// rewind
fdump.seekg(0, ios::beg);
char sbuf[256];
fdump.getline(sbuf, 256);
string entry(sbuf);
vector<string> fields;
split(fields, entry, is_any_of("=, \t"), token_compress_on);
int nOrbs = 0;
for(int i = 0; i < fields.size(); ++i)
{
if(fields[i] == "NORB")
{
nOrbs = atoi(fields[i+1].c_str());
break;
}
}
K.ReSize(nact, nact); K = 0.0;
while(fdump >> entry) if((entry == "&END") || (entry == "/")) break;
int i, j, k, l;
double v;
while(fdump >> v >> i >> j >> k >> l)
{
if(i == 0 && j == 0 && k == 0 && l == 0) break;
if (i > nact) break;
//* Read by Mulliken Notation
if(i == k && j == l)
{
i--; j--;
K.element(i, j) += fabs(v);
K.element(j, i) = K.element(i, j);
}
if(k == 0 && l == 0)
{
i--; j--;
K.element(i, j) += 1.0e-7 * fabs(v);
K.element(j, i) = K.element(i, j);
}
}
// set file pointer to original position
fdump.clear();
fdump.seekg(fp);
}
void genetic::Permute(const Matrix& K, const vector<int>& sequence, Matrix& Kp)
{
int nSize = sequence.size();
Kp.ReSize(nSize, nSize); Kp = 0.0;
for(int i = 0; i < nSize; ++i)
{
int ip = sequence[i];
for(int j = i + 1; j < nSize; ++j)
{
int jp = sequence[j];
Kp.element(i, j) = K.element(ip, jp);
}
}
}
double RelateMatrices(const Matrix& m, const Matrix& m2)
{
double val = 0.0;
double sumM1 = m.element(0,3)*m.element(0,3) + m.element(1,3)*m.element(1,3) + m.element(2,3)*m.element(2,3);
double sumM2 = m2.element(0,3)*m2.element(0,3) + m2.element(1,3)*m2.element(1,3) + m2.element(2,3)*m2.element(2,3);
val = (sumM2 - sumM1) / sumM2;
return val < 0 ? 0.0 : val;
}
void SpectClust::colToVector(const Matrix &M, int colIx , std::vector<double> &v) {
v.clear();
v.reserve(M.Nrows());
for(int i =0; i < M.Nrows(); i++) {
v.push_back(M.element(i,colIx));
}
}
void SpinAdapted::xsolve_AxeqB(const Matrix& a, const ColumnVector& b, ColumnVector& x)
{
FORTINT ar = a.Nrows();
int bc = 1;
int info=0;
FORTINT* ipiv = new FORTINT[ar];
double* bwork = new double[ar];
for(int i = 0;i<ar;++i)
bwork[i] = b.element(i);
double* workmat = new double[ar*ar];
for(int i = 0;i<ar;++i)
for(int j = 0;j<ar;++j)
workmat[i*ar+j] = a.element(j,i);
GESV(ar, bc, workmat, ar, ipiv, bwork, ar, info);
delete[] ipiv;
delete[] workmat;
for(int i = 0;i<ar;++i)
x.element(i) = bwork[i];
delete[] bwork;
if(info != 0)
{
pout << "Xsolve failed with info error " << info << endl;
abort();
}
}
irr::core::matrix4 sqrt(irr::core::matrix4 A) {
//1- [E D] = eig(A); sqrtm(A) = E * sqrt(D) * E' where D is a diagonal matrix.
//> sqrt(D) is formed by taking the square root of the diagonal entries in D.
SymmetricMatrix M(3);
DiagonalMatrix D(3);
Matrix E(3,3);
for(int j=0;j<3;j++) {
for(int i=j;i<3;i++) {
M.element(j,i) = A(i,j);
}
}
Jacobi(M,D,E);
for(int i=0;i<3;i++)
D.element(i,i) = sqrt(D.element(i,i));
Matrix S = E * D * E.t() ;
for(int j=0;j<3;j++) {
for(int i=0;i<3;i++) {
A(i,j) = S.element(j,i);
//std::cout << "i:" << i << ",j:" << j << " > " << A(j,i) ;
}
//std::cout << std::endl;
}
return A;
}
void SpectClustTest::testNCutDynamicProgram() {
Matrix Dist;
RFileToMatrix(Dist, "data/spike-in.norm.angleDist.b12.txt");
bool converged;
vector<double> eVals;
Matrix EVec;
converged = SpectClust::findNLargestEvals(Dist, 2, eVals, EVec, 200);
vector<double> cutVals;
std::vector<std::pair<double,int> > indices;
for(int i = 0; i < Dist.Ncols(); i++) {
std::pair<double,int> p;
p.first = EVec.element(i,1);// / E.element(i,0) ;
p.second = i;
indices.push_back(p);
}
std::sort(indices.begin(), indices.end(), SpectClust::PairLess());
SpectClust::calcNormalizedCutValues(Dist, indices, cutVals);
for(int i = 0; i < cutVals.size(); i++) {
double nCut = SpectClust::calcNormalizedCut(Dist, indices, i);
double diff = nCut - cutVals[i];
// cout << "\t" << nCut << "\t" << cutVals[i] << "\t" << diff << endl;
if(fabs(diff) > .000001) {
CPPUNIT_ASSERT(false);
}
}
}
double SpectClust::rowMedian(const Matrix &M, int rowIx) {
std::vector<double> r(M.Ncols(),0);
for(int i = 0; i < M.Ncols(); i++) {
r[i] = M.element(rowIx,i);
}
double med = median_in_place(r.begin(), r.end());
return med;
}
double SpinAdapted::CheckSum (Matrix& a)
{
double val = 0.;
for (int i = 0; i < a.Nrows (); ++i)
for (int j = 0; j < a.Ncols (); ++j)
val += a.element (i, j);
return val;
}
void SpectClust::rowMedianDivide(Matrix &M) {
for(int rowIx = 0; rowIx < M.Nrows(); rowIx++) {
double med = rowMedian(M, rowIx);
for(int colIx = 0; colIx < M.Ncols(); colIx++) {
M.element(rowIx,colIx) /= med;
}
}
}
Matrix argpermute(const Matrix& m, const int* indices)
{
Matrix newm(m.Nrows(),m.Ncols());
newm=0.;
for (int i=0;i<m.Nrows();++i)
for (int j=0;j<m.Ncols();++j)
newm.element(i,j)=m.element(indices[i],indices[j]);
return newm;
}
/**
* Compute a distance metric between two columns of a
* matrix. <b>Note that the indexes are *1* based (not 0) as that is
* Newmat's convention</b>. Note that dist(M,i,j) must equal dist(M,j,i);
*
* @param M - Matrix whose columns represent individual items to be clustered.
* @param col1Ix - Column index to be compared (1 based).
* @param col2Ix - Column index to be compared (1 based).
*
* @return - "Distance" or "dissimilarity" metric between two columns of matrix.
*/
double CorrelationMetric::dist(const Matrix &M, int col1Ix, int col2Ix) const {
std::vector<double> c1(M.Nrows()), c2(M.Nrows());
for(int i = 0; i < M.Nrows(); i++) {
c1[i] = M.element(i,col1Ix-1);
c2[i] = M.element(i,col2Ix-1);
}
double dist = 1 + correlation_coeff(c1.begin(), c1.end(), c2.begin());
return dist;
}
// Convert target position from the robot coordinate system to the image coordinate system
void MrsvrTransform::invTransform(MrsvrVector x, MrsvrVector y)
{
Matrix vx;
Matrix vy;
vx.ReSize(4,1);
vy.ReSize(4,1);
vx.element(0, 0) = x[0];
vx.element(1, 0) = x[1];
vx.element(2, 0) = x[2];
vx.element(3, 0) = 1.0;
vy = Trp*vx;
y[0] = vy.element(0, 0);
y[1] = vy.element(1, 0);
y[2] = vy.element(2, 0);
}
/** Utility printing function. */
void printMatrix(Matrix &m, std::ostream *out, const std::string& delim) {
int nRow = m.Nrows();
int nCol = m.Ncols();
int i = 0;
int j = 0;
if(out == NULL)
out = &cout;
for(i = 0; i < nRow-1; i++) {
for(j = 0; j < nCol - 1; j++) {
(*out) << m.element(i,j) << delim;
}
(*out) << m.element(i,j) << delim;
}
for(j = 0; j < nCol - 1; j++) {
(*out) << m.element(i,j) << delim;
}
(*out) << m.element(i,j);
}
bool Camera::CanSee (RelPose &pose) const
{
if(m_relPose == NULL)
return false;
if(pose.m_uniqueID == m_relPose->m_uniqueID) /*lazy people just search in front of the camera, allow it*/
return true;
RelPose* pose_rel = RelPoseFactory::GetRelPose(pose.m_uniqueID, m_relPose->m_uniqueID);
if(pose_rel != NULL)
{
Matrix m = pose_rel->GetMatrix(0);
RelPoseFactory::FreeRelPose(&pose_rel);
double x = m.element(0,3);
double y = m.element(1,3);
double z = m.element(2,3);
if(z > 0.0)
{
try
{
Halcon::HTuple R, C;
Halcon::project_3d_point(x,y,z, m_calibration.CamParam(), &R , &C);
if(R >= 0 && R < m_calibration.m_height
&& C >= 0 && C < m_calibration.m_width)
return true;
}
catch(Halcon::HException ex)
{
printf("Error: %s\n", ex.message);
}
}
}
else
{
printf("Queried position does not exist (or position of the camera), so better assume the camera can see this position\n");
return true;
}
return false;
}
void PrincipalComponentsAnalysis::_sortEigens(Matrix& eigenVectors,
DiagonalMatrix& eigenValues)
{
// simple bubble sort, slow, but I don't expect very large matrices. Lots of room for
// improvement.
bool change = true;
while (change)
{
change = false;
for (int c = 0; c < eigenVectors.Ncols() - 1; c++)
{
if (eigenValues.element(c) < eigenValues.element(c + 1))
{
std::swap(eigenValues.element(c), eigenValues.element(c + 1));
for (int r = 0; r < eigenVectors.Nrows(); r++)
{
std::swap(eigenVectors.element(r, c), eigenVectors.element(r, c + 1));
}
}
}
}
}
void SpectClust::partitionClusters(const Matrix &D, const Matrix &E, std::vector<Numeric> &eVals, int numClusters,
std::vector<int> &clusters, int hardMin, double cutVal, double margin) {
clusters.clear();
clusters.resize(D.Ncols(), -1);
std::vector<std::pair<double,int> > indices;
/* Load up all the values and their indexes. */
for(int i = 0; i < D.Ncols(); i++) {
std::pair<double,int> p;
p.first = E.element(i,1);
p.second = i;
indices.push_back(p);
}
std::sort(indices.begin(), indices.end(), PairLess());
vector<double> indexNVals;
calcNormalizedCutValues(D, indices, indexNVals);
double minNormCut = DBL_MAX;
int minNCutIx = 0;
if(cutVal == DBL_MAX) {
for(int i = 0; i < indexNVals.size(); i++) {
double nCutI = indexNVals[i];
if(nCutI < minNormCut) {
minNCutIx = i;
minNormCut = nCutI;
}
}
cutVal = minNormCut;
}
else {
for(int i = 0; i < indices.size() - 1; i++) {
if(indices[i].first <= cutVal && indices[i+1].first >= cutVal) {
minNCutIx = i;
}
}
}
clusters.resize(indices.size());
fill(clusters.begin(), clusters.end(), -1);
int maxNCutIx = minNCutIx;
int diff = indices.size() - minNCutIx;
diff = (int) (margin*diff);
maxNCutIx = Max((int)indices.size() - diff, maxNCutIx);
if(minNCutIx >= hardMin)
minNCutIx = Max((int)(margin*minNCutIx), hardMin);
for(int i = 0; i < indices.size(); i++) {
if(i <= minNCutIx) {
clusters[indices[i].second] = 0;
}
else if(i > maxNCutIx) {
clusters[indices[i].second] = 1;
}
}
}
bool SpectClust::MaxEigen(const Matrix &M, double &maxValue, ColumnVector &MaxVec, int maxIterations) {
double maxDelta = 1e-6;
bool converged = false;
int i = 0;
int nRows = M.Ncols();
if(M.Ncols() != M.Nrows())
Err::errAbort("MaxEigen() - Can't get eigen values of non square matrices.");
if(M.Ncols() <= 0)
Err::errAbort("MaxEigen() - Must have positive number of rows and columns.");
ColumnVector V(M.Ncols());
V = 1.0 / M.Nrows(); // any vector really...
V = V / Norm1(V);
MaxVec.ReSize(M.Ncols());
for(i = 0; i < maxIterations; ++i) {
// MaxVec = M * V;
multByMatrix(MaxVec, V, M);
double delta = 0;
double norm = sqrt(SumSquare(MaxVec));
for(int vIx = 0; vIx < nRows; vIx++) {
MaxVec.element(vIx) = MaxVec.element(vIx) / norm; // scale so we don't get too big.
}
for(int rowIx = 0; rowIx < nRows; rowIx++) {
delta += fabs((double)MaxVec.element(rowIx) - V.element(rowIx));
}
if(delta < maxDelta)
break; // we've already converged to eigen vector.
V = MaxVec;
}
if(i < maxIterations) {
converged = true;
}
// calculate approximate max eigen value using Rayleigh quotient (x'*M*x/x'*x).
Matrix num = (MaxVec.t() * M * MaxVec);
Matrix denom = (MaxVec.t() * MaxVec);
maxValue = num.element(0,0) / denom.element(0,0);
return converged;
}
// Convert target position from the patient coordinate system to the robot coordinate system
void MrsvrTransform::transform(MrsvrVector x, MrsvrVector y)
{
Matrix vx;
Matrix vy;
vx.ReSize(4,1);
vy.ReSize(4,1);
vx.element(0, 0) = x[0];
vx.element(1, 0) = x[1];
vx.element(2, 0) = x[2];
vx.element(3, 0) = 1.0;
vy = Tpr*vx;
//std::cout << vy << std::endl;
//std::cout << Tpr << std::endl;
//std::cout << vx << std::endl;
y[0] = vy.element(0, 0);
y[1] = vy.element(1, 0);
y[2] = vy.element(2, 0);
}
double genetic::Evaluate(const double& scale, const double& np, const Gene& gene, const Matrix& K)
{
Matrix Kp;
Permute(K, gene.Sequence(), Kp);
double weight = 0.0;
for(int i = 0; i < Kp.Nrows(); ++i)
for(int j = i + 1; j < Kp.Ncols(); ++j)
{
double d = j - i;
// weight += Kp.element(i, j) * pow(d, np);
weight += Kp.element(i, j) * d * d;
}
return scale * weight;
}
void genetic::ReadKmatrix(ifstream& fdump, Matrix& K) {
ifstream::pos_type fp = fdump.tellg();
fdump.seekg(0, ios::beg);
int nOrbs = 0;
fdump >> nOrbs;
K.ReSize(nOrbs, nOrbs); K = 0.0;
for (int i = 0; i < nOrbs; ++i) {
for (int j = 0; j < nOrbs; ++j) {
fdump >> K.element(i,j);
}
}
// set file pointer to original position
fdump.clear();
fdump.seekg(fp);
}
void QRZT(const Matrix& X, Matrix& Y, Matrix& M)
{
REPORT
Tracer et("QRZT(2)");
int n = X.Ncols(); int s = X.Nrows(); int t = Y.Nrows();
if (Y.Ncols() != n)
{ Throw(ProgramException("Unequal row lengths",X,Y)); }
M.resize(t,s);
Real* xi = X.Store(); int k;
for (int i=0; i<s; i++)
{
Real* xj0 = Y.Store(); Real* xi0 = xi;
for (int j=0; j<t; j++)
{
Real sum=0.0;
xi=xi0; Real* xj=xj0; k=n; while(k--) { sum += *xi++ * *xj++; }
xi=xi0; k=n; while(k--) { *xj0++ -= sum * *xi++; }
M.element(j,i) = sum;
}
}
}
void SpinAdapted::Wavefunction::FlattenInto (Matrix& C)
{
int flatIndex = 0;
for (int lQ = 0; lQ < nrows (); ++lQ)
for (int rQ = 0; rQ < ncols (); ++rQ)
if (allowed(lQ, rQ))
flatIndex += operator_element(lQ,rQ).Nrows()*operator_element(lQ,rQ).Ncols();
C.ReSize(flatIndex,1);
flatIndex = 0;
for (int lQ = 0; lQ < nrows (); ++lQ)
for (int rQ = 0; rQ < ncols (); ++rQ)
if (allowed(lQ, rQ))
for (int lQState = 0; lQState < operator_element(lQ, rQ).Nrows (); ++lQState)
for (int rQState = 0; rQState < operator_element(lQ, rQ).Ncols (); ++rQState)
{
C.element (flatIndex,0) = operator_element(lQ, rQ).element (lQState, rQState);
++flatIndex;
}
}
Halcon::HTuple RelPoseHTuple::m2HT(Matrix m)
{
Halcon::HTuple hommat(12);
(hommat)[0] = m.element(0,0);
(hommat)[1] = m.element(0,1);
(hommat)[2] = m.element(0,2);
(hommat)[3] = m.element(0,3);
(hommat)[4] = m.element(1,0);
(hommat)[5] = m.element(1,1);
(hommat)[6] = m.element(1,2);
(hommat)[7] = m.element(1,3);
(hommat)[8] = m.element(2,0);
(hommat)[9] = m.element(2,1);
(hommat)[10] = m.element(2,2);
(hommat)[11] = m.element(2,3);
return hommat;
}
void IsolatedWordRecogDialog::recognize(MotionClip* pmc, int nStartFn, int nEndFn)
{
MotionEditor editor;
MotionClip* pmctmp = editor.CreateSubMotion(pmc, nStartFn, nEndFn);
if (pmctmp)
{
// The first layer
RuleNode* prn = SwiftModel::instance().m_prm->findCandidateNode(pmctmp, SwiftModel::instance().m_prm->m_pRootRuleNode);
if (prn)
{
std::map<std::string, FStrongClassifier*>::iterator itfsc;
std::vector<std::string> vFResult;
// The second layer
int result=0;
for (int i=0; i<prn->m_vSigns.size(); i++)
{
itfsc = SwiftModel::instance().m_mFStrongClassifier.find(prn->m_vSigns.at(i));
result = itfsc->second->recognize(pmctmp);
if (result == 1)
{
vFResult.push_back(itfsc->first);
}
}
// The third layer
MotionJoint *pmjlw, *pmjle, *pmjrw, *pmjre, *pmjlw1, *pmjle1, *pmjrw1, *pmjre1;
pmjlw = pmctmp->findJoint("l_wrist");
pmjle = pmctmp->findJoint("l_elbow");
pmjrw = pmctmp->findJoint("r_wrist");
pmjre = pmctmp->findJoint("r_elbow");
Matrix test;
long nSamples = pmctmp->getFrameCount();
int nDimention = 20;
test.ReSize(nDimention,nSamples);
std::vector<double> vFlex;
std::vector<double> vDist;
std::vector<double> vOriX, vOriY, vOriZ;
MotionJoint *pmj0, *pmj1, *pmj2, *pmj3, *pmj4;
InfoCalcualtor calculator;
// Right thumb finger
pmj0 = pmctmp->findJoint("r_thumb0");
pmj1 = pmctmp->findJoint("r_thumb1");
pmj2 = pmctmp->findJoint("r_thumb2");
pmj3 = pmctmp->findJoint("r_thumb3");
pmj4 = pmctmp->getChild(pmj3, 0);
calculator.calFingerFlex(pmj0, pmj1, pmj2, pmj3, pmj4, 0, nSamples-1, vFlex, 190);
int k=0;
for (int j=0; j<nSamples; j++)
{
test.element(k,j)=vFlex.at(j);
}
vFlex.clear();
// Right index finger
pmj0 = pmctmp->findJoint("r_index0");
pmj1 = pmctmp->findJoint("r_index1");
pmj2 = pmctmp->findJoint("r_index2");
pmj3 = pmctmp->findJoint("r_index3");
pmj4 = pmctmp->getChild(pmj3, 0);
calculator.calFingerFlex(pmj0, pmj1, pmj2, pmj3, pmj4, 0, nSamples-1, vFlex, 250);
k++;
for (int j=0; j<nSamples; j++)
{
test.element(k,j)=vFlex.at(j);
}
vFlex.clear();
// Right middle finger
pmj0 = pmctmp->findJoint("r_middle0");
pmj1 = pmctmp->findJoint("r_middle1");
pmj2 = pmctmp->findJoint("r_middle2");
pmj3 = pmctmp->findJoint("r_middle3");
pmj4 = pmctmp->getChild(pmj3, 0);
calculator.calFingerFlex(pmj0, pmj1, pmj2, pmj3, pmj4, 0, nSamples-1, vFlex, 250);
k++;
for (int j=0; j<nSamples; j++)
{
test.element(k,j)=vFlex.at(j);
}
vFlex.clear();
// Right ring finger
pmj0 = pmctmp->findJoint("r_ring0");
pmj1 = pmctmp->findJoint("r_ring1");
pmj2 = pmctmp->findJoint("r_ring2");
pmj3 = pmctmp->findJoint("r_ring3");
pmj4 = pmctmp->getChild(pmj3, 0);
calculator.calFingerFlex(pmj0, pmj1, pmj2, pmj3, pmj4, 0, nSamples-1, vFlex, 250);
k++;
for (int j=0; j<nSamples; j++)
{
test.element(k,j)=vFlex.at(j);
}
vFlex.clear();
// Right little finger
pmj0 = pmctmp->findJoint("r_pinky0");
pmj1 = pmctmp->findJoint("r_pinky1");
pmj2 = pmctmp->findJoint("r_pinky2");
pmj3 = pmctmp->findJoint("r_pinky3");
pmj4 = pmctmp->getChild(pmj3, 0);
calculator.calFingerFlex(pmj0, pmj1, pmj2, pmj3, pmj4, 0, nSamples-1, vFlex, 250);
k++;
for (int j=0; j<nSamples; j++)
{
test.element(k,j)=vFlex.at(j);
}
vFlex.clear();
// Left thumb finger
pmj0 = pmctmp->findJoint("l_thumb0");
pmj1 = pmctmp->findJoint("l_thumb1");
pmj2 = pmctmp->findJoint("l_thumb2");
pmj3 = pmctmp->findJoint("l_thumb3");
pmj4 = pmctmp->getChild(pmj3, 0);
calculator.calFingerFlex(pmj0, pmj1, pmj2, pmj3, pmj4, 0, nSamples-1, vFlex, 190);
k++;
for (int j=0; j<nSamples; j++)
{
test.element(k,j)=vFlex.at(j);
}
vFlex.clear();
// Left index finger
pmj0 = pmctmp->findJoint("l_index0");
pmj1 = pmctmp->findJoint("l_index1");
pmj2 = pmctmp->findJoint("l_index2");
pmj3 = pmctmp->findJoint("l_index3");
pmj4 = pmctmp->getChild(pmj3, 0);
calculator.calFingerFlex(pmj0, pmj1, pmj2, pmj3, pmj4, 0, nSamples-1, vFlex, 250);
k++;
for (int j=0; j<nSamples; j++)
{
test.element(k,j)=vFlex.at(j);
}
vFlex.clear();
// Left middle finger
pmj0 = pmctmp->findJoint("l_middle0");
pmj1 = pmctmp->findJoint("l_middle1");
pmj2 = pmctmp->findJoint("l_middle2");
pmj3 = pmctmp->findJoint("l_middle3");
pmj4 = pmctmp->getChild(pmj3, 0);
calculator.calFingerFlex(pmj0, pmj1, pmj2, pmj3, pmj4, 0, nSamples-1, vFlex, 250);
k++;
for (int j=0; j<nSamples; j++)
{
test.element(k,j)=vFlex.at(j);
}
vFlex.clear();
// Left ring finger
pmj0 = pmctmp->findJoint("l_ring0");
pmj1 = pmctmp->findJoint("l_ring1");
pmj2 = pmctmp->findJoint("l_ring2");
pmj3 = pmctmp->findJoint("l_ring3");
pmj4 = pmctmp->getChild(pmj3, 0);
calculator.calFingerFlex(pmj0, pmj1, pmj2, pmj3, pmj4, 0, nSamples-1, vFlex, 250);
k++;
for (int j=0; j<nSamples; j++)
{
test.element(k,j)=vFlex.at(j);
}
vFlex.clear();
// Left little finger
pmj0 = pmctmp->findJoint("l_pinky0");
pmj1 = pmctmp->findJoint("l_pinky1");
pmj2 = pmctmp->findJoint("l_pinky2");
pmj3 = pmctmp->findJoint("l_pinky3");
pmj4 = pmctmp->getChild(pmj3, 0);
calculator.calFingerFlex(pmj0, pmj1, pmj2, pmj3, pmj4, 0, nSamples-1, vFlex, 250);
k++;
for (int j=0; j<nSamples; j++)
{
test.element(k,j)=vFlex.at(j);
}
vFlex.clear();
pmj1 = pmctmp->findJoint("r_wrist");
pmj2 = pmctmp->findJoint("root");
calculator.calDist(pmj1, pmj2, 0, nSamples-1, vDist);
k++;
for (int j=0; j<nSamples; j++)
{
test.element(k,j)=vDist.at(j);
}
vDist.clear();
pmj2 = pmctmp->findJoint("skullbase");
calculator.calDist(pmj1, pmj2, 0, nSamples-1, vDist);
k++;
for (int j=0; j<nSamples; j++)
{
test.element(k,j)=vDist.at(j);
}
vDist.clear();
pmj1 = pmctmp->findJoint("l_wrist");
pmj2 = pmctmp->findJoint("root");
calculator.calDist(pmj1, pmj2, 0, nSamples-1, vDist);
k++;
for (int j=0; j<nSamples; j++)
{
test.element(k,j)=vDist.at(j);
}
vDist.clear();
pmj2 = pmctmp->findJoint("skullbase");
calculator.calDist(pmj1, pmj2, 0, nSamples-1, vDist);
k++;
for (int j=0; j<nSamples; j++)
{
test.element(k,j)=vDist.at(j);
}
vDist.clear();
pmj1 = pmctmp->findJoint("r_wrist");
pmj2 = pmctmp->findJoint("r_index1");
pmj3 = pmctmp->findJoint("r_ring1");
calculator.calPalmOri(pmctmp, pmj1, pmj2, pmj3, 0, nSamples-1, vOriX, vOriY, vOriZ);
for (int j=0; j<nSamples; j++)
{
test.element(k+1,j)=vOriX.at(j);
test.element(k+2,j)=vOriY.at(j);
test.element(k+3,j)=vOriZ.at(j);
}
k+=3;
vOriX.clear();
vOriY.clear();
vOriZ.clear();
pmj1 = pmctmp->findJoint("l_wrist");
pmj2 = pmctmp->findJoint("l_index1");
pmj3 = pmctmp->findJoint("l_ring1");
calculator.calPalmOri(pmc, pmj1, pmj2, pmj3, 0, nSamples-1, vOriX, vOriY, vOriZ);
for (int j=0; j<nSamples; j++)
{
test.element(k+1,j)=vOriX.at(j);
test.element(k+2,j)=vOriY.at(j);
test.element(k+3,j)=vOriZ.at(j);
}
k+=3;
vOriX.clear();
vOriY.clear();
vOriZ.clear();
double dmax = -1.0e100, d;
std::string str;
RecogResult rr;
std::vector<RecogResult> vResult;
QString strBestN = ui.lineEdit_BestN->text();
int nBestN = strBestN.toInt();
std::map<std::string, HMM*>::iterator it;
for (int i=0; i<vFResult.size(); i++)
{
it = HMMModel::instance().m_mHMM.find(vFResult.at(i));
if (it != HMMModel::instance().m_mHMM.end())
{
d = it->second->viterbi_p(test);
//if (d > dmax)
//{
// dmax = d;
// str = it->first;
//}
rr.strSign = it->first;
rr.dScore = d;
if(vResult.size() == nBestN)
{
HMMModel::instance().sortResut(vResult);
if (vResult.back().dScore < d)
{
vResult.erase(vResult.end()-1);
vResult.push_back(rr);
}
}
else
{
vResult.push_back(rr);
}
}
}
HMMModel::instance().sortResut(vResult);
ui.textBrowser_Result->setText("The best "+QString::number(vResult.size())+" candidates and their scores:");
for (int i=0; i<vResult.size(); i++)
{
ui.textBrowser_Result->append(QString::fromStdString(vResult.at(i).strSign)+": "+QString::number(vResult.at(i).dScore));
}
ui.textBrowser_Result->append("");
}
}
}
void SpinAdapted::Linear::Lanczos(vector<Wavefunction>& b, DiagonalMatrix& e, double normtol, Davidson_functor& h_multiply, int nroots)
{
int iter = 0;
pout.precision(12);
if(mpigetrank() == 0) {
//b[0].Randomise();
Normalise(b[0]);
b.resize(dmrginp.max_lanczos_dimension());
}
double beta_minus = 1.0;
std::vector<double> diagonal(dmrginp.max_lanczos_dimension(), 0);
std::vector<double> offdiagonal(dmrginp.max_lanczos_dimension(), 0);
Wavefunction b0;
Wavefunction* bptr = &b0;
if (mpigetrank() == 0)
bptr = &b[0];
else
bptr = &b0;
#ifndef SERIAL
mpi::communicator world;
mpi::broadcast(world, *bptr, 0);
#endif
Wavefunction r(*bptr);
bool notconverged = true;
while (notconverged && iter < dmrginp.max_lanczos_dimension()) {
r.Clear();
pout << "\t\t\t Lanczos Iteration :: "<<iter<<endl;
iter ++;
#ifndef SERIAL
mpi::broadcast(world, *bptr, 0);
#endif
h_multiply(*bptr, r);
if(mpigetrank() == 0) {
if (iter > 1)
ScaleAdd(-beta_minus, b[iter-2], r);
diagonal[iter-1] = DotProduct(*bptr, r);
double alphai = diagonal[iter-1];
Matrix alpha;
std::vector<double> diagonal_temporary(iter,0);
std::vector<double> offdiagonal_temporary(iter,0);
for (int i=0; i<iter; i++) {
diagonal_temporary[i] = diagonal[i];
offdiagonal_temporary[i] = offdiagonal[i];
}
diagonalise_tridiagonal(diagonal_temporary, offdiagonal_temporary, iter, alpha);
ScaleAdd(-alphai, *bptr, r);
for (int i=iter-1; i>=0; i--) {
double overlap = DotProduct(b[i], r);
ScaleAdd(-overlap, b[i], r);
}
offdiagonal[iter-1] = sqrt(DotProduct(r, r));
if (dmrginp.outputlevel() > 0) {
if (iter > 1)
notconverged = false;
for (int i = 1; i <= min(iter-1, nroots); ++i) {
pout << "\t\t\t " << i << " :: " << diagonal_temporary[i-1]+dmrginp.get_coreenergy() <<" "<<pow(offdiagonal[iter-1]*alpha(iter,i),2)<<endl;
if (pow(offdiagonal[iter-1]*alpha(iter,i),2) >= normtol) {
notconverged = true;
break;
}
}
}
if (iter == dmrginp.max_lanczos_dimension()) {
pout << "Reached the maximum number of allowed iterations!!"<<endl;
exit(0);
}
//cout << offdiagonal[iter-1]<<" "<<iter<<endl;
if(!notconverged) {
for (int i=0; i<nroots; i++)
e(i+1) = diagonal_temporary[i];
vector<Wavefunction> btmp = b;
for (int i=0; i<nroots; i++)
Scale(alpha.element(i,i), b[i]);
for (int i=0; i<nroots; i++)
for (int j=0; j<iter; j++)
if (i != j)
ScaleAdd(alpha.element(j,i), btmp[j], b[i]);
break;
}
Normalise(r);
b[iter] = r;
if(mpigetrank() == 0)
bptr = &b[iter];
}
#ifndef SERIAL
mpi::broadcast(world, notconverged, 0);
#endif
r.Clear();
}
}
