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
temp=(lw.array())*.25 ;
break;
case 3 : // Gauss
temp = ((-.5*(llx.row(1).array().pow(2))).exp()) * invSqrt2pi *
((-.5*(llx.row(0).array().pow(2))).exp()) * invSqrt2pi *
(lw.transpose().array());
break;
case 4 : // GausVar
temp = (lw.transpose().array()) *
((-0.5 * llx.row(0).array().pow(2)).array().exp() * invSqrt2pi).array() *
((-0.5 * llx.row(1).array().pow(2)).array().exp() * invSqrt2pi).array() *
(1.25 - (0.25 * (llx.row(0).array().pow(2))).array()) *
(1.50 - (0.50 * (llx.row(1).array().pow(2))).array());
break;
case 5 : // Quar
temp = (lw.transpose().array()) *
((1.-llx.row(0).array().pow(2)).array().pow(2)).array() *
((1.-llx.row(1).array().pow(2)).array().pow(2)).array() * (225./256.);
break;
}
// make the design matrix
Eigen::MatrixXd X(indxSize ,3);
X.setOnes();
X.col(1) = (lx.row(0).array() - rxygrid(0,i)).array().pow(2);
X.col(2) = (lx.row(1).array() - rxygrid(1,i));
Eigen::LDLT<Eigen::MatrixXd> ldlt_XTWX(X.transpose() * temp.asDiagonal() *X);
Eigen::VectorXd beta = ldlt_XTWX.solve(X.transpose() * temp.asDiagonal() * ly);
mu(i)=beta(0);
} else if ( ly.size() == 1 && !bwCheck) { // Why only one but not two is handled?
mu(i) = ly(0);
} else if ( ly.size() != 1 && (ly.size() < npoly+1) ) {
if ( bwCheck ){
Eigen::VectorXd checker(1);
checker(0) = 0.;
return(checker);
} else {
Rcpp::stop("No enough points in local window, please increase bandwidth.");
}
}
}
if (bwCheck){
Eigen::VectorXd checker(1);
checker(0) = 1.;
return(checker);
}
return ( mu );
}
//==============================================================================
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;
++idx;
// Jacobian for directional friction 1
bodyDirection1.noalias()
= mBodyNode1->getTransform().linear().transpose() * D.col(0);
bodyDirection2.noalias()
= mBodyNode2->getTransform().linear().transpose() * -D.col(0);
// bodyPoint1.noalias()
// = mBodyNode1->getWorldTransform().inverse() * ct.point;
// bodyPoint2.noalias()
// = mBodyNode2->getWorldTransform().inverse() * ct.point;
// std::cout << "bodyDirection2: " << std::endl << bodyDirection2 << std::endl;
mJacobians1[idx].head<3>() = bodyPoint1.cross(bodyDirection1);
mJacobians2[idx].head<3>() = bodyPoint2.cross(bodyDirection2);
mJacobians1[idx].tail<3>() = bodyDirection1;
mJacobians2[idx].tail<3>() = bodyDirection2;
++idx;
// Jacobian for directional friction 2
bodyDirection1.noalias()
= mBodyNode1->getTransform().linear().transpose() * D.col(1);
bodyDirection2.noalias()
= mBodyNode2->getTransform().linear().transpose() * -D.col(1);
// bodyPoint1.noalias()
// = mBodyNode1->getWorldTransform().inverse() * ct.point;
// bodyPoint2.noalias()
// = mBodyNode2->getWorldTransform().inverse() * ct.point;
// std::cout << "bodyDirection2: " << std::endl << bodyDirection2 << std::endl;
mJacobians1[idx].head<3>() = bodyPoint1.cross(bodyDirection1);
mJacobians2[idx].head<3>() = bodyPoint2.cross(bodyDirection2);
mJacobians1[idx].tail<3>() = bodyDirection1;
mJacobians2[idx].tail<3>() = bodyDirection2;
++idx;
}
}
else
{
// Set the dimension of this constraint.
mDim = mContacts.size();
mJacobians1.resize(mDim);
mJacobians2.resize(mDim);
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];
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[i].head<3>().noalias() = bodyPoint1.cross(bodyDirection1);
mJacobians2[i].head<3>().noalias() = bodyPoint2.cross(bodyDirection2);
mJacobians1[i].tail<3>().noalias() = bodyDirection1;
mJacobians2[i].tail<3>().noalias() = bodyDirection2;
}
}
//----------------------------------------------------------------------------
// Union finding
//----------------------------------------------------------------------------
// uniteSkeletons();
}
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");
mitk::IOUtil::Save(probs_2, outputdir + "probs_2.nrrd");
mitk::IOUtil::Save(probs_3, outputdir + "probs_3.nrrd");
// mitk::IOUtil::Save(probs_2, outputdir + "test.nrrd");
return EXIT_SUCCESS;
}
