Esempio n. 1
0
void calcCovar(){
	int nIter=100;
	cMtx.init(nfiles);

	for(int i=0; i<nfiles; i++){
		for(int j=i; j<nfiles; j++){
			verb("Covariations %i~%i: <%s>~<%s> ",i,j,tracks[i]->name,tracks[j]->name);
			cMtx.calc(i,j);
		}
	}
	char b[2048];
	sprintf(b,"%s.cvr",confFile);
	FILE *f=xopen(b,"wt");
	cMtx.print(f);
	Matrix *x = new Matrix(&cMtx);
	getMem(eValues,nfiles+10,"eigenVal"); zeroMem(eValues,nfiles);
	eVectors=eigenVectors(x,eValues,nIter,1.E-3);
	fputs("eigenVectors\n",f);
	eVectors->printMtx(f);
	fputs("eigenValues\n",f);
	for(int i=0; i<nfiles; i++)
		fprintf(f,"%.5f; ",eValues[i]);
	fprintf(f,"\n");
	fclose(f);
	del(x);
}
Esempio n. 2
0
Mat myPCA::getEigenVectors()
{
	//get difference
	Mat diffFace(sampleMatrix.size(), CV_32FC1);
	for(int i = 0; i < sampleMatrix.cols; ++i)
	{
		Mat temp1 = diffFace.col(i);
		Mat temp2 = sampleMatrix.col(i) - meanFace;
		temp2.copyTo(temp1);
	}
	//get covariance matrix
	//AA'is too large, comput A'A instead
	Mat dt = diffFace.t();
	Mat covMatrix = dt * diffFace;
	
	SVD isvd(diffFace,SVD::FULL_UV);
	Mat U = isvd.u;
	Mat W = isvd.w;
	
	/*
	Mat W;
	Mat U;
	eigen(covMatrix,W,U);*/

	double sum = 0;
	int i;
	for(i = 1; i < W.rows; ++i)
	{
		float t = W.at<float>(i,0)* W.at<float>(i,0);
		//cout << t << endl;
		sum += t;
	}
	sum *= this->a;

	i = 1;
	double x = 0;
	while(x < sum)
	{
		float t = W.at<float>(i,0)* W.at<float>(i,0);
		x += t;
		++i;
	}

	//number of eigen faces
	int p = i - 1;
	
	if(p > 80)
		p = 80;
	Mat eigenVectors(sampleMatrix.rows, p, CV_32FC1);
	for(int i = 0; i < p; ++i)
	{
		Mat temp1 = eigenVectors.col(i);
		Mat temp2 = U.col(i);//diffFace * U.col(i) / sqrt(W.at<float>(i,0));
		temp2.copyTo(temp1);
	}

	this->eigenVectors = eigenVectors;
	return eigenVectors;
}
MatrixXd PCA::project(MatrixXd &dataPoints, unsigned int dimSpace)
{
	if(!k) return MatrixXd();

	int m = dataPoints.rows();
	int n = dataPoints.cols();

	switch(kernelType)
	{
	case 0:
		k = new LinearKernel();
		break;
	case 1:
		k = new PolyKernel(degree);
		break;
	case 2:
		k = new RBFKernel(gamma);
		break;
	default:
		k = new Kernel();
	}
	k->Compute(dataPoints, sourcePoints);

	//std::cout << "K:\n" << k->get() << "\n";

	// ''centralize''
	MatrixXd K = k->get()
				 - MatrixXd::Ones(k->get().rows(), k->get().rows())*k->get()
				 - k->get()*MatrixXd::Ones(k->get().cols(), k->get().cols())
				 + MatrixXd::Ones(k->get().rows(), k->get().rows())*k->get()*MatrixXd::Ones(k->get().cols(), k->get().cols());

	MatrixXd results = MatrixXd::Zero(n, dimSpace);
	for (unsigned int i = 0; i < dimSpace; i++)
	{
		for (int j=0; j<n; j++)
			for (int w=0; w<eigenVectors.rows(); w++)
				results(j,i) += K(j,w) * eigenVectors(w,pi[i].second); // permutation indices
	}

	MatrixXd sqrtE = MatrixXd::Zero(dimSpace, dimSpace);
	for (unsigned int i = 0; i < dimSpace; i++)
	{
//		sqrtE(i, i) = 0.9;
	}

	for(int i=0; i < results.rows(); i++)
	{
		for(int j=0; j < results.cols(); j++)
		{
			results(i,j) *= 0.9;
		}
	}

	// get the final data projection
	//results = (sqrtE * results.transpose()).transpose();

	return results;
}
dimensionedTensor eigenVectors(const dimensionedSymmTensor& dt)
{
    return dimensionedTensor
    (
        "eigenVectors("+dt.name()+')',
        dimless,
        eigenVectors(dt.value())
    );
}
Esempio n. 5
0
int main(int argc, char const *argv[]){
    if(argc != 3){
        cout << "Parameters should be: (i: input file), (o: output file)" << endl;
        return 0;
    }

    ifstream input(argv[1]);

    string inFileDir, line;
    int alpha;

    input >> inFileDir >> alpha;
    getline(input, line);

    getline(input, line);
    stringstream lineStream(line);
    DigitImages imagesTrain, imagesTest;
    Matrix eigenVectors(alpha, vector<double>(DEFAULT_IMAGE_SIZE));
    vector<double> eigenValues(alpha);
    int niter = 1000;

    populateDigitImages(imagesTrain, imagesTest, inFileDir, lineStream);
    imagesTrain.getMeans();
    imagesTrain.calculateCentralized();
    // imagesTrain.calculateCovariances();
    imagesTest.calculateCentralizedTest(imagesTrain.means, (int)imagesTrain.images.size());

    // PCA(imagesTrain.covariances, eigenVectors, eigenValues, alpha, niter);
    PLSDA(imagesTrain, eigenVectors, eigenValues, alpha, niter);

    vector<double> aux(DEFAULT_IMAGE_SIZE);
    for (int k = 0; k < eigenVectors.size(); ++k){
        ofstream output(argv[2] + to_string(k));
        double max = eigenVectors[k][0], min = eigenVectors[k][0];
        for (int i = 1; i < DEFAULT_IMAGE_SIZE; ++i){
            if(eigenVectors[k][i] > max)
                max = eigenVectors[k][i];
            else if(eigenVectors[k][i] < min)
                min = eigenVectors[k][i];
        }
        for (int i = 0; i < DEFAULT_IMAGE_SIZE; ++i){
            aux[i] = eigenVectors[k][i] - min;
            aux[i] *= 255;
            aux[i] /= (max - min);
        }
        printImage(aux, output);
        output.close();
    }

    input.close();
    return 0;
}
float PCA::test(VectorXd point)
{
	if(!k) return 0;

	int n = 1;
	int dimSpace = 1;
	int m = point.rows();

	switch(kernelType)
	{
	case 0:
		k = new LinearKernel();
		break;
	case 1:
		k = new PolyKernel(degree);
		break;
	case 2:
		k = new RBFKernel(gamma);
		break;
	default:
		k = new Kernel();
	}
	MatrixXd onePoint = MatrixXd::Zero(m,1);
	for(int i=0; i<m; i++) onePoint(i,0) = point(i);
	k->Compute(onePoint, sourcePoints);

	//std::cout << "K:\n" << k->get() << "\n";

	// ''centralize''
	MatrixXd K = k->get()
				 - MatrixXd::Ones(k->get().rows(), k->get().rows())*k->get()
				 - k->get()*MatrixXd::Ones(k->get().cols(), k->get().cols())
				 + MatrixXd::Ones(k->get().rows(), k->get().rows())*k->get()*MatrixXd::Ones(k->get().cols(), k->get().cols());


//	std::cout << K.row(0) << "\n";
//	std::cout << eigenvalues << "\n";
//	std::cout << k->get().row(0) << "\n";

	float result = 0;	
	for (int w=0; w<eigenVectors.rows(); w++)
	{
		result += k->get()(0,w) * eigenVectors(w,pi[0].second); // permutation indices
	}
	result = (result * 0.25f - 1)*2;
	//result *= eigenvalues(pi[0].second);
	//std::cout << result << "\n";
	return result;
}
Esempio n. 7
0
const Vector &
DOF_Group::getDampingBetaForce(int mode, double beta)
{
  // to return beta * M * phi(mode)
  const Matrix & mass = myNode->getMass();
  const Matrix & eigenVectors = myNode->getEigenvectors();
  int numDOF = eigenVectors.noRows();

  Vector eigenvector(numDOF);

  for (int i=0; i<numDOF; i++)
    eigenvector(i) = eigenVectors(i,mode);

  unbalance->addMatrixVector(0.0, mass, eigenvector, -beta);
  return *unbalance;
}
Esempio n. 8
0
double 
DOF_Group::getDampingBetaFactor(int mode, double ratio, double wn)
{
  // to return 2.0 * ratio * wn * phi(mode)' * M * v
  double beta = 0;
  const Matrix & mass = myNode->getMass();
  const Matrix & eigenVectors = myNode->getEigenvectors();
  const Vector & vel = myNode->getTrialVel();
  int numDOF = eigenVectors.noRows();
  int numMode = eigenVectors.noCols();
  Vector Mv = mass * vel;
  if (mode < numMode) {
    for (int i = 0; i < numDOF; i++) 
      beta += 2.0 * ratio * wn * eigenVectors(i, mode) * Mv(i);
  }
  return beta;
}
Esempio n. 9
0
// from numerical recipes in c
// Jacobian method for computing the eigenvectors of a symmetric matrix
dMatrix dMatrix::JacobiDiagonalization (dVector & eigenValues, const dMatrix & initialMatrix) const
{
   dFloat thresh;
   dFloat b[3];
   dFloat z[3];
   dFloat d[3];
   dFloat EPSILON = 1.0e-5f;
   dMatrix mat (*this);
   dMatrix eigenVectors (initialMatrix);
   b[0] = mat[0][0];
   b[1] = mat[1][1];
   b[2] = mat[2][2];
   d[0] = mat[0][0];
   d[1] = mat[1][1];
   d[2] = mat[2][2];
   z[0] = 0.0f;
   z[1] = 0.0f;
   z[2] = 0.0f;
   int nrot = 0;
   for (int i = 0; i < 50; i++)
   {
      dFloat sm = dAbs (mat[0][1]) + dAbs (mat[0][2]) + dAbs (mat[1][2]);
      if (sm < EPSILON * 1e-5)
      {
         dAssert (dAbs ((eigenVectors.m_front % eigenVectors.m_front) - 1.0f) < EPSILON);
         dAssert (dAbs ((eigenVectors.m_up % eigenVectors.m_up) - 1.0f) < EPSILON);
         dAssert (dAbs ((eigenVectors.m_right % eigenVectors.m_right) - 1.0f) < EPSILON);
         eigenValues = dVector (d[0], d[1], d[2], dFloat (0.0f));
         return eigenVectors.Inverse();
      }
      if (i < 3)
         thresh = (dFloat) (0.2f / 9.0f) * sm;
      else
         thresh = 0.0;
      // First row
      dFloat g = 100.0f * dAbs (mat[0][1]);
      if ((i > 3) && (dAbs (d[0]) + g == dAbs (d[0])) && (dAbs (d[1]) + g == dAbs (d[1])))
         mat[0][1] = 0.0f;
      else
         if (dAbs (mat[0][1]) > thresh)
         {
            dFloat t;
            dFloat h = d[1] - d[0];
            if (dAbs (h) + g == dAbs (h))
               t = mat[0][1] / h;
            else
            {
               dFloat theta = dFloat (0.5f) * h / mat[0][1];
               t = dFloat (1.0f) / (dAbs (theta) + dSqrt (dFloat (1.0f) + theta * theta));
               if (theta < 0.0f)
                  t = -t;
            }
            dFloat c = dFloat (1.0f) / dSqrt (1.0f + t * t);
            dFloat s = t * c;
            dFloat tau = s / (dFloat (1.0f) + c);
            h = t * mat[0][1];
            z[0] -= h;
            z[1] += h;
            d[0] -= h;
            d[1] += h;
            mat[0][1] = 0.0f;
            ROT (mat, 0, 2, 1, 2, s, tau);
            ROT (eigenVectors, 0, 0, 0, 1, s, tau);
            ROT (eigenVectors, 1, 0, 1, 1, s, tau);
            ROT (eigenVectors, 2, 0, 2, 1, s, tau);
            nrot++;
         }
      // second row
      g = 100.0f * dAbs (mat[0][2]);
      if ((i > 3) && (dAbs (d[0]) + g == dAbs (d[0])) && (dAbs (d[2]) + g == dAbs (d[2])))
         mat[0][2] = 0.0f;
      else
         if (dAbs (mat[0][2]) > thresh)
         {
            dFloat t;
            dFloat h = d[2] - d[0];
            if (dAbs (h) + g == dAbs (h))
               t = (mat[0][2]) / h;
            else
            {
               dFloat theta = dFloat (0.5f) * h / mat[0][2];
               t = dFloat (1.0f) / (dAbs (theta) + dSqrt (dFloat (1.0f) + theta * theta));
               if (theta < 0.0f)
                  t = -t;
            }
            dFloat c = dFloat (1.0f) / dSqrt (1 + t * t);
            dFloat s = t * c;
            dFloat tau = s / (dFloat (1.0f) + c);
            h = t * mat[0][2];
            z[0] -= h;
            z[2] += h;
            d[0] -= h;
            d[2] += h;
            mat[0][2] = 0.0;
            ROT (mat, 0, 1, 1, 2, s, tau);
            ROT (eigenVectors, 0, 0, 0, 2, s, tau);
            ROT (eigenVectors, 1, 0, 1, 2, s, tau);
            ROT (eigenVectors, 2, 0, 2, 2, s, tau);
         }
      // trird row
      g = 100.0f * dAbs (mat[1][2]);
      if ((i > 3) && (dAbs (d[1]) + g == dAbs (d[1])) && (dAbs (d[2]) + g == dAbs (d[2])))
         mat[1][2] = 0.0f;
      else
         if (dAbs (mat[1][2]) > thresh)
         {
            dFloat t;
            dFloat h = d[2] - d[1];
            if (dAbs (h) + g == dAbs (h))
               t = mat[1][2] / h;
            else
            {
               dFloat theta = dFloat (0.5f) * h / mat[1][2];
               t = dFloat (1.0f) / (dAbs (theta) + dSqrt (dFloat (1.0f) + theta * theta));
               if (theta < 0.0f)
                  t = -t;
            }
            dFloat c = dFloat (1.0f) / dSqrt (1 + t * t);
            dFloat s = t * c;
            dFloat tau = s / (dFloat (1.0f) + c);
            h = t * mat[1][2];
            z[1] -= h;
            z[2] += h;
            d[1] -= h;
            d[2] += h;
            mat[1][2] = 0.0f;
            ROT (mat, 0, 1, 0, 2, s, tau);
            ROT (eigenVectors, 0, 1, 0, 2, s, tau);
            ROT (eigenVectors, 1, 1, 1, 2, s, tau);
            ROT (eigenVectors, 2, 1, 2, 2, s, tau);
            nrot++;
         }
      b[0] += z[0];
      d[0] = b[0];
      z[0] = 0.0f;
      b[1] += z[1];
      d[1] = b[1];
      z[1] = 0.0f;
      b[2] += z[2];
      d[2] = b[2];
      z[2] = 0.0f;
   }
   dAssert (0);
   eigenValues = dVector (d[0], d[1], d[2], dFloat (0.0f));
   return dGetIdentityMatrix();
}
Esempio n. 10
0
void dgMatrix::EigenVectors (dgVector &eigenValues, const dgMatrix& initialGuess)
{
	hacd::HaF32 b[3];
	hacd::HaF32 z[3];
	hacd::HaF32 d[3];

	dgMatrix& mat = *this;
	dgMatrix eigenVectors (initialGuess.Transpose4X4());
	mat = initialGuess * mat * eigenVectors;

	b[0] = mat[0][0]; 
	b[1] = mat[1][1];
	b[2] = mat[2][2];

	d[0] = mat[0][0]; 
	d[1] = mat[1][1]; 
	d[2] = mat[2][2]; 

	z[0] = hacd::HaF32 (0.0f);
	z[1] = hacd::HaF32 (0.0f);
	z[2] = hacd::HaF32 (0.0f);

	for (hacd::HaI32 i = 0; i < 50; i++) {
		hacd::HaF32 sm = dgAbsf(mat[0][1]) + dgAbsf(mat[0][2]) + dgAbsf(mat[1][2]);

		if (sm < hacd::HaF32 (1.0e-6f)) {
			HACD_ASSERT (dgAbsf((eigenVectors.m_front % eigenVectors.m_front) - hacd::HaF32(1.0f)) < dgEPSILON);
			HACD_ASSERT (dgAbsf((eigenVectors.m_up % eigenVectors.m_up) - hacd::HaF32(1.0f)) < dgEPSILON);
			HACD_ASSERT (dgAbsf((eigenVectors.m_right % eigenVectors.m_right) - hacd::HaF32(1.0f)) < dgEPSILON);

			// order the eigenvalue vectors	
			dgVector tmp (eigenVectors.m_front * eigenVectors.m_up);
			if (tmp % eigenVectors.m_right < hacd::HaF32(0.0f)) {
				eigenVectors.m_right = eigenVectors.m_right.Scale (-hacd::HaF32(1.0f));
			}

			eigenValues = dgVector (d[0], d[1], d[2], hacd::HaF32 (0.0f));
			*this = eigenVectors.Inverse();
			return;
		}

		hacd::HaF32 thresh = hacd::HaF32 (0.0f);
		if (i < 3) {
			thresh = (hacd::HaF32)(0.2f / 9.0f) * sm;
		}

		for (hacd::HaI32 ip = 0; ip < 2; ip ++) {
			for (hacd::HaI32 iq = ip + 1; iq < 3; iq ++) {
				hacd::HaF32 g = hacd::HaF32 (100.0f) * dgAbsf(mat[ip][iq]);
				//if ((i > 3) && (dgAbsf(d[0]) + g == dgAbsf(d[ip])) && (dgAbsf(d[1]) + g == dgAbsf(d[1]))) {
				if ((i > 3) && ((dgAbsf(d[ip]) + g) == dgAbsf(d[ip])) && ((dgAbsf(d[iq]) + g) == dgAbsf(d[iq]))) {
					mat[ip][iq] = hacd::HaF32 (0.0f);
				} else if (dgAbsf(mat[ip][iq]) > thresh) {

					hacd::HaF32 t;
					hacd::HaF32 h = d[iq] - d[ip];
					if (dgAbsf(h) + g == dgAbsf(h)) {
						t = mat[ip][iq] / h;
					} else {
						hacd::HaF32 theta = hacd::HaF32 (0.5f) * h / mat[ip][iq];
						t = hacd::HaF32(1.0f) / (dgAbsf(theta) + dgSqrt(hacd::HaF32(1.0f) + theta * theta));
						if (theta < hacd::HaF32 (0.0f)) {
							t = -t;
						}
					}
					hacd::HaF32 c = hacd::HaF32(1.0f) / dgSqrt (hacd::HaF32 (1.0f) + t * t); 
					hacd::HaF32 s = t * c; 
					hacd::HaF32 tau = s / (hacd::HaF32(1.0f) + c); 
					h = t * mat[ip][iq];
					z[ip] -= h; 
					z[iq] += h; 
					d[ip] -= h; 
					d[iq] += h;
					mat[ip][iq] = hacd::HaF32(0.0f);

					for (hacd::HaI32 j = 0; j <= ip - 1; j ++) {
						//ROT (mat, j, ip, j, iq, s, tau); 
						//ROT(dgMatrix &a, hacd::HaI32 i, hacd::HaI32 j, hacd::HaI32 k, hacd::HaI32 l, hacd::HaF32 s, hacd::HaF32 tau) 
						hacd::HaF32 g = mat[j][ip]; 
						hacd::HaF32 h = mat[j][iq]; 
						mat[j][ip] = g - s * (h + g * tau); 
						mat[j][iq] = h + s * (g - h * tau);

					}
					for (hacd::HaI32 j = ip + 1; j <= iq - 1; j ++) {
						//ROT (mat, ip, j, j, iq, s, tau); 
						//ROT(dgMatrix &a, hacd::HaI32 i, hacd::HaI32 j, hacd::HaI32 k, hacd::HaI32 l, hacd::HaF32 s, hacd::HaF32 tau) 
						hacd::HaF32 g = mat[ip][j]; 
						hacd::HaF32 h = mat[j][iq]; 
						mat[ip][j] = g - s * (h + g * tau); 
						mat[j][iq] = h + s * (g - h * tau);
					}
					for (hacd::HaI32 j = iq + 1; j < 3; j ++) {
						//ROT (mat, ip, j, iq, j, s, tau); 
						//ROT(dgMatrix &a, hacd::HaI32 i, hacd::HaI32 j, hacd::HaI32 k, hacd::HaI32 l, hacd::HaF32 s, hacd::HaF32 tau) 
						hacd::HaF32 g = mat[ip][j]; 
						hacd::HaF32 h = mat[iq][j]; 
						mat[ip][j] = g - s * (h + g * tau); 
						mat[iq][j] = h + s * (g - h * tau);
					}

					for (hacd::HaI32 j = 0; j < 3; j ++) {
						//ROT (eigenVectors, j, ip, j, iq, s, tau); 
						//ROT(dgMatrix &a, hacd::HaI32 i, hacd::HaI32 j, hacd::HaI32 k, hacd::HaI32 l, hacd::HaF32 s, hacd::HaF32 tau) 
						hacd::HaF32 g = eigenVectors[j][ip]; 
						hacd::HaF32 h = eigenVectors[j][iq]; 
						eigenVectors[j][ip] = g - s * (h + g * tau); 
						eigenVectors[j][iq] = h + s * (g - h * tau);
					}
				}
			}
		}
		b[0] += z[0]; d[0] = b[0]; z[0] = hacd::HaF32 (0.0f);
		b[1] += z[1]; d[1] = b[1]; z[1] = hacd::HaF32 (0.0f);
		b[2] += z[2]; d[2] = b[2]; z[2] = hacd::HaF32 (0.0f);
	}

	eigenValues = dgVector (d[0], d[1], d[2], hacd::HaF32 (0.0f));
	*this = dgGetIdentityMatrix();
}
Esempio n. 11
0
void dgMatrix::EigenVectors (dgVector &eigenValues, const dgMatrix* const initialGuess)
{
	dgMatrix& mat = *this;
	dgMatrix eigenVectors (dgGetIdentityMatrix());
	if (initialGuess) {
		eigenVectors = *initialGuess;
		mat = eigenVectors.Transpose4X4() * mat * eigenVectors;
	}

	dgVector d (mat[0][0], mat[1][1], mat[2][2], dgFloat32 (0.0f)); 
	dgVector b (d);
	for (dgInt32 i = 0; i < 50; i++) {
		dgFloat32 sm = dgAbsf(mat[0][1]) + dgAbsf(mat[0][2]) + dgAbsf(mat[1][2]);

		if (sm < dgFloat32 (1.0e-12f)) {
			// order the eigenvalue vectors	
			dgVector tmp (eigenVectors.m_front * eigenVectors.m_up);
			if (tmp % eigenVectors.m_right < dgFloat32(0.0f)) {
				dgAssert (0.0f);
				eigenVectors.m_right = eigenVectors.m_right.Scale3 (-dgFloat32(1.0f));
			}
			break;
		}

		dgFloat32 thresh = dgFloat32 (0.0f);
		if (i < 3) {
			thresh = (dgFloat32)(0.2f / 9.0f) * sm;
		}

		dgVector z (dgVector::m_zero);
		for (dgInt32 ip = 0; ip < 2; ip ++) {
			for (dgInt32 iq = ip + 1; iq < 3; iq ++) {
				dgFloat32 g = dgFloat32 (100.0f) * dgAbsf(mat[ip][iq]);
				if ((i > 3) && ((dgAbsf(d[ip]) + g) == dgAbsf(d[ip])) && ((dgAbsf(d[iq]) + g) == dgAbsf(d[iq]))) {
					mat[ip][iq] = dgFloat32 (0.0f);
				} else if (dgAbsf(mat[ip][iq]) > thresh) {

					dgFloat32 t;
					dgFloat32 h = d[iq] - d[ip];
					if (dgAbsf(h) + g == dgAbsf(h)) {
						t = mat[ip][iq] / h;
					} else {
						dgFloat32 theta = dgFloat32 (0.5f) * h / mat[ip][iq];
						t = dgFloat32(1.0f) / (dgAbsf(theta) + dgSqrt(dgFloat32(1.0f) + theta * theta));
						if (theta < dgFloat32 (0.0f)) {
							t = -t;
						}
					}
					dgFloat32 c = dgRsqrt (dgFloat32 (1.0f) + t * t); 
					dgFloat32 s = t * c; 
					dgFloat32 tau = s / (dgFloat32(1.0f) + c); 
					h = t * mat[ip][iq];
					z[ip] -= h; 
					z[iq] += h; 
					d[ip] -= h; 
					d[iq] += h;
					mat[ip][iq] = dgFloat32(0.0f);

					for (dgInt32 j = 0; j <= ip - 1; j ++) {
						dgFloat32 g = mat[j][ip]; 
						dgFloat32 h = mat[j][iq]; 
						mat[j][ip] = g - s * (h + g * tau); 
						mat[j][iq] = h + s * (g - h * tau);
					}
					for (dgInt32 j = ip + 1; j <= iq - 1; j ++) {
						dgFloat32 g = mat[ip][j]; 
						dgFloat32 h = mat[j][iq]; 
						mat[ip][j] = g - s * (h + g * tau); 
						mat[j][iq] = h + s * (g - h * tau);
					}
					for (dgInt32 j = iq + 1; j < 3; j ++) {
						dgFloat32 g = mat[ip][j]; 
						dgFloat32 h = mat[iq][j]; 
						mat[ip][j] = g - s * (h + g * tau); 
						mat[iq][j] = h + s * (g - h * tau);
					}

					dgVector sv (s);
					dgVector tauv (tau);
					dgVector gv (eigenVectors[ip]);
					dgVector hv (eigenVectors[iq]);
					eigenVectors[ip] -= sv.CompProduct4 (hv + gv.CompProduct4(tauv)); 
					eigenVectors[iq] += sv.CompProduct4 (gv - hv.CompProduct4(tauv));
				}
			}
		}

		b += z; 
		d = b; 
	}

	eigenValues = d;
	*this = eigenVectors;
}
Esempio n. 12
0
void SecondOrderTrustRegion::doOptimize(OptimizationProblemSecond& problem) {
#ifdef NICE_USELIB_LINAL
  bool previousStepSuccessful = true;
  double previousError = problem.objective();
  problem.computeGradientAndHessian();
  double delta = computeInitialDelta(problem.gradientCached(), problem.hessianCached());
  double normOldPosition = 0.0;
  
  // iterate
  for (int iteration = 0; iteration < maxIterations; iteration++) {
//     Log::debug() << "iteration, objective: " << iteration << ", "
//                   << problem.objective() << std::endl;
    
    if (previousStepSuccessful && iteration > 0) {
      problem.computeGradientAndHessian();
    }
  
    // gradient-norm stopping condition
    if (problem.gradientNormCached() < epsilonG) {
      Log::debug() << "SecondOrderTrustRegion stopped: gradientNorm "
                   << iteration << std::endl;
      break;
    }
    
    LinAlVector gradient(problem.gradientCached().linalCol());
    LinAlVector negativeGradient(gradient);
    negativeGradient.multiply(-1.0);

    double lambda;
    int lambdaMinIndex = -1;
    // FIXME will this copy the matrix? no copy needed here!
    LinAlMatrix hessian(problem.hessianCached().linal());
    LinAlMatrix l(hessian);
    try {
      //l.CHdecompose(); // FIXME
      choleskyDecompose(hessian, l);
      
      lambda = 0.0;
    } catch (...) { //(LinAl::BLException& e) { // FIXME
      const LinAlVector& eigenValuesHessian = LinAl::eigensym(hessian);

      // find smallest eigenvalue
      lambda = std::numeric_limits<double>::infinity();
      for (unsigned int i = 0; i < problem.dimension(); i++) {
        const double eigenValue = eigenValuesHessian(i);
        if (eigenValue < lambda) {
          lambda = eigenValue;
          lambdaMinIndex = i;
        }
      }
      const double originalLambda = lambda;
      lambda = -lambda * (1.0 + epsilonLambda);
      
      l = hessian;
      for (unsigned int i = 0; i < problem.dimension(); i++) {
        l(i, i) += lambda;
      }
      try {
        //l.CHdecompose(); // FIXME
        LinAlMatrix temp(l);
        choleskyDecompose(temp, l);
      } catch (...) { // LinAl::BLException& e) { // FIXME
        /*
         * Cholesky factorization failed, which should theortically not be
         * possible (can still happen due to numeric problems,
         * also note that there seems to be a bug in CHdecompose()).
         * Try a really great lambda as last attempt.
         */        
//         lambda = -originalLambda * (1.0 + epsilonLambda * 100.0)
//                  + 2.0 * epsilonM;
        lambda = fabs(originalLambda) * (1.0 + epsilonLambda * 1E5)
                 + 1E3 * epsilonM;
//        lambda = fabs(originalLambda);// * 15.0;
        l = hessian;
        for (unsigned int i = 0; i < problem.dimension(); i++) {
          l(i, i) += lambda;
        }
        try {
          //l.CHdecompose(); // FIXME
          LinAlMatrix temp(l);
          choleskyDecompose(temp, l);
        } catch (...) { // (LinAl::BLException& e) { // FIXME
          // Cholesky factorization failed again, give up.
          l = hessian;
          for (unsigned int i = 0; i < problem.dimension(); i++) {
            l(i, i) += lambda;
          }

          const LinAlVector& eigenValuesL = LinAl::eigensym(l);
                    
          Log::detail()
               << "l.CHdecompose: exception" << std::endl
              //<< e.what() << std::endl // FIXME
               << "lambda=" << lambda << std::endl
               << "l" << std::endl << l
               << "hessian" << std::endl << hessian
               << "gradient" << std::endl << gradient
               << "eigenvalues hessian" << std::endl << eigenValuesHessian
               << "eigenvalues l" << std::endl << eigenValuesL
               << std::endl;
          return;
        }
      }
    }

    // FIXME will the copy the vector? copy is needed here
    LinAlVector step(negativeGradient);
    l.CHsubstitute(step);
    double normStepSquared = normSquared(step);
    double normStep = sqrt(normStepSquared);
    // exact: if normStep <= delta
    if (normStep - delta <= tolerance) {
      // exact: if lambda == 0 || normStep == delta
      if (std::fabs(lambda) < tolerance
          || std::fabs(normStep - delta) < tolerance) {
        // done
      } else {
        LinAlMatrix eigenVectors(problem.dimension(), problem.dimension());
        eigensym(hessian, eigenVectors);
        double a = 0.0;
        double b = 0.0;
        double c = 0.0;
        for (unsigned int i = 0; i < problem.dimension(); i++) {
          const double ui = eigenVectors(i, lambdaMinIndex);
          const double si = step(i);
          a += ui * ui;
          b += si * ui;
          c += si * si;
        }
        b *= 2.0;
        c -= delta * delta;
        const double sq = sqrt(b * b - 4.0 * a * c);
        const double root1 = 0.5 * (-b + sq) / a;
        const double root2 = 0.5 * (-b - sq) / a;
        LinAlVector step1(step);
        LinAlVector step2(step);
        for (unsigned int i = 0; i < problem.dimension(); i++) {
          step1(i) += root1 * eigenVectors(i, lambdaMinIndex);
          step2(i) += root2 * eigenVectors(i, lambdaMinIndex);
        }
        const double psi1
          = dotProduct(gradient, step1)
            + 0.5 * productVMV(step1, hessian, step1);
        const double psi2
          = dotProduct(gradient, step2)
            + 0.5 * productVMV(step2, hessian, step2);
        if (psi1 < psi2) {
          step = step1;
        } else {
          step = step2;
        }
      }
    } else {
      for (unsigned int subIteration = 0; 
           subIteration < maxSubIterations; 
           subIteration++) {
        if (std::fabs(normStep - delta) <= kappa * delta) {
          break;
        }

        // NOTE specialized algorithm may be more effifient than solvelineq
        //      (l is lower triangle!)
        // Only lower triangle values of l are valid (other implicitly = 0.0),
        // but solvelineq doesn't know that -> explicitly set to 0.0
        for (int i = 0; i < l.rows(); i++) {
          for (int j = i + 1; j < l.cols(); j++) {
            l(i, j) = 0.0;
          }
        }
        LinAlVector y(step.size());
        try {
          y = solvelineq(l, step);
        } catch (LinAl::Exception& e) {
          // FIXME if we end up here, something is pretty wrong!
          // give up the whole thing
          Log::debug() << "SecondOrderTrustRegion stopped: solvelineq failed "
                       << iteration << std::endl;
          return;
        }
        
        lambda += (normStep - delta) / delta
                  * normStepSquared / normSquared(y);
        l = hessian;
        for (unsigned int i = 0; i < problem.dimension(); i++) {
          l(i, i) += lambda;
        }
        try {
          //l.CHdecompose(); // FIXME
          LinAlMatrix temp(l);
          choleskyDecompose(temp, l);
        } catch (...) { // (LinAl::BLException& e) { // FIXME
          Log::detail()
               << "l.CHdecompose: exception" << std::endl
               // << e.what() << std::endl // FIXME
               << "lambda=" << lambda << std::endl
               << "l" << std::endl << l
               << std::endl;
          return;
        }
        step = negativeGradient;
        l.CHsubstitute(step);
        normStepSquared = normSquared(step);
        normStep = sqrt(normStepSquared);
      }
    }
    
    // computation of step is complete, convert to NICE::Vector
    Vector stepLimun(step);
    
    // minimal change stopping condition
    if (changeIsMinimal(stepLimun, problem.position())) {
      Log::debug() << "SecondOrderTrustRegion stopped: change is minimal "
                   << iteration << std::endl;
      break;
    }
  
    if (previousStepSuccessful) {
      normOldPosition = problem.position().normL2();
    }

    // set new region parameters (to be verified later)
    problem.applyStep(stepLimun);
    
    // compute reduction rate
    const double newError = problem.objective();
//Log::debug() << "newError: " << newError << std::endl;
    const double errorReduction = newError - previousError;
    const double psi = problem.gradientCached().scalarProduct(stepLimun)
                       + 0.5 * productVMV(step, hessian, step);
    double rho;
    if (std::fabs(psi) <= epsilonRho
        && std::fabs(errorReduction) <= epsilonRho) {
      rho = 1.0;
    } else {
      rho = errorReduction / psi;
    }

    // NOTE psi and errorReduction checks added to the algorithm 
    //      described in Ferid Bajramovic's Diplomarbeit
    if (rho < eta1 || psi >= 0.0 || errorReduction > 0.0) {
      previousStepSuccessful = false;
      problem.unapplyStep(stepLimun);
      delta = alpha2 * normStep;
    } else {
      previousStepSuccessful = true;
      previousError = newError;
      if (rho >= eta2) {
        const double newDelta = alpha1 * normStep;
        if (newDelta > delta) {
          delta = newDelta;
        }
      } // else: don't change delta
    }
    
    // delta stopping condition
    if (delta < epsilonDelta * normOldPosition) {
      Log::debug() << "SecondOrderTrustRegion stopped: delta too small "
                   << iteration << std::endl;
      break;
    }
  }
#else // no linal
  fthrow(Exception,
         "SecondOrderTrustRegion needs LinAl. Please recompile with LinAl");
#endif
}