C++ MatrixXf::transpose方法代码示例

bool FirthRegression::FitFirthModel(Matrix & X, Vector & y, int nrrounds)
{
  this-> Reset(X);

  G_to_Eigen(X, &this->w->X);
  G_to_Eigen(y, &this->w->y);

  int rounds = 0;
  // double lastDeviance, currentDeviance;
  Eigen::MatrixXf xw; // W^(1/2) * X
  // Newton-Raphson  
  while (rounds < nrrounds) {
    // std::cout << "beta = " << this->w->beta << "\n";
    this->w->eta = this->w->X * this->w->beta;
    this->w->p = (1.0 + (-this->w->eta.array()).exp()).inverse();
    this->w->V = this->w->p.array() * (1.0 - this->w->p.array());

    xw = (this->w->V.array().sqrt().matrix().asDiagonal() * this->w->X).eval(); // W^(1/2) * X 
    this->w->D = xw.transpose() * xw; // X' V X
    this->w->covB = this->w->D.eval().ldlt().solve(Eigen::MatrixXf::Identity(this->w->D.rows(), this->w->D.rows()));

    // double rel = ((this->w->D * this->w->covB).array() - Eigen::MatrixXf::Identity(this->w->D.rows(), this->w->D.rows()).array()).matrix().norm() / this->w->D.rows() / this->w->D.rows();
    // // printf("norm = %g\n", rel);
    // if (rel > 1e-6) { // use relative accuracy to evalute convergence
    if ((this->w->D * this->w->covB - Eigen::MatrixXf::Identity(this->w->D.rows(), this->w->D.rows())).norm() > 1e-3) {      
      // cannot inverse
      // printToFile(this->w->D, "matD", "D");
      // printToFile(this->w->covB, "matCovB", "B");
      return false;
    }
    this->w->h = (xw * this->w->covB * xw.transpose()).diagonal();
    this->w->r = this->w->X.transpose() * (this->w->y - this->w->p + (this->w->h.array() * (0.5 - this->w->p.array())).matrix()); // X' (y-mu)
    this->w->delta_beta = this->w->covB * this->w->r;
    this->w->beta += this->w->delta_beta;
    // printf("norm = %g\n", this->w->delta_beta.norm());
    // use relative accuracy to evalute convergence
    if (rounds > 1 && (this->w->beta.norm() > 0 && this->w->delta_beta.norm() / this->w->beta.norm() < 1e-6)) {
      rounds = 0;
      break;
    }
    rounds ++;
  }
  if (rounds == nrrounds)
  {
    printf("Not enough iterations!");
    return false;
  }
  // this->w->covB = this->w->D.eval().llt().solve(Eigen::MatrixXf::Identity(this->w->D.rows(), this->w->D.rows()));

  Eigen_to_G(this->w->beta, &B);
  Eigen_to_G(this->w->covB, &covB);
  Eigen_to_G(this->w->p, &p);
  Eigen_to_G(this->w->V, &V);

  return true;
}

 void getBetaSigma2(double delta) {
   Eigen::MatrixXf x = (this->lambda.array() + delta).sqrt().matrix().asDiagonal() * this->ux;
   Eigen::MatrixXf y = (this->lambda.array() + delta).sqrt().matrix().asDiagonal() * this->uy;
   this->beta = (x.transpose() * x).eval().ldlt().solve(x.transpose() * y);
   double sumResidual2 = getSumResidual2(delta);
   // if ( model == GrammarGamma::MLE) {
   this->sigma2_g = sumResidual2 / x.rows();
   // } else {
   //   this->sigma2 = sumResidual2 / (x.rows() - x.cols());
   // }
 }

 // NOTE: need to fit null model fit before calling this function
 double GetAF(const EigenMatrix& kinshipU, const EigenMatrix& kinshipS) const{
   const Eigen::MatrixXf& U = kinshipU.mat;
   Eigen::MatrixXf u1 = Eigen::MatrixXf::Ones(U.rows(), 1);
   u1 = U.transpose() *  u1;
   Eigen::MatrixXf x = (this->lambda.array() + delta).sqrt().matrix().asDiagonal() * u1;
   Eigen::MatrixXf y = (this->lambda.array() + delta).sqrt().matrix().asDiagonal() * this->ug;
   Eigen::MatrixXf beta = (x.transpose() * x).inverse() * x.transpose() * y;
   // here x is represented as 0, 1, 2, so beta(0, 0) is the mean genotype
   // multiply by 0.5 to get AF
   double af = 0.5 * beta(0, 0);
   return af;
 }

// Perform a linear regression on the power ratio in each order
// Omit l=2 - tends to be abnormally small due to non-isotropic brain-wide fibre distribution
std::pair<float, float> get_regression (const std::vector<float>& ratios)
{
  const size_t n = ratios.size() - 1;
  Eigen::VectorXf Y (n), b (2);
  Eigen::MatrixXf A (n, 2);
  for (size_t i = 1; i != ratios.size(); ++i) {
    Y[i-1] = ratios[i];
    A(i-1,0) = 1.0f;
    A(i-1,1) = (2*i)+2;
  }
  b = (A.transpose() * A).ldlt().solve (A.transpose() * Y);
  return std::make_pair (b[0], b[1]);
}

//params is a matrix of nx2 where n is the number of landmarks
//for each landmark, the x and y pose of where it is
//pose is a matrix of 2x1 containing the initial guess of the robot pose
//delta is a matrix of 2x1 returns the increment in the x and y of the robot
void LMAlgr::computeIncrement(Eigen::MatrixXf params, Eigen::MatrixXf pose, double lambda, Eigen::MatrixXf error, Eigen::MatrixXf &delta){
	Eigen::MatrixXf Jac;
	Jac.resize(params.rows(), 2);
	//initialize the jacobian matrix
	for(int i = 0; i < params.rows(); i++){
		Jac(i, 0) = (params(i, 1) - pose(1, 0)) / (pow(params(i, 0) - pose(0, 0), 2) + pow(params(i, 1) - pose(1, 0), 2));
		Jac(i, 1) = -1 * (params(i, 0) - pose(0, 0)) / (pow(params(i, 0) - pose(0, 0), 2) + pow(params(i, 1) - pose(1, 0), 2));
	}
	Eigen::MatrixXf I;
	I = Eigen::MatrixXf::Identity(2, 2);
	Eigen::MatrixXf tmp = (Jac.transpose() * Jac) + (lambda * I);
	Eigen::MatrixXf incr = tmp.inverse() * Jac.transpose() * error;
	delta = incr;
}

float pseudo_inv(const Eigen::MatrixXf *mat_in,
                 Eigen::MatrixXf *mat_out) {
  int dim = 0;

  // Get matrices dimension :
  if (mat_in->cols () != mat_in->rows ()) {
    THROW_ERR("Cannot compute matrix pseudo_inverse");
  } else {
    dim = mat_in->cols ();
  }

  mat_out->resize (dim, dim);

  Eigen::MatrixXf U (dim,dim);
  Eigen::MatrixXf eig_val (dim, 1);
  Eigen::MatrixXf eig_val_inv (dim, dim);
  Eigen::MatrixXf V (dim, dim);

  float det;

  eig_val_inv = Eigen::MatrixXf::Identity(dim,dim);

  // Compute the SVD decomposition
  Eigen::JacobiSVD<Eigen::MatrixXf> svd(*mat_in, Eigen::ComputeFullU | Eigen::ComputeFullV);

  eig_val = svd.singularValues();
  U = svd.matrixU();
  V = svd.matrixV();

  // Compute pseudo-inverse
  // - quick'n'dirty inversion of eigen matrix
  for (int i = 0; i<dim; ++i) {
    if (eig_val(i,0) != 0.f)
      eig_val_inv(i,i) = 1.f / eig_val(i,0);
    else
      eig_val_inv(i,i) = 0.f;
  }

  *mat_out = V.transpose() * eig_val_inv * U.transpose();

  // Compute determinant from eigenvalues..
  det = 1.f;
  for (int i=0; i<dim; ++i) {
    det *= eig_val(i,0);
  }

  return det;
}

	void gaussNewtonFromSamplesWeighed(const Eigen::VectorXf &xb, const Eigen::VectorXf &rb, const Eigen::MatrixXf &X, const Eigen::VectorXf &weights, const Eigen::VectorXf &residuals, float regularization, Eigen::VectorXf &out_result)
	{
		//Summary:
		//out_result=xb - G rb
		//xb is the best sample, rb is the best sample residual vector
		//G=AB'inv(BB'+kI)
		//A.col(i)=weights[i]*(X.row(i)-best sample)'
		//B.col(i)=weights[i]*(residuals - rb)'
		//k=regularization

		//Get xb, r(xb)
		//cv::Mat xb=X.row(bestIndex);
		//cv::Mat rb=residuals.row(bestIndex);

		//Compute A and B
		MatrixXf A=X.transpose();
		MatrixXf B=residuals.transpose();
		for (int i=0; i<A.cols(); i++)
		{
			A.col(i)=weights[i]*(X.row(i).transpose()-xb);
			B.col(i)=weights[i]*(residuals.row(i).transpose()-rb);
		}
		MatrixXf I=MatrixXf::Identity(B.rows(),B.rows());
		I=I*regularization;
		MatrixXf G=(A*B.transpose())*(B*B.transpose()+I).inverse();
		out_result=xb - G * rb;
	}

Eigen::MatrixXf PhotoCamera::getPseudoInverse()
{
    Eigen::MatrixXf P = intrinsicMatrix*extrinsicMatrix.matrix();
    Eigen::MatrixXf Pt = P.transpose();
    Eigen::MatrixXf pseudoInverse = Pt*(P*Pt).inverse();
    return pseudoInverse;
}

Eigen::MatrixXf JacobiProvider::computePseudoInverseJacobianMatrix(const Eigen::MatrixXf &m) const
{
#ifdef CHECK_PERFORMANCE
	clock_t startT = clock();
#endif
	MatrixXf pseudo;
	switch (inverseMethod)
	{
	case eTranspose:
		{
			if (jointWeights.rows() == m.cols())
			{
				Eigen::MatrixXf W = jointWeights.asDiagonal();
				Eigen::MatrixXf W_1 = W.inverse();
				pseudo = W_1 * m.transpose() * (m*W_1*m.transpose()).inverse();
			}
			else
			{
				pseudo = m.transpose() * (m*m.transpose()).inverse();
			}
			break;
		}
	case eSVD:
		{
				 float pinvtoler = 0.00001f;
				 pseudo = MathTools::getPseudoInverse(m, pinvtoler);
				 break;
		}
	case eSVDDamped:
		{
				 float pinvtoler = 0.00001f;
				 pseudo = MathTools::getPseudoInverseDamped(m,pinvtoler);
				 break;
		}
	default:
		THROW_VR_EXCEPTION("Inverse Jacobi Method nyi...");
	}
#ifdef CHECK_PERFORMANCE
	clock_t endT = clock();
	float diffClock = (float)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0f);
	//if (diffClock>10.0f)
	cout << "Inverse Jacobi time:" << diffClock << endl;
#endif
	return pseudo;
}

tf::Transform SvdTransformOptimization::svdOwnImpl(
		std::vector<tf::Vector3> pointcloudX,
		std::vector<tf::Vector3> pointcloudP) {

	// Calculate center of mass for both pointclouds.
	int numOfPoints = pointcloudX.size();
	tf::Vector3 centerOfMassX, centerOfMassP;
	for (int i = 0; i < numOfPoints; i++) {
		centerOfMassX += pointcloudX[i];
		centerOfMassP += pointcloudP[i];
	}
	centerOfMassX /= numOfPoints;
	centerOfMassP /= numOfPoints;

	// Extract the center of mass from the corresponding points.
	std::vector<tf::Vector3> pointcloudXPrime, pointcloudPPrime;
	for (int i = 0; i < numOfPoints; i++) {
		pointcloudXPrime.push_back(pointcloudX[i] - centerOfMassX);
		pointcloudPPrime.push_back(pointcloudP[i] - centerOfMassP);
	}

	// Calculate matrix W
	Eigen::MatrixXf W = Eigen::MatrixXf::Zero(3, 3);
	for (int i = 0; i < numOfPoints; i++) {
		Eigen::Vector3f currentPointXPrime(pointcloudXPrime[i].getX(),
				pointcloudXPrime[i].getY(), pointcloudXPrime[i].getZ());
		Eigen::Vector3f currentPointPPrime(pointcloudPPrime[i].getX(),
				pointcloudPPrime[i].getY(), pointcloudPPrime[i].getZ());
		W += currentPointXPrime * currentPointPPrime.transpose();
	}

	// Perform the SVD.
	Eigen::JacobiSVD<Eigen::MatrixXf> svd(W);
	svd.compute(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
	Eigen::MatrixXf U = svd.matrixU();
	Eigen::MatrixXf V = svd.matrixV();

	// Caclulate Rotation and translation and convert to tf.
	Eigen::MatrixXf R = U * V.transpose();
	Eigen::Vector3f centerOfMassXEigen(centerOfMassX.getX(),
			centerOfMassX.getY(), centerOfMassX.getZ());
	Eigen::Vector3f centerOfMassPEigen(centerOfMassP.getX(),
			centerOfMassP.getY(), centerOfMassP.getZ());
	Eigen::MatrixXf t = centerOfMassXEigen - R * (centerOfMassPEigen);

	tf::Matrix3x3 Rtf(R(0, 0), R(0, 1), R(0, 2), R(1, 0), R(1, 1), R(1, 2),
			R(2, 0), R(2, 1), R(2, 2));
	tf::Vector3 ttf(t(0), t(1), t(2));

	// Create and return the new transform.
	tf::Transform newTransform(Rtf, ttf);
	return newTransform;
}

	virtual Eigen::VectorXf gradient( const Eigen::MatrixXf & a, const Eigen::MatrixXf & b ) const {
		if (ktype_ == CONST_KERNEL)
			return Eigen::VectorXf();
		Eigen::MatrixXf fg = featureGradient( a, b );
		if (ktype_ == DIAG_KERNEL)
			return (f_.array()*fg.array()).rowwise().sum();
		else {
			Eigen::MatrixXf p = fg*f_.transpose();
			p.resize( p.cols()*p.rows(), 1 );
			return p;
		}
	}

	void calcMeanAndCovarWeighedVectorized(const Eigen::MatrixXf &input, const Eigen::VectorXd &inputWeights, Eigen::MatrixXf &out_covMat, Eigen::VectorXf &out_mean,Eigen::MatrixXf &temp)
	{
		out_mean=input.col(0); //to resize
		out_mean.setZero();
		double wSumInv=1.0/inputWeights.sum();
		for (int k=0;k<inputWeights.size();k++){
			double w=inputWeights[k];
			out_mean+=input.col(k)*(float)(w*wSumInv);
		}
		out_mean = input.rowwise().mean();
		temp = (input.colwise() - out_mean);
		for (int k=0;k<inputWeights.size();k++){
			temp.col(k) *= (float)(sqrt(inputWeights(k)*wSumInv));	//using square roots, as we only want the normalized weights to be included once for each result element in the multiplication below
		}
		out_covMat = temp*temp.transpose();
	}

/**
 * estimateRigidTransformationSVD
 */
void RigidTransformationRANSAC::estimateRigidTransformationSVD(
      const std::vector<Eigen::Vector3f > &srcPts,
      const std::vector<int> &srcIndices,
      const std::vector<Eigen::Vector3f > &tgtPts,
      const std::vector<int> &tgtIndices,
      Eigen::Matrix4f &transform)
{
  Eigen::Vector3f srcCentroid, tgtCentroid;

  // compute the centroids of source, target
  ComputeCentroid (srcPts, srcIndices, srcCentroid);
  ComputeCentroid (tgtPts, tgtIndices, tgtCentroid);

  // Subtract the centroids from source, target
  Eigen::MatrixXf srcPtsDemean;
  DemeanPoints(srcPts, srcIndices, srcCentroid, srcPtsDemean);

  Eigen::MatrixXf tgtPtsDemean;
  DemeanPoints(tgtPts, tgtIndices, tgtCentroid, tgtPtsDemean);

  // Assemble the correlation matrix H = source * target'
  Eigen::Matrix3f H = (srcPtsDemean * tgtPtsDemean.transpose ()).topLeftCorner<3, 3>();

  // Singular Value Decomposition
  Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3f u = svd.matrixU ();
  Eigen::Matrix3f v = svd.matrixV ();

  // Compute R = V * U'
  if (u.determinant () * v.determinant () < 0)
  {
    for (int x = 0; x < 3; ++x)
      v (x, 2) *= -1;
  }

  // Return the transformation
  Eigen::Matrix3f R = v * u.transpose ();
  Eigen::Vector3f t = tgtCentroid - R * srcCentroid;

  // set rotation
  transform.block(0,0,3,3) = R;
  // set translation
  transform.block(0,3,3,1) = t;
  transform.block(3, 0, 1, 3).setZero();  
  transform(3,3) = 1.;
}

  void run(Mat& A, const int rank){
    if (A.cols() == 0 || A.rows() == 0) return;
    int r = (rank < A.cols()) ? rank : A.cols();
    r = (r < A.rows()) ? r : A.rows();
    
    // Gaussian Random Matrix
    Eigen::MatrixXf O(A.rows(), r);
    Util::sampleGaussianMat(O);
    
    // Compute Sample Matrix of A
    Eigen::MatrixXf Y = A.transpose() * O;
    
    // Orthonormalize Y
    Util::processGramSchmidt(Y);

    Eigen::MatrixXf B = Y.transpose() * A * Y;
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigenOfB(B);
    
    eigenValues_ = eigenOfB.eigenvalues();
    eigenVectors_ = Y * eigenOfB.eigenvectors();
  }

  int TestCovariate(Matrix& Xnull, Matrix& Y, Matrix& Xcol,
                    const EigenMatrix& kinshipU, const EigenMatrix& kinshipS){
    Eigen::MatrixXf g;
    G_to_Eigen(Xcol, &g);

    // store U'*G for computing AF later.
    const Eigen::MatrixXf& U = kinshipU.mat;
    this->ug = U.transpose() * g;

    Eigen::RowVectorXf g_mean = g.colwise().mean();
    g = g.rowwise() - g_mean;

    double gTg = g.array().square().sum();
    double t_new = (g.array() * this->transformedY.array()).sum();
    t_new = t_new * t_new / gTg;
    double t_score = t_new / this->gamma;
    this->betaG = (g.transpose() * this->transformedY).sum() / gTg / this->gamma;
    this->betaGVar = this->ySigmaY / gTg / this->gamma;

    this->pvalue = gsl_cdf_chisq_Q(t_score, 1.0);
    return 0;
  }