//#############################################################################
bool Matrix::operator==( const Matrix& B) const
{
assert( !(*this).isEmpty() );
assert( ! B.isEmpty() );
assert( (*this).getRow() == B.getRow() );
assert( (*this).getCol() == B.getCol() );
int kontrol=0;
int j=0;
while( j != B.getRow() )
{
int i=0;
while( i != B.getCol() )
{
if((*this)(j,i)!=B(j,i))
++kontrol;
++i;
}
++j;
}
if(kontrol)
return false;
return true;
}
//#############################################################################
//Abgleichen ob 2 Matrizen "gleich" sind
bool Matrix::NearEqual( const Matrix& B, double threshold) const
{
assert( !(*this).isEmpty() );
assert( ! B.isEmpty() );
assert( (*this).getRow() == B.getRow() );
assert( (*this).getCol() == B.getCol() );
//double threshold = 9.99999e-8;
int kontrol=0;
int j=0;
while( j != B.getRow() )
{
int i=0;
while( i != B.getCol() )
{
double diff=fabs((*this)(j,i)-B(j,i));
if(diff>threshold)
++kontrol;
++i;
}
++j;
}
if(kontrol)
return false;
return true;
}
Matrix Matrix::MultT2(const Matrix& left, const Matrix& right) {
// left * right^T
Matrix ret(left.getRow(), right.getRow());
for (int row = 0; row < ret.getRow(); row++) {
for (int col = 0; col < ret.getCol(); col++) {
double sum = 0;
for (int sumIndex = 0; sumIndex < left.getCol(); sumIndex++) {
sum += left(row, sumIndex) * right(col, sumIndex);
}
ret(row, col) = sum;
}}
return ret;
}
Matrix vertcat(Matrix a, Matrix b)
{
//Matrix concatenation
//assume same dimension on cols
int mTotal=a.getm()+b.getm();
Matrix c=Matrix(mTotal,a.getn(),false);
for(int i=0;i<a.getm();i++){
c.setRow(i,a.getRow(i));
}
for(int i=0;i<b.getm();i++){
c.setRow(i+a.getm(),b.getRow(i));
}
return c;
}
/**
* Overloading ->* operator (matrix multiplication)
* \ingroup overload
* @param m The right operand
* @return The matrix multiplication
*/
Matrix Matrix::operator->*(const Matrix &m) const
{
int i, j, nr, nc, p;
Matrix temp;
double tmp;
nr = m.getRow();
nc = m.getCol();
if (col != nr) {
cerr << "OPERATOR->*: "
<< "Matrix size is not consistent, cannot do multiplication\n";
exit(3);
}
temp.createMatrix(row, nc);
for (i=0; i<row; i++)
for (j=0; j<nc; j++) {
tmp = 0.0;
for (p=0; p<col; p++) {
tmp += matrix[i*col+p] * m(p,j);
}
temp(i,j) = tmp;
}
return temp;
}
/**
* Overloading / operator (piecewise division)
* \ingroup overload
* @param m The right operand
* @return The division
*/
Matrix Matrix::operator/(const Matrix &m) const
{
int i, j, nr, nc;
Matrix temp;
nr = m.getRow();
nc = m.getCol();
if (nr != row || nc != col) {
cerr << "OPERATOR/: "
<< "Matrices are not of the same size, cannot do multiplication\n";
exit(3);
}
temp.createMatrix(row, col);
for (i=0; i<row; i++)
for (j=0; j<col; j++) {
if (m(i,j)!=0)
temp(i,j) = matrix[i*col+j] / m(i,j);
else
temp(i,j) = matrix[i*col+j] / 0.000001;
}
return temp;
}
/**
* Overloading * operator (piecewise multiplication)
* \ingroup overload
* @param m The right operand
* @return The component-wise multiplication
*/
Matrix Matrix::operator*(const Matrix &m) const
{
int i, j, nr, nc;
Matrix temp;
nr = m.getRow();
nc = m.getCol();
if (col != nr) {
cerr << "OPERATOR*: "
<< "Cannot do multiplication\n";
exit(3);
}
temp.createMatrix(row, nc);
for (i=0; i<row; i++)
for (j=0; j<nc; j++) {
double aux = 0;
for (int k = 0; k < col; k++) {
aux += matrix[i*col + k] * m(k, j);
}
//temp(i,j) = matrix[i*nc+j] * m(i,j);
temp(i, j) = aux;
}
return temp;
}
returnValue ShootingMethod::differentiateBackward( const int &idx ,
const Matrix &seed,
Matrix &Gx ,
Matrix &Gp ,
Matrix &Gu ,
Matrix &Gw ){
uint run1;
Gx.init( seed.getNumRows(), nx );
Gp.init( seed.getNumRows(), np );
Gu.init( seed.getNumRows(), nu );
Gw.init( seed.getNumRows(), nw );
for( run1 = 0; run1 < seed.getNumRows(); run1++ ){
Vector tmp = seed.getRow( run1 );
Vector tmpX( nx );
Vector tmpP( np );
Vector tmpU( nu );
Vector tmpW( nw );
ACADO_TRY( integrator[idx]->setBackwardSeed( 1, tmp ) );
ACADO_TRY( integrator[idx]->integrateSensitivities( ) );
ACADO_TRY( integrator[idx]->getBackwardSensitivities( tmpX, tmpP, tmpU, tmpW , 1 ) );
Gx.setRow( run1, tmpX );
Gp.setRow( run1, tmpP );
Gu.setRow( run1, tmpU );
Gw.setRow( run1, tmpW );
}
return SUCCESSFUL_RETURN;
}
void HorizontalMultiply(Matrix& MatrixA, Matrix& MatrixB, Matrix& MatrixC)
{
int cores = (int)sysconf(_SC_NPROCESSORS_ONLN); //Get # of cores from system
pthread_t tid[cores]; //Make thread ids for number of cores available in system
Pass passlist[cores];// Make an array of struct for number of cores available
Pass *temp; // Pointer to the struct
cout << "Your computer has " << cores << " cores available " << endl;
vector<int>array[cores]; //Arrays made for number of cores
for(int row=0;row<MatrixA.getRow();row++) //Partition to where the threads would be
{
array[row%cores].push_back(row);
}
for(int i=0; i< cores; i++)
{ /*Set struct pointer to corresponding values*/
temp=&passlist[i];
temp->RowIndex=array+i;
temp->MatrixA=&MatrixA;
temp->MatrixB=&MatrixB;
temp->MatrixC=&MatrixC;
pthread_create(&tid[i],NULL,runner,(void*)temp);
}
/*After threads are done with execution we will join*/
for(int i=0; i< cores; i++)
{
pthread_join(tid[i],NULL);
}
}//End function
void Perceptron::fit(const Matrix<float>& M, const Matrix<bool>& label)
{
coeff = zeros<float>(M.colNb(), 1);
std::size_t cpt = 0;
bool coeff_changed = true;
while(coeff_changed && cpt<max_iteration)
{
coeff_changed = false;
for(std::size_t i=0;i<M.rowNb();++i)
{
Matrix<float> X = M.getRow(i);
bool res = predict(X)(0, 0);
if(res!=label(i, 0))
{
X.transpose();
coeff_changed = true;
if(label(i, 0))
{
coeff+=alpha*X;
thresh-=alpha;
}
else
{
coeff-=alpha*X;
thresh+=alpha;
}
}
}
++cpt;
}
}
bool SIMoutput::evalProjSolution (const Vector& ssol,
std::vector<PointValue>& maxVal)
{
if (ssol.empty())
return true;
else if (!myVtf)
return true;
Matrix field;
Vector lovec;
size_t i, j;
int geomID = myGeomID;
for (i = 0; i < myModel.size(); i++)
{
if (myModel[i]->empty()) continue; // skip empty patches
// Evaluate the solution variables at the visualization points
myModel[i]->extractNodeVec(ssol,lovec,myProblem->getNoFields(2));
if (!myModel[i]->evalSolution(field,lovec,opt.nViz))
return false;
const ElementBlock* grid = myVtf->getBlock(++geomID);
for (j = 0; j < field.rows() && j < maxVal.size(); j++)
{
// Update extremal values
size_t indx = 0;
double cmax = field.getRow(1+j).normInf(indx);
if (fabs(cmax) > fabs(maxVal[j].second))
maxVal[j] = std::make_pair(grid->getCoord(indx-1),cmax);
}
}
return true;
}
bool SIMoutput::writeGlvE (const Vector& vec, int iStep, int& nBlock,
const char* name)
{
if (!myVtf)
return false;
Matrix infield(1,vec.size());
infield.fillRow(1,vec.ptr());
Matrix field;
int geomID = myGeomID;
IntVec sID;
for (size_t i = 0; i < myModel.size(); i++)
{
if (myModel[i]->empty()) continue; // skip empty patches
if (msgLevel > 1)
IFEM::cout <<"Writing element field "<< name
<<" for patch "<< i+1 << std::endl;
myModel[i]->extractElmRes(infield,field);
if (!myVtf->writeEres(field.getRow(1),++nBlock,++geomID))
return false;
sID.push_back(nBlock);
}
int idBlock = 300;
if (!myVtf->writeSblk(sID,name,++idBlock,iStep,true))
return false;
return true;
}
//#############################################################################
// Matizen multiplizieren
Matrix Matrix::MatMult(const Matrix &B )
{
assert(!(*this).isEmpty() && !B.isEmpty() );
assert( (*this).getCol() == B.getRow() );
Matrix Z( (*this).getRow() , B.getCol() , Empty );
int i=0,j=0,k=0;
j=0;
while( j != Z.getRow() )
{
i=0;
while( i != Z.getCol() )
{
Z(j,i)=0.0;
k=0;
while( k != (*this).getCol() )
{
Z(j,i)+=(*this)(j,k)*B(k,i);
++k;
}
++i;
}
++j;
}
return Z;
}
Matrix::Matrix(const Matrix& other) :
row(other.getRow()),
col(other.getCol()),
components(new double[row * col]())
{
for (int i = 0; i < row * col; i++) {
components[i] = other(i);
}
}
void setToZero(Matrix &m) {
bool should_zero_col0 = false, should_zero_row0 = false;
for (int i = 0; i < m.getRow(); i++) {
if (0 == m.data[i][0]) {
should_zero_col0 = true;
break;
}
}
for (int i = 0; i < m.getCol(); i++) {
if (0 == m.data[0][i]) {
should_zero_row0 = true;
break;
}
}
for (int i = 1; i < m.getRow(); i++) {
for (int j = 1; j < m.getCol(); j++) {
if (0 == m.data[i][j]) {
m.data[i][0] = 0; m.data[0][j] = 0;
}
}
}
for (int i = 1; i < m.getRow(); i++) {
if (0 == m.data[i][0]) {
for (int j = 1; j < m.getCol(); j++) m.data[i][j] = 0;
}
}
for (int j = 1; j < m.getCol(); j++) {
if (0 == m.data[0][j]) {
for (int i = 1; i < m.getRow(); i++) m.data[i][j] = 0;
}
}
if (should_zero_col0) {
for (int i = 0; i<m.getRow(); i++) m.data[i][0] = 0;
}
if (should_zero_row0) {
for (int j = 0; j<m.getCol(); j++) m.data[0][j] = 0;
}
}
bool SIMoutput::writeGlvP (const Vector& ssol, int iStep, int& nBlock,
int idBlock, const char* prefix,
std::vector<PointValue>* maxVal)
{
if (ssol.empty())
return true;
else if (!myVtf)
return false;
Matrix field;
Vector lovec;
std::vector<IntVec> sID(myProblem->getNoFields(2));
size_t i, j;
int geomID = myGeomID;
for (i = 0; i < myModel.size(); i++)
{
if (myModel[i]->empty()) continue; // skip empty patches
if (msgLevel > 1)
IFEM::cout <<"Writing projected solution for patch "<< i+1 << std::endl;
// Evaluate the solution variables at the visualization points
myModel[i]->extractNodeVec(ssol,lovec,sID.size());
if (!myModel[i]->evalSolution(field,lovec,opt.nViz))
return false;
// Write out to VTF-file as scalar fields
const ElementBlock* grid = myVtf->getBlock(++geomID);
for (j = 0; j < field.rows() && j < sID.size(); j++)
{
Vector comp(field.getRow(1+j));
if (!myVtf->writeNres(comp,++nBlock,geomID))
return false;
else
sID[j].push_back(nBlock);
if (maxVal && j < maxVal->size())
{
// Update extremal values
size_t indx = 0;
double cmax = comp.normInf(indx);
PointValue& pv = (*maxVal)[j];
if (fabs(cmax) > fabs(pv.second))
pv = std::make_pair(grid->getCoord(indx-1),cmax);
}
}
}
// Write result block identifications
for (j = 0; j < sID.size() && !sID[j].empty(); j++)
if (!myVtf->writeSblk(sID[j],myProblem->getField2Name(j,prefix).c_str(),
++idBlock,iStep)) return false;
return true;
}
/**
* Copy constructor.
* @param m Copy matrix.
* @return The created matrix.
*/
Matrix::Matrix(const Matrix &m)
{
int i, j;
createMatrix(m.getRow(), m.getCol()); // allocate memory
for (i=0; i<row; i++)
for (j=0; j<col; j++)
matrix[i*col+j] = m(i,j);
}
Matrix Matrix::operator+(const Matrix& right) const
{
if(getRow() != right.getRow() || getColumn() != right.getColumn())
throw OperandsNotMatched();
Matrix sum(getRow(), getColumn());
for(int i = 0; i < sum.getRow(); i++)
for(int j = 0; j < sum.getColumn(); j++)
sum.setEntry(i, j, getEntry(i, j) + right.getEntry(i, j));
return sum;
}
Matrix gassianKernel(Matrix x, double sigma)
{
int m=x.length();
Matrix kernel(x.length(),x.length());
for(int i=0;i<m;i++)
{
for(int j=i;j<m;j++)
{
Matrix xi = x.getRow(i,i);
Matrix xj=x.getRow(j,j);
double tmp =((xi-xj).powm(2)).sumPerRow().matrix_[0]*(-1.0/(2*pow(sigma,2))); //if m row, then m row
*kernel(i,j) = exp(tmp);
*kernel(j,i)=exp(tmp);
}
}
cout<<"kernel\n"<<kernel;
return kernel;
}
Matrix::Matrix(const Matrix & right)
{
setRC(right.getRow(), right.getColumn());
mat = new T*[getRow()];
for(int i = 0; i < getRow(); i++)
mat[i] = new T[getColumn()];
for(int i = 0; i < getRow(); i++)
for(int j = 0; j < getColumn(); j++)
mat[i][j] = right.getEntry(i, j);
}
//***************************************
//check whether a matrix has nan
//****************************************
int checkNan(Matrix& p)
{
double* ptr = p.getDataPtr();
int num = p.getCol() * p.getRow();
for (int i = 0; i < num; i++)
{
if (isnan(ptr[i]))
return 1;
}
return 0;
}
Matrix GaussJordanInverse(const Matrix& mat)
{
// Augment matrix with I - [mat | I]
Matrix A = horzcat(mat, Matrix(mat.getm(), mat.getn(), true));
int i,j;
unsigned int i_max = 0;
int k = 0;
double C;
for (k = 0; k < A.getm(); ++k)
{
// find max pivot.
i_max = k;
for (i = k; i < A.getm(); ++i)
{
if(fabs(A[i_max][k]) < fabs(A[i][k]))
i_max = i;
}
// Check if singular
if(A[i_max][k] == 0)
std::cout << "Matrix is singular" << std::endl;
A.swapRows(k, i_max);
for(i = k+1; i < A.getm(); ++i)
{
C = A[i][k] / A[k][k];
for(j = k+1; j < A.getn(); ++j)
{
A[i][j] -= A[k][j] * C;
}
A[i][k] = 0;
}
}
// Back substitution
for(k = A.getm()-1; k >= 0; --k)
{
C = A[k][k];
for(i=0; i < k; ++i)
{
for(j=A.getn()-1; j > k-1; --j)
{
A[i][j] -= A[k][j] * A[i][k] / C;
}
}
A.setRow(k, A.getRow(k) / C);
}
// Un-Augment matrix from I - [I | result]
Matrix result(mat.getm(), mat.getn(), false);
for(i=0; i < mat.getn(); ++i)
result.setCol(i, A.getCol(i + mat.getn()));
return result;
}
MatrixCrossSection getCrossSection(const Matrix& mat, int row_begin, int col_begin, int num_cols_and_rows)
{
MatrixCrossSection cs;
cs.row_id = row_begin;
cs.col_id = col_begin;
for(int i = 0; i < num_cols_and_rows; ++i){
cs.row_data.push_back( mat.getRow(row_begin + i) );
cs.col_data.push_back( mat.getCol(col_begin + i) );
}
return cs;
}
bool read(ConnectionReader &connection)
{
Bottle data;
data.read(connection);
if ((data.size()>=2) && enabled)
{
Vector xa,oa;
iarm->getPose(xa,oa);
Vector xe,oe;
if (eye=="left")
igaze->getLeftEyePose(xe,oe);
else
igaze->getRightEyePose(xe,oe);
Matrix Ha=axis2dcm(oa);
Ha.setSubcol(xa,0,3);
Matrix He=axis2dcm(oe);
He.setSubcol(xe,0,3);
Matrix H=Prj*SE3inv(He)*Ha;
Vector p(2);
p[0]=data.get(0).asDouble();
p[1]=data.get(1).asDouble();
if (logPort.getOutputCount()>0)
{
Vector &log=logPort.prepare();
log=p;
for (int i=0; i<H.rows(); i++)
log=cat(log,H.getRow(i));
for (int i=0; i<Prj.rows(); i++)
log=cat(log,Prj.getRow(i));
for (int i=0; i<Ha.rows(); i++)
log=cat(log,Ha.getRow(i));
for (int i=0; i<He.rows(); i++)
log=cat(log,He.getRow(i));
logPort.write();
}
mutex.wait();
solver.addItem(p,H);
mutex.post();
}
return true;
}
Matrix Matrix::operator*(const Matrix& right) const
{
if(getColumn() != right.getRow()) throw OperandsNotMatched();
Matrix temp(getRow(), right.getColumn());
for(int i = 0; i < temp.getRow(); i++)
for(int j = 0; j < temp.getColumn(); j++)
{
T ent = getEntry(i, 0) * right.getEntry(0, j);
for(int k = 1; k < getColumn(); k++)
ent = ent + getEntry(i, k) * right.getEntry(k, j);
temp.setEntry(i, j, ent);
}
return temp;
}
/**
* Overloading - operator. The left operand is the matrix and the right
* is a double point number. Each element in the matrix is
* subtracted by the double point number.
* \ingroup overload
* @param left Matrix as the left operand.
* @param dev A double point number as the right operand.
* @result Matrix subtracted by a double point number.
*/
Matrix operator-(Matrix &left, double s)
{
int i, j;
int nr, nc;
Matrix temp;
nr = left.getRow();
nc = left.getCol();
temp.createMatrix(nr, nc);
for (i=0; i<nr; i++)
for (j=0; j<nc; j++)
temp(i,j) = left(i,j) - s;
return temp;
}
//! \brief Write a field to VTF/VTU file
//! \param locvec The patch-level basis coefficients
//! \param components Number of components in field
//! \param patch The patch the field is defined on
//! \param model The evaluation points for this part of the fake FE model
//! \param geomID The ID associated with this patch
//! \param name Name of field
//! \param vlist List of vector fields stored in VTF/VTU
//! \param slist List of scalar fields stored in VTF/VTU
//! \param myVtf The VTF/VTU file to write to
bool writeFieldPatch(const Vector& locvec, int components,
ASMbase& patch, RealArray* model, int geomID, int& nBlock,
const std::string& name, VTFList& vlist, VTFList& slist,
VTF& myVtf, const std::string& description, const std::string& type)
{
if (dynamic_cast<VTU*>(&myVtf) && type == "displacement")
return true;
Matrix field;
if (!patch.evalSolution(field, locvec, model))
return false;
if (components > 1 || type == "eigenmodes") {
if (!myVtf.writeVres(field,++nBlock,geomID,components))
return false;
else {
vlist[description].Type = type;
vlist[description].Name = name;
vlist[description].Blocks.push_back(nBlock);
}
if (type == "displacement") {
vlist["combined displacement"].Type = type;
vlist["combined displacement"].Name = "combined displacements";
vlist["combined displacement"].Blocks.push_back(nBlock);
}
}
if (type == "eigenmodes")
return true;
for (size_t j = 0; j < field.rows(); j++) {
std::string nam = name;
if (field.rows() > 1) {
nam += "_";
nam += (char)('x'+j);
}
if (!myVtf.writeNres(field.getRow(1+j),++nBlock,geomID))
return false;
else {
slist[nam].Name = nam;
slist[nam].Blocks.push_back(nBlock);
}
}
return true;
}
void SVM::getEvalRslt()
{
Matrix x = X_orig;
Matrix y = Y_orig;
if(bRand)
{
x = X_rand;
y = Y_rand;
}
Matrix hypo = predictSvm(x);
//01. get relative matrixs
int m = y.length();
int train_n= (int)(rate_para.train_rate*m);
Matrix y_train = y.getRow(0,train_n-1);
Matrix hypo_train = hypo.getRow(0,train_n-1);
ML_SVM_INFO("train from 0 to "<<train_n-1);
int cv_n = (int)((rate_para.train_rate+rate_para.cv_rate)*m);
//int cv_m = cv_n-train_n;
Matrix y_cv =y.getRow(train_n,cv_n-1);
Matrix hypo_cv =hypo.getRow(train_n,cv_n-1);
ML_SVM_INFO("cv from "<<train_n<<" to "<<cv_n-1);
int test_m = m-cv_n;
Matrix y_test =y.getRow(cv_n,m-1);
Matrix hypo_test =hypo.getRow(cv_n,m-1);
ML_SVM_INFO("test from "<<cv_n<<" to "<<m-1);
//01. get accuracy
eval_rslt.train_accuracy = eval_rslt.getAccuracy(hypo_train,y_train);
eval_rslt.test_accuracy = eval_rslt.getAccuracy(hypo.getRow(train_n,m-1),y.getRow(train_n,m-1));
eval_rslt.getPrecision(hypo,y,labels);
//02. get cost, these are used for model selection
//eval_rslt.Jtrain=computeCostFunc(hypo_train,y_train);
//eval_rslt.Jcv=computeCostFunc(hypo_cv,y_cv);
//eval_rslt.Jtest =computeCostFunc(hypo_test,y_test);
ML_SVM_INFO("the evaulation parameters are as follows:"<<eval_rslt);
return;
}
/**
* Overloading = operator
* \ingroup overload
* @param m The right operand
* @return The assignment
*/
const Matrix Matrix::operator=(const Matrix &m)
{
int i, j;
if (this == &m)
return *this;
if (matrix)
delete [] matrix;
createMatrix(m.getRow(), m.getCol()); // allocate memory
for (i=0; i<row; i++)
for (j=0; j<col; j++)
matrix[i*col+j] = m(i,j);
return *this;
}
Matrix Matrix::operator=(const Matrix & right)
{
for(int i = 0; i < getRow(); i++)
{
delete[] mat[i];
}
delete[] mat;
setRC(right.getRow(), right.getColumn());
mat = new T*[getRow()];
for(int i = 0; i < getRow(); i++)
mat[i] = new T[getColumn()];
for(int i = 0; i < getRow(); i++)
for(int j = 0; j < getColumn(); j++)
mat[i][j] = right.getEntry(i, j);
return *this;
}
