C++ Matrix3d::block方法代码示例

//! Function to numerical compute the partial derivative of an acceleration w.r.t. a body state.
Eigen::Matrix3d calculateAccelerationWrtStatePartials(
        boost::function< void( basic_mathematics::Vector6d ) > setBodyState,
        boost::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > accelerationModel,
        basic_mathematics::Vector6d originalState,
        Eigen::Vector3d statePerturbation,
        int startIndex,
        boost::function< void( ) > updateFunction )
{
    Eigen::Matrix3d upAccelerations = Eigen::Matrix3d::Zero( );
    Eigen::Matrix3d downAccelerations = Eigen::Matrix3d::Zero( );

    basic_mathematics::Vector6d perturbedState = originalState;

    // Calculate perturbed accelerations for up-perturbed state entries.
    for( int i = 0; i < 3; i++ )
    {
        perturbedState( i + startIndex ) += statePerturbation( i );
        setBodyState( perturbedState );
        updateFunction( );
        upAccelerations.block( 0, i, 3, 1 ) = basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >(
                    accelerationModel );
        accelerationModel->resetTime( TUDAT_NAN );
        perturbedState = originalState;
    }

    // Calculate perturbed accelerations for down-perturbed state entries.
    for( int i = 0; i < 3; i++ )
    {
        perturbedState( i + startIndex ) -= statePerturbation( i );
        setBodyState( perturbedState );
        updateFunction( );
        downAccelerations.block( 0, i, 3, 1 ) = basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >(
                    accelerationModel );
        accelerationModel->resetTime( TUDAT_NAN );
        perturbedState = originalState;
    }

    // Reset state/environment to original state.
    setBodyState( perturbedState );
    updateFunction( );

    basic_astrodynamics::updateAndGetAcceleration< Eigen::Vector3d >(
                        accelerationModel );

    // Numerically compute partial derivatives.
    Eigen::Matrix3d accelerationPartials = upAccelerations - downAccelerations;
    for( int i = 0; i < 3; i++ )
    {
        accelerationPartials.block( 0, i, 3, 1 ) /= ( 2.0 * statePerturbation( i ) );
    }

    return accelerationPartials;
}

/**
 * @function getPlaneTransform
 * @brief Assumes plane coefficients are of the form ax+by+cz+d=0, normalized
*/
Eigen::Matrix4d getPlaneTransform ( pcl::ModelCoefficients &coeffs,
				    double up_direction,
				    bool flatten_plane ) {
  Eigen::Matrix4d tf = Eigen::Matrix4d::Identity();
  if( coeffs.values.size() <= 3 ) {
    std::cout << "[ERROR] Coefficient size less than 3. I will output nonsense values"<<std::endl;
    return tf;
  }
  double a = coeffs.values[0];
  double b = coeffs.values[1];
  double c = coeffs.values[2];
  double d = coeffs.values[3];
  //asume plane coefficients are normalized
  Eigen::Vector3d position(-a*d, -b*d, -c*d);
  Eigen::Vector3d z(a, b, c);
  //if we are flattening the plane, make z just be (0,0,up_direction)
  if(flatten_plane) {
    z << 0, 0, up_direction;
  }
  else {
    //make sure z points "up" (the plane normal and the table must have a > 90 angle, hence the cosine must be negative)
    if ( z.dot( Eigen::Vector3d(0, 0, up_direction) ) > 0) {
      z = -1.0 * z;
      printf("Changing sign \n");
      coeffs.values[0]*= -1; coeffs.values[1]*= -1; coeffs.values[2]*= -1; coeffs.values[3]*= -1;
    }
  }
  //try to align the x axis with the x axis of the original frame
  //or the y axis if z and x are too close too each other
  Eigen::Vector3d x; x << 1, 0, 0;
  if ( fabs(z.dot(x)) > 1.0 - 1.0e-4) x = Eigen::Vector3d(0, 1, 0);
  Eigen::Vector3d y = z.cross(x); y.normalized();
  x = y.cross(z); x.normalized();
  Eigen::Matrix3d rotation;
  rotation.block(0,0,3,1) = x;
  rotation.block(0,1,3,1) = y;
  rotation.block(0,2,3,1) = z;
  Eigen::Quaterniond orientation( rotation );
  tf.block(0,0,3,3) = orientation.matrix();
  tf.block(0,3,3,1) = position;
  return tf;
}

//! Function to compute the partial w.r.t. time-independent empirical acceleration components
void EmpiricalAccelerationPartial::wrtEmpiricalAccelerationCoefficientFromIndices(
        const int numberOfAccelerationComponents,
        const std::map< basic_astrodynamics::EmpiricalAccelerationFunctionalShapes, std::vector< int > >& accelerationIndices,
        Eigen::MatrixXd& partial )
{
    // Retrieve rotation matrix to inertial frame
    Eigen::Matrix3d rotationMatrix = Eigen::Matrix3d( empiricalAcceleration_->getCurrentToInertialFrame( ) );

    // Initialize partial derivatives
    partial = Eigen::Matrix< double, 3, Eigen::Dynamic >::Zero( 3, numberOfAccelerationComponents );

    // Iterate over all terms, and set partial
    int currentIndex = 0;
    double multiplier = 0.0;
    for( std::map<  basic_astrodynamics::EmpiricalAccelerationFunctionalShapes, std::vector< int > >::const_iterator
         indexIterator = accelerationIndices.begin( );
         indexIterator != accelerationIndices.end( ); indexIterator++ )
    {
        // Get multiplier associated with functional shape of empirical acceleration
        switch( indexIterator->first )
        {
        case basic_astrodynamics::constant_empirical:
            multiplier = 1.0;
            break;
        case basic_astrodynamics::sine_empirical:
            multiplier = std::sin( empiricalAcceleration_->getCurrentTrueAnomaly( ) );
            break;
        case basic_astrodynamics::cosine_empirical:
            multiplier = std::cos( empiricalAcceleration_->getCurrentTrueAnomaly( ) );
            break;
        default:
            throw std::runtime_error(
                        "Error when calculating partial w.r.t. empirical accelerations, could not find functional shape " );
        }

        // Set partial value for current component and shape
        for( unsigned int i = 0; i < indexIterator->second.size( ); i++ )
        {
            partial.block( 0, currentIndex, 3, 1 ) = multiplier * rotationMatrix.block ( 0, indexIterator->second.at( i ), 3, 1 );
            currentIndex++;
        }

    }
}