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

void KF_joseph_update(VectorXf &x, MatrixXf &P,float v,float R, MatrixXf H)
{
    VectorXf PHt = P*H.transpose();
    MatrixXf S = H*PHt;
    S(0,0) += R;
    MatrixXf Si = S.inverse();
    Si = make_symmetric(Si);
    MatrixXf PSD_check = Si.llt().matrixL(); //chol of scalar is sqrt
    PSD_check.transpose();
    PSD_check.conjugate();

    VectorXf W = PHt*Si;
    x = x+W*v;
    
    //Joseph-form covariance update
    MatrixXf eye(P.rows(), P.cols());
    eye.setIdentity();
    MatrixXf C = eye - W*H;
    P = C*P*C.transpose() + W*R*W.transpose();  

    float eps = 2.2204*pow(10.0,-16); //numerical safety 
    P = P+eye*eps;

    PSD_check = P.llt().matrixL();
    PSD_check.transpose();
    PSD_check.conjugate(); //for upper tri
}

VectorXf param_sensitivity_widget::computeSensitivity(
    MatrixXf &parameterMatrix, VectorXf &responseVector)
{
    MatrixXf Ctemp = parameterMatrix.transpose()*parameterMatrix;
    MatrixXf C;
    C = Ctemp.inverse();

    VectorXf b = C*parameterMatrix.transpose()*responseVector;

    VectorXf Y_hat = parameterMatrix*b;

    int p = b.rows();

    VectorXf sigma2Vec = responseVector-Y_hat;

    float sigma2 = sigma2Vec.squaredNorm();
    sigma2= sigma2/(parameterMatrix.rows() - p);

    Ctemp = C*sigma2;

    MatrixXf denominator = Ctemp.diagonal();

    // Do element-wise division
    VectorXf t = b;
    for (int i = 0; i < b.rows(); i++)
    {
        t(i) = abs(b(i)/sqrt(denominator(i)));
    }

    return t;
}

MatrixXf Arm::pseudo_inverse() {
    MatrixXf Jacovian = compute_Jacobian();
    MatrixXf jjtInv = (Jacovian * Jacovian.transpose());
    jjtInv = jjtInv.inverse();
    
    return (Jacovian.transpose() * jjtInv);
}

void call_ref()
{
  VectorXcf ca  = VectorXcf::Random(10);
  VectorXf a    = VectorXf::Random(10);
  RowVectorXf b = RowVectorXf::Random(10);
  MatrixXf A    = MatrixXf::Random(10,10);
  RowVector3f c = RowVector3f::Random();
  const VectorXf& ac(a);
  VectorBlock<VectorXf> ab(a,0,3);
  const VectorBlock<VectorXf> abc(a,0,3);
  

  VERIFY_EVALUATION_COUNT( call_ref_1(a,a), 0);
  VERIFY_EVALUATION_COUNT( call_ref_1(b,b.transpose()), 0);
//   call_ref_1(ac,a<c);           // does not compile because ac is const
  VERIFY_EVALUATION_COUNT( call_ref_1(ab,ab), 0);
  VERIFY_EVALUATION_COUNT( call_ref_1(a.head(4),a.head(4)), 0);
  VERIFY_EVALUATION_COUNT( call_ref_1(abc,abc), 0);
  VERIFY_EVALUATION_COUNT( call_ref_1(A.col(3),A.col(3)), 0);
//   call_ref_1(A.row(3),A.row(3));    // does not compile because innerstride!=1
  VERIFY_EVALUATION_COUNT( call_ref_3(A.row(3),A.row(3).transpose()), 0);
  VERIFY_EVALUATION_COUNT( call_ref_4(A.row(3),A.row(3).transpose()), 0);
//   call_ref_1(a+a, a+a);          // does not compile for obvious reason

  MatrixXf tmp = A*A.col(1);
  VERIFY_EVALUATION_COUNT( call_ref_2(A*A.col(1), tmp), 1);     // evaluated into a temp
  VERIFY_EVALUATION_COUNT( call_ref_2(ac.head(5),ac.head(5)), 0);
  VERIFY_EVALUATION_COUNT( call_ref_2(ac,ac), 0);
  VERIFY_EVALUATION_COUNT( call_ref_2(a,a), 0);
  VERIFY_EVALUATION_COUNT( call_ref_2(ab,ab), 0);
  VERIFY_EVALUATION_COUNT( call_ref_2(a.head(4),a.head(4)), 0);
  tmp = a+a;
  VERIFY_EVALUATION_COUNT( call_ref_2(a+a,tmp), 1);            // evaluated into a temp
  VERIFY_EVALUATION_COUNT( call_ref_2(ca.imag(),ca.imag()), 1);      // evaluated into a temp

  VERIFY_EVALUATION_COUNT( call_ref_4(ac.head(5),ac.head(5)), 0);
  tmp = a+a;
  VERIFY_EVALUATION_COUNT( call_ref_4(a+a,tmp), 1);           // evaluated into a temp
  VERIFY_EVALUATION_COUNT( call_ref_4(ca.imag(),ca.imag()), 0);

  VERIFY_EVALUATION_COUNT( call_ref_5(a,a), 0);
  VERIFY_EVALUATION_COUNT( call_ref_5(a.head(3),a.head(3)), 0);
  VERIFY_EVALUATION_COUNT( call_ref_5(A,A), 0);
//   call_ref_5(A.transpose(),A.transpose());   // does not compile because storage order does not match
  VERIFY_EVALUATION_COUNT( call_ref_5(A.block(1,1,2,2),A.block(1,1,2,2)), 0);
  VERIFY_EVALUATION_COUNT( call_ref_5(b,b), 0);             // storage order do not match, but this is a degenerate case that should work
  VERIFY_EVALUATION_COUNT( call_ref_5(a.row(3),a.row(3)), 0);

  VERIFY_EVALUATION_COUNT( call_ref_6(a,a), 0);
  VERIFY_EVALUATION_COUNT( call_ref_6(a.head(3),a.head(3)), 0);
  VERIFY_EVALUATION_COUNT( call_ref_6(A.row(3),A.row(3)), 1);           // evaluated into a temp thouth it could be avoided by viewing it as a 1xn matrix
  tmp = A+A;
  VERIFY_EVALUATION_COUNT( call_ref_6(A+A,tmp), 1);                // evaluated into a temp
  VERIFY_EVALUATION_COUNT( call_ref_6(A,A), 0);
  VERIFY_EVALUATION_COUNT( call_ref_6(A.transpose(),A.transpose()), 1);      // evaluated into a temp because the storage orders do not match
  VERIFY_EVALUATION_COUNT( call_ref_6(A.block(1,1,2,2),A.block(1,1,2,2)), 0);
  
  VERIFY_EVALUATION_COUNT( call_ref_7(c,c), 0);
}

FeatureModel::FeatureModel(	const CamModel & _camera,
								const MotionModel & _motion,
								const FeaturesState & _featuresState,
								const Mat & frame,
								const Point2f & pf,
								int patchSize,
								float rho_0,
								float sigma_rho):
								camera(_camera),
								motion(_motion),
								features(_featuresState)
	{
    Mat newPatch(cv::Mat(frame, cv::Rect(pf.x-patchSize/2, pf.y-patchSize/2, patchSize,patchSize)));
    this->imageLocation << pf.x, pf.y;

    this->Patch = newPatch.clone();


    Vector2f hd = (Vector2f << (float)pf.x, (float)pf.y);

    // TODO: convert using motionmodel
    Vector3f r = motion->get_r();
    Quatenionf q = motion->get_q();


    Vector3f hC = this->camera.unproject(hd, true);
    Matrix3f Jac_hCHd = this->camera.getUnprojectionJacobian();

    Matrix3f Rot = quat2rot(q);

    Vector3f hW = Rot*hC;

    float hx = hW(0);
    float hy = hW(1);
    float hz = hW(2);



    float theta = atan2(hx,hz);
    float phi = atan2(-hy, sqrt(hx*hx+hz*hz));

	// Updating state and Sigma
    MatrixXf Jx = this->computeJx();
	MatrixXf Jm = this->computeJm();
	Matrix2f Sigma_mm = this->computeSigma_mm(sigma_pixel, sigma_pixel);

	this->f = (VectorXf(6) << r, theta, phi, rho_0);
	this->Sigma_ii = Jm*Sigma_mm*Jm.transpose() + Jx*motion->getSigma_xx()*Jx.transpose();

	this->motion->updateVariaceBlocks(Jx);
	this->features->updateVariaceBlocks(Jx);
	this->features.push_back((*this));
    return 1;

}

MatrixXf LinkedStructure::pseudoInverse()
{
  // Simple math that represents the mathematics
  // explained on the website to computing the
  // pseudo inverse. this is exactly the math
  // discussed in the tutorial!!!
    MatrixXf j = jacobian();
    MatrixXf jjtInv = (j * j.transpose());
    jjtInv = jjtInv.inverse();

    return (j.transpose() * jjtInv);
}

void Lu::Initialize(MatrixXf x3d_h,MatrixXf x2d_h,  Matrix3f A) {
 

 

 x2d=x2d_h.transpose();
  // std::cout<<"x2dtrasn"<<x2d<<std::endl;
 x3d=x3d_h.transpose();
 //std::cout<<"x3dtrasn"<<x3d<<std::endl;

 x2dn=A.inverse ()*x2d;
 //std::cout<<"x2dn"<<x2dn<<std::endl;
 K=A;
  //std::cout<<"Intialize points "<<std::endl; 
}

void multi(dym tdim,MatrixXf &c)
{
 	 if(dim==tdim)
 	     data=data*c.transpose();
     else
         rotate_top(tdim),data=c*data;
}

void computeHomographyResidue(MatrixXf pts1, MatrixXf pts2, const Matrix3f &H, MatrixXf &residue){
  // cross residue
  filterPointAtInfinity(pts1, pts2);
  residue.resize(pts1.rows(), 1);
  MatrixXf Hx1 = (H*pts1.transpose()).transpose();
  MatrixXf invHx2 = (H.inverse()*pts2.transpose()).transpose();

  noHomogeneous(Hx1);
  noHomogeneous(invHx2);
  noHomogeneous(pts1);
  noHomogeneous(pts2);

  MatrixXf diffHx1pts2 = Hx1 - pts2;
  MatrixXf diffinvHx2pts1 = invHx2 - pts1;
  residue = diffHx1pts2.rowwise().squaredNorm() + diffinvHx2pts1.rowwise().squaredNorm();
}

void TestLeastSquares() {
	MatrixXf A = MatrixXf::Random(10, 2);
	VectorXf b = VectorXf::Random(10);
	Vector2f x;

	std::cout << "=============================" << std::endl;
	std::cout << "Testing least squares solvers" << std::endl;
	std::cout << "=============================" << std::endl;

	x = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
	std::cout << "Solution using Jacobi SVD = " << x.transpose() << std::endl;

	x = A.colPivHouseholderQr().solve(b);
	std::cout << "Solution using column pivoting Householder QR = " << x.transpose() << std::endl;

	// If the matrix A is ill-conditioned, then this is not a good method
	x = (A.transpose() * A).ldlt().solve(A.transpose() * b);
	std::cout << "Solution using normal equation = " << x.transpose() << std::endl;
}

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

VectorXf project1D( const RMatrixXf & Y, int * rep_label=NULL ) {
// 	const int MAX_SAMPLE = 20000;
	const bool fast = true, very_fast = true;
	// Remove the DC (Y : N x M)
	RMatrixXf dY = Y.rowwise() - Y.colwise().mean();
// 	RMatrixXf sY = dY;
// 	if( 0 < MAX_SAMPLE && MAX_SAMPLE < dY.rows() ) {
// 		VectorXi samples = randomChoose( dY.rows(), MAX_SAMPLE );
// 		std::sort( samples.data(), samples.data()+samples.size() );
// 		sY = RMatrixXf( samples.size(), dY.cols() );
// 		for( int i=0; i<samples.size(); i++ )
// 			sY.row(i) = dY.row( samples[i] );
// 	}
	
	// ... and use (pc > 0)
	VectorXf lbl = VectorXf::Zero( Y.rows() );
	
	// Find the largest PC of (dY.T * dY) and project onto it
	if( very_fast ) {
		// Find the largest PC using poweriterations
		VectorXf U = VectorXf::Random( dY.cols() );
		U = U.array() / U.norm()+std::numeric_limits<float>::min();
		for( int it=0; it<20; it++ ){
			// Normalize
			VectorXf s = dY.transpose()*(dY*U);
			s.array() /= s.norm()+std::numeric_limits<float>::min();
			if ( (U-s).norm() < 1e-6 )
				break;
			U = s;
		}
		// Project onto the PC
		lbl = dY*U;
	}
	else if(fast) {
		// Compute the eigen values of the covariance (and project onto the largest eigenvector)
		MatrixXf cov = dY.transpose()*dY;
		SelfAdjointEigenSolver<MatrixXf> eigensolver(0.5*(cov+cov.transpose()));
		MatrixXf ev = eigensolver.eigenvectors();
		lbl = dY * ev.col( ev.cols()-1 );
	}
	else {
		// Use the SVD
		JacobiSVD<RMatrixXf> svd = dY.jacobiSvd(ComputeThinU | ComputeThinV );
		// Project onto the largest PC
		lbl = svd.matrixU().col(0) * svd.singularValues()[0];
	}
	// Find the representative label
	if( rep_label )
		dY.array().square().rowwise().sum().minCoeff( rep_label );
	
	return (lbl.array() < 0).cast<float>();
}

Image smooth_detector(const Image& source, Interpolation level, int r) {
  Image output(source.rows(), source.columns(), 1, numeric_limits<float>::max());
  const MatrixXf reg_matrix = ComputeRegMatrix(level, r);
  const LDLT<MatrixXf> solver = (reg_matrix.transpose() * reg_matrix).ldlt();
  for (int pr = 0; pr <= source.rows() - r; ++pr) {
    for (int pc = 0; pc <= source.columns() - r; ++pc) {
      VectorXf dist = VectorXf::Zero(r * r);
      for (int ch = 0; ch < source.channels(); ++ch) {
        EigenImage y = ExtractPatch(source, r, pr, pc, ch);
        VectorXf reg_surf = solver.solve(reg_matrix.transpose() * y.asvector());
        dist += (reg_matrix * reg_surf - y.asvector()).cwiseAbs2();
      }
      dist = dist.cwiseSqrt();
      for (int row = pr; row < min(output.rows(), pr + r); ++row) {
        for (int col = pc; col < min(output.columns(), pc + r); ++col) {
          output.val(col, row) = min(output.val(col, row), dist((row - pr) * r + col - pc));
        }
      }
    }
  }
  return output;
}

int main(int, char**)
{
  cout.precision(3);
  SelfAdjointEigenSolver<MatrixXf> es(4);
MatrixXf X = MatrixXf::Random(4,4);
MatrixXf A = X + X.transpose();
es.compute(A);
cout << "The eigenvalues of A are: " << es.eigenvalues().transpose() << endl;
es.compute(A + MatrixXf::Identity(4,4)); // re-use es to compute eigenvalues of A+I
cout << "The eigenvalues of A+I are: " << es.eigenvalues().transpose() << endl;

  return 0;
}

// todo: some weighting scheme
SparseArray calcCorrNN(const vector<btVector3>& estPts, const vector<btVector3>& obsPts, const vector<float>& pVis) {
  int nEst = estPts.size();
  int nObs = obsPts.size();
  float r = (float)nObs/nEst;
  SparseArray out(nEst);
  MatrixXf distsEstObs = pairwiseSquareDist(toEigenMatrix(estPts), toEigenMatrix(obsPts));
  vector<int> estToObs = argminAlongRows(distsEstObs);
  vector<int> obsToEst = argminAlongRows(distsEstObs.transpose());
  for (int iEst=0; iEst<nEst; iEst++) 
    out[iEst].push_back(IndVal(estToObs[iEst],.5*pVis[iEst]));
  for (int iObs=0; iObs<nObs; iObs++) out[obsToEst[iObs]].push_back(IndVal(iObs,.5/r));
  return out;
}