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

template <typename PointNT> void
pcl::MarchingCubesRBF<PointNT>::voxelizeData ()
{
  // Initialize data structures
  unsigned int N = static_cast<unsigned int> (input_->size ());
  Eigen::MatrixXd M (2*N, 2*N),
                  d (2*N, 1);

  for (unsigned int row_i = 0; row_i < 2*N; ++row_i)
  {
    // boolean variable to determine whether we are in the off_surface domain for the rows
    bool row_off = (row_i >= N) ? 1 : 0;
    for (unsigned int col_i = 0; col_i < 2*N; ++col_i)
    {
      // boolean variable to determine whether we are in the off_surface domain for the columns
      bool col_off = (col_i >= N) ? 1 : 0;
      M (row_i, col_i) = kernel (Eigen::Vector3f (input_->points[col_i%N].getVector3fMap ()).cast<double> () + Eigen::Vector3f (input_->points[col_i%N].getNormalVector3fMap ()).cast<double> () * col_off * off_surface_epsilon_,
                                 Eigen::Vector3f (input_->points[row_i%N].getVector3fMap ()).cast<double> () + Eigen::Vector3f (input_->points[row_i%N].getNormalVector3fMap ()).cast<double> () * row_off * off_surface_epsilon_);
    }

    d (row_i, 0) = row_off * off_surface_epsilon_;
  }

  // Solve for the weights
  Eigen::MatrixXd w (2*N, 1);

  // Solve_linear_system (M, d, w);
  w = M.fullPivLu ().solve (d);

  std::vector<double> weights (2*N);
  std::vector<Eigen::Vector3d> centers (2*N);
  for (unsigned int i = 0; i < N; ++i)
  {
    centers[i] = Eigen::Vector3f (input_->points[i].getVector3fMap ()).cast<double> ();
    centers[i + N] = Eigen::Vector3f (input_->points[i].getVector3fMap ()).cast<double> () + Eigen::Vector3f (input_->points[i].getNormalVector3fMap ()).cast<double> () * off_surface_epsilon_;
    weights[i] = w (i, 0);
    weights[i + N] = w (i + N, 0);
  }

  for (int x = 0; x < res_x_; ++x)
    for (int y = 0; y < res_y_; ++y)
      for (int z = 0; z < res_z_; ++z)
      {
        Eigen::Vector3d point;
        point[0] = min_p_[0] + (max_p_[0] - min_p_[0]) * float (x) / float (res_x_);
        point[1] = min_p_[1] + (max_p_[1] - min_p_[1]) * float (y) / float (res_y_);
        point[2] = min_p_[2] + (max_p_[2] - min_p_[2]) * float (z) / float (res_z_);

        double f = 0.0;
        std::vector<double>::const_iterator w_it (weights.begin());
        for (std::vector<Eigen::Vector3d>::const_iterator c_it = centers.begin ();
             c_it != centers.end (); ++c_it, ++w_it)
          f += *w_it * kernel (*c_it, point);

        grid_[x * res_y_*res_z_ + y * res_z_ + z] = float (f);
      }
}

//observations weighted by inverse stddev
QuadrCoefs QuadrLogFit( const map<double,UniModalSearch::MS> & y_by_x ) 
{
    unsigned n = y_by_x.size();

    //prepare for case weights: inverse of stddev; beware of zero stddev
    double minstddev=numeric_limits<double>::max();
    for (map<double,UniModalSearch::MS>::const_iterator itr=y_by_x.begin(); itr!=y_by_x.end(); itr++) {
        if (0 < itr->second.s && itr->second.s < minstddev) {
        	minstddev = itr->second.s;
        }
    }
    const double zeroadjust = minstddev<numeric_limits<double>::max() ? 100.0/minstddev //non-zeroes present
        : 1.0; //all weights will be equal

    Eigen::MatrixXd X( n, 3 ); //nRows, nCols
    Eigen::MatrixXd XTW( 3, n ); //nRows, nCols
    Eigen::VectorXd Y( n );
    int i = 0;
    for (map<double,UniModalSearch::MS>::const_iterator itr=y_by_x.begin(); itr!=y_by_x.end();
        itr++, i++) {
        double weight = itr->second.s>0 ? 1/itr->second.s : zeroadjust;
        X(i,0) = XTW(0,i) = 1.0;
        X(i,1) = XTW(1,i) = log( itr->first );
        X(i,2) = XTW(2,i) = log( itr->first ) * log( itr->first );
        for (int j=0; j < 3; j++) { //for no weighting, just skip this
            XTW(j,i) *= weight;
        }
        Y(i) = itr->second.m;
    }

    Eigen::MatrixXd XTX =  XTW * X;
    Eigen::VectorXd XTY = XTW * Y;
    Eigen::VectorXd b_hat = XTX.fullPivLu().solve(XTY);

    // TODO Check numerical accuracy and throw error

    QuadrCoefs ret;
    ret.c0 = b_hat(0);
    ret.c1 = b_hat(1);
    ret.c2 = b_hat(2);
    return ret;
}

void ExperimentalTrajectory::calculateRbfnWeights()
{
    int nC = kernelCenters.rows();
    Eigen::MatrixXd rbfnDesignMatrix = rbfnKernelFunction(kernelCenters);
    // rbfnWeights = Eigen::MatrixXd::Zero(nDoF, nC);
    Eigen::MatrixXd wayTrans = waypoints.transpose();

    // std::cout << "phi = " << rbfnDesignMatrix.rows() << " x " << rbfnDesignMatrix.cols() << std::endl;
    // std::cout << "way = " << wayTrans.rows()<< " x " << wayTrans.cols() << std::endl;
    Eigen::MatrixXd A = rbfnDesignMatrix * rbfnDesignMatrix.transpose();
    // std::cout << "A = " << A.rows()<< " x " << A.cols() << std::endl;

    Eigen::MatrixXd b = rbfnDesignMatrix * wayTrans;

    // std::cout << "b = " << b.rows()<< " x " << b.cols() << std::endl;


    // rbfnWeights = (A.inverse() * b).transpose(); // the transpose makes weights = nDoF x nCenters which is better for the output function.

    // rbfnWeights = A.fullPivLu().solve(b).transpose();
    rbfnWeights = rbfnDesignMatrix.fullPivLu().solve(wayTrans).transpose();
    // std::cout << "rbfn weights:\n" << rbfnWeights << std::endl;

}

//	EulerImplicit Single step
void Integrator::EulerImplicit_Singlestep(double dt, double& t, Eigen::VectorXd& yNext) {
	Eigen::MatrixXd J		=	Jacobian(t, yNext);
	Eigen::MatrixXd A		=	Eigen::MatrixXd::Identity(J.rows(),J.cols())-dt*J;
	yNext+=dt*A.fullPivLu().solve(function(t,yNext));
};

//.........这里部分代码省略.........
	centroid+= points_mat.col(ipt); //add all the column vectors together
    }
    centroid/=npts; //divide by the number of points to get the centroid
    cout<<"centroid: "<<centroid.transpose()<<endl;
    
    
    // subtract this centroid from all points in points_mat:
    Eigen::MatrixXd points_offset_mat = points_mat;
    for (int ipt =0;ipt<npts;ipt++) {
        points_offset_mat.col(ipt)  = points_offset_mat.col(ipt)-centroid;
    }
    //compute the covariance matrix w/rt x,y,z:
    Eigen::Matrix3d CoVar;
    CoVar = points_offset_mat*(points_offset_mat.transpose());  //3xN matrix times Nx3 matrix is 3x3
    cout<<"covariance: "<<endl;
    cout<<CoVar<<endl;
    
    // here is a more complex object: a solver for eigenvalues/eigenvectors;
    // we will initialize it with our covariance matrix, which will induce computing eval/evec pairs
    Eigen::EigenSolver<Eigen::Matrix3d> es3d(CoVar);
    
    Eigen::VectorXd evals; //we'll extract the eigenvalues to here
    //cout<<"size of evals: "<<es3d.eigenvalues().size()<<endl;
    //cout<<"rows,cols = "<<es3d.eigenvalues().rows()<<", "<<es3d.eigenvalues().cols()<<endl;
    cout << "The eigenvalues of CoVar are:" << endl << es3d.eigenvalues().transpose() << endl;
    cout<<"(these should be real numbers, and one of them should be zero)"<<endl;
    cout << "The matrix of eigenvectors, V, is:" << endl;
    cout<< es3d.eigenvectors() << endl << endl;
    cout<< "(these should be real-valued vectors)"<<endl;
    // in general, the eigenvalues/eigenvectors can be complex numbers
    //however, since our matrix is self-adjoint (symmetric, positive semi-definite), we expect
    // real-valued evals/evecs;  we'll need to strip off the real parts of the solution

    evals= es3d.eigenvalues().real(); // grab just the real parts
    cout<<"real parts of evals: "<<evals.transpose()<<endl;

    // our solution should correspond to an e-val of zero, which will be the minimum eval
    //  (all other evals for the covariance matrix will be >0)
    // however, the solution does not order the evals, so we'll have to find the one of interest ourselves
    
    double min_lambda = evals[0]; //initialize the hunt for min eval
    Eigen::Vector3cd complex_vec; // here is a 3x1 vector of double-precision, complex numbers
    Eigen::Vector3d est_plane_normal;
    complex_vec=es3d.eigenvectors().col(0); // here's the first e-vec, corresponding to first e-val
    //cout<<"complex_vec: "<<endl;
    //cout<<complex_vec<<endl;
    est_plane_normal = complex_vec.real();  //strip off the real part
    //cout<<"real part: "<<est_plane_normal.transpose()<<endl;
    //est_plane_normal = es3d.eigenvectors().col(0).real(); // evecs in columns

    double lambda_test;
    int i_normal=0;
    //loop through "all" ("both", in this 3-D case) the rest of the solns, seeking min e-val
    for (int ivec=1;ivec<3;ivec++) {
        lambda_test = evals[ivec];
    	if (lambda_test<min_lambda) {
		min_lambda =lambda_test;
                i_normal= ivec; //this index is closer to index of min eval
		est_plane_normal = es3d.eigenvectors().col(ivec).real();
        }
    }
    // at this point, we have the minimum eval in "min_lambda", and the plane normal
    // (corresponding evec) in "est_plane_normal"/
    // these correspond to the ith entry of i_normal
    cout<<"min eval is "<<min_lambda<<", corresponding to component "<<i_normal<<endl;
    cout<<"corresponding evec (est plane normal): "<<est_plane_normal.transpose()<<endl;
    cout<<"correct answer is: "<<normal_vec.transpose()<<endl;
    double est_dist = est_plane_normal.dot(centroid);
    cout<<"est plane distance from origin = "<<est_dist<<endl;
    cout<<"correct answer is: "<<dist<<endl;
    cout<<endl<<endl;
           
    
    //xxxx  one_vec*dist = point.dot(nx,ny,nz)
    // so, one_vec = points_mat.transpose()*x_vec, where x_vec = [nx;ny;nz]/dist (does not work if dist=0)
    // this is of the form: b = A*x, an overdetermined system of eqns
    // solve this using one of many Eigen methods
    // see: http://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html
    
    ROS_INFO("2ND APPROACH b = A*x SOLN");
    Eigen::VectorXd  ones_vec= Eigen::MatrixXd::Ones(npts,1); // this is our "b" vector in b = A*x
    Eigen::MatrixXd A = points_mat.transpose(); // make this a Nx3 matrix, where points are along the rows
    // we'll pick the "full pivot LU" solution approach; see: http://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html
    // a matrix in "Eigen" has member functions that include solution methods to this common problem, b = A*x
    // use: x = A.solution_method(b)
    Eigen::Vector3d x_soln = A.fullPivLu().solve(ones_vec);
    //cout<<"x_soln: "<<x_soln.transpose()<<endl;
    double dist_est2 = 1.0/x_soln.norm();
    x_soln*=dist_est2;
    cout<<"normal vec, 2nd approach: "<<x_soln.transpose()<<endl;
    cout<<"plane distance = "<<dist_est2<<endl;
    
    

    return 0;

   // while (ros::ok()) {
   //     sleep_timer.sleep();
   // }
}