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

int main(int argc, char** argv) {

    ros::init(argc, argv, "eigen_test");
    ros::NodeHandle nh_jntPub; // node handle for joint command publisher

 //   ros::Publisher pub_joint_commands; //
//    pub_joint_commands = nh_jntPub.advertise<atlas_msgs::AtlasCommand>("/atlas/atlas_command", 1, true); 
    
    ROS_INFO("test eigen program");
    
    Eigen::Matrix3f A;
    Eigen::Vector3f b;
    A << 1,2,3, 4,5,6, 7,8,10;
    
    A(1,2)=0; // how to access one element of matrix; start from 0; no warning out of range...
    
 
    b << 3,3,4;
    std::cout <<"b = "<<b <<std::endl;   
    
    // column operaton: replace first column of A with vector b:
    A.col(0)= b;  // could copy columns of matrices w/ A.col(0) = B.col(0);
    
    std::cout <<"A = "<<A <<std::endl;

    Eigen::MatrixXd mat1 = Eigen::MatrixXd::Zero(6, 6); //6x6 matrix full of zeros
    Eigen::MatrixXd mat2 = Eigen::MatrixXd::Identity(6, 6); //6x6 identity matrix  

    std::cout<<mat1<<std::endl;
    std::cout<<mat2<<std::endl;

    Eigen::Vector3f xtest = A.colPivHouseholderQr().solve(b);
    std::cout<<"soln xtest = "<<xtest<<std::endl;
    
    Eigen::Vector3f x = A.partialPivLu().solve(b); //dec.solve(b); //A.colPivHouseholderQr().solve(b);
    std::cout<<"soln x = "<<x<<std::endl;
    
    Eigen::Vector3f btest = A*x;
    std::cout<<"test soln: A*x = " <<btest<<std::endl;
    
    //extend to 6x6 test: v = M*z, find z using 2 methods
    // use double-precision matrices/vectors
    Eigen::MatrixXd M = Eigen::MatrixXd::Random(6,6);

    std::cout<<"test 6x6: M = "<<M<<std::endl;
    Eigen::VectorXd v(6);   
    v << 1,2,3,4,5,6;
    std::cout<<"v = "<<v<<std::endl;
    Eigen::VectorXd z(6); 
    Eigen::VectorXd ztest(6);   
    ztest = M.colPivHouseholderQr().solve(v);
    std::cout<<"soln ztest = "<<ztest<<std::endl;
    z = M.partialPivLu().solve(v);   
    std::cout<<"soln 6x6: z = "<<z<<std::endl;
    Eigen::VectorXd vtest(6);
    vtest = M*z;
    std::cout<<"test soln: M*z = "<<vtest<<std::endl;

    // .norm() operator...
    double relative_error = (M*z - v).norm() / v.norm(); // norm() is L2 norm
    std::cout << "The relative error is:\n" << relative_error << std::endl;

    
    std::cout<<"dot prod, v, z: "<< v.dot(z)<<std::endl;
    std::cout<<"cross prod, b-cross-x: " << b.cross(x)<<std::endl;
    
    
    
    return 0;
}

int Lemke(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q,
          Eigen::VectorXd* _z) {
  int n = _q.size();

  const double zer_tol = 1e-5;
  const double piv_tol = 1e-8;
  int maxiter = 1000;
  int err = 0;

  if (_q.minCoeff() > 0) {
    // LOG(INFO) << "Trivial solution exists.";
    *_z = Eigen::VectorXd::Zero(n);
    return err;
  }

  *_z = Eigen::VectorXd::Zero(2 * n);
  int iter = 0;
  double theta = 0;
  double ratio = 0;
  int leaving  = 0;
  Eigen::VectorXd Be = Eigen::VectorXd::Constant(n, 1);
  Eigen::VectorXd x = _q;
  std::vector<int> bas;
  std::vector<int> nonbas;

  int t = 2 * n + 1;
  int entering = t;

  bas.clear();
  nonbas.clear();

  for (int i = 0; i < n; ++i) {
    bas.push_back(i);
  }

  Eigen::MatrixXd B = -Eigen::MatrixXd::Identity(n, n);

  for (int i = 0; i < bas.size(); ++i) {
    B.col(bas[i]) = _M.col(bas[i]);
  }

  x = -B.partialPivLu().solve(_q);

  Eigen::VectorXd minuxX = -x;
  int lvindex;
  double tval = minuxX.maxCoeff(&lvindex);
  leaving = bas[lvindex];
  bas[lvindex] = t;

  Eigen::VectorXd U = Eigen::VectorXd::Zero(n);
  for (int i = 0; i < n; ++i) {
    if (x[i] < 0)
      U[i] = 1;
  }
  Be = -(B * U);
  x += tval * U;
  x[lvindex] = tval;
  B.col(lvindex) = Be;

  for (iter = 0; iter < maxiter; ++iter) {
    if (leaving == t) {
      break;
    } else if (leaving < n) {
      entering = n + leaving;
      Be = Eigen::VectorXd::Zero(n);
      Be[leaving] = -1;
    } else {
      entering = leaving - n;
      Be = _M.col(entering);
    }

    Eigen::VectorXd d = B.partialPivLu().solve(Be);

    std::vector<int> j;
    for (int i = 0; i < n; ++i) {
      if (d[i] > piv_tol)
        j.push_back(i);
    }
    if (j.empty()) {
      // err = 2;
      break;
    }

    int jSize = static_cast<int>(j.size());
    Eigen::VectorXd minRatio(jSize);
    for (int i = 0; i < jSize; ++i) {
      minRatio[i] = (x[j[i]] + zer_tol) / d[j[i]];
    }
    double theta = minRatio.minCoeff();

    std::vector<int> tmpJ;
    std::vector<double> tmpMinRatio;
    for (int i = 0; i < jSize; ++i) {
      if (x[j[i]] / d[j[i]] <= theta) {
        tmpJ.push_back(j[i]);
        tmpMinRatio.push_back(minRatio[i]);
      }
    }
//    if (tmpJ.empty())
//    {
//.........这里部分代码省略.........