envire::smurf::GraphLoader getLoaderWithStructuredGraph(const smurf::Robot & robot)
{
envire::core::Transform iniPose;
std::shared_ptr<envire::core::EnvireGraph> transformGraph(new envire::core::EnvireGraph) ;
iniPose.transform.orientation = base::Quaterniond::Identity();
iniPose.transform.translation << 1.0, 1.0, 1.0;
envire::smurf::GraphLoader graphLoader(transformGraph, iniPose);
envire::core::FrameId center = "center";
transformGraph->addFrame(center);
graphLoader.loadStructure(transformGraph->getVertex(center), robot);
return graphLoader;
}
void updateSigma2_MII(int *seed,
const int *nTry,
int *nAccept,
const double *epsilon,
double *sigma2,
const int *Q,
const int *G,
const int *S,
const double *x,
const int *psi,
const double *nu,
const int *delta,
const double *Delta,
const double *gamma2,
const double *r,
const double *rho,
const double *phi,
const double *t,
const double *l,
const double *tau2R,
const double *tau2Rho,
const double *a,
const double *b,
const double *Omega,
const int *nClique,
const int *oldClique,
const int *nOldComponents,
const int *nNewComponents,
const int *oldComponents) {
unsigned int seedU = (unsigned int) *seed;
vector<vector<vector<double> > > OmegaTransformed;
vector<int> oldCliqueTransformed;
vector<vector<int> > oldComponentsTransformed;
transformGraph(nClique,oldClique,nOldComponents,oldComponents,oldCliqueTransformed,oldComponentsTransformed);
transformOmega(nClique,nOldComponents,nNewComponents,Omega,OmegaTransformed);
updateSigma2_HyperInverseWishart(&seedU,*nTry,nAccept,*epsilon,sigma2,*Q,*G,S,x,psi,nu,
delta,Delta,*gamma2,r,rho,phi,t,l,tau2R,tau2Rho,a,b,
OmegaTransformed,oldCliqueTransformed,oldComponentsTransformed);
*seed = (int) seedU;
return;
}
void updateBDDeltaStar_MII(unsigned int *seed,
int *nTry,
int *nAccept,
double *epsilon,
double *b,
double *Delta,
const int *Q,
const int *G,
const int *S,
const double *x,
const int *psi,
const double *nu,
const int *delta,
const double *r,
const double *sigma2,
const double *phi,
const double *tau2R,
const double *pB0,
const double *pB1,
const double *alphaB,
const double *betaB,
const double *Omega,
const int *nClique,
const int *oldClique,
const int *nOldComponents,
const int *nNewComponents,
const int *oldComponents) {
unsigned int seedU = (unsigned int) *seed;
vector<vector<vector<double> > > OmegaTransformed;
vector<int> oldCliqueTransformed;
vector<vector<int> > oldComponentsTransformed;
transformGraph(nClique,oldClique,nOldComponents,oldComponents,oldCliqueTransformed,oldComponentsTransformed);
transformOmega(nClique,nOldComponents,nNewComponents,Omega,OmegaTransformed);
updateBDDeltaStar_HyperInverseWishart(&seedU,*nTry,nAccept,*epsilon,b,Delta,*Q,
*G,S,x,psi,nu,delta,r,sigma2,phi,tau2R,
*pB0,*pB1,*alphaB,*betaB,
OmegaTransformed,oldCliqueTransformed,
oldComponentsTransformed);
*seed = (int) seedU;
}
void updateOmega_MII(unsigned int *seed,
int *nAccept,
double *Omega,
const int *Q,
const int *G,
const double *Delta,
const double *r,
const double *sigma2,
const double *tau2R,
const double *b,
const double *zeta,
const double *D,
const int *nClique,
const int *oldClique,
const int *nOldComponents,
const int *nNewComponents,
const int *oldComponents) {
unsigned int seedU = (unsigned int) *seed;
vector<vector<vector<double> > > OmegaTransformed;
vector<vector<vector<double> > > DTransformed;
vector<int> oldCliqueTransformed;
vector<vector<int> > oldComponentsTransformed;
transformGraph(nClique,oldClique,nOldComponents,oldComponents,oldCliqueTransformed,oldComponentsTransformed);
transformOmega(nClique,nOldComponents,nNewComponents,D,DTransformed);
transformOmega(nClique,nOldComponents,nNewComponents,Omega,OmegaTransformed);
updateOmega_HyperInverseWishart(&seedU,nAccept,OmegaTransformed,*Q,*G,Delta,r,sigma2,
tau2R,b,*zeta,DTransformed,oldCliqueTransformed,
oldComponentsTransformed);
inverseTransformOmega(OmegaTransformed,Omega);
*seed = (int) seedU;
}
