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

//==============================================================================
void Controller::update(const Eigen::Vector3d& _targetPosition)
{
  using namespace dart;

  // Get equation of motions
  Eigen::Vector3d x    = mEndEffector->getTransform().translation();
  Eigen::Vector3d dx   = mEndEffector->getLinearVelocity();
  Eigen::MatrixXd invM = mRobot->getInvMassMatrix();                   // n x n
  Eigen::VectorXd Cg   = mRobot->getCoriolisAndGravityForces();        // n x 1
  math::LinearJacobian Jv   = mEndEffector->getLinearJacobian();       // 3 x n
  math::LinearJacobian dJv  = mEndEffector->getLinearJacobianDeriv();  // 3 x n
  Eigen::VectorXd dq        = mRobot->getVelocities();                 // n x 1

  // Compute operational space values
  Eigen::MatrixXd A = Jv*invM;                 // 3 x n
  Eigen::Vector3d b = /*-(A*Cg) + */dJv*dq;    // 3 x 1
  Eigen::MatrixXd M2 = Jv*invM*Jv.transpose(); // 3 x 3

  // Compute virtual operational space spring force at the end effector
  Eigen::Vector3d f = -mKp*(x - _targetPosition) - mKv*dx;

  // Compute desired operational space acceleration given f
  Eigen::Vector3d desired_ddx = b + M2*f;

  // Gravity compensation
  mForces = Cg;

  // Compute joint space forces to acheive the desired acceleration by solving:
  // A tau + b = desired_ddx
  mForces += A.colPivHouseholderQr().solve(desired_ddx - b);

  // Apply the joint space forces to the robot
  mRobot->setForces(mForces);
}

		void MLRModel::train()
		{	
			if (descriptor_matrix_.cols() == 0)
			{
				throw Exception::InconsistentUsage(__FILE__, __LINE__, "Data must be read into the model before training!"); 
			}
			if (Y_.cols() == 0)
			{
				throw Exception::InconsistentUsage(__FILE__, __LINE__, "No response values have been read! Model can not be trained!");
			}
			if (descriptor_matrix_.cols() >= descriptor_matrix_.rows())
			{	
				throw Exception::SingularMatrixError(__FILE__, __LINE__, "For MLR model, matrix must have more rows than columns in order to be invertible!!");
				//training_result_.ReSize(0, 0);
				//return;
			}

  			Eigen::MatrixXd m = descriptor_matrix_.transpose()*descriptor_matrix_;

				
			try
			{
				training_result_ = m.colPivHouseholderQr().solve(descriptor_matrix_.transpose()*Y_);
			}
			catch(BALL::Exception::GeneralException e)
			{
				training_result_.resize(0, 0);
				throw Exception::SingularMatrixError(__FILE__, __LINE__, "Matrix for MLR training is singular!! Check that descriptor_matrix_ does not contain empty columns!"); 
				return;
			}

			calculateOffsets();
		}

void PointSolver::solveForCorrection ()
{
	Eigen::MatrixXd jat = Jac.transpose() * Jac;
	Eigen::VectorXd jae = Jac.transpose() * pointErrs;
	Pcorrect = jat.colPivHouseholderQr().solve (jae);
	std::cout << Pcorrect << std::endl;
}

void PointSolver2::solveForCorrection ()
{
	Eigen::MatrixXd jat = Jac.transpose() * Jac;
	Eigen::VectorXd jae = Jac.transpose() * pointErrs;
	Pcorrect = jat.colPivHouseholderQr().solve (jae);
	position0.x() -= Pcorrect[0];
	position0.y() -= Pcorrect[1];
	position0.z() -= Pcorrect[2];
	orientation0.x() -= Pcorrect[3];
	orientation0.y() -= Pcorrect[4];
	orientation0.z() -= Pcorrect[5];
	orientation0.w() -= Pcorrect[6];
//	orientation0.normalize();
//	std::cout << Jac << std::endl;
//	exit (1);
//	std::cout << Pcorrect << std::endl;
}

Eigen::VectorXd ALLModel::predict(const vector<double> & substance, bool transform)
{
    if (descriptor_matrix_.cols() == 0)
    {
        throw Exception::InconsistentUsage(__FILE__, __LINE__, "Training data must be read into the ALL-model before the activity of a substance can be predicted!");
    }
    Eigen::VectorXd v0 = getSubstanceVector(substance, transform);

    Eigen::MatrixXd v(1, v0.rows());
    v.row(0) = v0;
    Eigen::MatrixXd dist;

    // calculate distances between the given substance and the substances of X
    // dimension of dist: 1xn
    calculateEuclDistanceMatrix(v, descriptor_matrix_, dist);
    Eigen::VectorXd w;
    calculateWeights(dist, w);

    Eigen::MatrixXd XX;

    // calculate X.t()*X as cross-products weighted by the similarity of the given substance to the respective row of X
    calculateXX(w, XX);

    Eigen::MatrixXd XY;

    // calculate X.t()*Y_ as cross-products weighted by the similarity of the given substance to the respective row of X
    calculateXY(w, XY);

    // rigde regression in order to be able to cope with linearly dependent columns, i.e. singular matrices
    Eigen::MatrixXd im(XX.rows(), XX.rows());
    im.setIdentity();
    im *= lambda_;
    XX += im;

    Eigen::MatrixXd train = XX.colPivHouseholderQr().solve(XY);

    Eigen::VectorXd res(Y_.cols());
    res = v0.transpose()*train;

    if (transform && y_transformations_.cols() != 0)
    {
        backTransformPrediction(res);
    }

    return res;
}

		void KPLSModel::train()
		{	
			if (descriptor_matrix_.cols() == 0)
			{
				throw Exception::InconsistentUsage(__FILE__, __LINE__, "Data must be read into the model before training!"); 
			}
		// 	if (descriptor_matrix_.cols() < no_components_)
		// 	{
		// 		throw Exception::TooManyPLSComponents(__FILE__, __LINE__, no_components_, descriptor_matrix_.cols());
		// 	}
			
			kernel->calculateKernelMatrix(descriptor_matrix_, K_);

			Eigen::MatrixXd P;  // Matrix P saves all vectors p

			int cols = K_.cols();
			
			// determine the number of components that are to be created.
			// no_components_ contains the number of components desired by the user, 
			// but obviously we cannot calculate more PLS components than features
			int features = descriptor_matrix_.cols();
			unsigned int components_to_create = no_components_;
			if (features < no_components_) components_to_create = features; 

			U_.resize(K_.rows(), components_to_create);
			loadings_.resize(cols, components_to_create);
			weights_.resize(Y_.cols(), components_to_create);
			latent_variables_.resize(K_.rows(), components_to_create);
			P.resize(cols, components_to_create);
			
			Eigen::VectorXd w;
			Eigen::VectorXd t;
			Eigen::VectorXd c;
			Eigen::VectorXd u = Y_.col(0);

			Eigen::VectorXd u_old;
			
			for (unsigned int j = 0; j < components_to_create; j++)
			{
				for (int i = 0; i < 10000 ; i++)
				{	
					w = K_.transpose()*u / Statistics::scalarProduct(u);
					w = w / Statistics::euclNorm(w);
					t = K_*w;
					c = Y_.transpose()*t / Statistics::scalarProduct(t);
					u_old = u;
					u = Y_*c / Statistics::scalarProduct(c); 
				
					if (Statistics::euclDistance(u, u_old)/Statistics::euclNorm(u) > 0.0000001) 
					{ 
						continue;				
					}
					else  // if u has converged
					{
						break;
					}
				}

				Eigen::VectorXd p = K_.transpose() * t / Statistics::scalarProduct(t);
				K_ -= t * p.transpose();

				U_.col(j) = u;
				loadings_.col(j) = w;
				weights_.col(j) = c;
				P.col(j) = p;
				latent_variables_.col(j) = t;
			}

		// 	try // p's are not orthogonal to each other, so that in rare cases P.t()*loadings_ is not invertible
		// 	{
		// 		loadings_ = loadings_*(P.t()*loadings_).i();
		// 	}
		// 	catch(BALL::Exception::MatrixIsSingular e)
		// 	{
		// 		Eigen::MatrixXd I; I.setToIdentity(P.cols());
		// 		I *= 0.001;
		// 		loadings_ = loadings_*(P.t()*loadings_+I).i();
		// 	}

			Eigen::MatrixXd m = P.transpose()*loadings_;
			training_result_ = loadings_*m.colPivHouseholderQr().solve(weights_.transpose());

			calculateOffsets();
		}

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;
}

//.........这里部分代码省略.........
		A1.coeffRef( i - 1 , 1 ) = pow( t.coeff( i - 1 , 0 ) , 4 );
		A1.coeffRef( i - 1 , 2 ) = pow( t.coeff( i - 1 , 0 ) , 5 );
	}

	Eigen::MatrixXd A2 = Eigen::MatrixXd::Zero( n , n );

	for ( i = 1; i <= n; i++ )
	{
		for ( j = 1; j <= n; j++ )
		{
			if ( i > j )
				k = i;
			else
				k = j;

			A2.coeffRef( j - 1 , i - 1 ) =
					pow( ( t.coeff( k - 1 , 0 ) - t.coeff( i - 1 , 0 ) ) , 5 ) / 120;
		}
	}

	Eigen::MatrixXd A3 = Eigen::MatrixXd::Zero( 3 , n + 3 );

	A3.coeffRef( 0 , 0 ) = pow( tf , 3 );
	A3.coeffRef( 0 , 1 ) = pow( tf , 4 );
	A3.coeffRef( 0 , 2 ) = pow( tf , 5 );

	A3.coeffRef( 1 , 0 ) = 3 * pow( tf , 2 );
	A3.coeffRef( 1 , 1 ) = 4 * pow( tf , 3 );
	A3.coeffRef( 1 , 2 ) = 5 * pow( tf , 4 );

	A3.coeffRef( 2 , 0 ) = 6 * tf;
	A3.coeffRef( 2 , 1 ) = 12 * pow( tf , 2 );
	A3.coeffRef( 2 , 2 ) = 20 * pow( tf , 3 );

	for ( i = 1; i <= n; i++ )
	{
		A3.coeffRef( 0 , i + 2 ) = pow( tf - t.coeff( i - 1 , 0 ) , 5 ) / 120;
		A3.coeffRef( 1 , i + 2 ) = pow( tf - t.coeff( i - 1 , 0 ) , 4 ) / 24;
		A3.coeffRef( 2 , i + 2 ) = pow( tf - t.coeff( i - 1 , 0 ) , 3 ) / 6;
	}

	Eigen::MatrixXd A = Eigen::MatrixXd::Zero( n + 3 , n + 3 );

	A.block( 0 , 0 , n , 3 ) = A1;
	A.block( 0 , 3 , n , n ) = A2;
	A.block( n , 0 , 3 , n + 3 ) = A3;

	/* Calculation Matrix C (coefficient of polynomial function) */

	Eigen::MatrixXd C( 2 * n + 3 , 1 );
	//C = A.inverse() * B;
	C = A.colPivHouseholderQr().solve(B);

	/* Time */

	int NN;
	double N;

	N = tf / smp ;
	NN = round( N ) ;

	Eigen::MatrixXd Time = Eigen::MatrixXd::Zero( NN + 1 , 1 );

	for ( i = 1; i <= NN + 1; i++ )
		Time.coeffRef( i - 1 , 0 ) = ( i - 1 ) * smp;

	/* Time_via */

	Eigen::MatrixXd Time_via = Eigen::MatrixXd::Zero( n , 1 );

	for ( i = 1; i <= n; i++ )
		Time_via.coeffRef( i - 1 , 0 ) = round( t.coeff( i - 1 , 0 ) / smp ) + 2;

	/* Minimum Jerk Trajectory with Via-points */

	Eigen::MatrixXd Tra_jerk_via = Eigen::MatrixXd::Zero( NN + 1 , 1 );

	for ( i = 1; i <= NN + 1; i++ )
	{
		Tra_jerk_via.coeffRef( i - 1 , 0 ) =
				x0 +
				v0 * Time.coeff( i - 1 , 0 ) +
				0.5 * a0 * pow( Time.coeff( i - 1 , 0 ) , 2 ) +
				C.coeff( 0 , 0 ) * pow( Time.coeff( i - 1 , 0 ) , 3 ) +
				C.coeff( 1 , 0 ) * pow( Time.coeff( i - 1 , 0 ) , 4 ) +
				C.coeff( 2 , 0 ) * pow( Time.coeff( i - 1 , 0 ) , 5 ) ;
	}

	for ( i = 1 ; i <= n; i++ )
	{
		for ( j = Time_via.coeff( i - 1 , 0 ); j <= NN + 1; j++ )
		{
			Tra_jerk_via.coeffRef( j - 1 , 0 ) =
					Tra_jerk_via.coeff( j - 1 , 0 ) +
					C.coeff( i + 2 , 0 ) * pow( ( Time.coeff( j - 1 , 0 ) - t.coeff( i - 1 , 0 ) ) , 5 ) / 120 ;
		}
	}

	return Tra_jerk_via;
}