C++ VectorXd::segment方法代码示例

void DiagonalBlockSparseMatrix::Solve(const Eigen::VectorXd& rhs, Eigen::VectorXd& sol)
{
	int numRows = GetNumRows();
	int numCols = GetNumCols();
	LOG_IF(FATAL, numRows != numCols) << "DiagonalBlockSparseMatrix is not a square matrix and noninvertible.";
	int vecLength = rhs.size();
	LOG_IF(FATAL, numCols != vecLength) << "DiagonalBlockSparseMatrix and right hand side vector are of different dimensions.";

	sol.resize(numCols);
	int numUntilLast = 0;
	int numDiagElements = static_cast<int>(mDiagElements.size());
	for (int i = 0; i < numDiagElements; ++i)
	{
		int numColsElement = mDiagElements[i].GetNumCols();	
		Eigen::VectorXd partSol(numColsElement);
		Eigen::VectorXd partRhs = rhs.segment(numUntilLast, numColsElement);
#if LLT_SOLVE
		mDiagElements[i].LltSolve(partRhs, partSol);
#else
		mDiagElements[i].ConjugateGradientSolve(partRhs, partSol);
#endif		
		sol.segment(numUntilLast, numColsElement) = partSol;
		numUntilLast += numColsElement;
	}
}

// Compute the average contour, by calling compute_contour_vals repeatedly
//
// [[Rcpp::export]]
Eigen::MatrixXd rcppstandardize_rates(const Eigen::VectorXd &tiles, const Eigen::VectorXd &rates,
                                      const Eigen::VectorXd &xseed, const Eigen::VectorXd &yseed,
                                      const Eigen::MatrixXd &marks, const Eigen::VectorXd &nmrks,
                                      const std::string &distm) {
    bool use_weighted_mean = true;
    int nxmrks = nmrks(0);
    int nymrks = nmrks(1);
    Eigen::MatrixXd Zvals = Eigen::MatrixXd::Zero(nymrks,nxmrks);
    Eigen::MatrixXd zvals = Eigen::MatrixXd::Zero(nymrks,nxmrks);
    int niters = tiles.size();
    for ( int i = 0, pos = 0 ; i < niters ; i++ ) {
        int now_tiles = (int)tiles(i);
        Eigen::VectorXd now_rates = rates.segment(pos,now_tiles);
        Eigen::VectorXd now_xseed = xseed.segment(pos,now_tiles);
        Eigen::VectorXd now_yseed = yseed.segment(pos,now_tiles);
        Eigen::MatrixXd now_seeds(now_tiles, 2);
        now_seeds << now_xseed,now_yseed;
        if (use_weighted_mean) {
            compute_contour_vals(zvals,marks,now_rates,now_seeds,distm);
            zvals = zvals.array() - zvals.mean();
        } else {
            now_rates = now_rates.array() - now_rates.mean();
            compute_contour_vals(zvals,marks,now_rates,now_seeds,distm);
        }
        Zvals += zvals;
        pos += now_tiles;
    }
    // Do not divide by niters here but in 'average.eems.contours' instead
    // Zvals = Zvals.array() / niters;
    return Zvals.transpose();
}

void BallJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
    getJacobian();
    _J1.block(_rowIndex, 0, 3, mBodyNode1->getSkeleton()->getNumGenCoords()) = mJ1;
    Eigen::Vector3d worldP1 = mBodyNode1->getWorldTransform() * mOffset1;
    Eigen::VectorXd qDot1 = mBodyNode1->getSkeleton()->get_dq();
    _C.segment(_rowIndex, 3) = worldP1 - mOffset2;
    _CDot.segment(_rowIndex, 3) = mJ1 * qDot1;
}

//! Convert Cartesian to spherical state.
Eigen::VectorXd convertCartesianToSphericalState( const Eigen::VectorXd& cartesianState )
{
    // Create spherical state vector, initialized with zero entries.
    Eigen::VectorXd convertedSphericalState = Eigen::VectorXd::Zero( 6 );

    // Compute radius.
    convertedSphericalState( 0 ) = cartesianState.segment( 0, 3 ).norm( );

    // Check if radius is nonzero.
    /*
     * If r > 0, the elevation and azimuth angles are computed using trigonometric relationships.
     * If r = 0, the coordinates are at the origin, the elevation and azimuth angles equal to zero.
     * Since the state vector was initialized with zeroes, this is already the case.
     */
    if ( convertedSphericalState( 0 ) > std::numeric_limits< double >::epsilon( ) )
    {
        // Compute elevation and azimuth angles using trigonometric relationships.
        // Azimuth angle.
        convertedSphericalState( 1 ) = std::atan2( cartesianState( 1 ), cartesianState( 0 ) );
        // Elevation angle.
        convertedSphericalState( 2 ) = std::asin( cartesianState( 2 )
                                                   / convertedSphericalState( 0 ) );
    }

    // Precompute sine/cosine of angles, which has multiple usages, to save computation time.
    const double cosineOfElevationAngle = std::cos( convertedSphericalState( 2 ) );
    const double sineOfElevationAngle = std::sin( convertedSphericalState( 2 ) );
    const double cosineOfAzimuthAngle = std::cos( convertedSphericalState( 1 ) );
    const double sineOfAzimuthAngle = std::sin( convertedSphericalState( 1 ) );

    // Set up transformation matrix for cylindrical to spherical conversion.
    Eigen::MatrixXd transformationMatrixCylindricalToSpherical = Eigen::MatrixXd::Zero( 3, 3 );
    transformationMatrixCylindricalToSpherical( 0, 0 ) = cosineOfElevationAngle;
    transformationMatrixCylindricalToSpherical( 0, 2 ) = sineOfElevationAngle;
    transformationMatrixCylindricalToSpherical( 1, 1 ) = 1.0;
    transformationMatrixCylindricalToSpherical( 2, 0 ) = -sineOfElevationAngle;
    transformationMatrixCylindricalToSpherical( 2, 2 ) = cosineOfElevationAngle;

    // Set up transformation matrix for Cartesian to cylindrical conversion.
    Eigen::MatrixXd transformationMatrixCartesianToCylindrical = Eigen::MatrixXd::Zero( 3, 3 );
    transformationMatrixCartesianToCylindrical( 0, 0 ) = cosineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 0, 1 ) = sineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 1, 0 ) = -sineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 1, 1 ) = cosineOfAzimuthAngle;
    transformationMatrixCartesianToCylindrical( 2, 2 ) = 1.0;

    // Compute transformation matrix for Cartesian to spherical conversion.
    const Eigen::MatrixXd transformationMatrixCartesianToSpherical
            = transformationMatrixCylindricalToSpherical
            * transformationMatrixCartesianToCylindrical;

    // Perform transformation of velocity vector.
    convertedSphericalState.segment( 3, 3 )
            = transformationMatrixCartesianToSpherical * cartesianState.segment( 3, 3 );

    // Return spherical state vector.
    return convertedSphericalState;
}

 void PointConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
     getJacobian();
     SkeletonDynamics *skel = (SkeletonDynamics*)mBody->getSkel();
     _J[mSkelIndex].block(_rowIndex, 0, 3, skel->getNumDofs()) = mJ;
     Vector3d worldP = xformHom(mBody->getWorldTransform(), mOffset);
     VectorXd qDot = skel->getQDotVector();
     _C.segment(_rowIndex, 3) = worldP - mTarget;
     _CDot.segment(_rowIndex, 3) = mJ * qDot;
 }

void PointConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
    getJacobian();
    dynamics::Skeleton *skel = mBody->getSkeleton();
    _J[mSkelIndex].block(_rowIndex, 0, 3, skel->getDOF()) = mJ;
    Eigen::Vector3d worldP = mBody->getWorldTransform() * mOffset;
    Eigen::VectorXd qDot = skel->get_dq();
    _C.segment(_rowIndex, 3) = worldP - mTarget;
    _CDot.segment(_rowIndex, 3) = mJ * qDot;
}

Eigen::VectorXd IEFSolver::computeCharge_impl(const Eigen::VectorXd & potential, int irrep) const
{
  // The potential and charge vector are of dimension equal to the
  // full dimension of the cavity. We have to select just the part
  // relative to the irrep needed.
  int fullDim = fullPCMMatrix_.rows();
  Eigen::VectorXd charge = Eigen::VectorXd::Zero(fullDim);
  int nrBlocks = blockPCMMatrix_.size();
  int irrDim = fullDim/nrBlocks;
  charge.segment(irrep*irrDim, irrDim) =
    - blockPCMMatrix_[irrep] * potential.segment(irrep*irrDim, irrDim);
  return charge;
}

    void ClosedLoopConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
        getJacobian();
        _J[mSkelIndex2].block(_rowIndex, 0, 3, mBody2->getSkel()->getNumDofs()).setZero();
        _J[mSkelIndex1].block(_rowIndex, 0, 3, mBody1->getSkel()->getNumDofs()) = mJ1;
        _J[mSkelIndex2].block(_rowIndex, 0, 3, mBody2->getSkel()->getNumDofs()) += mJ2;

        Vector3d worldP1 = xformHom(mBody1->getWorldTransform(), mOffset1);
        Vector3d worldP2 = xformHom(mBody2->getWorldTransform(), mOffset2);
        VectorXd qDot1 = ((SkeletonDynamics*)mBody1->getSkel())->getPoseVelocity();
        VectorXd qDot2 = ((SkeletonDynamics*)mBody2->getSkel())->getPoseVelocity();
        _C.segment(_rowIndex, 3) = worldP1 - worldP2;
        _CDot.segment(_rowIndex, 3) = mJ1 * qDot1 + mJ2 * qDot2;
    }

void BallJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::MatrixXd & _J2, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
    getJacobian();
    _J2.block(_rowIndex, 0, 3, mBodyNode2->getSkeleton()->getNumGenCoords()).setZero();
    _J1.block(_rowIndex, 0, 3, mBodyNode1->getSkeleton()->getNumGenCoords()) = mJ1;
    _J2.block(_rowIndex, 0, 3, mBodyNode2->getSkeleton()->getNumGenCoords()) += mJ2;

    Eigen::Vector3d worldP1 = mBodyNode1->getWorldTransform() * mOffset1;
    Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->getGenVels();
    Eigen::VectorXd worldP2 = mBodyNode2->getWorldTransform() * mOffset2;
    Eigen::VectorXd qDot2 = ((dynamics::Skeleton*)mBodyNode2->getSkeleton())->getGenVels();
    
    _C.segment(_rowIndex, 3) = worldP1 - worldP2;
    _CDot.segment(_rowIndex, 3) = mJ1 * qDot1 + mJ2 * qDot2;
}

//! Convert spherical to Cartesian state.
Eigen::VectorXd convertSphericalToCartesianState( const Eigen::VectorXd& sphericalState )
{
    // Create Cartesian state vector, initialized with zero entries.
    Eigen::VectorXd convertedCartesianState = Eigen::VectorXd::Zero( 6 );

    // Create local variables.
    const double radius = sphericalState( 0 );
    const double azimuthAngle = sphericalState( 1 );
    const double elevationAngle = sphericalState( 2 );

    // Precompute sine/cosine of angles, which has multiple usages, to save computation time.
    const double cosineOfElevationAngle = std::cos( elevationAngle );
    const double sineOfElevationAngle = std::sin( elevationAngle );
    const double cosineOfAzimuthAngle = std::cos( azimuthAngle );
    const double sineOfAzimuthAngle = std::sin( azimuthAngle );

    // Set up transformation matrix for spherical to cylindrical conversion.
    Eigen::MatrixXd transformationMatrixSphericalToCylindrical = Eigen::MatrixXd::Zero( 3, 3 );
    transformationMatrixSphericalToCylindrical( 0, 0 ) = cosineOfElevationAngle;
    transformationMatrixSphericalToCylindrical( 0, 2 ) = -sineOfElevationAngle;
    transformationMatrixSphericalToCylindrical( 1, 1 ) = 1.0;
    transformationMatrixSphericalToCylindrical( 2, 0 ) = sineOfElevationAngle;
    transformationMatrixSphericalToCylindrical( 2, 2 ) = cosineOfElevationAngle;

    // Set up transformation matrix for cylindrical to Cartesian conversion.
    Eigen::MatrixXd transformationMatrixCylindricalToCartesian = Eigen::MatrixXd::Zero( 3, 3 );
    transformationMatrixCylindricalToCartesian( 0, 0 ) = cosineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 0, 1 ) = -sineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 1, 0 ) = sineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 1, 1 ) = cosineOfAzimuthAngle;
    transformationMatrixCylindricalToCartesian( 2, 2 ) = 1.0;

    // Compute transformation matrix for spherical to Cartesian conversion.
    const Eigen::MatrixXd transformationMatrixSphericalToCartesian
            = transformationMatrixCylindricalToCartesian
            * transformationMatrixSphericalToCylindrical;

    // Perform transformation of position coordinates.
    convertedCartesianState( 0 ) = radius * cosineOfAzimuthAngle * cosineOfElevationAngle;
    convertedCartesianState( 1 ) = radius * sineOfAzimuthAngle * cosineOfElevationAngle;
    convertedCartesianState( 2 ) = radius * sineOfElevationAngle;

    // Perform transformation of velocity vector.
    convertedCartesianState.segment( 3, 3 ) =
        transformationMatrixSphericalToCartesian * sphericalState.segment( 3, 3 );

    // Return Cartesian state vector.
    return convertedCartesianState;
}

void inertialParametersVectorToUndirectedTreeLoop(const Eigen::VectorXd & parameters_vector,
                                                  UndirectedTree & undirected_tree)
{
    for(int i=0; i < (int)undirected_tree.getNrOfLinks(); i++ ) {
            undirected_tree.getLink(i,false)->setInertia(deVectorize(parameters_vector.segment(i*10,10)));
    }
}

/**
* Performs eigenvector decomposition of an affinity matrix
*
* @param data 		the affinity matrix
* @param numDims	the number of dimensions to consider when clustering
*/
SpectralClustering::SpectralClustering(Eigen::MatrixXd& data, int numDims):
	mNumDims(numDims),
	mNumClusters(0)
{
	Eigen::MatrixXd Deg = Eigen::MatrixXd::Zero(data.rows(),data.cols());

	// calc normalised laplacian 
	for ( int i=0; i < data.cols(); i++) {
		Deg(i,i)=1/(sqrt((data.row(i).sum())) );
	}
	Eigen::MatrixXd Lapla = Deg * data * Deg;

	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> s(Lapla, true);
	Eigen::VectorXd val = s.eigenvalues();
	Eigen::MatrixXd vec = s.eigenvectors();

	//sort eigenvalues/vectors
	int n = data.cols();
	for (int i = 0; i < n - 1; ++i) {
		int k;
		val.segment(i, n - i).maxCoeff(&k);
		if (k > 0) {
			std::swap(val[i], val[k + i]);
			vec.col(i).swap(vec.col(k + i));
		}
	}

	//choose the number of eigenvectors to consider
	if (mNumDims < vec.cols()) {
		mEigenVectors = vec.block(0,0,vec.rows(),mNumDims);
	} else {
		mEigenVectors = vec;
	}
}

//! Function to evaluate a set of double and vector-returning functions and concatenate the results.
Eigen::VectorXd evaluateListOfFunctions(
        const std::vector< boost::function< double( ) > >& doubleFunctionList,
        const std::vector< std::pair< boost::function< Eigen::VectorXd( ) >, int > > vectorFunctionList,
        const int totalSize)
{
    Eigen::VectorXd variableList = Eigen::VectorXd::Zero( totalSize );
    int currentIndex = 0;

    for( unsigned int i = 0; i < doubleFunctionList.size( ); i++ )
    {
        variableList( i ) = doubleFunctionList.at( i )( );
        currentIndex++;
    }

    for( unsigned int i = 0; i < vectorFunctionList.size( ); i++ )
    {
        variableList.segment( currentIndex, vectorFunctionList.at( i ).second ) =
                vectorFunctionList.at( i ).first( );
        currentIndex += vectorFunctionList.at( i ).second;
    }

    // Check consistency with input
    if( currentIndex != totalSize )
    {
        std::string errorMessage = "Error when evaluating lists of functions, sizes are inconsistent: " +
                boost::lexical_cast< std::string >( currentIndex ) + " and " +
                boost::lexical_cast< std::string >( totalSize );
        throw std::runtime_error( errorMessage );
    }

    return variableList;
}

void inertialParametersVectorLoop(const UndirectedTree & undirected_tree,
                                  Eigen::VectorXd & parameters_vector)
{
    for(int i=0; i < (int)undirected_tree.getNrOfLinks(); i++ ) {
            parameters_vector.segment(i*10,10) = Vectorize(undirected_tree.getLink(i)->getInertia());
    }
}

 bool operator() (
     Eigen::VectorXd& newx,
     const Eigen::VectorXd& x,
     const Eigen::VectorXd& g,
     double step_size) {
   newx.noalias() = x - step_size * g;
   for (ssize_t n = 0; n < newx.size(); n += 3)
     newx.segment (n,3).normalize();
   return newx != x;
 }