C++ MatrixXd::inverse方法代码示例

double weight_gaussian_predictive_rev(Gaussian &g1, Gaussian &g2)
{
    int dim = g1.dim;
    double energy1 = 0.;
            Eigen::MatrixXd cov = g1.predictive_covariance + g1.predictive_covariance;
            Eigen::VectorXd mean = g1.predictive_mean - g1.predictive_mean;
            Eigen::MatrixXd invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy1 += gauss;

    double energy2 = 0.;

            cov = g1.predictive_covariance + g2.predictive_covariance;
            mean = g1.predictive_mean - g2.predictive_mean;
            invij = cov.inverse();
            a = mean.transpose()*invij*mean;
            gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy2 += gauss;

    double energy3 = 0.;
            cov = g2.predictive_covariance + g2.predictive_covariance;
            mean = g2.predictive_mean - g2.predictive_mean;
            invij = cov.inverse();
            a = mean.transpose()*invij*mean;
            gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy3 += gauss;
//    cout<<(maxDist - (energy1 - 2*energy2 + energy3))/maxDist<<endl;
    return energy1 - 2*energy2 + energy3;

}

double weight_l2_rev(PCObject &o1, PCObject &o2)
{
    double last = pcl::getTime ();
    // reference :
    // Robust Point Set Registration Using Gaussian Mixture Models
    // Bing Jina, and Baba C. Vemuri
    // IEEE Transactions on Pattern Analysis and Machine Intelligence 2010
    int n = o1.gmm.size();
    int m = o2.gmm.size();

    double energy1 = 0.;
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            int dim = o1.gmm.at(i).dim;
            Eigen::MatrixXd cov = o1.gmm.at(i).covariance + o1.gmm.at(j).covariance;
            Eigen::VectorXd mean = o1.gmm.at(i).mean - o1.gmm.at(j).mean;
            Eigen::MatrixXd invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy1 += o1.gmm.at(i).weight*o1.gmm.at(j).weight*gauss;
        }
    }
    double energy2 = 0.;
    for(int i=0;i<n;i++){
        for(int j=0;j<m;j++){
            int dim = o1.gmm.at(i).dim;
            Eigen::MatrixXd cov = o1.gmm.at(i).covariance + o2.gmm.at(j).covariance;
            Eigen::VectorXd mean = o1.gmm.at(i).mean - o2.gmm.at(j).mean;
            Eigen::MatrixXd invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy2 += o1.gmm.at(i).weight*o2.gmm.at(j).weight*gauss;
        }
    }
    double energy3 = 0.;
    for(int i=0;i<m;i++){
        for(int j=0;j<m;j++){
            int dim = o2.gmm.at(i).dim;
            Eigen::MatrixXd cov = o2.gmm.at(i).covariance + o2.gmm.at(j).covariance;
            Eigen::VectorXd mean = o2.gmm.at(i).mean - o2.gmm.at(j).mean;
            Eigen::MatrixXd invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy3 += o2.gmm.at(i).weight*o2.gmm.at(j).weight*gauss;
        }
    }
    double now = pcl::getTime ();
//    cout << "l2-distance time " << now-last << " second" << endl;
    //    cout<<"l2distance"<<energy1 - 2*energy2 + energy3<<endl;

    return (energy1 - 2*energy2 + energy3);
}

void Assembler2D::precomputeShapeFunctionDerivatives() {
    Eigen::MatrixXd selector = Eigen::MatrixXd::Zero(3,2);
    selector << 0.0, 0.0,
             1.0, 0.0,
             0.0, 1.0;

    m_DN.resize(m_mesh->getNbrElements());
    for (size_t i=0; i<m_mesh->getNbrElements(); ++i)
    {
        VectorI idx = m_mesh->getElement(i);
        assert(idx.size() == 3);
        VectorF u[3];
        u[0] = m_mesh->getNode(idx[0]);
        u[1] = m_mesh->getNode(idx[1]);
        u[2] = m_mesh->getNode(idx[2]);

        Eigen::MatrixXd P = Eigen::MatrixXd::Zero(3,3);
        P <<     1.0,     1.0,     1.0,
        u[0][0], u[1][0], u[2][0],
        u[0][1], u[1][1], u[2][1];

        // DN is a 4x3 matrix containing the gradients of the
        // 4 shape functions (one for each node)
        //
        m_DN[i] = P.inverse() * selector /* * -1.0 */;
    }
}

void benchmark( )
{
    for (size_t i = 1; i < 12; i++) 
    {
        const size_t N = (size_t)pow(2, i);
        Eigen::MatrixXd m = MatrixXd::Random(N, N);

        clock_t t0 = clock();
        auto mInv = m.inverse();
        clock_t t1 = clock();
        double eigenT = (double)(t1-t0)/CLOCKS_PER_SEC;

        t0 = clock();
        auto m1 = m;
        invert( m1 );
        t1 = clock();

        if( (m1-mInv).norm() > 1e-6 )
        {
            cout << "Got " << endl << m1 << endl;
            cout << "Expected " << endl << mInv << endl;
            cout << "Original matrix was " << endl << m << endl;
            cout<< "Error : " << endl << (m1 - mInv).norm() << endl;
            throw;
        }

        cout << N << ' ' << (double)(t1-t0) / CLOCKS_PER_SEC <<  ' ' << eigenT << endl;
    }

}

// Convert joint torques to end effector force/torque
void computeFT(Eigen::VectorXd& ee_ft) {
	Eigen::MatrixXd jac;
	mRobot->getJacobian(jac);
	Eigen::MatrixXd trans = jac*jac.transpose() + singular_tol*Eigen::MatrixXd::Identity(jac.rows(), jac.rows());
	// f = inv(J*J')*J*tau
	ee_ft = (trans.inverse()*jac)*read_torque;
}

void IEFSolver::buildIsotropicMatrix(const Cavity & cav, const IGreensFunction & gf_i, const IGreensFunction & gf_o)
{
    // The total size of the cavity
    size_t cavitySize = cav.size();
    // The number of irreps in the group
    int nrBlocks = cav.pointGroup().nrIrrep();
    // The size of the irreducible portion of the cavity
    int dimBlock = cav.irreducible_size();

    // Compute SI and DI on the whole cavity, regardless of symmetry
    Eigen::MatrixXd SI = gf_i.singleLayer(cav.elements());
    Eigen::MatrixXd DI = gf_i.doubleLayer(cav.elements());

    // Perform symmetry blocking
    // If the group is C1 avoid symmetry blocking, we will just pack the fullPCMMatrix
    // into "block diagonal" when all other manipulations are done.
    if (cav.pointGroup().nrGenerators() != 0) {
        symmetryBlocking(DI, cavitySize, dimBlock, nrBlocks);
        symmetryBlocking(SI, cavitySize, dimBlock, nrBlocks);
    }

    Eigen::MatrixXd a = cav.elementArea().asDiagonal();
    Eigen::MatrixXd aInv = Eigen::MatrixXd::Zero(cavitySize, cavitySize);
    aInv = a.inverse();

    // Tq = -Rv -> q = -(T^-1 * R)v = -Kv
    // T = (2 * M_PI * fact * aInv - DI) * a * SI; R = (2 * M_PI * aInv - DI)
    // fullPCMMatrix_ = K = T^-1 * R * a
    // 1. Form T
    double epsilon = profiles::epsilon(gf_o.permittivity());
    double fact = (epsilon + 1.0)/(epsilon - 1.0);
    fullPCMMatrix_ = (2 * M_PI * fact * aInv - DI) * a * SI;
    // 2. Invert T using LU decomposition with full pivoting
    //    This is a rank-revealing LU decomposition, this allows us
    //    to test if T is invertible before attempting to invert it.
    Eigen::FullPivLU<Eigen::MatrixXd> T_LU(fullPCMMatrix_);
    if (!(T_LU.isInvertible()))
        PCMSOLVER_ERROR("T matrix is not invertible!");
    fullPCMMatrix_ = T_LU.inverse();
    // 3. Multiply T^-1 and R
    fullPCMMatrix_ *= (2 * M_PI * aInv - DI);
    // 4. Multiply by a
    fullPCMMatrix_ *= a;
    // 5. Symmetrize K := (K + K+)/2
    if (hermitivitize_) {
        hermitivitize(fullPCMMatrix_);
    }
    // Pack into a block diagonal matrix
    // For the moment just packs into a std::vector<Eigen::MatrixXd>
    symmetryPacking(blockPCMMatrix_, fullPCMMatrix_, dimBlock, nrBlocks);
    std::ofstream matrixOut("PCM_matrix");
    matrixOut << "fullPCMMatrix" << std::endl;
    matrixOut << fullPCMMatrix_ << std::endl;
    for (int i = 0; i < nrBlocks; ++i) {
        matrixOut << "Block number " << i << std::endl;
        matrixOut << blockPCMMatrix_[i] << std::endl;
    }

    built_ = true;
}

void MSF_MeasurementBase<EKFState_T>::calculateAndApplyCorrection(
    shared_ptr<EKFState_T> state, MSF_Core<EKFState_T>& core,
    const Eigen::MatrixXd& H_delayed, const Eigen::MatrixXd & res_delayed,
    const Eigen::MatrixXd& R_delayed) {

  // Get measurements.
  /// Correction from EKF update.
  Eigen::Matrix<double, MSF_Core<EKFState_T>::nErrorStatesAtCompileTime, 1> correction_;

  Eigen::MatrixXd S;
  Eigen::MatrixXd K(
      static_cast<int>(MSF_Core<EKFState_T>::nErrorStatesAtCompileTime),
      R_delayed.rows());
  typename MSF_Core<EKFState_T>::ErrorStateCov & P = state->P;

  S = H_delayed * P * H_delayed.transpose() + R_delayed;
  K = P * H_delayed.transpose() * S.inverse();

  correction_ = K * res_delayed;
  const typename MSF_Core<EKFState_T>::ErrorStateCov KH =
      (MSF_Core<EKFState_T>::ErrorStateCov::Identity() - K * H_delayed);
  P = KH * P * KH.transpose() + K * R_delayed * K.transpose();

  // Make sure P stays symmetric.
  P = 0.5 * (P + P.transpose());

  core.applyCorrection(state, correction_);
}

double* simulatePlant(double *state_vect, double *input_vect, double *state_op_vect, double sampling_time)
{
	double input_op_vect[4] = {0.0, 0.0, 0.0, 0.0};
	Eigen::Map<Eigen::VectorXd> x(state_vect, num_states_);
	Eigen::Map<Eigen::VectorXd> x_bar(state_op_vect, num_states_);
	Eigen::Map<Eigen::VectorXd> u(input_vect, num_inputs_);
	Eigen::Map<Eigen::VectorXd> u_bar(&input_op_vect[0], num_inputs_);
	
	Eigen::MatrixXd A = Eigen::MatrixXd::Identity(num_states_, num_states_);
	Eigen::MatrixXd B = Eigen::MatrixXd::Zero(num_states_, num_inputs_);
	
	Eigen::MatrixXd M = Eigen::MatrixXd::Zero(num_inputs_, num_inputs_);
	Eigen::VectorXd f_bar = Eigen::MatrixXd::Zero(num_inputs_, 1);
	
	double phi = x_bar(6);
	double theta = x_bar(7);
	double p = x_bar(9);
	double q = x_bar(10);
	double r = x_bar(11);
	
	M << 1., 1., 1., 1., 0., -1., 0., 1., 1., 0., -1., 0., -1., 1., -1., 1.;
	f_bar(0) = g_ * m_ / (Ct_ * cos(phi) * cos(theta));
	f_bar(1) = (Izz_ - Iyy_) * q * r / (Ct_ * d_);
	f_bar(2) = (Izz_ - Ixx_) * p * r / (Ct_ * d_);
	f_bar(3) = (Iyy_ - Ixx_) * p * q / Cq_;
	u_bar = M.inverse() * f_bar;
	u_bar = u_bar.cwiseSqrt();
	
	// Compute the matrices of the linear dynamic model	
	computeLTIModel(A, B, state_op_vect, input_op_vect);
	
	x = x_bar + A * (x - x_bar) + B * (u - u_bar);
		
	return state_vect;
}

void IEFSolver::buildAnisotropicMatrix(const Cavity & cav, const IGreensFunction & gf_i, const IGreensFunction & gf_o)
{
    // The total size of the cavity
    size_t cavitySize = cav.size();
    // The number of irreps in the group
    int nrBlocks = cav.pointGroup().nrIrrep();
    // The size of the irreducible portion of the cavity
    int dimBlock = cav.irreducible_size();

    // Compute SI, DI and SE, DE on the whole cavity, regardless of symmetry
    Eigen::MatrixXd SI = gf_i.singleLayer(cav.elements());
    Eigen::MatrixXd DI = gf_i.doubleLayer(cav.elements());
    Eigen::MatrixXd SE = gf_o.singleLayer(cav.elements());
    Eigen::MatrixXd DE = gf_o.doubleLayer(cav.elements());

    // Perform symmetry blocking
    // If the group is C1 avoid symmetry blocking, we will just pack the fullPCMMatrix
    // into "block diagonal" when all other manipulations are done.
    if (cav.pointGroup().nrGenerators() != 0) {
        symmetryBlocking(DI, cavitySize, dimBlock, nrBlocks);
        symmetryBlocking(SI, cavitySize, dimBlock, nrBlocks);
        symmetryBlocking(DE, cavitySize, dimBlock, nrBlocks);
        symmetryBlocking(SE, cavitySize, dimBlock, nrBlocks);
    }

    Eigen::MatrixXd a = cav.elementArea().asDiagonal();
    Eigen::MatrixXd aInv = a.inverse();

    // 1. Form T
    fullPCMMatrix_ = ((2 * M_PI * aInv - DE) * a * SI + SE * a * (2 * M_PI * aInv + DI.adjoint().eval()));
    // 2. Invert T using LU decomposition with full pivoting
    //    This is a rank-revealing LU decomposition, this allows us
    //    to test if T is invertible before attempting to invert it.
    Eigen::FullPivLU<Eigen::MatrixXd> T_LU(fullPCMMatrix_);
    if (!(T_LU.isInvertible())) PCMSOLVER_ERROR("T matrix is not invertible!");
    fullPCMMatrix_ = T_LU.inverse();
    Eigen::FullPivLU<Eigen::MatrixXd> SI_LU(SI);
    if (!(SI_LU.isInvertible())) PCMSOLVER_ERROR("SI matrix is not invertible!");
    fullPCMMatrix_ *= ((2 * M_PI * aInv - DE) - SE * SI_LU.inverse() * (2 * M_PI * aInv - DI));
    fullPCMMatrix_ *= a;
    // 5. Symmetrize K := (K + K+)/2
    if (hermitivitize_) {
        hermitivitize(fullPCMMatrix_);
    }
    // Pack into a block diagonal matrix
    // For the moment just packs into a std::vector<Eigen::MatrixXd>
    symmetryPacking(blockPCMMatrix_, fullPCMMatrix_, dimBlock, nrBlocks);
    std::ofstream matrixOut("PCM_matrix");
    matrixOut << "fullPCMMatrix" << std::endl;
    matrixOut << fullPCMMatrix_ << std::endl;
    for (int i = 0; i < nrBlocks; ++i) {
        matrixOut << "Block number " << i << std::endl;
        matrixOut << blockPCMMatrix_[i] << std::endl;
    }

    built_ = true;
}

void calc()
{
  Eigen::MatrixXd M_inv1 = M1.inverse();
  Eigen::MatrixXd M_inv2 = M2.inverse();
  Eigen::MatrixXd Jv1 = J1.block(0, 0, 3, J1.cols());
  Eigen::MatrixXd Jv2 = J2.block(0, 0, 3, J2.cols());
  Eigen::MatrixXd Lambda_inv1 = Jv1 * M_inv1 * Jv1.transpose();
  Eigen::MatrixXd Lambda_inv2 = Jv2 * M_inv2 * Jv2.transpose();
  Eigen::MatrixXd Lambda1 = Lambda_inv1.inverse();
  Eigen::MatrixXd Lambda2 = Lambda_inv2.inverse();
  Eigen::MatrixXd J_dyn_inv1 = M_inv1 * Jv1.transpose() * Lambda1;
  Eigen::MatrixXd J_dyn_inv2 = M_inv2 * Jv2.transpose() * Lambda2;
  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(M_inv1.rows(), M_inv1.rows());
  Eigen::MatrixXd N1 = I - J_dyn_inv1 * Jv1;
  Eigen::MatrixXd N2 = I - J_dyn_inv2 * Jv2;

  //std::cout << N.transpose() << std::endl << std::endl;
}

void PseudoInverse(const ::Eigen::MatrixXd& inputMatrix, ::Eigen::MatrixXd& outputMatrix, const ::Eigen::MatrixXd& weight)
{
	Eigen::MatrixXd tmp;
	if (weight.size() > 0)
		tmp = inputMatrix.transpose() * weight.transpose() * weight * inputMatrix;
	else
		tmp = inputMatrix * inputMatrix.transpose();
	tmp = tmp.inverse();
	outputMatrix =  inputMatrix.transpose() * tmp;

}

double weight_l2_gauss(PCObject &o1, PCObject &o2)
{
    // l2 distance
    Eigen::MatrixXd covsum = o1.gaussian.covariance+o2.gaussian.covariance;
    Eigen::VectorXd meandiff = o1.gaussian.mean - o2.gaussian.mean;
    Eigen::MatrixXd inv = covsum.inverse();
    double det = covsum.determinant();
    double a = meandiff.transpose()*inv*meandiff;
    double l2 = 2.-2.*(1./sqrt(pow(2*pi,3)*det)) * exp(-0.5*a);
    if(l2 < 0) l2 = 0.;
    return l2;
}

  double multivariateGaussianDensity(const Eigen::VectorXd& mean,
                                         const Eigen::MatrixXd& cov,
                                         const Eigen::VectorXd& z)
  {
    Eigen::VectorXd diff = mean - z;

    Eigen::VectorXd exponent = -0.5 * (diff.transpose() * cov.inverse() * diff);

    return pow(2 * M_PI, (double) z.size() / -2.0) * pow(cov.determinant(), -0.5) *
    exp(exponent(0));

  }

void calc()
{
  //std::cout << "M : " << std::endl << M.inverse() << std::endl;
  //          << "J : " << std::endl << J << std::endl << std::endl;
  Eigen::MatrixXd M_inv = M.inverse();
  Eigen::MatrixXd Jv = J.block(0, 0, 3, J.cols());
  Eigen::MatrixXd Lambda_inv = Jv * M_inv * Jv.transpose();
  Eigen::MatrixXd Lambda = Lambda_inv.inverse();
  Eigen::MatrixXd J_dyn_inv = M_inv * Jv.transpose() * Lambda;
  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(15, 15);
  Eigen::MatrixXd N = I - J_dyn_inv * Jv;

  //std::cout << N.transpose() << std::endl << std::endl;
}

/**
 * @function Residual 
 */
void Residual( Eigen::MatrixXd _M, std::vector<Eigen::VectorXi> _points2D, 
                                   std::vector<Eigen::VectorXd> _points3D,
                                   std::vector<Eigen::VectorXd> _normPoints2D, 
                                   std::vector<Eigen::VectorXd> _normPoints3D,
                                   Eigen::MatrixXd T2 )
{
   int N = _points2D.size();
   Eigen::VectorXd uvh;
   double u; double v;
   std::vector<Eigen::VectorXd> uv_normPredicted;
   std::vector<Eigen::VectorXd> uv_predicted;
   std::vector<Eigen::VectorXd> normResidual;
   std::vector<Eigen::VectorXd> residual;
   Eigen::MatrixXd T2inv;

   for( int i = 0; i < N; i++ )
   {
      Eigen::VectorXd normPoint3D_h(4);
      normPoint3D_h << _normPoints3D[i](0), _normPoints3D[i](1), _normPoints3D[i](2), 1; 
      uvh = _M*normPoint3D_h;
      u = uvh(0) / uvh(2);  v = uvh(1) / uvh(2);

      uv_normPredicted.push_back( Eigen::Vector2d( u, v ) );
      double nru = u - _normPoints2D[i](0); 
      double nrv = v - _normPoints2D[i](1);
      double nrd = sqrt( nru*nru + nrv*nrv );
      normResidual.push_back( Eigen::Vector3d( nru, nrv, nrd) );

      T2inv = T2.inverse();
      Eigen::VectorXd uvp = T2inv*Eigen::Vector3d( u, v, 1 );
      double ru = uvp(0) - _points2D[i](0); 
      double rv = uvp(1) - _points2D[i](1);
      double rd = sqrt( ru*ru + rv*rv );
      uv_predicted.push_back( Eigen::Vector2d( uvp(0), uvp(1) ) );
      residual.push_back( Eigen::Vector3d( ru, rv, rd ) );

   }


   /** Display */
   printf("Residual!! \n");
   for( unsigned int i = 0; i < N; i++ )
   {
      printf("[%d] P2D: (%d , %d) -- Predicted: (%.4f , %.4f) - Residual: (%.4f %.4f) Mod: %.4f \n",i, _points2D[i](0), _points2D[i](1), uv_predicted[i](0), uv_predicted[i](1), residual[i](0), residual[i](1), residual[i](2));
   }

}