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

Eigen::VectorXd Rrotatedmullwlsk( const Eigen::Map<Eigen::VectorXd> & bw, const std::string kernel_type, const Eigen::Map<Eigen::MatrixXd> & tPairs, const Eigen::Map<Eigen::MatrixXd> & cxxn, const Eigen::Map<Eigen::VectorXd> & win,  const Eigen::Map<Eigen::MatrixXd> & xygrid, const unsigned int npoly, const bool & bwCheck){ 

  // tPairs : xin (in MATLAB code)
  // cxxn : yin (in MATLAB code)
  // xygrid: d (in MATLAB code)
  // npoly: redundant?

  const double invSqrt2pi=  1./(sqrt(2.*M_PI));

  // Map the kernel name so we can use switches  
  std::map<std::string,int> possibleKernels; 
  possibleKernels["epan"]    = 1;   possibleKernels["rect"]    = 2;
  possibleKernels["gauss"]   = 3;   possibleKernels["gausvar"] = 4; 
  possibleKernels["quar"]    = 5; 
   
  // The following test is here for completeness, we mightwant to move it up a 
  // level (in the wrapper) in the future. 

  // If the kernel_type key exists set KernelName appropriately
  int KernelName = 0;
  if ( possibleKernels.count( kernel_type ) != 0){ 
    KernelName = possibleKernels.find( kernel_type )->second; //Set kernel choice
  } else {
  // otherwise use "epan"as the kernel_type 
  // Rcpp::Rcout << "Kernel_type argument was not set correctly; Epanechnikov kernel used." << std::endl;
    Rcpp::warning("Kernel_type argument was not set correctly; Epanechnikov kernel used.");
    KernelName = possibleKernels.find( "epan" )->second;;
  }

  // Check that we do not have zero weights // Should do a try-catch here
  // Again this might be best moved a level-up. 
  if ( !(win.all()) ){  // 
    Rcpp::Rcout << "Cases with zero-valued windows are not yet implemented" << std::endl;
    return (tPairs);
  } 

  Eigen::Matrix2d RC;  // Rotation Coordinates
  RC << 1, -1, 1, 1; 
  RC *= sqrt(2.)/2.; 

  Eigen::MatrixXd rtPairs = RC * tPairs;
  Eigen::MatrixXd rxygrid = RC * xygrid;
  
  unsigned int xygridN = rxygrid.cols();  
  
  Eigen::VectorXd mu(xygridN);
  mu.setZero();     

  for (unsigned int i = 0; i !=  xygridN; ++i){   

    //locating local window (LOL) (bad joke)
    std::vector <unsigned int> indx; 
    //if the kernel is not Gaussian or Gaussian-like
    if ( KernelName != 3 && KernelName != 4 ) { 
      //construct listX as vectors / size is unknown originally
      std::vector <unsigned int> list1, list2; 
      for (unsigned int y = 0; y != tPairs.cols(); y++){ 
        if ( std::abs( rtPairs(0,y) - rxygrid(0,i) ) <= bw(0)  ) {
          list1.push_back(y);
        }         
        if ( std::abs( rtPairs(1,y) - rxygrid(1,i) ) <= bw(1)  ) {
          list2.push_back(y);
        }
      }
      
    //get intersection between the two lists 
    std::set_intersection(list1.begin(), list1.begin() + list1.size(), list2.begin(), list2.begin() + list2.size(), std::back_inserter(indx));   
  
    } else { // just get the whole deal
      for (unsigned int y = 0; y != tPairs.cols(); ++y){
        indx.push_back(y);
      }
    }   

    unsigned int indxSize = indx.size();
    Eigen::VectorXd lw(indxSize);  
    Eigen::VectorXd ly(indxSize);
    Eigen::MatrixXd lx(2,indxSize);
    for (unsigned int u = 0; u !=indxSize; ++u){ 
      lx.col(u) = rtPairs.col(indx[u]); 
      lw(u) = win(indx[u]); 
      ly(u) = cxxn(indx[u]); 
    }


    if (ly.size()>=npoly+1 && !bwCheck ){

      //computing weight matrix 
      Eigen::VectorXd temp(indxSize);
      Eigen::MatrixXd llx(2, indxSize );  
      llx.row(0) = (lx.row(0).array() - rxygrid(0,i))/bw(0);  
      llx.row(1) = (lx.row(1).array() - rxygrid(1,i))/bw(1); 
 
      //define the kernel used 
      switch (KernelName){
        case 1: // Epan
          temp=  ((1-llx.row(0).array().pow(2))*(1- llx.row(1).array().pow(2))).array() * 
                 ((9./16)*lw).transpose().array(); 
          break;  
        case 2 : // Rect
//.........这里部分代码省略.........

//==============================================================================
ContactConstraint::ContactConstraint(const collision::Contact& _contact)
  : Constraint(),
    mFirstFrictionalDirection(Eigen::Vector3d::UnitZ()),
    mIsFrictionOn(true),
    mAppliedImpulseIndex(-1),
    mIsBounceOn(false),
    mActive(false)
{
  // TODO(JS): Assumed single contact
  mContacts.push_back(_contact);

  // TODO(JS):
  mBodyNode1 = _contact.bodyNode1;
  mBodyNode2 = _contact.bodyNode2;

  //----------------------------------------------
  // Bounce
  //----------------------------------------------
  mRestitutionCoeff = mBodyNode1->getRestitutionCoeff()
                      * mBodyNode2->getRestitutionCoeff();
  if (mRestitutionCoeff > DART_RESTITUTION_COEFF_THRESHOLD)
    mIsBounceOn = true;
  else
    mIsBounceOn = false;

  //----------------------------------------------
  // Friction
  //----------------------------------------------
  // TODO(JS): Assume the frictional coefficient can be changed during
  //           simulation steps.
  // Update mFrictionalCoff
  mFrictionCoeff = std::min(mBodyNode1->getFrictionCoeff(),
                            mBodyNode2->getFrictionCoeff());
  if (mFrictionCoeff > DART_FRICTION_COEFF_THRESHOLD)
  {
    mIsFrictionOn = true;

    // Update frictional direction
    updateFirstFrictionalDirection();
  }
  else
  {
    mIsFrictionOn = false;
  }

  // Compute local contact Jacobians expressed in body frame
  if (mIsFrictionOn)
  {
    // Set the dimension of this constraint. 1 is for Normal direction constraint.
    // TODO(JS): Assumed that the number of contact is not static.
    // TODO(JS): Adjust following code once use of mNumFrictionConeBases is
    //           implemented.
    //  mDim = mContacts.size() * (1 + mNumFrictionConeBases);
    mDim = mContacts.size() * 3;

    mJacobians1.resize(mDim);
    mJacobians2.resize(mDim);

    // Intermediate variables
    size_t idx = 0;

    Eigen::Vector3d bodyDirection1;
    Eigen::Vector3d bodyDirection2;

    Eigen::Vector3d bodyPoint1;
    Eigen::Vector3d bodyPoint2;

    for (size_t i = 0; i < mContacts.size(); ++i)
    {
      const collision::Contact& ct = mContacts[i];

      // TODO(JS): Assumed that the number of tangent basis is 2.
      Eigen::MatrixXd D = getTangentBasisMatrixODE(ct.normal);

      assert(std::fabs(ct.normal.dot(D.col(0))) < DART_EPSILON);
      assert(std::fabs(ct.normal.dot(D.col(1))) < DART_EPSILON);
//      if (D.col(0).dot(D.col(1)) > 0.0)
//        std::cout << "D.col(0).dot(D.col(1): " << D.col(0).dot(D.col(1)) << std::endl;
      assert(fabs(D.col(0).dot(D.col(1))) < DART_EPSILON);

//      std::cout << "D: " << std::endl << D << std::endl;

      // Jacobian for normal contact
      bodyDirection1.noalias()
          = mBodyNode1->getTransform().linear().transpose() * ct.normal;
      bodyDirection2.noalias()
          = mBodyNode2->getTransform().linear().transpose() * -ct.normal;

      bodyPoint1.noalias()
          = mBodyNode1->getTransform().inverse() * ct.point;
      bodyPoint2.noalias()
          = mBodyNode2->getTransform().inverse() * ct.point;

      mJacobians1[idx].head<3>() = bodyPoint1.cross(bodyDirection1);
      mJacobians2[idx].head<3>() = bodyPoint2.cross(bodyDirection2);

      mJacobians1[idx].tail<3>() = bodyDirection1;
      mJacobians2[idx].tail<3>() = bodyDirection2;

//.........这里部分代码省略.........

bool pre_draw(igl::viewer::Viewer & viewer)
{
  using namespace Eigen;
  using namespace std;
  if(resolve)
  {
    MatrixXd bc(b.size(),V.cols());
    VectorXd Beq(3*b.size());
    for(int i = 0;i<b.size();i++)
    {
      bc.row(i) = V.row(b(i));
      switch(i%4)
      {
        case 2:
          bc(i,0) += 0.15*bbd*sin(0.5*anim_t);
          bc(i,1) += 0.15*bbd*(1.-cos(0.5*anim_t));
          break;
        case 1:
          bc(i,1) += 0.10*bbd*sin(1.*anim_t*(i+1));
          bc(i,2) += 0.10*bbd*(1.-cos(1.*anim_t*(i+1)));
          break;
        case 0:
          bc(i,0) += 0.20*bbd*sin(2.*anim_t*(i+1));
          break;
      }
      Beq(3*i+0) = bc(i,0);
      Beq(3*i+1) = bc(i,1);
      Beq(3*i+2) = bc(i,2);
    }
    switch(mode)
    {
      case MODE_TYPE_ARAP:
        igl::arap_solve(bc,arap_data,U);
        break;
      case MODE_TYPE_ARAP_GROUPED:
        igl::arap_solve(bc,arap_grouped_data,U);
        break;
      case MODE_TYPE_ARAP_DOF:
      {
        VectorXd L0 = L;
        arap_dof_update(arap_dof_data,Beq,L0,30,0,L);
        const auto & Ucol = M*L;
        U.col(0) = Ucol.block(0*U.rows(),0,U.rows(),1);
        U.col(1) = Ucol.block(1*U.rows(),0,U.rows(),1);
        U.col(2) = Ucol.block(2*U.rows(),0,U.rows(),1);
        break;
      }
    }
    viewer.data.set_vertices(U);
    viewer.data.set_points(bc,sea_green);
    viewer.data.compute_normals();
    if(viewer.core.is_animating)
    {
      anim_t += anim_t_dir;
    }else
    {
      resolve = false;
    }
  }
  return false;
}

int main(int argc, char* argv[])
{
  mitkCommandLineParser parser;
  parser.setArgumentPrefix("--", "-");

  // required params
  parser.addArgument("inputdir", "i", mitkCommandLineParser::InputDirectory, "Input Directory", "Contains input feature files.", us::Any(), false);
  parser.addArgument("outputdir", "o", mitkCommandLineParser::OutputDirectory, "Output Directory", "Destination of output files.", us::Any(), false);
  parser.addArgument("classmask", "m", mitkCommandLineParser::InputFile, "Class mask image", "Contains several classes.", us::Any(), false);

  // optional params
  parser.addArgument("select", "s", mitkCommandLineParser::String, "Item selection", "Using Regular expression, seperated by space e.g.: '*.nrrd *.vtk *test*'",std::string("*.nrrd"),true);
  parser.addArgument("treecount", "tc", mitkCommandLineParser::Int, "Treecount", "Number of trees.",50,true);
  parser.addArgument("treedepth", "td", mitkCommandLineParser::Int, "Treedepth", "Maximal tree depth.",50,true);
  parser.addArgument("minsplitnodesize", "min", mitkCommandLineParser::Int, "Minimum split node size.", "Minimum split node size.",2,true);
  parser.addArgument("precision", "p", mitkCommandLineParser::Float, "Split precision.", "Precision.", mitk::eps,true);
  parser.addArgument("fraction", "f", mitkCommandLineParser::Float, "Fraction of samples per tree.", "Fraction of samples per tree.", 0.6f,true);
  parser.addArgument("replacment", "r", mitkCommandLineParser::Bool, "Sample with replacement.", "Sample with replacement.", true,true);

  // Miniapp Infos
  parser.setCategory("Classification Tools");
  parser.setTitle("Random Forest Training");
  parser.setDescription("Vigra RF impl.");
  parser.setContributor("MBI");

  // Params parsing
  map<string, us::Any> parsedArgs = parser.parseArguments(argc, argv);
  if (parsedArgs.size()==0)
    return EXIT_FAILURE;

  std::string inputdir = us::any_cast<string>(parsedArgs["inputdir"]);
  std::string outputdir = us::any_cast<string>(parsedArgs["outputdir"]);
  std::string classmask = us::any_cast<string>(parsedArgs["classmask"]);

  int treecount = parsedArgs.count("treecount") ? us::any_cast<int>(parsedArgs["treecount"]) : 50;
  int treedepth = parsedArgs.count("treedepth") ? us::any_cast<int>(parsedArgs["treedepth"]) : 50;
  int minsplitnodesize = parsedArgs.count("minsplitnodesize") ? us::any_cast<int>(parsedArgs["minsplitnodesize"]) : 2;
  float precision = parsedArgs.count("precision") ? us::any_cast<float>(parsedArgs["precision"]) : mitk::eps;
  float fraction = parsedArgs.count("fraction") ? us::any_cast<float>(parsedArgs["fraction"]) : 0.6;
  bool withreplacement = parsedArgs.count("replacment") ? us::any_cast<float>(parsedArgs["replacment"]) : true;
  std::string filt_select =/* parsedArgs.count("select") ? us::any_cast<string>(parsedArgs["select"]) :*/ "*.nrrd";

  QString filter(filt_select.c_str());

  // **** in principle repeat this block to create a feature matrix X_all for all subjects (in dir)
  // Get nrrd filepath
  QDir dir(inputdir.c_str());
  auto strl = dir.entryList(filter.split(" "),QDir::Files);

  // load class mask
  mitk::Image::Pointer mask = mitk::IOUtil::LoadImage(classmask);
  unsigned int num_samples = 0;
  mitk::CLUtil::CountVoxel(mask,num_samples);

  // initialize featurematrix [num_samples, num_featureimages]
  Eigen::MatrixXd X(num_samples, strl.size());

  for(int i = 0 ; i < strl.size(); i++)
  {
    // load feature image
    mitk::Image::Pointer img = mitk::IOUtil::LoadImage(inputdir + strl[i].toStdString());
    // transfom it into a [num_samples, 1] vector depending on the classmask
    Eigen::MatrixXd _x = mitk::CLUtil::Transform<double>(img,mask);
    // replace i-th (empty) col with feature vector in _x
    X.block(0,i,num_samples,1) = _x;
  }
  // ****

  // transform classmask into the label-vector [num_samples, 1]
  Eigen::MatrixXi Y = mitk::CLUtil::Transform<int>(mask,mask);

  mitk::VigraRandomForestClassifier::Pointer classifier = mitk::VigraRandomForestClassifier::New();
  classifier->SetTreeCount(treecount);
  classifier->SetMaximumTreeDepth(treedepth);
  classifier->SetMinimumSplitNodeSize(minsplitnodesize);
  classifier->SetPrecision(precision);
  classifier->SetSamplesPerTree(fraction);
  classifier->UseSampleWithReplacement(withreplacement);

  classifier->PrintParameter();
  classifier->Train(X,Y);

  MITK_INFO << classifier->IsEmpty();

  // no metainformations are saved currently
  // only the raw vigra rf data
  mitk::IOUtil::Save(classifier, outputdir + "RandomForest.hdf5");

  Eigen::MatrixXi Y_pred = classifier->Predict(X);
  Eigen::MatrixXd Probs = classifier->GetPointWiseProbabilities();

  MITK_INFO << Y_pred.rows() << " " << Y_pred.cols();
  MITK_INFO << Probs.rows() << " " << Probs.cols();

  //    mitk::Image::Pointer prediction = mitk::CLUtil::Transform<int>(Y_pred,mask);
  mitk::Image::Pointer probs_1 = mitk::CLUtil::Transform<double>(Probs.col(0),mask);
  mitk::Image::Pointer probs_2 = mitk::CLUtil::Transform<double>(Probs.col(1),mask);
  mitk::Image::Pointer probs_3 = mitk::CLUtil::Transform<double>(Probs.col(2),mask);

  mitk::IOUtil::Save(probs_1, outputdir + "probs_1.nrrd");
//.........这里部分代码省略.........