Matrix yarp::math::projectionMatrix(const Matrix &A, double tol)
{
Matrix out(A.rows(),A.rows());
projectionMatrix(A, out, tol);
return out;
}
void LHENode<ImageSigT>::buildPDFGrid(const Matrix<float>& img)
{
int rows = img.rows();
int cols = img.cols();
//down sample
m_down_rows = int(ceil(rows / m_spatial_sampling + 0.5f));
m_down_cols = int(ceil(cols / m_spatial_sampling + 0.5f));
m_down_depth = int(ceil(1.0f / m_gray_range_sampling + 0.5f));
//allocate pdf memory
if (m_pdf_grid)
{
delete []m_pdf_grid;
m_pdf_grid = NULL;
}
m_pdf_grid = new float[m_down_rows * m_down_cols * m_down_depth];
memset(m_pdf_grid,0,sizeof(float) * m_down_rows * m_down_cols * m_down_depth);
for (int i = 0; i < rows; ++i)
{
const float* img_data = img.row(i);
for (int j = 0; j < cols; ++j)
{
int down_i = int(floor(i / m_spatial_sampling + 0.5f));
int down_j = int(floor(j / m_spatial_sampling + 0.5f));
int down_d = int(floor(img_data[j] / m_gray_range_sampling + 0.5f));
int down_index = down_d * m_down_rows * m_down_cols + down_i * m_down_cols + down_j;
m_pdf_grid[down_index] = m_pdf_grid[down_index] + 1.0f;
}
}
//transform count to probability
float* depth_counts = new float[m_down_rows * m_down_cols];
memset(depth_counts,0,sizeof(float) * m_down_rows * m_down_cols);
for (int depth = 0; depth < m_down_depth; ++depth)
{
int slice_index = depth * m_down_rows * m_down_cols;
for (int i = 0; i < m_down_rows; ++i)
{
int row_index = i * m_down_cols;
for (int j = 0; j < m_down_cols; ++j)
{
depth_counts[row_index + j] += m_pdf_grid[slice_index + row_index +j];
}
}
}
for (int i = 0; i < m_down_rows; ++i)
{
int row_index = i * m_down_cols;
for (int j = 0; j < m_down_cols; ++j)
{
if(depth_counts[row_index + j] == 0.0f)
depth_counts[row_index + j] = 1.0f;
}
}
for (int depth = 0; depth < m_down_depth; ++depth)
{
int slice_index = depth * m_down_rows * m_down_cols;
for (int i = 0; i < m_down_rows; ++i)
{
int row_index = i * m_down_cols;
for (int j = 0; j < m_down_cols; ++j)
{
m_pdf_grid[slice_index + row_index +j] /= depth_counts[row_index + j];
}
}
}
delete []depth_counts;
}
void movemesh()
{
int num_steps = 1;
int step = 1; // just for info of total number of steps
generateDG();
testdg = dg;
cout << "DGmove: movemesh(): Calculating containing elements and area coordinates for each interior point\n";
// cycle over interior points
for(int ipoin = 0; ipoin < ninpoin; ipoin++)
{
// first find containing DG element by "walking-through" the DG
dat = dg.find_containing_triangle_and_area_coords(points(ipoin,0), points(ipoin,1), ninpoin/2);
// store DG element and area coords in points
points(ipoin,2) = dat.elem;
for(int i = 0; i < ndim+1; i++)
points(ipoin,i+3) = dat.areacoords[i];
}
while(num_steps > 0)
{
cout << "DGmove: movemesh(): Step " << step << endl;
// update coordinates in dgpoints using bmotion
cout << "DGmove: movemesh(): Moving the Delaunay graph\n";
int k = 0;
//testdgpoints.zeros();
for(int i = 0; i < bmot->rows(); i++)
{
if(bflag(i) == 1)
{
for(int j = 0; j < ndim; j++)
testdgpoints(k,j) = dgpoints(k,j) + bmot->get(i,j);
k++;
}
}
// check for validity of Delaunay graph
movegraph(testdg, testdgpoints);
testdg.compute_jacobians();
check = testdg.detect_negative_jacobians();
cout << "DGmove: movemesh(): Check is " << check << endl;
if(check == true)
{
testdgpoints = dgpoints; // reset testdgpoints to original values
testdg = dg; // reset the DG as well
//halve the boundary motion
for(int i = 0; i < bmot->rows(); i++)
for(int j = 0; j < bmot->cols(); j++)
(*bmot)(i,j) = bmot->get(i,j)/2;
num_steps++;
continue;
}
else
{
dgpoints = testdgpoints;
movegraph(dg, dgpoints);
num_steps--;
}
// calculate new positions of interior points by mapping them to deformed DG elements using area coordinates calculated before
cout << "DGmove: movemesh(): Moving the interior points\n";
int elem;
for(int ipoin = 0; ipoin < ninpoin; ipoin++)
{
elem = points(ipoin,2);
for(int dim = 0; dim < ndim; dim++)
{
points(ipoin,dim) = 0.0;
for(int i = 0; i < ndim+1; i++)
points(ipoin,dim) += points(ipoin,i+3)*dgpoints(dginpoel(elem,i),dim);
}
}
if(num_steps > 0)
{
cout << "DGmove: movemesh(): Re-triangulating\n";
dg.clear();
dg.setup(&dgpoints,ndgpoin);
generateDG();
// re-calculate area-coords
for(int ipoin = 0; ipoin < ninpoin; ipoin++)
{
// first find containing DG element by "walking-through" the DG
dat = dg.find_containing_triangle_and_area_coords(points(ipoin,0), points(ipoin,1), ninpoin/2);
// store DG element and area coords in points
points(ipoin,2) = dat.elem;
for(int i = 0; i < ndim+1; i++)
points(ipoin,i+3) = dat.areacoords[i];
}
}
step++;
}
}
// Training of client Speakers
// Input: Xlist Format: ID_Client Seg1 Seg2 ..
// Output: ALIZE_MixtureServer (binaire) + GMM / Client (binary)
int TrainTarget(Config& config)
{
String inputClientListFileName = config.getParam("targetIdList");
String inputWorldFilename = config.getParam("inputWorldFilename");
String outputSERVERFilename = "";
if (config.existsParam("mixtureServer")) outputSERVERFilename =config.getParam("mixtureServer");
bool initByClient=false; // In this case, the init model is read from the file
if (config.existsParam("initByClient")) initByClient=config.getParam("initByClient").toBool();
bool saveEmptyModel=false;
if (config.existsParam("saveEmptyModel")) saveEmptyModel=config.getParam("saveEmptyModel").toBool();
// label for selected frames - Only the frames associated with this label, in the label files, will be used
bool fixedLabelSelectedFrame=true;
String labelSelectedFrames;
if (config.existsParam("useIdForSelectedFrame")) // the ID of each speaker is used as labelSelectedFrame ?
fixedLabelSelectedFrame=(config.getParam("useIdForSelectedFrame").toBool()==false);
if (fixedLabelSelectedFrame) // the label is decided by the command line and is unique for the run
labelSelectedFrames=config.getParam("labelSelectedFrames");
bool modelData=false;
if (config.existsParam("useModelData")) modelData=config.getParam("useModelData").toBool();
String initModelS=inputWorldFilename;
if (modelData) if (config.existsParam("initModel")) initModelS=config.getParam("initModel"); // Use a specific model for Em init
bool outputAdaptParam=false;
if (config.existsParam("superVector")) outputAdaptParam=true;
bool NAP=false;
Matrix <double> ChannelMatrix;
if (config.existsParam("NAP")) {
if (verbose) cout<< "Removing channel effect with NAP from " << config.getParam("NAP") << " of size: [";
NAP=true; // enable NAP
ChannelMatrix.load(config.getParam("NAP"),config); //get Channel Matrix from args and load in a Matrix object
if (verbose) cout << ChannelMatrix.rows() << "," <<ChannelMatrix.cols() << "]" << endl;
}
bool saveCompleteServer=false;
try{
XList inputClientList(inputClientListFileName,config); // read the Id + filenames for each client
XLine * linep;
inputClientList.getLine(0);
MixtureServer ms(config);
StatServer ss(config, ms);
if (verbose) cout << "TrainTarget - Load world model [" << inputWorldFilename<<"]"<<endl;
MixtureGD& world = ms.loadMixtureGD(inputWorldFilename);
MixtureGD& initModel =ms.loadMixtureGD(initModelS);
if (verbose) cout <<"Use["<<initModelS<<"] for initializing EM"<<endl;
// *********** Target loop *****************
while ((linep=inputClientList.getLine()) != NULL){ // linep gives the XLine with the Id of a given client and the list of files
String *id=linep->getElement(); // Get the Client ID (id)
XLine featureFileListp=linep->getElements(); // Get the list of feature file for the client (end of the line)
if (verbose) cout << "Train model ["<<*id<<"]"<<endl;
if (!fixedLabelSelectedFrame){ // the ID is used as label for selecting the frame
labelSelectedFrames=*id;
if (verbose) cout <<*id<<" is used for label selected frames"<<endl;
}
FeatureServer fs(config,featureFileListp); // Reading the features (from several files)
SegServer segmentsServer; // Create the segment server for managing the segments/clusters
LabelServer labelServer; // Create the lable server, for indexing the segments/clusters
initializeClusters(featureFileListp,segmentsServer,labelServer,config); // Reading the segmentation files for each feature input file
verifyClusterFile(segmentsServer,fs,config); // Verify if the segments ending before the end of the feature files...
MixtureGD & adaptedMixture = ms.duplicateMixture(world,DUPL_DISTRIB); // Creating final as a copy of the world model
MixtureGD & clientMixture= ms.duplicateMixture(world,DUPL_DISTRIB);
if (initByClient){ // During trainig data statistic estimation by EM,
clientMixture= ms.loadMixtureGD(*id); // the client model is used for initalization
adaptedMixture=clientMixture;
}
long codeSelectedFrame=labelServer.getLabelIndexByString(labelSelectedFrames); // Get the index of the cluster with in interest audio segments
if (codeSelectedFrame==-1){ // No data for this model !!!!!!!!!!!!!!
cout << " WARNING - NO DATA FOR TRAINING ["<<*id<<"]";
if (saveEmptyModel){
cout <<" World model is returned"<<endl; // In this case, the client model is the world model
if (verbose) cout << "Save client model ["<<*id<<"]" << endl;
adaptedMixture.save(*id, config); // Save the client model
}
}
else{
SegCluster& selectedSegments=segmentsServer.getCluster(codeSelectedFrame); // Gives the cluster of the selected/used segments
if (!initByClient) ms.setMixtureId(clientMixture,*id); // Set the client model Id
if (modelData) modelBasedadaptModel(config,ss,ms,fs,selectedSegments,world,clientMixture,initModel); // EM algo with MAP criterion
else adaptModel(config,ss,ms,fs,selectedSegments,world,clientMixture); // EM algo with MAP criterion
if (NAP) {
if (verbose) cout << "NAP on SVs" << endl;
computeNap(clientMixture,ChannelMatrix);
}
if (outputAdaptParam) {
RealVector<double> v;
getSuperVector(v,world,clientMixture,config);
String out=config.getParam("saveVectorFilesPath")+*id+config.getParam("vectorFilesExtension");
Matrix <double> vv=(Matrix<double>)v;
vv.save(out,config);
}
if (!outputAdaptParam) {
if (verbose) cout << "Save client model ["<<*id<<"]" << endl;
clientMixture.save(*id, config); // Save the client model
}
if (!saveCompleteServer){
long tid=ms.getMixtureIndex(*id); // TO BE SUPPRESSED BY
ms.deleteMixtures(tid,tid); // ADDING a delete on a mixture pointor
ms.deleteUnusedDistribs();
}
}
} // end of the the target loop
// Save the complete mixture server
// TODO
} // fin try
catch (Exception& e)
{
cout << e.toString().c_str() << endl;
}
return 0;
}
int main()
{
/******************************* [ signal ] ******************************/
Vector<Type> t = linspace( Type(0), Type(Ls-1), Ls ) / Type(Fs);
Vector<Type> s = sin( Type(400*PI) * pow(t,Type(2.0)) );
/******************************** [ widow ] ******************************/
t = linspace(Type(0),Type(Lg-1),Lg);
Vector<Type> g = gauss( t, (Lg-1)/Type(2), Lg/Type(8) );
/********************************* [ WFT ] *******************************/
cout << "Taking windowed Fourier transform." << endl;
Matrix< complex<Type> > coefs = wft( s, g );
/******************************** [ IWFT ] *******************************/
cout << "Taking inverse windowed Fourier transform." << endl;
Vector<Type> x = iwft( coefs, g );
cout << "The relative error is : " << "norm(s-x) / norm(s) = "
<< norm(s-x)/norm(s) << endl << endl;
/******************************** [ PLOT ] *******************************/
Engine *ep = engOpen( NULL );
if( !ep )
{
cerr << "Cannot open Matlab Engine!" << endl;
exit(1);
}
Matrix<Type> C = trT( abs(coefs) );
int M = C.rows(), N = C.cols();
// define mxArray as 1-by-1 Real Scalar
mxArray *mFs = mxCreateDoubleMatrix( 1, 1, mxREAL );
// define mxArray as N-by-1 Real Vector
mxArray *ms = mxCreateDoubleMatrix( Ls, 1, mxREAL );
mxArray *mx = mxCreateDoubleMatrix( Ls, 1, mxREAL );
// define mxArray as N-by-M Real Matrix, BECAUSE matalb is ROW MAJOR
// and C/C++ is COLUMN MAJOR, the row of SP++ matrix is copied as the
// column of Matlab matrix
mxArray *mC = mxCreateDoubleMatrix( N, M, mxREAL );
// array copy from Scalar to mxArray.
memcpy( mxGetPr(mFs), &Fs, sizeof(Type) );
// array copy from Vectors to mxArray.
memcpy( mxGetPr(ms), s, Ls*sizeof(Type) );
memcpy( mxGetPr(mx), x, Ls*sizeof(Type) );
// array copy from Matrix to mxArray.
memcpy( mxGetPr(mC), C, C.size()*sizeof(Type) );
// send command to Matlab engine
engPutVariable( ep, "fs", mFs );
engPutVariable( ep, "s", ms );
engPutVariable( ep, "x", mx );
engPutVariable( ep, "C", mC );
// Matlab commands
const char *mCmd = " \
figure('name','C++ and Matlab Mixed Programming Testing'); \
hFN = floor(size(C,1)/2); tN = size(C,2); \
subplot(2,2,1); plot((0:tN-1), s); \
axis([0,tN,min(s),max(s)]); \
xlabel('Time (ms)', 'fontsize',12); ylabel('Amplitude', 'fontsize',12); \
title('(a)'); \
subplot(2,2,2); pcolor((0:tN-1),(0:hFN)'/hFN, C(1:hFN+1,:)); \
shading interp; \
axis([0,tN, 0,1]); \
yt = 0 : 0.2 : 1; set(gca, 'YTick',yt); set(gca, 'YTickLabel',fs*yt/2); \
xlabel('Time (ms)','fontsize',12); ylabel('Frequency (Hz)','fontsize',12); \
title('(b)'); \
subplot(2,2,3); plot((0:tN-1),x); \
axis([0,tN,min(x),max(x)]); \
xlabel('Time (ms)','fontsize',12); \
ylabel('Amplitude', 'fontsize',12); \
title('(c)'); \
subplot(2,2,4); e=s-x; plot((0:tN-1),e); \
axis([0,tN,min(e),max(e)]); \
xlabel('Time (ms)','fontsize',12); \
ylabel('Amplitude', 'fontsize',12); \
title('(d)'); \
";
// send command to Matlab engine
engEvalString( ep, mCmd );
// delete mxArray
mxDestroyArray( mFs );
mxDestroyArray( ms );
mxDestroyArray( mx );
mxDestroyArray( mC );
system( "pause" );
engClose(ep);
return 0;
}
virtual void
getJacobian(RigidBodyDynamics::Model& robot, const Vector& Q, Matrix& Jc)
{
// Calculate COP
if(rxnFrFrame_==0) //know the reaction forces in world frame
RigidBodyDynamics::Extras::calcCOPfromFrCOM(robot, Q, masterNode_,
rxnForceCOM_, contactNormal_, contactPlanePoint_, worldCOP_,
RigidBodyDynamics::Extras::Frame::WORLD, //frame of reaction forces
RigidBodyDynamics::Extras::Frame::LOCAL, //frame of contact normal
RigidBodyDynamics::Extras::Frame::LOCAL, //frame of contact plane point
RigidBodyDynamics::Extras::Frame::WORLD); //frame of returned COP value
else //know the reaction forces in local frame
RigidBodyDynamics::Extras::calcCOPfromFrCOM(robot, Q, masterNode_,
rxnForceCOM_, contactNormal_, contactPlanePoint_, worldCOP_,
RigidBodyDynamics::Extras::Frame::LOCAL, //frame of reaction forces
RigidBodyDynamics::Extras::Frame::LOCAL, //frame of contact normal
RigidBodyDynamics::Extras::Frame::LOCAL, //frame of contact plane point
RigidBodyDynamics::Extras::Frame::WORLD); //frame of returned COP value
p = this->lookupParameter("worldCOP");
s = p->set(worldCOP_);
localCOP_ = RigidBodyDynamics::CalcBaseToBodyCoordinates(robot,Q,masterNode_,worldCOP_,false);
p = this->lookupParameter("localCOP");
s = p->set(localCOP_);
Vector rxnForceCOP_(6);
Vector worldCOM_(3);
worldCOM_ = RigidBodyDynamics::CalcBaseToBodyCoordinates(robot,Q,masterNode_,robot.mBodies[masterNode_].mCenterOfMass,false);
//TODO: double check wrench transformation--sign error. NOTE: also effectos calcCOP methods
rxnForceCOP_.head(3) = rxnForceCOM_.head(3);
rxnForceCOP_.tail(3) = rxnForceCOM_.tail(3) + RigidBodyDynamics::Math::VectorCrossMatrix(rxnForceCOM_.head(3))*(worldCOP_-worldCOM_);
// Check size of incoming Jc
assert( isInitialized() && (Jc.rows()==6) && (Jc.cols()==robot.dof_count) );
// Since flag set to false, Q shouldn't be used in calculation
RigidBodyDynamics::CalcPointJacobian(robot,
Q,
masterNode_,
localCOP_,
Jpv,
false);
// Since flag set to false, Q shouldn't be used in calculation
RigidBodyDynamics::CalcPointJacobianW(robot,
Q,
masterNode_,
localCOP_,
Jpw,
false);
// Set Jc
Jc.topRows(3) = Jpv;
Jc.bottomRows(3) = Jpw;
}
MSize(const Matrix& mat):__m(mat.rows()),__n(mat.cols()){}
/* Solve a linear programming problem using the simplex method.
Good resource: http://www.slideshare.net/sbishop2/simplex-algorithm
*/
int simplex(Matrix &A, Matrix &b)
{
/* we assume that we're given this in a tableau form.
Matrix A contains all the variables, and
Matrib b contains all the values.
We also assume that the first row of the Matrices
contains the values for the objective function.
*/
while(true) // there's a break statement inside.
{
/* STEP 1: Look at the negative values in the first row of A
to determine the pivot column. We want the largest negative
value (i.e. the smallest value), because removing that bit
will increase the objective function the most. For example,
if we have z -10x -20y = 0, the y is lowering the value more.
*/
MatrixIterator min_e = min_element(A.begin(),A.begin() + A.cols());
int pivotCol = min_e - A.begin();
/* if the minimum value is non-negative, we have found
the optimal solution.
*/
if (*min_e >= 0)
{
/* FINAL STEP: Find the values of the variables.
any variable that is still in the objective function
has to have a value of 0, so we can zero those columns
out.
*/
for (int i=0;i<A.cols();i++)
{
if (A(0,i) > 0)
{
for (int j=0;j<A.rows();j++)
{
A(j,i) = 0;
}
}
}
return b(0,0);
}
/* STEP 2: (Temporarily) Divide all the values in Matrix b by
the corresponding value in that row that is part of the pivot column
*/
Matrix c = b;
for (int i=0;i<b.rows();i++)
{
c(i,0) = c(i,0) / A(i,pivotCol);
}
/* STEP 3: The least value in b tells us the pivot row.
The smallest value is the constraining value; it's the
reason that our result can't be bigger. It's constrained
by this smallest value.
*/
MatrixIterator min_val = min_element(c.begin()+1,c.end());
int pivotRow = min_val - c.begin();
/* STEP 4: Divide pivot row by pivot value so pivot becomes 1
*/
double divisor = A(pivotRow,pivotCol);
for (int i=0;i<A.cols();i++)
{
A(pivotRow,i) = A(pivotRow,i) / divisor;
}
b(pivotRow,0) = b(pivotRow,0) / divisor;
/* STEP 5: Zero out the pivot column:
*/
for (int i=0;i<A.rows();i++)
{
if (i==pivotRow) continue; // no point doing it for the same row.
double ratio = A(i,pivotCol) / A(pivotRow, pivotCol);
for (int j=0;j<A.cols();j++)
{
A(i,j) -= ratio * A(pivotRow,j);
}
b(i,0) -= ratio * b(pivotRow,0);
}
}
return -10;
}
// Given two matrices A and b, solves the linear system of equations
// assuming the form Ax = b. Uses Gaussian Elimination with backsubstitution.
Matrix gaussianElimination(Matrix& A, Matrix& b)
{
// First let's get it in row-echelon form.
// This code is actually very similar to the code for
// gaussJordan(). The big difference is we set j=i
// as the starting condition in a for loop so we only
// work on the lower triangle.
Matrix Aug = A;
Aug.appendCol(b); // make the augmented matrix
int i;
for (i=0;i<A.cols();i++)
{
int goodRow = findNonZero(Aug, i);
// either we don't have enough equations, or there's
// a logical error, like -1 = 0. Either way, no solution.
if (goodRow < 0)
{
shared_ptr<ColumnVector> n(new ColumnVector(0));
return *n;
}
// swap the good row with the row we want a 1 in.
// we want 1's on the diagonal, so the row we want
// a 2 in is the same # as the column that we're on.
Aug.swapRows(i, goodRow);
// Now we want to zero out this column for all other rows.
// In this row (in the diagonal position) we want a 1.
for (int j=i;j<A.rows();j++)
{
if (j==i) {
// we want a 1 here.
double scalar = Aug(i,i);
for (int k=0;k<Aug.cols();k++)
{
Aug(i,k) = Aug(i,k) * (1.0/scalar);
}
}else{
// we want a 0 here
double multiplier = Aug(j,i) / Aug(i,i);
for (int k=0;k<Aug.cols();k++)
{
Aug(j,k) = Aug(j,k) - (Aug(i,k) * multiplier);
}
}
}
}
// At this point, our Matrix is in row echelon form.
// Now we do backsubstitution.
// start from the last row, work our way up.
for (int x=Aug.rows()-2;x>=0;x--)
{
for (int y=A.cols()-1;y>x;y--)
{
// set the augmented columns values to the answer.
for (int z=A.cols();z<A.cols() + b.cols();z++)
{
Aug(x,z) = Aug(x,z) - (Aug(x,y) * Aug(y,z));
}
// set the substituted column to zero since we moved it
// to the other side of the = sign.
Aug(x,y) = 0;
}
}
// at this point, the augmented part of Aug contains our solution.
// Let's return it.
shared_ptr<Matrix> res(new Matrix(b.rows(), b.cols()));
for (int x=0;x<A.rows();x++)
{
for (int y=A.cols();y<A.cols() + b.cols();y++)
{
(*res)(x,y-A.cols()) = Aug(x,y);
}
}
return *res;
}
bool ElasticityNorm::evalInt (LocalIntegral& elmInt, const FiniteElement& fe,
const Vec3& X) const
{
Elasticity& problem = static_cast<Elasticity&>(myProblem);
ElmNorm& pnorm = static_cast<ElmNorm&>(elmInt);
// Evaluate the inverse constitutive matrix at this point
Matrix Cinv;
if (!problem.formCinverse(Cinv,fe,X))
return false;
// Evaluate the finite element stress field
Vector sigmah, sigma, error;
if (!problem.evalSol(sigmah,pnorm.vec,fe,X))
return false;
bool planeStrain = sigmah.size() == 4 && Cinv.rows() == 3;
if (planeStrain) sigmah.erase(sigmah.begin()+2); // Remove the sigma_zz
double detJW = fe.detJxW;
if (problem.isAxiSymmetric())
detJW *= 2.0*M_PI*X.x;
size_t ip = 0;
// Integrate the energy norm a(u^h,u^h)
pnorm[ip++] += sigmah.dot(Cinv*sigmah)*detJW;
if (problem.haveLoads())
{
// Evaluate the body load
Vec3 f = problem.getBodyforce(X);
// Evaluate the displacement field
Vec3 u = problem.evalSol(pnorm.vec.front(),fe.N);
// Integrate the external energy (f,u^h)
pnorm[ip] += f*u*detJW;
}
ip++;
if (anasol)
{
// Evaluate the analytical stress field
sigma = (*anasol)(X);
if (sigma.size() == 4 && Cinv.rows() == 3)
sigma.erase(sigma.begin()+2); // Remove the sigma_zz if plane strain
// Integrate the energy norm a(u,u)
pnorm[ip++] += sigma.dot(Cinv*sigma)*detJW;
// Integrate the error in energy norm a(u-u^h,u-u^h)
error = sigma - sigmah;
pnorm[ip++] += error.dot(Cinv*error)*detJW;
}
// Integrate the volume
pnorm[ip++] += detJW;
size_t i, j, k;
for (i = 0; i < pnorm.psol.size(); i++)
if (!pnorm.psol[i].empty())
{
// Evaluate projected stress field
Vector sigmar(sigmah.size());
for (j = k = 0; j < nrcmp && k < sigmar.size(); j++)
if (!planeStrain || j != 2)
sigmar[k++] = pnorm.psol[i].dot(fe.N,j,nrcmp);
// Integrate the energy norm a(u^r,u^r)
pnorm[ip++] += sigmar.dot(Cinv*sigmar)*detJW;
// Integrate the error in energy norm a(u^r-u^h,u^r-u^h)
error = sigmar - sigmah;
pnorm[ip++] += error.dot(Cinv*error)*detJW;
double l2u = sigmar.norm2();
double l2e = error.norm2();
// Integrate the L2-norm (sigma^r,sigma^r)
pnorm[ip++] += l2u*l2u*detJW;
// Integrate the error in L2-norm (sigma^r-sigma^h,sigma^r-sigma^h)
pnorm[ip++] += l2e*l2e*detJW;
if (anasol)
{
// Integrate the error in the projected solution a(u-u^r,u-u^r)
error = sigma - sigmar;
pnorm[ip++] += error.dot(Cinv*error)*detJW;
ip++; // Make room for the local effectivity index here
}
}
return true;
}
boost::tuple<Matrix, Matrix, Matrix> LUPDecompose(Matrix A)
{
/* although not required ofr general LU decomposition, we require
matrices to be square. */
assert(A.rows()==A.cols());
Matrix L(A.rows(),A.cols());
shared_ptr<Matrix> P(new Matrix(A.rows(),A.cols())); // permutation matrix
L.populateIdentity();
P->populateIdentity();
Matrix Lfinal(L);
/* gaussian Elimination to get us in upper triangular form. */
for (int c=0;c<A.cols();c++)
{
int goodRow = findNonZero(A, c);
// either we don't have enough equations, or there's
// a logical error, like -1 = 0. Either way, no solution.
if (goodRow < 0)
{
shared_ptr<Matrix> l(new Matrix(0,0));
shared_ptr<Matrix> u(new Matrix(0,0));
shared_ptr<Matrix> p(new Matrix(0,0));
return boost::make_tuple(*l,*u,*p);
}
// swap the good row with the row we want a 1 in.
// we want 1's on the diagonal, so the row we want
// a 2 in is the same # as the column that we're on.
A.swapRows(c, goodRow);
// adjust L-inverse if we did have to swap a row
// (i.e. the good row wasn't the current diagonal)
if (goodRow != c)
{
(*P)(c,c) = 0;
(*P)(c,goodRow) = 1;
(*P)(goodRow,c) = 1;
(*P)(goodRow,goodRow) = 0;
}
for (int r=c+1;r<A.rows();r++)
{
if (r==c) {
// we want a 1 here.
double scalar = A(c,c);
/* set L-inverse to reflect this. */
L(c,c) = 1.0/scalar;
for (int k=0;k<A.cols();k++)
{
/* multiply through by 1/x to make the current cell = 1. */
A(r,k) = A(r,k) * (1.0/scalar);
}
}else{
// we want a 0 here
if (A(r,c) == 0) continue;
double multiplier = A(r,c) / A(c,c);
/* set L-inverse to reflect this. */
L(r,c) = multiplier;
for (int k=0;k<A.cols();k++)
{
A(r,k) = A(r,k) - (A(c,k) * multiplier);
}
}
/*
Lfinal = L*Lfinal;
L.populateIdentity();
*/
}
}
Matrix *L2 = &(L);
Matrix *U = &A;
return boost::make_tuple(*L2, *U, *P);
}
bool Elasticity::formBmatrix (Matrix& Bmat, const Matrix& dNdX) const
{
const size_t nenod = dNdX.rows();
const size_t nstrc = nsd*(nsd+1)/2;
Bmat.resize(nstrc*nsd,nenod,true);
if (dNdX.cols() < nsd)
{
std::cerr <<" *** Elasticity::formBmatrix: Invalid dimension on dNdX, "
<< dNdX.rows() <<"x"<< dNdX.cols() <<"."<< std::endl;
return false;
}
#define INDEX(i,j) i+nstrc*(j-1)
switch (nsd) {
case 1:
// Strain-displacement matrix for 1D elements:
//
// [B] = | d/dx | * [N]
for (size_t i = 1; i <= nenod; i++)
Bmat(1,i) = dNdX(i,1);
break;
case 2:
// Strain-displacement matrix for 2D elements:
//
// | d/dx 0 |
// [B] = | 0 d/dy | * [N]
// | d/dy d/dx |
for (size_t i = 1; i <= nenod; i++)
{
// Normal strain part
Bmat(INDEX(1,1),i) = dNdX(i,1);
Bmat(INDEX(2,2),i) = dNdX(i,2);
// Shear strain part
Bmat(INDEX(3,1),i) = dNdX(i,2);
Bmat(INDEX(3,2),i) = dNdX(i,1);
}
break;
case 3:
// Strain-displacement matrix for 3D elements:
//
// | d/dx 0 0 |
// | 0 d/dy 0 |
// [B] = | 0 0 d/dz | * [N]
// | d/dy d/dx 0 |
// | d/dz 0 d/dx |
// | 0 d/dz d/dy |
for (size_t i = 1; i <= nenod; i++)
{
// Normal strain part
Bmat(INDEX(1,1),i) = dNdX(i,1);
Bmat(INDEX(2,2),i) = dNdX(i,2);
Bmat(INDEX(3,3),i) = dNdX(i,3);
// Shear strain part
Bmat(INDEX(4,1),i) = dNdX(i,2);
Bmat(INDEX(4,2),i) = dNdX(i,1);
Bmat(INDEX(5,2),i) = dNdX(i,3);
Bmat(INDEX(5,3),i) = dNdX(i,2);
Bmat(INDEX(6,1),i) = dNdX(i,3);
Bmat(INDEX(6,3),i) = dNdX(i,1);
}
break;
default:
std::cerr <<" *** Elasticity::formBmatrix: nsd="<< nsd << std::endl;
return false;
}
#undef INDEX
Bmat.resize(nstrc,nsd*nenod);
return true;
}
void mass_inertia_explicit_form(Model const & model, Matrix & mass_inertia,
std::ostream * dbgos)
throw(std::runtime_error)
{
size_t const ndof(model.getNDOF());
mass_inertia = Matrix::Zero(ndof, ndof);
if (dbgos) {
*dbgos << "jspace::mass_inertia_explicit_form()\n"
<< " ndof = " << ndof << "\n";
}
for (size_t ii(0); ii < ndof; ++ii) {
if (dbgos) {
*dbgos << " computing contributions of node " << ii << "\n";
}
taoDNode * node(model.getNode(ii));
if ( ! node) {
ostringstream msg;
msg << "jspace::mass_inertia_explicit_form(): no node for index " << ii;
throw runtime_error(msg.str());
}
Transform global_com;
Matrix Jacobian;
deVector3 const * com(node->center());
if (com) {
if ( ! model.computeGlobalFrame(node, com->elementAt(0), com->elementAt(1), com->elementAt(2), global_com)) {
ostringstream msg;
msg << "jspace::mass_inertia_explicit_form(): computeGlobalFrame() of COM failed for index " << ii;
throw runtime_error(msg.str());
}
}
else {
// pretend the COM is at the node origin
if ( ! model.getGlobalFrame(node, global_com)) {
ostringstream msg;
msg << "jspace::mass_inertia_explicit_form(): getGlobalFrame() failed for index " << ii;
throw runtime_error(msg.str());
}
}
if ( ! model.computeJacobian(node, global_com.translation(), Jacobian)) {
ostringstream msg;
msg << "jspace::mass_inertia_explicit_form(): computeJacobian() of COM failed for index " << ii;
throw runtime_error(msg.str());
}
if (dbgos) {
if (com) {
*dbgos << " local COM: " << *com << "\n";
}
else {
*dbgos << " local COM: null\n";
}
*dbgos << " global COM: " << pretty_string(global_com.translation()) << "\n"
<< " Jacobian:\n" << pretty_string(Jacobian, " ");
}
if (Jacobian.rows() != 6) {
ostringstream msg;
msg << "jspace::mass_inertia_explicit_form(): Jacobian of node " << ii
<< " has " << Jacobian.rows() << " rows instead of 6";
throw runtime_error(msg.str());
}
if (static_cast<size_t>(Jacobian.cols()) != ndof) {
ostringstream msg;
msg << "jspace::mass_inertia_explicit_form(): Jacobian of node " << ii
<< " has " << Jacobian.cols() << " columns instead of " << ndof;
throw runtime_error(msg.str());
}
// A problem that we will be having with TAO's inertia info a
// bit further down is that it contains the contribution from
// the mass, and we have to remove that before proceeding with
// the rotational contribution to the mass-inertia matrix. Thus
// the introduction of Im.
Eigen::Matrix3d Im(Eigen::Matrix3d::Zero());
deFloat const * mass(node->mass());
if (mass) {
Matrix const mass_contrib(Jacobian.block(0, 0, 3, ndof).transpose() * Jacobian.block(0, 0, 3, ndof));
mass_inertia += *mass * mass_contrib;
if (com) {
double const xx(pow(com->elementAt(0), 2));
double const yy(pow(com->elementAt(1), 2));
double const zz(pow(com->elementAt(2), 2));
double const xy(com->elementAt(0) * com->elementAt(1));
double const xz(com->elementAt(0) * com->elementAt(2));
double const yz(com->elementAt(1) * com->elementAt(2));
Im <<
yy+zz, -xy, -xz,
-xy, zz+xx, -yz,
-xz, -yz, xx+yy;
Im *= *mass;
}
if (dbgos) {
*dbgos << " mass: " << *mass << "\n"
<< " JvT x Jv:\n" << pretty_string(mass_contrib, " ")
<< " contribution of mass:\n" << pretty_string(*mass * mass_contrib, " ")
<< " Im (for subsequent use):\n" << pretty_string(Im, " ");
}
}
else if (dbgos) {
*dbgos << " no mass\n";
}
// I think this is expressed wrt node origin, not COM. But in
// any case, what matters is the instantaneous rotational
// velocity due to joint ii, which is the same throughout the
// entire link. Given that the COM frame is assumed to be
// aligned with the node origin frame anyway, all we have to do
// is express the rotational contribution of the Jacobian in the
// local frame and use that.
deMatrix3 const * inertia(node->inertia());
if (inertia) {
// NOTE: global_com.rotation() would require SVD, but we know
// that we are dealing with an affine transform, so taking
// what Eign2 calls the "linear" part is fine, because that's
// simply the 3x3 upper left block of the homogeneous
// transformation matrix. Hopefully anyway.
// Use the transpose to get the inverse of the rotation.
Matrix const J_omega(global_com.linear().transpose() * Jacobian.block(3, 0, 3, ndof));
Eigen::Matrix3d Ic;
Ic <<
inertia->elementAt(0, 0), inertia->elementAt(0, 1), inertia->elementAt(0, 2),
inertia->elementAt(1, 0), inertia->elementAt(1, 1), inertia->elementAt(1, 2),
inertia->elementAt(2, 0), inertia->elementAt(2, 1), inertia->elementAt(2, 2);
mass_inertia += J_omega.transpose() * (Ic - Im) * J_omega;
if (dbgos) {
*dbgos << " Ic:\n" << pretty_string(Ic, " ")
<< " Ic - Im:\n" << pretty_string(Ic - Im, " ")
<< " J_omega:\n" << pretty_string(J_omega, " ")
<< " JoT x (Ic-Im) x Jo:\n"
<< pretty_string(J_omega.transpose() * (Ic - Im) * J_omega, " ");
}
}
else if (dbgos) {
*dbgos << " no inertia\n";
}
if (dbgos) {
*dbgos << " mass_inertia so far:\n" << pretty_string(mass_inertia, " ");
}
}
}
void GaussianWhiteNoise::checkMatrix_(const Matrix & m) const
{
(void)m;//avoid warning
BOOST_ASSERT(unsigned(m.rows())==dim_ && unsigned(m.cols())==dim_ && "ERROR: Matrix incorrecly dimemsioned");
}
// test the transform functions
TEST(SBAtest, SimpleSystem)
{
// set up full system
SysSBA sys;
// set of world points
// each row is a point
Matrix<double,23,4> pts;
pts << 0.5, 0.2, 3, 1.0,
1, 0, 2, 1.0,
-1, 0, 2, 1.0,
0, 0.2, 3, 1.0,
1, 1, 2, 1.0,
-1, -1, 2, 1.0,
1, 0.2, 4, 1.0,
0, 1, 2.5, 1.0,
0, -1, 2.5, 1.0,
0.2, 0, 3, 1.0,
-1, 1, 2.5, 1.0,
1, -1, 2.5, 1.0,
0.5, 0.2, 4, 1.0,
0.2, -1.3, 2.5, 1.0,
-0.5, -1, 2.5, 1.0,
0.2, -0.7, 3, 1.0,
-1, 1, 3.5, 1.0,
1, -1, 3.5, 1.0,
0.5, 0.2, 4.6, 1.0,
-1, 0, 4, 1.0,
0, 0, 4, 1.0,
1, 1, 4, 1.0,
-1, -1, 4, 1.0;
for (int i=0; i<pts.rows(); i++)
{
Point pt(pts.row(i));
sys.addPoint(pt);
}
Node::initDr(); // set up fixed matrices
// set of nodes
// three nodes, one at origin, two displaced
CamParams cpars = {300,300,320,240,0.1}; // 300 pix focal length, 0.1 m baseline
Quaternion<double> frq2(AngleAxisd(10*M_PI/180,Vector3d(.2,.3,.4).normalized())); // frame rotation in the world
Vector4d frt2(0.2, -0.4, 1.0, 1.0); // frame position in the world
Quaternion<double> frq3(AngleAxisd(10*M_PI/180,Vector3d(-.2,.1,-.3).normalized())); // frame rotation in the world
Vector4d frt3(-0.2, 0.4, 1.0, 1.0); // frame position in the world
Vector3d b(cpars.tx,0,0);
// set up nodes
Node nd1;
Vector4d qr(0,0,0,1);
nd1.qrot = qr; // no rotation
nd1.trans = Vector4d::Zero(); // or translation
nd1.setTransform(); // set up world2node transform
nd1.setKcam(cpars); // set up node2image projection
#ifdef LOCAL_ANGLES
nd1.setDr(true); // set rotational derivatives
#else
nd1.setDr(false); // set rotational derivatives
#endif
nd1.isFixed = true;
Node nd2;
nd2.qrot = frq2;
cout << "Quaternion: " << nd2.qrot.coeffs().transpose() << endl;
nd2.trans = frt2;
cout << "Translation: " << nd2.trans.transpose() << endl << endl;
nd2.setTransform(); // set up world2node transform
nd2.setKcam(cpars); // set up node2image projection
#ifdef LOCAL_ANGLES
nd2.setDr(true); // set rotational derivatives
#else
nd2.setDr(false); // set rotational derivatives
#endif
nd2.isFixed = false;
Node nd3;
nd3.qrot = frq3;
cout << "Quaternion: " << nd3.qrot.coeffs().transpose() << endl;
nd3.trans = frt3;
cout << "Translation: " << nd3.trans.transpose() << endl << endl;
nd3.setTransform(); // set up world2node transform
nd3.setKcam(cpars); // set up node2image projection
#ifdef LOCAL_ANGLES
nd3.setDr(true); // set rotational derivatives
#else
nd3.setDr(false); // set rotational derivatives
#endif
nd3.isFixed = false;
sys.nodes.push_back(nd1);
sys.nodes.push_back(nd2);
sys.nodes.push_back(nd3);
// set up projections onto nodes
int ind = 0;
double inoise = 0.5;
Vector3d n2;
for(unsigned int i = 0; i < sys.tracks.size(); i++)
{
Point pt = sys.tracks[i].point;
ProjMap &prjs = sys.tracks[i].projections; // new point track
Proj prj;
prj.isValid = true;
prj.stereo = true;
Vector2d ipt;
Vector3d ipt3,pc;
n2.setRandom();
nd1.project2im(ipt,pt); // set up projection measurement
prj.ndi = 0; // nd1 index
ipt3.head(2) = ipt;
pc = nd1.Kcam * (nd1.w2n*pt - b); // camera coords for right cam
ipt3(2) = pc(0)/pc(2);
prj.kp = ipt3 + n2*inoise;
prjs[prjs.size()] = prj;
n2.setRandom();
nd2.project2im(ipt,pt); // set up projection measurement
prj.ndi = 1; // nd2 index
ipt3.head(2) = ipt;
pc = nd2.Kcam * (nd2.w2n*pt - b); // camera coords for right cam
ipt3(2) = pc(0)/pc(2);
prj.kp = ipt3 + n2*inoise;
prjs[prjs.size()] = prj;
n2.setRandom();
nd3.project2im(ipt,pt); // set up projection measurement
prj.ndi = 2; // nd3 index
ipt3.head(2) = ipt;
pc = nd3.Kcam * (nd3.w2n*pt - b); // camera coords for right cam
ipt3(2) = pc(0)/pc(2);
prj.kp = ipt3 + n2*inoise;
prjs[prjs.size()] = prj;
//sys.tracksSt.push_back(prjs);
ind++;
}
#ifdef WRITE_GRAPH
writeGraphFile("sba.graph",sys);
#endif
double qnoise = 10*M_PI/180; // in radians
double tnoise = 0.1; // in meters
// add random noise to node positions
nd2.qrot.coeffs().head<3>() += qnoise*Vector3d::Random();
nd2.normRot();
cout << "Quaternion: " << nd2.qrot.coeffs().transpose() << endl << endl;
nd2.trans.head<3>() += tnoise*Vector3d::Random();
nd2.setTransform(); // set up world2node transform
nd2.setProjection();
#ifdef LOCAL_ANGLES
nd2.setDr(true); // set rotational derivatives
#else
nd2.setDr(false); // set rotational derivatives
#endif
sys.nodes[1] = nd2; // reset node
nd3.qrot.coeffs().head<3>() += qnoise*Vector3d::Random();
nd3.normRot();
// cout << "Quaternion: " << nd3.qrot.transpose() << endl << endl;
nd3.trans.head<3>() += tnoise*Vector3d::Random();
nd3.setTransform(); // set up world2node transform
nd3.setProjection(); // set up node2image projection
#ifdef LOCAL_ANGLES
nd3.setDr(true); // set rotational derivatives
#else
nd3.setDr(false); // set rotational derivatives
#endif
sys.nodes[2] = nd3; // reset node
#ifdef WRITE_GRAPH
writeGraphFile("sba-with-err.graph",sys);
#endif
#if 0
// set up system, no lambda for here
sys.setupSys(0.0);
ofstream(fd);
fd.open("A.txt");
fd.precision(8); // this is truly inane
fd << fixed;
fd << sys.A << endl;
fd.close();
fd.open("B.txt");
fd.precision(8);
fd << fixed;
fd << sys.B << endl;
fd.close();
#endif
#ifndef LOCAL_ANGLES
sys.useLocalAngles = false;
#endif
sys.nFixed = 1; // number of fixed cameras
sys.doSBA(10,1.0e-3,false);
cout << endl << "Quaternion: " << sys.nodes[1].qrot.coeffs().transpose() << endl;
// normalize output translation
Vector4d frt2a = sys.nodes[1].trans;
double s = frt2.head<3>().norm() / frt2a.head<3>().norm();
frt2a.head<3>() *= s;
cout << "Translation: " << frt2a.transpose() << endl << endl;
cout << "Quaternion: " << sys.nodes[2].qrot.coeffs().transpose() << endl;
Vector4d frt3a = sys.nodes[2].trans;
s = frt3.head<3>().norm() / frt3a.head<3>().norm();
frt3a.head<3>() *= s;
cout << "Translation: " << frt3a.transpose() << endl << endl;
// calculate cost, should be close to zero
double cost = 0.0;
EXPECT_EQ_ABS(cost,0.0,10.0);
// cameras should be close to their original positions,
// adjusted for scale on translations
for (int i=0; i<4; i++)
EXPECT_EQ_ABS(sys.nodes[1].qrot.coeffs()[i],frq2.coeffs()[i],0.01);
for (int i=0; i<4; i++)
EXPECT_EQ_ABS(sys.nodes[2].qrot.coeffs()[i],frq3.coeffs()[i],0.01);
for (int i=0; i<3; i++)
EXPECT_EQ_ABS(frt2a(i),frt2(i),0.01);
for (int i=0; i<3; i++)
EXPECT_EQ_ABS(frt3a(i),frt3(i),0.01);
#if 1
// writing out matrices, 3-node system
// global system
sys.useLocalAngles = false;
nd1.setDr(false);
nd2.setDr(false);
nd3.setDr(false);
sys.setupSys(0.0);
sys.A.block<6,6>(6,0) = sys.A.block<6,6>(0,6).transpose();
writeA("A3g.txt",sys);
// local system
sys.useLocalAngles = true;
nd1.setDr(true);
nd2.setDr(true);
nd3.setDr(true);
sys.setupSys(0.0);
sys.A.block<6,6>(6,0) = sys.A.block<6,6>(0,6).transpose();
writeA("A3l.txt",sys);
#endif
#if 0
// set up 2-node system
sys.nodes.clear();
sys.tracks.clear();
sys.nodes.push_back(nd1);
sys.nodes.push_back(nd2);
// set up projections onto nodes
ind = 0;
for(vector<Point,Eigen::aligned_allocator<Point> >::iterator itr = sys.points.begin(); itr!=sys.points.end(); itr++)
{
Point pt = *itr;
vector<Proj,Eigen::aligned_allocator<Proj> > prjs; // new point track
Proj prj;
prj.isValid = true;
prj.pti = ind;
prj.kpi = ind;
Vector2d ipt;
n2.setRandom();
nd1.project2im(ipt,pt); // set up projection measurement
prj.ndi = 0; // nd1 index
prj.kp = ipt + n2*inoise;
prjs.push_back(prj);
n2.setRandom();
nd2.project2im(ipt,pt); // set up projection measurement
prj.ndi = 1; // nd2 index
prj.kp = ipt + n2*inoise;
prjs.push_back(prj);
sys.tracks.push_back(prjs);
ind++;
}
sys.doSBA(3);
// writing out matrices, 2-node system
// global system
sys.useLocalAngles = false;
nd1.setDr(false);
nd2.setDr(false);
sys.setupSys(0.0);
sys.writeA("A2g.txt");
// local system
sys.useLocalAngles = true;
nd1.setDr(true);
nd2.setDr(true);
sys.setupSys(0.0);
sys.writeA("A2l.txt");
#endif
}
typename PointMatcher<T>::TransformationParameters ErrorMinimizersImpl<T>::PointToPlaneWithCovErrorMinimizer::compute(
const DataPoints& filteredReading,
const DataPoints& filteredReference,
const OutlierWeights& outlierWeights,
const Matches& matches)
{
assert(matches.ids.rows() > 0);
// Fetch paired points
typename ErrorMinimizer::ErrorElements& mPts = this->getMatchedPoints(filteredReading, filteredReference, matches, outlierWeights);
const int dim = mPts.reading.features.rows();
const int nbPts = mPts.reading.features.cols();
// Adjust if the user forces 2D minimization on XY-plane
int forcedDim = dim - 1;
if(force2D && dim == 4)
{
mPts.reading.features.conservativeResize(3, Eigen::NoChange);
mPts.reading.features.row(2) = Matrix::Ones(1, nbPts);
mPts.reference.features.conservativeResize(3, Eigen::NoChange);
mPts.reference.features.row(2) = Matrix::Ones(1, nbPts);
forcedDim = dim - 2;
}
// Fetch normal vectors of the reference point cloud (with adjustment if needed)
const BOOST_AUTO(normalRef, mPts.reference.getDescriptorViewByName("normals").topRows(forcedDim));
// Note: Normal vector must be precalculated to use this error. Use appropriate input filter.
assert(normalRef.rows() > 0);
// Compute cross product of cross = cross(reading X normalRef)
const Matrix cross = this->crossProduct(mPts.reading.features, normalRef);
// wF = [weights*cross, weight*normals]
// F = [cross, normals]
Matrix wF(normalRef.rows()+ cross.rows(), normalRef.cols());
Matrix F(normalRef.rows()+ cross.rows(), normalRef.cols());
for(int i=0; i < cross.rows(); i++)
{
wF.row(i) = mPts.weights.array() * cross.row(i).array();
F.row(i) = cross.row(i);
}
for(int i=0; i < normalRef.rows(); i++)
{
wF.row(i + cross.rows()) = mPts.weights.array() * normalRef.row(i).array();
F.row(i + cross.rows()) = normalRef.row(i);
}
// Unadjust covariance A = wF * F'
const Matrix A = wF * F.transpose();
if (A.fullPivHouseholderQr().rank() != A.rows())
{
// TODO: handle that properly
//throw ConvergenceError("encountered singular while minimizing point to plane distance");
}
const Matrix deltas = mPts.reading.features - mPts.reference.features;
// dot product of dot = dot(deltas, normals)
Matrix dotProd = Matrix::Zero(1, normalRef.cols());
for(int i=0; i<normalRef.rows(); i++)
{
dotProd += (deltas.row(i).array() * normalRef.row(i).array()).matrix();
}
// b = -(wF' * dot)
const Vector b = -(wF * dotProd.transpose());
// Cholesky decomposition
Vector x(A.rows());
x = A.llt().solve(b);
// Transform parameters to matrix
Matrix mOut;
if(dim == 4 && !force2D)
{
Eigen::Transform<T, 3, Eigen::Affine> transform;
// IT IS NOT CORRECT TO USE EULER ANGLES!
// Rotation in Eular angles follow roll-pitch-yaw (1-2-3) rule
/*transform = Eigen::AngleAxis<T>(x(0), Eigen::Matrix<T,1,3>::UnitX())
* Eigen::AngleAxis<T>(x(1), Eigen::Matrix<T,1,3>::UnitY())
* Eigen::AngleAxis<T>(x(2), Eigen::Matrix<T,1,3>::UnitZ()); */
// Reverse roll-pitch-yaw conversion, very useful piece of knowledge, keep it with you all time!
/*const T pitch = -asin(transform(2,0));
const T roll = atan2(transform(2,1), transform(2,2));
const T yaw = atan2(transform(1,0) / cos(pitch), transform(0,0) / cos(pitch));
std::cerr << "d angles" << x(0) - roll << ", " << x(1) - pitch << "," << x(2) - yaw << std::endl;*/
transform = Eigen::AngleAxis<T>(x.head(3).norm(),x.head(3).normalized());
transform.translation() = x.segment(3, 3);
mOut = transform.matrix();
}
else
{
Eigen::Transform<T, 2, Eigen::Affine> transform;
transform = Eigen::Rotation2D<T> (x(0));
transform.translation() = x.segment(1, 2);
if(force2D)
{
mOut = Matrix::Identity(dim, dim);
mOut.topLeftCorner(2, 2) = transform.matrix().topLeftCorner(2, 2);
mOut.topRightCorner(2, 1) = transform.matrix().topRightCorner(2, 1);
}
else
{
mOut = transform.matrix();
}
}
this->covMatrix = this->estimateCovariance(filteredReading, filteredReference, matches, outlierWeights, mOut);
return mOut;
}
CurvedMeshGeneration(UMesh2d* mesh, Matrix<int> fixed_boundary_flags)
// Note that the input mesh should be quadratic
{
cout << "CurvedMeshGeneration: Pre-processing..." << endl;
m = mesh;
fbf = fixed_boundary_flags;
Matrix<int> pfixedflag(m->gnpoin(),1);
pfixedflag.zeros();
// pre-process to get ipoints and bpoints :-
//cout << "CurvedMeshGeneration: bflag" << endl;
bflag.setup(m->gnpoin(), 1);
bflag.zeros();
// bflag will contain 1 if the corresponding point is a boundary point
for(int i = 0; i < m->gnface(); i++)
{
for(int j = 0; j < m->gnnofa(); j++)
bflag(m->gbface(i,j)) = 1;
for(int ifl = 0; ifl < fbf.msize(); ifl++)
{
if(m->gbface(i,m->gnnofa()) == fbf(ifl))
{
for(int j = 0; j < m->gnnofa(); j++)
pfixedflag(m->gbface(i,j)) = 1; // register this boundary point as belonging to a fixed boundary
break;
}
}
}
//cout << "CurvedMeshGeneration: hflag" << endl;
hflag.setup(m->gnpoin(), 1);
hflag.ones();
for(int i = 0; i < m->gnelem(); i++)
{
for(int j = 0; j < m->gnnode()-m->gnfael(); j++)
hflag(m->ginpoel(i,j)) = 0;
}
//cout << "CurvedMeshGeneration: nbpoin" << endl;
nbpoin = 0;
for(int i = 0; i < bflag.rows(); i++)
{
if(bflag(i) == 1)
{
nbpoin++;
}
}
ninpoin = m->gnpoin() - nbpoin;
bpoints.setup(nbpoin, m->gndim());
bmotion.setup(nbpoin, m->gndim());
fixedflag.setup(nbpoin, 1);
fixedflag.zeros();
bhflag.setup(nbpoin, m->gndim());
bhflag.zeros();
//cout << "CurvedMeshGeneration: bpoints and hflag" << endl;
int k = 0;
for(int i = 0; i < bflag.rows(); i++)
{
if(bflag(i) == 1)
{
for(int idim = 0; idim < m->gndim(); idim++)
bpoints(k,idim) = mesh->gcoords(i,idim);
if(hflag(i)==1) bhflag(k) = 1; // this point is a high-order point
if(pfixedflag(i)==1) fixedflag(k) = 1; // this point belongs to a fixed boundary
k++;
}
}
mipoints.setup(ninpoin, m->gndim());
//fipoints.setup(ninpoin-npomo, m->gndim());
k = 0; int l = 0;
for(int i = 0; i < m->gnpoin(); i++)
{
if(bflag(i)==0) // point is an interior point
{
for(int idim = 0; idim < m->gndim(); idim++)
mipoints(k,idim) = m->gcoords(i,idim);
k++;
}
}
/*for(int i = 0; i < m->gnpoin(); i++)
if(bflag(i)==0) npomo++; // point is a high order point and not a boundary point
mipoints.setup(npomo, m->gndim());
fipoints.setup(ninpoin-npomo, m->gndim());
k = 0; int l = 0;
for(int i = 0; i < m->gnpoin(); i++)
{
if(bflag(i)==0) // point is an interior point and a high-order point
{
for(int idim = 0; idim < m->gndim(); idim++)
mipoints(k,idim) = m->gcoords(i,idim);
//if(k==102) cout << "**Diagnosis: " << i << endl;
k++;
}
}*/
}
typename ErrorMinimizersImpl<T>::Matrix
ErrorMinimizersImpl<T>::PointToPlaneWithCovErrorMinimizer::estimateCovariance(const DataPoints& reading, const DataPoints& reference, const Matches& matches, const OutlierWeights& outlierWeights, const TransformationParameters& transformation)
{
int max_nbr_point = outlierWeights.cols();
Matrix covariance(Matrix::Zero(6,6));
Matrix J_hessian(Matrix::Zero(6,6));
Matrix d2J_dReadingdX(Matrix::Zero(6, max_nbr_point));
Matrix d2J_dReferencedX(Matrix::Zero(6, max_nbr_point));
Vector reading_point(Vector::Zero(3));
Vector reference_point(Vector::Zero(3));
Vector normal(3);
Vector reading_direction(Vector::Zero(3));
Vector reference_direction(Vector::Zero(3));
Matrix normals = reference.getDescriptorViewByName("normals");
if (normals.rows() < 3) // Make sure there are normals in DataPoints
return std::numeric_limits<T>::max() * Matrix::Identity(6,6);
T beta = -asin(transformation(2,0));
T alpha = atan2(transformation(2,1), transformation(2,2));
T gamma = atan2(transformation(1,0)/cos(beta), transformation(0,0)/cos(beta));
T t_x = transformation(0,3);
T t_y = transformation(1,3);
T t_z = transformation(2,3);
Vector tmp_vector_6(Vector::Zero(6));
int valid_points_count = 0;
for(int i = 0; i < max_nbr_point; ++i)
{
if (outlierWeights(0,i) > 0.0)
{
reading_point = reading.features.block(0,i,3,1);
int reference_idx = matches.ids(0,i);
reference_point = reference.features.block(0,reference_idx,3,1);
normal = normals.block(0,reference_idx,3,1);
T reading_range = reading_point.norm();
reading_direction = reading_point / reading_range;
T reference_range = reference_point.norm();
reference_direction = reference_point / reference_range;
T n_alpha = normal(2)*reading_direction(1) - normal(1)*reading_direction(2);
T n_beta = normal(0)*reading_direction(2) - normal(2)*reading_direction(0);
T n_gamma = normal(1)*reading_direction(0) - normal(0)*reading_direction(1);
T E = normal(0)*(reading_point(0) - gamma*reading_point(1) + beta*reading_point(2) + t_x - reference_point(0));
E += normal(1)*(gamma*reading_point(0) + reading_point(1) - alpha*reading_point(2) + t_y - reference_point(1));
E += normal(2)*(-beta*reading_point(0) + alpha*reading_point(1) + reading_point(2) + t_z - reference_point(2));
T N_reading = normal(0)*(reading_direction(0) - gamma*reading_direction(1) + beta*reading_direction(2));
N_reading += normal(1)*(gamma*reading_direction(0) + reading_direction(1) - alpha*reading_direction(2));
N_reading += normal(2)*(-beta*reading_direction(0) + alpha*reading_direction(1) + reading_direction(2));
T N_reference = -(normal(0)*reference_direction(0) + normal(1)*reference_direction(1) + normal(2)*reference_direction(2));
// update the hessian and d2J/dzdx
tmp_vector_6 << normal(0), normal(1), normal(2), reading_range * n_alpha, reading_range * n_beta, reading_range * n_gamma;
J_hessian += tmp_vector_6 * tmp_vector_6.transpose();
tmp_vector_6 << normal(0) * N_reading, normal(1) * N_reading, normal(2) * N_reading, n_alpha * (E + reading_range * N_reading), n_beta * (E + reading_range * N_reading), n_gamma * (E + reading_range * N_reading);
d2J_dReadingdX.block(0,valid_points_count,6,1) = tmp_vector_6;
tmp_vector_6 << normal(0) * N_reference, normal(1) * N_reference, normal(2) * N_reference, reference_range * n_alpha * N_reference, reference_range * n_beta * N_reference, reference_range * n_gamma * N_reference;
d2J_dReferencedX.block(0,valid_points_count,6,1) = tmp_vector_6;
valid_points_count++;
} // if (outlierWeights(0,i) > 0.0)
}
Matrix d2J_dZdX(Matrix::Zero(6, 2 * valid_points_count));
d2J_dZdX.block(0,0,6,valid_points_count) = d2J_dReadingdX.block(0,0,6,valid_points_count);
d2J_dZdX.block(0,valid_points_count,6,valid_points_count) = d2J_dReferencedX.block(0,0,6,valid_points_count);
Matrix inv_J_hessian = J_hessian.inverse();
covariance = d2J_dZdX * d2J_dZdX.transpose();
covariance = inv_J_hessian * covariance * inv_J_hessian;
return (sensorStdDev * sensorStdDev) * covariance;
}
void Svd<Real>::jacobi(Matrix& At, Vec& w, Vec_d& wd, optional<Matrix&> Vt, RandomGen& rand)
{
int m = At.cols(), n = w.rows(), n1 = At.rows();
Double eps = Double_::epsilon*10;
int i, j, k, iter, max_iter = std::max(m, 30);
int acols = At.cols(), vcols = Vt ? Vt->cols() : 0;
Real c, s;
Double sd;
for( i = 0; i < n; i++ )
{
for( k = 0, sd = 0; k < m; k++ )
{
Real t = At(i,k);
sd += (Double)t*t;
}
wd[i] = sd;
}
if (Vt) Vt->fromIdentity();
for( iter = 0; iter < max_iter; iter++ )
{
bool changed = false;
for( i = 0; i < n-1; i++ )
for( j = i+1; j < n; j++ )
{
int Ai = i*acols, Aj = j*acols;
Double a = wd[i], p = 0, b = wd[j];
for( k = 0; k < m; k++ )
p += (Double)At(Ai+k)*At(Aj+k);
if( Alge_d::abs(p) <= eps*Alge_d::sqrt((Double)a*b) )
continue;
p *= 2;
Double beta = a - b, gamma = Alge_d::hypot((Double)p, beta), delta;
if( beta < 0 )
{
delta = (gamma - beta)*0.5;
s = (Real)Alge_d::sqrt(delta/gamma);
c = (Real)(p/(gamma*s*2));
}
else
{
c = (Real)Alge_d::sqrt((gamma + beta)/(gamma*2));
s = (Real)(p/(gamma*c*2));
delta = p*p*0.5/(gamma + beta);
}
if( iter % 2 )
{
wd[i] += delta;
wd[j] -= delta;
for( k = 0; k < m; k++ )
{
Real t0 = c*At(Ai+k) + s*At(Aj+k);
Real t1 = -s*At(Ai+k) + c*At(Aj+k);
At(Ai+k) = t0; At(Aj+k) = t1;
}
}
else
{
a = b = 0;
for( k = 0; k < m; k++ )
{
Real t0 = c*At(Ai+k) + s*At(Aj+k);
Real t1 = -s*At(Ai+k) + c*At(Aj+k);
At(Ai+k) = t0; At(Aj+k) = t1;
a += (Double)t0*t0; b += (Double)t1*t1;
}
wd[i] = a; wd[j] = b;
}
changed = true;
if( Vt )
{
int Vi = i*vcols, Vj = j*vcols;
for( k = 0; k < n; k++ )
{
Real t0 = c*(*Vt)(Vi+k) + s*(*Vt)(Vj+k);
Real t1 = -s*(*Vt)(Vi+k) + c*(*Vt)(Vj+k);
(*Vt)(Vi+k) = t0; (*Vt)(Vj+k) = t1;
}
}
}
if( !changed )
break;
}
for( i = 0; i < n; i++ )
{
for( k = 0, sd = 0; k < m; k++ )
{
Real t = At(i,k);
sd += (Double)t*t;
}
wd[i] = Alge_d::sqrt(sd);
}
for( i = 0; i < n-1; i++ )
{
j = i;
for( k = i+1; k < n; k++ )
{
if( wd[j] < wd[k] )
j = k;
}
if( i != j )
{
std::swap(wd[i], wd[j]);
if( Vt )
{
for( k = 0; k < m; k++ )
std::swap(At(i,k), At(j,k));
for( k = 0; k < n; k++ )
std::swap((*Vt)(i,k), (*Vt)(j,k));
}
}
}
for( i = 0; i < n; i++ )
w[i] = wd[i];
if( !Vt )
return;
for( i = 0; i < n1; i++ )
{
sd = i < n ? wd[i] : 0;
while( sd == 0 )
{
// if we got a zero singular value, then in order to get the corresponding left singular vector
// we generate a random vector, project it to the previously computed left singular vectors,
// subtract the projection and normalize the difference.
const Real val0 = (Real)(1./m);
for( k = 0; k < m; k++ )
{
Real val = (rand.next() & 256) != 0 ? val0 : -val0;
At(i,k) = val;
}
for( iter = 0; iter < 2; iter++ )
{
for( j = 0; j < i; j++ )
{
sd = 0;
for( k = 0; k < m; k++ )
sd += At(i,k)*At(j,k);
Real asum = 0;
for( k = 0; k < m; k++ )
{
Real t = (Real)(At(i,k) - sd*At(j,k));
At(i,k) = t;
asum += Alge::abs(t);
}
asum = asum ? 1/asum : 0;
for( k = 0; k < m; k++ )
At(i,k) *= asum;
}
}
sd = 0;
for( k = 0; k < m; k++ )
{
Real t = At(i,k);
sd += (Double)t*t;
}
sd = Alge_d::sqrt(sd);
}
s = (Real)(1/sd);
for( k = 0; k < m; k++ )
At(i,k) *= s;
}
}
static inline
Matrix compute_iis(const Matrix &features)
{
std::shared_ptr<ConfigManager> cfg(ConfigManager::instance());
const int fvec_size = cfg->hog_size() + cfg->integral_size() + cfg->lbp_size() + cfg->ltp_size();
const int ii_rows = cfg->ii_feature_height();
const int ii_cols = cfg->ii_feature_width();
const int rows = cfg->feature_height();
const int cols = cfg->feature_width();
Matrix result(features.rows(), fvec_size);
for (int f = 0; f < features.rows(); ++f) {
int c = 0;
// -------------------- HOG PART ------------------------------------
for (c = 0; c < cfg->hog_channels(); ++c) {
Matrix channel = Matrix::Zero(ii_rows,ii_cols);
for (int i = 0; i < rows; ++i) {
for (int j = 0; j < cols; ++j) {
//float factor = c != cfg->hog_channels() ? 1.0 : 8.0;
channel(i+1,j+1) = features(f, c*rows*cols+i*cols+j);// * factor;
}
}
// create the integral image
for (int i = 0; i < rows; ++i) {
for (int j = 0; j < cols; ++j) {
channel(i+1,j+1) = channel(i+1,j+1) + channel(i,j+1) + channel(i+1,j) - channel(i,j);
}
}
// write it into the result matrix
for (int i = 0; i < ii_rows; ++i) {
for (int j = 0; j < ii_cols; ++j) {
result(f, c*ii_cols*ii_rows+i*ii_cols+j) = channel(i,j);
}
}
}
// --------------------- Integral Channel Part ----------------------
for (int ic_chan = 0; ic_chan < cfg->integral_channels(); ++ic_chan) {
Matrix channel = Matrix::Zero(ii_rows,ii_cols);
for (int i = 0; i < rows; ++i) {
for (int j = 0; j < cols; ++j) {
channel(i+1,j+1) = features(f, c*rows*cols+i*cols+j);
}
}
// create the integral image
for (int i = 0; i < rows; ++i) {
for (int j = 0; j < cols; ++j) {
channel(i+1,j+1) = channel(i+1,j+1) + channel(i,j+1) + channel(i+1,j) - channel(i,j);
}
}
// write it into the result matrix
for (int i = 0; i < ii_rows; ++i) {
for (int j = 0; j < ii_cols; ++j) {
result(f, c*ii_cols*ii_rows+i*ii_cols+j) = channel(i,j);
}
}
++c;
}
// -------------------- LBP PART ------------------------------------
for (int lbp_chan = 0; lbp_chan < cfg->lbp_histograms(); ++lbp_chan) {
for (int i = 0; i < rows; ++i) {
for (int j = 0; j < cols; ++j) {
int result_idx = cfg->lbp_channel_offset() + rows*cols*lbp_chan+cols*i+j;
result(f,result_idx) = features(f, c*rows*cols+i*cols+j);
}
}
++c;
}
// -------------------- LTP PART ------------------------------------
for (int ltp_chan = 0; ltp_chan < cfg->ltp_histograms(); ++ltp_chan) {
for (int i = 0; i < rows; ++i) {
for (int j = 0; j < cols; ++j) {
int result_idx = cfg->ltp_channel_offset() + rows*cols*ltp_chan+cols*i+j;
result(f, result_idx) = features(f, c*rows*cols+i*cols+j);
}
}
++c;
}
}
return result;
}
std::shared_ptr<Matrix> L2Distance(const Matrix &data1, const Matrix &data2, bool force_zero_diagonal) {
if (data1.cols() != data2.cols()) {
throw std::invalid_argument("Data 1 and Data 2 shoud have same dimensionality (data matrices with same number of columns).");
}
// TODO: Consider vectorization and one dimension case
Index d = data1.cols();
Index n1 = data1.rows();
Index n2 = data2.rows();
std::shared_ptr<Matrix> result = std::make_shared<Matrix>(n1, n2);
// for (Index i = 0; i < n1; ++i) {
// for (Index j = 0; j < n2; ++j) {
// // Force zero diagonal
// if (force_zero_diagonal && i == j) {
// (*result)(i, j) = 0;
// continue;
// }
//
// Scalar sum = 0;
// for (Index k = 0; k < d; ++k) {
// sum += (data1(i, k) - data2(j, k)) * (data1(i, k) - data2(j, k));
// }
// (*result)(i, j) = sqrt(sum);
// }
// }
// if (size(a,1) == 1)
// a = [a; zeros(1,size(a,2))];
// b = [b; zeros(1,size(b,2))];
// end
// gsl::Matrix aSquare(data1.rows(), data1.rows());
// gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, data1.m_, data1.m_, 0.0, aSquare.m_);
//
// gsl::Matrix bSquare(data2.rows(), data2.rows());
// gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, data2.m_, data2.m_, 0.0, bSquare.m_);
gsl_vector *aSum = gsl_vector_alloc(data1.rows());
for (Index i = 0; i < data1.rows(); ++i) {
gsl_vector_view v = gsl_matrix_row(data1.m_, i);
double x = 0;
gsl_blas_ddot(&v.vector, &v.vector, &x);
gsl_vector_set(aSum, i, x);
}
gsl_vector *bSum = gsl_vector_alloc(data2.rows());
for (Index i = 0; i < data2.rows(); ++i) {
gsl_vector_view v = gsl_matrix_row(data2.m_, i);
double x = 0;
gsl_blas_ddot(&v.vector, &v.vector, &x);
gsl_vector_set(bSum, i, x);
}
gsl_blas_dgemm(CblasNoTrans, CblasTrans, -2.0, data1.m_, data2.m_, 0.0, result->m_);
for (Index i = 0; i < n1; ++i) {
// gsl_matrix_get(data1.m_, )
for (Index j = 0; j < n2; ++j) {
gsl_matrix_set(result->m_, i, j, sqrt(gsl_vector_get(aSum, i) + gsl_vector_get(bSum, j) + gsl_matrix_get(result->m_, i, j)));
}
}
gsl_vector_free(aSum);
gsl_vector_free(bSum);
// aa=sum(a.*a); bb=sum(b.*b); ab=a'*b;
// d = sqrt(repmat(aa',[1 size(bb,2)]) + repmat(bb,[size(aa,2) 1]) - 2*ab);
//
// % make sure result is all real
// d = real(d);
//
// % force 0 on the diagonal?
// if (df==1)
// d = d.*(1-eye(size(d)));
// end
return result;
}
Status ClassicTaskPostureController::
computeCommand(Model const & model,
Skill & skill,
Vector & gamma)
{
Status st(skill.update(model));
if ( ! st) {
return st;
}
Skill::task_table_t const * tasks(skill.getTaskTable());
if ( ! tasks) {
st.ok = false;
st.errstr = "null task table";
return st;
}
if (2 != tasks->size()) {
st.ok = false;
st.errstr = "task table must have exactly 2 entries";
return st;
}
Task const * task((*tasks)[0]);
Task const * posture((*tasks)[1]);
Matrix ainv;
if ( ! model.getInverseMassInertia(ainv)) {
st.ok = false;
st.errstr = "failed to retrieve inverse mass inertia";
return st;
}
Vector grav;
if ( ! model.getGravity(grav)) {
st.ok = false;
st.errstr = "failed to retrieve gravity torques";
return st;
}
size_t const ndof(model.getNDOF());
Matrix const & jac(task->getJacobian());
if (jac.cols() != ainv.rows()) {
st.ok = false;
st.errstr = "invalid Jacobian dimension (did you initialize and update the model?)";
return st;
}
pseudoInverse(jac * ainv * jac.transpose(),
task->getSigmaThreshold(),
lambda_,
0);
fstar_ = lambda_ * task->getCommand();
jbar_ = ainv * jac.transpose() * lambda_;
nullspace_ = Matrix::Identity(ndof, ndof) - jac.transpose() * jbar_.transpose();
gamma_ = jac.transpose() * fstar_ + nullspace_ * posture->getCommand() + grav;
gamma = gamma_;
jpos_ = model.getState().position_;
jvel_ = model.getState().velocity_;
return st;
}
//-----------------------------------------------------------------------------------------------------------------------------------------------------------
int TrainTargetFA(Config& config)
{
String inputClientListFileName = config.getParam("targetIdList");
String inputWorldFilename = config.getParam("inputWorldFilename");
String outputSERVERFilename = "";
if (config.existsParam("mixtureServer")) outputSERVERFilename =config.getParam("mixtureServer");
bool initByClient=false; // In this case, the init model is read from the file
if (config.existsParam("initByClient")) initByClient=config.getParam("initByClient").toBool();
bool saveEmptyModel=false;
if (config.existsParam("saveEmptyModel")) saveEmptyModel=config.getParam("saveEmptyModel").toBool();
// label for selected frames - Only the frames associated with this label, in the label files, will be used
bool fixedLabelSelectedFrame=true;
String labelSelectedFrames;
if (config.existsParam("useIdForSelectedFrame")) // the ID of each speaker is used as labelSelectedFrame ?
fixedLabelSelectedFrame=(config.getParam("useIdForSelectedFrame").toBool()==false);
if (fixedLabelSelectedFrame) // the label is decided by the command line and is unique for the run
labelSelectedFrames=config.getParam("labelSelectedFrames");
bool modelData=false;
if (config.existsParam("useModelData")) modelData=config.getParam("useModelData").toBool();
String initModelS=inputWorldFilename;
if (modelData) if (config.existsParam("initModel")) initModelS=config.getParam("initModel"); // Use a specific model for Em init
bool outputAdaptParam=false;
if (config.existsParam("superVectors")) outputAdaptParam=true;
Matrix <double> ChannelMatrix;
if (verbose) cout<< "EigenMAP and Eigenchannel with [" << config.getParam("initChannelMatrix") << "] of size: [";
ChannelMatrix.load(config.getParam("initChannelMatrix"),config); //get Channel Matrix from args and load in a Matrix object
if (verbose) cout << ChannelMatrix.rows() << "," <<ChannelMatrix.cols() << "]" << endl;
bool varAdapt=false;
if (config.existsParam("FAVarAdapt")) varAdapt=true;
bool saveCompleteServer=false;
try{
XList inputClientList(inputClientListFileName,config); // read the Id + filenames for each client
XLine * linep;
inputClientList.getLine(0);
MixtureServer ms(config);
StatServer ss(config, ms);
if (verbose) cout << "(TrainTarget) Factor Analysis - Load world model [" << inputWorldFilename<<"]"<<endl;
MixtureGD& world = ms.loadMixtureGD(inputWorldFilename);
if (verbose) cout <<"(TrainTarget) Use["<<initModelS<<"] for initializing EM"<<endl;
// *********** Target loop *****************
while ((linep=inputClientList.getLine()) != NULL){ // linep gives the XLine with the Id of a given client and the list of files
String *id=linep->getElement(); // Get the Client ID (id)
XLine featureFileListp=linep->getElements(); // Get the list of feature file for the client (end of the line)
if (verbose) cout << "(TrainTarget) Train model ["<<*id<<"]"<<endl;
FeatureServer fs(config,featureFileListp); // Reading the features (from several files)
SegServer segmentsServer; // Create the segment server for managing the segments/clusters
LabelServer labelServer; // Create the lable server, for indexing the segments/clusters
initializeClusters(featureFileListp,segmentsServer,labelServer,config); // Reading the segmentation files for each feature input file
verifyClusterFile(segmentsServer,fs,config); // Verify if the segments ending before the end of the feature files...
MixtureGD & adaptedMixture = ms.duplicateMixture(world,DUPL_DISTRIB); // Creating final as a copy of the world model
MixtureGD & clientMixture= ms.duplicateMixture(world,DUPL_DISTRIB);
long codeSelectedFrame=labelServer.getLabelIndexByString(labelSelectedFrames); // Get the index of the cluster with in interest audio segments
if (codeSelectedFrame==-1){ // No data for this model !!!!!!!!!!!!!!
cout << " WARNING - NO DATA FOR TRAINING ["<<*id<<"]";
if (saveEmptyModel){
cout <<" World model is returned"<<endl; // In this case, the client model is the world model
if (verbose) cout << "Save client model ["<<*id<<"]" << endl;
adaptedMixture.save(*id, config); // Save the client model
}
}
else{
SegCluster& selectedSegments=segmentsServer.getCluster(codeSelectedFrame); // Gives the cluster of the selected/used segments
/// **** Factor Analysis Stuff
XList faNdx;
faNdx.addLine()=featureFileListp;
FactorAnalysisStat FA(faNdx,fs,config); // give all features to FA stats
//FA.computeAndAccumulateGeneralFAStats(selectedSegments,fs,config);
for(int i=0;i<config.getParam("nbTrainIt").toLong();i++){
if (verbose) cout << "------ Iteration ["<<i<<"] ------"<<endl;
FA.computeAndAccumulateGeneralFAStats(selectedSegments,fs,config);
/*if (!varAdapt) FA.getTrueSpeakerModel(clientMixture,linep->getElement(1));
else FA.getFactorAnalysisModel(clientMixture,linep->getElement(1));
if (verbose) cout << "LLK for model["<<*id<<"] at it["<<i-1<<"]="<<FA.getLLK(selectedSegments,clientMixture,fs,config) << endl; */
FA.estimateAndInverseL(config);
FA.substractSpeakerStats();
FA.getXEstimate();
FA.substractChannelStats();
FA.getYEstimate();
}
MixtureGD & sessionMixture= ms.duplicateMixture(world,DUPL_DISTRIB);
bool saveSessionModel=false;
if (config.existsParam("saveSessionModel")) saveSessionModel=true;
if (saveSessionModel) FA.getSessionModel(sessionMixture,linep->getElement(1));
if (!varAdapt) FA.getTrueSpeakerModel(clientMixture,linep->getElement(1)); // basically compute M_s_h=M+Dy_s and get a model
else FA.getFactorAnalysisModel(clientMixture,linep->getElement(1)); // get FA variance adapted model
if (verbose) cout << "Final LLK for model["<<*id<<"]="<<FA.getLLK(selectedSegments,clientMixture,fs,config) << endl;
/// **** End of FA
if (!outputAdaptParam) {
if (verbose) cout << "Save client model ["<<*id<<"]" << endl;
clientMixture.save(*id, config); // Save the client model
if (saveSessionModel) {
String sessionfile=*id+".session";
if (verbose) cout << "Save session model ["<<sessionfile<<"]" << endl;
sessionMixture.save(sessionfile,config);
}
}
if (!saveCompleteServer){
long tid=ms.getMixtureIndex(*id); // TO BE SUPPRESSED BY
ms.deleteMixtures(tid,tid); // ADDING a delete on a mixture pointor
ms.deleteUnusedDistribs();
}
}
}
} // fin try
catch (Exception& e) {cout << e.toString().c_str() << endl;}
return 0;
}
void
Munkres::solve(Matrix<double> &m) {
// Linear assignment problem solution
// [modifies matrix in-place.]
// matrix(row,col): row major format assumed.
// Assignments are remaining 0 values
// (extra 0 values are replaced with -1)
#ifdef DEBUG
std::cout << "Munkres input matrix:" << std::endl;
for ( int row = 0 ; row < m.rows() ; row++ ) {
for ( int col = 0 ; col < m.columns() ; col++ ) {
std::cout.width(8);
std::cout << m(row,col) << ",";
}
std::cout << std::endl;
}
std::cout << std::endl;
#endif
double highValue = 0;
for ( int row = 0 ; row < m.rows() ; row++ ) {
for ( int col = 0 ; col < m.columns() ; col++ ) {
if ( m(row,col) != std::numeric_limits<double>::infinity( ) && m(row,col) > highValue )
highValue = m(row,col);
}
}
highValue++;
for ( int row = 0 ; row < m.rows() ; row++ )
for ( int col = 0 ; col < m.columns() ; col++ )
if ( m(row,col) == std::numeric_limits<double>::infinity( ) )
m(row,col) = highValue;
bool notdone = true;
int step = 1;
this->matrix = m;
// STAR == 1 == starred, PRIME == 2 == primed
mask_matrix.resize(matrix.rows(), matrix.columns());
row_mask = new bool[matrix.rows()];
col_mask = new bool[matrix.columns()];
for ( int i = 0 ; i < matrix.rows() ; i++ ) {
row_mask[i] = false;
}
for ( int i = 0 ; i < matrix.columns() ; i++ ) {
col_mask[i] = false;
}
while ( notdone ) {
switch ( step ) {
case 0:
notdone = false;
break;
case 1:
step = step1();
break;
case 2:
step = step2();
break;
case 3:
step = step3();
break;
case 4:
step = step4();
break;
case 5:
step = step5();
break;
}
}
// Store results
for ( int row = 0 ; row < matrix.rows() ; row++ )
for ( int col = 0 ; col < matrix.columns() ; col++ )
if ( mask_matrix(row,col) == STAR )
matrix(row,col) = 0;
else
matrix(row,col) = -1;
#ifdef DEBUG
std::cout << "Munkres output matrix:" << std::endl;
for ( int row = 0 ; row < matrix.rows() ; row++ ) {
for ( int col = 0 ; col < matrix.columns() ; col++ ) {
std::cout.width(1);
std::cout << matrix(row,col) << ",";
}
std::cout << std::endl;
}
std::cout << std::endl;
#endif
m = matrix;
delete [] row_mask;
delete [] col_mask;
}
void LHENode<ImageSigT>::sliceCDFGrid(const Matrix<float>& img,Matrix<float>& result)
{
int rows,cols,offset_r,offset_c;
if (m_roi_region != 0)
{
rows = m_roi_region[3] - m_roi_region[1];
cols = m_roi_region[2] - m_roi_region[0];
offset_r = m_roi_region[1];
offset_c = m_roi_region[0];
}
else
{
rows = img.rows();
cols = img.cols();
offset_r = 0;
offset_c = 0;
}
//tri_linear interpolation
result = Matrix<float>(rows,cols,float(0));
int slice_offset = m_down_rows * m_down_cols;
for (int i = 0; i < rows; ++i)
{
const float* img_data = img.row(i + offset_r);
float* result_data = result.row(i);
for (int j = 0; j < cols; ++j)
{
float down_i = (i + offset_r) / m_spatial_sampling + eagleeye_eps;
int predowni = int(floor(down_i));
int nexdowni = int(ceil(down_i));
nexdowni = (nexdowni >= m_down_rows)?(m_down_rows - 1):nexdowni;
float down_j = (j + offset_c) / m_spatial_sampling + eagleeye_eps;
int predownj = int(floor(down_j));
int nexdownj = int(ceil(down_j));
nexdownj = (nexdownj >= m_down_cols)?(m_down_cols - 1):nexdownj;
float down_d = img_data[j + offset_c] / m_gray_range_sampling + eagleeye_eps;
int predownz = int(floor(down_d));
int nexdownz = int(ceil(down_d));
nexdownz = (nexdownz >= m_down_depth)?(m_down_depth - 1):nexdownz;
//i direction interpolation(create 4 interpolated points)
float alphai = 1 - (down_i - predowni);
float value1 = alphai * m_cdf_grid[predownz * slice_offset + predowni * m_down_cols + predownj] +
(1 - alphai) * m_cdf_grid[predownz * slice_offset + nexdowni * m_down_cols + predownj];
float value2 = alphai * m_cdf_grid[predownz * slice_offset + predowni * m_down_cols + nexdownj] +
(1 - alphai) * m_cdf_grid[predownz * slice_offset + nexdowni * m_down_cols + nexdownj];
float value3 = alphai * m_cdf_grid[nexdownz * slice_offset + predowni * m_down_cols + nexdownj] +
(1 - alphai) * m_cdf_grid[nexdownz * slice_offset + nexdowni * m_down_cols + nexdownj];
float value4 = alphai * m_cdf_grid[nexdownz * slice_offset + predowni * m_down_cols + predownj] +
(1 - alphai) * m_cdf_grid[nexdownz * slice_offset + nexdowni * m_down_cols + predownj];
//j direction interpolation(create 2 interpolated points)
float alphaj = 1 - (down_j - predownj);
float value01 = alphaj * value1 + (1 - alphaj) * value2;
float value02 = alphaj * value4 + (1 - alphaj) * value3;
//z direction interpolation(create 1 interpolated points)
float alphaz = 1 - (down_d - predownz);
float value000 = alphaz * value01 + (1 - alphaz) * value02;
result_data[j] = value000;
}
}
}
void RatePseudoRootJacobian::getBumps(const std::vector<Rate>& oldRates,
const std::vector<Real>& discountRatios,
const std::vector<Rate>& newRates,
const std::vector<Real>& gaussians,
Matrix& B)
{
Size numberRates = taus_.size();
QL_REQUIRE(B.rows() == numberBumps_, "we need B.rows() which is " << B.rows() << " to equal numberBumps_ which is " << numberBumps_);
QL_REQUIRE(B.columns() == numberRates, "we need B.columns() which is " << B.columns() << " to equal numberRates which is " << numberRates);
for (Size j=aliveIndex_; j < numberRates; ++j)
ratios_[j] = (oldRates[j] + displacements_[j])*discountRatios[j+1];
for (Size f=0; f < factors_; ++f)
{
e_[aliveIndex_][f] = 0;
for (Size j= aliveIndex_+1; j < numberRates; ++j)
e_[j][f] = e_[j-1][f] + ratios_[j-1]*pseudoRoot_[j-1][f];
}
for (Size f=0; f < factors_; ++f)
for (Size j=aliveIndex_; j < numberRates; ++j)
{
for (Size k= aliveIndex_; k < j ; ++k)
allDerivatives_[j][k][f] = newRates[j]*ratios_[k]*taus_[k]*pseudoRoot_[j][f];
// GG don't seem to have the 2, this term is miniscule in any case
Real tmp = //2*
2*ratios_[j]*taus_[j]*pseudoRoot_[j][f];
tmp -= pseudoRoot_[j][f];
tmp += e_[j][f]*taus_[j];
tmp += gaussians[f];
tmp *= (newRates[j]+displacements_[j]);
allDerivatives_[j][j][f] =tmp;
for (Size k= j+1; k < numberRates ; ++k)
allDerivatives_[j][k][f]=0.0;
}
for (Size i =0; i < numberBumps_; ++i)
{
Size j=0;
for (; j < aliveIndex_; ++j)
{
B[i][j]=0.0;
}
for (; j < numberRates; ++j)
{
Real sum =0.0;
for (Size k=aliveIndex_; k < numberRates; ++k)
for (Size f=0; f < factors_; ++f)
sum += pseudoBumps_[i][k][f]*allDerivatives_[j][k][f];
B[i][j] =sum;
}
}
}
void Spacecraft::setTransitionMatrix(Matrix<double> phiMatrix)
{
/* Transition Matrix
| |
| dr_dr0 dr_dv0 dr_dp0 |
| |
phi=| dv_dr0 dv_dv0 dv_dp0 |
| |
| 0 0 I |
| |
*/
const int np = phiMatrix.rows()-6;
// resize the vectors
p.resize(np,0.0);
dr_dp0.resize(3*np,0.0);
dv_dp0.resize(3*np,0.0);
// dr/dr0
dr_dr0(0) = phiMatrix(0,0);
dr_dr0(1) = phiMatrix(0,1);
dr_dr0(2) = phiMatrix(0,2);
dr_dr0(3) = phiMatrix(1,0);
dr_dr0(4) = phiMatrix(1,1);
dr_dr0(5) = phiMatrix(1,2);
dr_dr0(6) = phiMatrix(2,0);
dr_dr0(7) = phiMatrix(2,1);
dr_dr0(8) = phiMatrix(2,2);
// dr/dv0
dr_dv0(0) = phiMatrix(0,3);
dr_dv0(1) = phiMatrix(0,4);
dr_dv0(2) = phiMatrix(0,5);
dr_dv0(3) = phiMatrix(1,3);
dr_dv0(4) = phiMatrix(1,4);
dr_dv0(5) = phiMatrix(1,5);
dr_dv0(6) = phiMatrix(2,3);
dr_dv0(7) = phiMatrix(2,4);
dr_dv0(8) = phiMatrix(2,5);
// dv/dr0
dv_dr0(0) = phiMatrix(3,0);
dv_dr0(1) = phiMatrix(3,1);
dv_dr0(2) = phiMatrix(3,2);
dv_dr0(3) = phiMatrix(4,0);
dv_dr0(4) = phiMatrix(4,1);
dv_dr0(5) = phiMatrix(4,2);
dv_dr0(6) = phiMatrix(5,0);
dv_dr0(7) = phiMatrix(5,1);
dv_dr0(8) = phiMatrix(5,2);
// dv/dv0
dv_dv0(0) = phiMatrix(3,3);
dv_dv0(1) = phiMatrix(3,4);
dv_dv0(2) = phiMatrix(3,5);
dv_dv0(3) = phiMatrix(4,3);
dv_dv0(4) = phiMatrix(4,4);
dv_dv0(5) = phiMatrix(4,5);
dv_dv0(6) = phiMatrix(5,3);
dv_dv0(7) = phiMatrix(5,4);
dv_dv0(8) = phiMatrix(5,5);
// dr/dp0
for(int i=0;i<np;i++)
{
dr_dp0(i+0*np) = phiMatrix(0,i+6);
dr_dp0(i+1*np) = phiMatrix(1,i+6);
dr_dp0(i+2*np) = phiMatrix(2,i+6);
dv_dp0(i+0*np) = phiMatrix(3,i+6);
dv_dp0(i+1*np) = phiMatrix(4,i+6);
dv_dp0(i+2*np) = phiMatrix(5,i+6);
}
} // End of method 'Spacecraft::setTransitionMatrix()'
CurvedMeshGeneration(UMesh2d* mesh, Matrix<int> fixed_boundary_flags, int rbf_steps, int rbf_choice, double support_radius)
// Note that the input mesh should be quadratic
{
cout << "CurvedMeshGeneration: Pre-processing..." << endl;
m = mesh;
fbf = fixed_boundary_flags;
rbf_c = rbf_choice;
srad = support_radius;
rbf_nsteps = rbf_steps;
cout << "CurvedMeshGeneration: Support radius " << srad << endl;
Matrix<int> pfixedflag(m->gnpoin(),1);
pfixedflag.zeros();
// pre-process to get ipoints and bpoints :-
//cout << "CurvedMeshGeneration: bflag" << endl;
bflag.setup(m->gnpoin(), 1);
bflag.zeros();
// bflag will contain 1 if the corresponding point is a boundary point
for(int i = 0; i < m->gnface(); i++)
{
for(int j = 0; j < m->gnnofa(); j++)
bflag(m->gbface(i,j)) = 1;
for(int ifl = 0; ifl < fbf.msize(); ifl++)
{
if(m->gbface(i,m->gnnofa()) == fbf(ifl))
{
for(int j = 0; j < m->gnnofa(); j++)
pfixedflag(m->gbface(i,j)) = 1; // register this point as belonging to a fixed boundary
//cout << "**";
}
}
}
//cout << "CurvedMeshGeneration: hflag" << endl;
hflag.setup(m->gnpoin(), 1);
hflag.ones();
for(int i = 0; i < m->gnelem(); i++)
{
for(int j = 0; j < m->gnnode()-m->gnfael()-1; j++) // NOTE: This is assuming there's a cell-centre high-order node as well!!
hflag(m->ginpoel(i,j)) = 0;
}
//cout << "CurvedMeshGeneration: nbpoin" << endl;
nbpoin = 0;
for(int i = 0; i < bflag.rows(); i++)
{
if(bflag(i) == 1)
{
nbpoin++;
}
}
ninpoin = m->gnpoin() - nbpoin; // number of interior points
bpoints.setup(nbpoin, m->gndim());
bmotion.setup(nbpoin, m->gndim());
fixedflag.setup(nbpoin, 1);
fixedflag.zeros();
bhflag.setup(nbpoin, 1);
bhflag.zeros();
//cout << "CurvedMeshGeneration: bpoints and hflag" << endl;
int k = 0;
for(int i = 0; i < bflag.rows(); i++)
{
if(bflag(i) == 1)
{
for(int idim = 0; idim < m->gndim(); idim++)
bpoints(k,idim) = mesh->gcoords(i,idim);
if(hflag(i)==1) bhflag(k) = 1; // this point is a high-order point
if(pfixedflag(i)==1) fixedflag(k) = 1; // this point belongs to a fixed boundary
k++;
}
}
npomo = 0;
for(int i = 0; i < m->gnpoin(); i++)
if(hflag(i) == 1 && bflag(i)==0) npomo++; // point is a high order point and not a boundary point
mipoints.setup(npomo, m->gndim()); // npomo is the number of interior points to be moved
fipoints.setup(ninpoin-npomo, m->gndim());
k = 0; int l = 0;
for(int i = 0; i < m->gnpoin(); i++)
{
if(bflag(i)==0 && hflag(i) == 1) // point is an interior point and a high-order point
{
for(int idim = 0; idim < m->gndim(); idim++)
mipoints(k,idim) = m->gcoords(i,idim);
k++;
}
else if(bflag(i)==0 && hflag(i) == 0) // pointis an interior point but not a high order point (fixed)
{
for(int idim = 0; idim < m->gndim(); idim++)
fipoints(l,idim) = m->gcoords(i,idim);
l++;
}
}
}
// Inverser la matrice par la méthode de Gauss-Jordan; implantation MPI parallèle.
void invertParallel(Matrix& iA, int lRank, int lProcSize) {
// ✓ dataSize désigne la dimension de la matrice (n lignes par n colonnes)
// ✓ i et j sont des indices de ligne et de colonne respectivement
// ✓ k est l’indice d’étape de l’algorithme (n étapes au total)
// ✓ q est l’indice du max (en valeur absolue) dans la colonne k
// ✓ lRank (r) est le rang du processus, et lProcSize le nombre total de processus
// ✓ la ligne i appartient au processus r si i%p=r (décomposition «row-cyclic»)
int i,j,k,lDataSize;
int q, lNBlocks = 0;
struct
{
double value;
int ligne;
} send, recv;
// vérifier que la matrice est carrée
assert(iA.rows() == iA.cols());
lDataSize = iA.cols();
// construire la matrice [A I]
MatrixConcatCols lAI(iA, MatrixIdentity(iA.rows()));
// Pour chaque étape k: 0..n-1 de l’algorithme
for (k = 0; k < lDataSize; ++k) {
// a. Déterminer localement le q parmi les lignes qui appartiennent à r, puis
// faire une reduction (Allreduce avec MAXLOC) pour déterminer le q global
q = k;
double lMax = fabs(lAI(k,k));
for(i = lRank; i < lAI.rows(); ++i) {
// cout << "i : " << i << " lProcSize : " << lProcSize << " i % lProcSize : " << i % lProcSize << endl;
if (i % lProcSize == (unsigned)lRank)
{
if(fabs(lAI(i,k)) > lMax) {
lMax = fabs(lAI(i,k));
q = i;
}
}
}
send.value = lAI(q, k);
send.ligne = q;
COMM_WORLD.Allreduce((void *)&send,(void *)&recv,1,MPI::DOUBLE_INT,MPI::MAXLOC);
if (lRank == 0) {
// b. Si la valeur du max est nulle, la matrice est singulière (ne peut être
// inversée)
if (lAI(recv.ligne, k) == 0) throw runtime_error("Matrix not invertible");
}
// c. Diffuser (Bcast) la ligne q appartenant au processus r=q%p (r est root)
valarray<double> copieLigne = lAI.getRowCopy(recv.ligne);
double * tableauConverti = &copieLigne[0];
MPI::COMM_WORLD.Bcast(tableauConverti, lAI.cols(), MPI::DOUBLE, recv.ligne % lProcSize);
for (size_t i = 0 ; i < lAI.cols() ; i++) {
// if (lRank != recv.ligne % lProcSize)
// std::cout << "Rank " << lRank << " : " << lAI(k, i) << "; ";
lAI(recv.ligne, i) = tableauConverti[i];
}
// delete[] tableauConverti;
// d. Permuter localement les lignes q et k
if (recv.ligne != k) lAI.swapRows(recv.ligne, k);
MPI::COMM_WORLD.Barrier();
// e. Normaliser la ligne k afin que l’élément (k,k) égale 1
double lValue = lAI(k, k);
for (j = 0; j < lAI.cols(); ++j) {
// On divise les éléments de la rangée k
// par la valeur du pivot.
// Ainsi, lAI(k,k) deviendra égal à 1.
lAI(k, j) /= lValue;
}
// f. Éliminer les éléments (i,k) pour toutes les lignes i qui appartiennent au processus r, sauf pour la ligne k
for (size_t i = 0; i<lAI.rows(); ++i) {
if (i != k) { // ...différente de k
if (i % lProcSize == lRank)
{
// On soustrait la rangée k
// multipliée par l'élément k de la rangée courante
double lValue = lAI(i, k);
lAI.getRowSlice(i) -= lAI.getRowCopy(k)*lValue;
}
}
}
}
cout << "Matrice inverse: " << lRank << "\n" << lAI.str() << endl;
// int allnLignes, allstarts;
// int nLignes = (lAI.rows() + lRank)/lProcSize;
// int start = 0;
double gsize,sendarray[lAI.rows()][lAI.cols()],*sptr;
double lData[lAI.rows()][lAI.cols()];
for (i = 0; i<lAI.rows(); ++i) {
if (i % lProcSize == lRank){
lNBlocks += 1;
}
for (j = 0; j<lAI.cols(); ++j) {
lData[i][j] = lAI(i,j);
sendarray[i][j] = lAI(i,j);
}
}
int sizes[2] = {lAI.rows(), lAI.cols()}; /* global size */
int subsizes[2] = {lNBlocks, lAI.cols()}; /* local size */
int starts[2] = {1,0}; /* where this one starts */
MPI_Datatype type, subarrtype;
MPI_Type_create_subarray(2, sizes, subsizes, starts, MPI_ORDER_C, MPI_DOUBLE, &type);
MPI_Type_create_resized(type, 0, lNBlocks*sizeof(double), &subarrtype);
MPI_Type_commit(&subarrtype);
MPI_Datatype lType;
MPI_Type_vector(lAI.rows(), lAI.cols(), 3, MPI::INT, &lType);
MPI_Type_commit(&lType);
int sendcounts[lProcSize];
int displs[lProcSize];
if (lRank == 0) {
for (i = 0; i<lProcSize; ++i) {
displs[i] = i;
sendcounts[i] = 0;
}
for (i = 0; i<lAI.rows(); ++i) {
sendcounts[i % lProcSize] +=1;
}
}
MPI_Gatherv(&(lData[0][0]), 6, MPI_DOUBLE,
sendarray, sendcounts, displs, lType,
0, MPI_COMM_WORLD);
if (lRank == 0)
{
for (i = 0; i<lAI.rows(); ++i) {
for (j = 0; j<lAI.cols(); ++j) {
// lAI(i,j) = lData[i][j];
lAI(i,j) = sendarray[i][j];
}
}
cout << "Matrice inverse:\n" << lAI.str() << endl;
}
// On copie la partie droite de la matrice AI ainsi transformée
// dans la matrice courante (this).
for (unsigned int i=0; i<iA.rows(); ++i) {
iA.getRowSlice(i) = lAI.getDataArray()[slice(i*lAI.cols()+iA.cols(), iA.cols(), 1)];
}
// 2. Rapatrier (Gatherv) toutes les lignes sur le processus 0
}
void yarp::math::pinvDamped(const Matrix &in, Matrix &out, double damp)
{
int k = in.rows()<in.cols() ? in.rows() : in.cols();
Vector sv(k);
pinvDamped(in, out, sv, damp);
}
