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

bool computePlaneToPlaneHomography( const Eigen::MatrixXd &points1, const Eigen::MatrixXd &points2, Eigen::MatrixXd &H,
	std::vector<bool> &mask, double reprojectionErrorThreshold )
{
	if( points1.size() != points2.size() )
	{
		throw std::runtime_error( "computePlaneToPlaneHomography: Point lists must be of the same length." );
	}

	if( H.cols() != 3 || H.rows() != 3 )
	{
		H.resize( 3, 3 );
	}

	bool result = false;
		
	// If we have exactly 4 points, compute the homography.
	if( points1.size() == 4 )
	{
		result = compute4PointPlaneToPlaneHomography( points1, points2, H );
	}
	else
	{
		// Otherwise, use RANSAC to remove the outliers.
		Detail::HomographyEstimator estimator(4);
		estimator.setThreshold( reprojectionErrorThreshold );
		result = estimator( points1, points2, H, mask );
	}

	return result;
}

void BaseOfSupport::buildCi(const Eigen::MatrixXd& Ab, const Eigen::MatrixXd& Cp) {
    if (Ab.size() < 1)
        OCRA_ERROR("Reference Ab hasn't been resized");
    if (Cp.size() < 1) 
        OCRA_ERROR("Reference Cp hasn't been resized");
    _Ci.setZero();
    _Ci.block(10,10,4,6) = _Ab*_Cp;
    OCRA_INFO("Built Ci");
}

void parse_rhs(
  const int nrhs, 
  const mxArray *prhs[], 
  Eigen::MatrixXd & V,
  Eigen::MatrixXi & F,
  Eigen::MatrixXd & P,
  Eigen::MatrixXd & N,
  int & num_samples)
{
  using namespace std;
  if(nrhs < 5)
  {
    mexErrMsgTxt("nrhs < 5");
  }

  const int dim = mxGetN(prhs[0]);
  if(dim != 3)
  {
    mexErrMsgTxt("Mesh vertex list must be #V by 3 list of vertex positions");
  }
  if(dim != (int)mxGetN(prhs[1]))
  {
   mexErrMsgTxt("Mesh facet size must be 3");
  }
  if(mxGetN(prhs[2]) != dim)
  {
    mexErrMsgTxt("Point list must be #P by 3 list of origin locations");
  }
  if(mxGetN(prhs[3]) != dim)
  {
    mexErrMsgTxt("Normal list must be #P by 3 list of origin normals");
  }
  if(mxGetN(prhs[4]) != 1 || mxGetM(prhs[4]) != 1)
  {
    mexErrMsgTxt("Number of samples must be scalar.");
  }


  V.resize(mxGetM(prhs[0]),mxGetN(prhs[0]));
  copy(mxGetPr(prhs[0]),mxGetPr(prhs[0])+V.size(),V.data());
  F.resize(mxGetM(prhs[1]),mxGetN(prhs[1]));
  copy(mxGetPr(prhs[1]),mxGetPr(prhs[1])+F.size(),F.data());
  F.array() -= 1;
  P.resize(mxGetM(prhs[2]),mxGetN(prhs[2]));
  copy(mxGetPr(prhs[2]),mxGetPr(prhs[2])+P.size(),P.data());
  N.resize(mxGetM(prhs[3]),mxGetN(prhs[3]));
  copy(mxGetPr(prhs[3]),mxGetPr(prhs[3])+N.size(),N.data());
  if(*mxGetPr(prhs[4]) != (int)*mxGetPr(prhs[4]))
  {
    mexErrMsgTxt("Number of samples should be non negative integer.");
  }
  num_samples = (int) *mxGetPr(prhs[4]);
}

void FixedJoint::v2qdot(const Eigen::Ref<const Eigen::VectorXd>& q, Eigen::MatrixXd& v_to_qdot, Eigen::MatrixXd* dv_to_qdot) const
{
  v_to_qdot.resize(getNumPositions(), getNumVelocities());
  if (dv_to_qdot) {
    dv_to_qdot->setZero(v_to_qdot.size(), getNumPositions());
  }
}

void FixedJoint::qdot2v(const Eigen::Ref<const Eigen::VectorXd>& q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
  qdot_to_v.resize(getNumVelocities(), getNumPositions());
  if (dqdot_to_v) {
    dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
  }
}

void FixedAxisOneDoFJoint::v2qdot(double* q, Eigen::MatrixXd& v_to_qdot, Eigen::MatrixXd* dv_to_qdot) const
{
  v_to_qdot.setIdentity(getNumPositions(), getNumVelocities());
  if (dv_to_qdot) {
    dv_to_qdot->setZero(v_to_qdot.size(), getNumPositions());
  }
}

void FixedAxisOneDoFJoint::qdot2v(double* q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
  qdot_to_v.setIdentity(getNumVelocities(), getNumPositions());
  if (dqdot_to_v) {
    dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
  }
}

void QuaternionFloatingJoint::v2qdot(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& v_to_qdot, Eigen::MatrixXd* dv_to_qdot) const
{
  v_to_qdot.resize(getNumPositions(), getNumVelocities());

  auto quat = q.middleRows<QUAT_SIZE>(SPACE_DIMENSION);
  Matrix3d R = quat2rotmat(quat);

  Matrix<double, QUAT_SIZE, SPACE_DIMENSION> M;
  if (dv_to_qdot) {
    auto dR = dquat2rotmat(quat);
    Gradient<decltype(M), QUAT_SIZE, 1>::type dM;
    angularvel2quatdotMatrix(quat, M, &dM);

    dv_to_qdot->setZero(v_to_qdot.size(), getNumPositions());

    setSubMatrixGradient<4>(*dv_to_qdot, dR, intRange<3>(0), intRange<3>(3), v_to_qdot.rows(), 3);
    auto dMR = matGradMultMat(M, R, dM, dR);
    setSubMatrixGradient<4>(*dv_to_qdot, dMR, intRange<4>(3), intRange<3>(0), v_to_qdot.rows(), 3);
  }
  else {
    angularvel2quatdotMatrix(quat, M, (Gradient<decltype(M), QUAT_SIZE, 1>::type*) nullptr);
  }

  v_to_qdot.block<3, 3>(0, 0).setZero();
  v_to_qdot.block<3, 3>(0, 3) = R;
  v_to_qdot.block<4, 3>(3, 0).noalias() = M * R;
  v_to_qdot.block<4, 3>(3, 3).setZero();
}

void RollPitchYawFloatingJoint::v2qdot(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& v_to_qdot, Eigen::MatrixXd* dv_to_qdot) const
{
  v_to_qdot.setIdentity(getNumPositions(), getNumVelocities());

  if (dv_to_qdot) {
    dv_to_qdot->setZero(v_to_qdot.size(), getNumPositions());
  }
}

void RollPitchYawFloatingJoint::qdot2v(const Eigen::Ref<const VectorXd>& q, Eigen::MatrixXd& qdot_to_v, Eigen::MatrixXd* dqdot_to_v) const
{
  qdot_to_v.setIdentity(getNumVelocities(), getNumPositions());

  if (dqdot_to_v) {
    dqdot_to_v->setZero(qdot_to_v.size(), getNumPositions());
  }
}

bool ClassifyMotion::load_model( const std::string& folder )
{
    m_nb_classes = 8;

    //------------------------------------------------------
    cout << "Load Priors" << endl;
    m_priors.resize( m_nb_classes );

    for( int i=0; i<int(m_priors.size()); i++)
    {
        ostringstream filename;
        filename << "Prior_1_" << i+1 << ".csv";
        //cout << folder + filename.str() << endl;
        Eigen::MatrixXd mat = load_from_csv( folder + filename.str() );
        if(  mat.size() == 0 )
            return false;

        m_priors[i] = mat.transpose();
    }

    //------------------------------------------------------
    cout << "Load Mu" << endl;
    m_mu.resize( m_nb_classes );

    for( int i=0; i<int(m_mu.size()); i++)
    {
        ostringstream filename;
        filename << "Mu_1_" << i+1 << ".csv";
        //cout << folder + filename.str() << endl;
         m_mu[i] = load_from_csv( folder + filename.str() );
        if(  m_mu[i].size() == 0 )
            return false;
    }

    //------------------------------------------------------
    cout << "Load Sigma" << endl;
    m_sigma.resize( m_nb_classes );
    m_nb_states = m_priors[0].size();

    for( int i=0; i<int(m_sigma.size()); i++)
    {
        m_sigma[i].resize( m_nb_states );

        for( int j=0; j<int(m_sigma[i].size()); j++)
        {
            ostringstream filename;
            filename << "Sigma_1_" << j+1 << "_"<< i+1   << ".csv";
            m_sigma[i][j] = load_from_csv( folder + filename.str() );
            if( m_sigma[i][j].size() == 0 )
                return false;
        }
    }

    return true;
}

void PseudoInverse(const ::Eigen::MatrixXd& inputMatrix, ::Eigen::MatrixXd& outputMatrix, const ::Eigen::MatrixXd& weight)
{
	Eigen::MatrixXd tmp;
	if (weight.size() > 0)
		tmp = inputMatrix.transpose() * weight.transpose() * weight * inputMatrix;
	else
		tmp = inputMatrix * inputMatrix.transpose();
	tmp = tmp.inverse();
	outputMatrix =  inputMatrix.transpose() * tmp;

}

void Mesh::relative_to_mesh(Eigen::MatrixXd & C) const
{
  if(C.size() == 0)
  {
    return;
  }
  C = C * rotation.matrix();
  C /= scale;
  C.col(0).array() -= shift(0);
  C.col(1).array() -= shift(1);
  C.col(2).array() -= shift(2);
}

void Mesh::unrelative_to_mesh(Eigen::MatrixXd & C) const
{
  if(C.size() == 0)
  {
    return;
  }
  C.col(0).array() += shift(0);
  C.col(1).array() += shift(1);
  C.col(2).array() += shift(2);
  C *= scale;
  C = C * rotation.conjugate().matrix();
}

void parse_rhs(
  const int nrhs, 
  const mxArray *prhs[], 
  Eigen::MatrixXd & V,
  Eigen::MatrixXi & Ele,
  Eigen::MatrixXd & Q,
  Eigen::MatrixXd & bb_mins,
  Eigen::MatrixXd & bb_maxs,
  Eigen::VectorXi & elements)
{
  using namespace std;
  using namespace igl;
  using namespace igl::matlab;
  mexErrMsgTxt(nrhs >= 3, "The number of input arguments must be >=3.");

  const int dim = mxGetN(prhs[0]);
  mexErrMsgTxt(dim == 3 || dim == 2,
    "Mesh vertex list must be #V by 2 or 3 list of vertex positions");

  mexErrMsgTxt(dim+1 == mxGetN(prhs[1]),
    "Mesh \"face\" simplex size must equal dimension+1");

  parse_rhs_double(prhs,V);
  parse_rhs_index(prhs+1,Ele);
  parse_rhs_double(prhs+2,Q);
  mexErrMsgTxt(Q.cols() == dim,"Dimension of Q should match V");
  if(nrhs > 3)
  {
    mexErrMsgTxt(nrhs >= 6, "The number of input arguments must be 3 or >=6.");
    parse_rhs_double(prhs+3,bb_mins);
    if(bb_mins.size()>0)
    {
      mexErrMsgTxt(bb_mins.cols() == dim,"Dimension of bb_mins should match V");
      mexErrMsgTxt(bb_mins.rows() >= Ele.rows(),"|bb_mins| should be > |Ele|");
    }
    parse_rhs_double(prhs+4,bb_maxs);
    mexErrMsgTxt(bb_maxs.cols() == bb_mins.cols(),
      "|bb_maxs| should match |bb_mins|");
    mexErrMsgTxt(bb_mins.rows() == bb_maxs.rows(),
      "|bb_mins| should match |bb_maxs|");
    parse_rhs_index(prhs+5,elements);
    mexErrMsgTxt(elements.cols() == 1,"Elements should be column vector");
    mexErrMsgTxt(bb_mins.rows() == elements.rows(),
      "|bb_mins| should match |elements|");
  }else
  {
    // Defaults
    bb_mins.resize(0,dim);
    bb_maxs.resize(0,dim);
    elements.resize(0,1);
  }
}