/////////////////////////
// operator overloads
/////////////////////////
Matrix Matrix::operator+(const Matrix & toAdd)
{
if (rows != toAdd.getRows() || cols != toAdd.getCols())
{
cerr << "Invalid Dimensions" << endl;
exit(0);
}
Matrix sum(rows,cols);
for (int i = 0; i < rows; i++)
{
for (int j = 0; j < cols ; j++)
{
Number sumNum = at(i).at(j)+ toAdd.getNum(i, j);
sum.setNum(i, j, sumNum);
}
}
return sum;
}
int
solveLUP(Matrix<T> &m, Vector<T> &x, Vector<T> &y, Vector<int> &p, T ep)
{
// must be a square matrix
MustBeTrue(m.getRows() == m.getCols() && m.getRows() > 0);
// check epsilon, set if invalid
T minep = calcEpsilon(T(0));
if ((ep = fabs(ep)) < minep)
ep = minep;
// get number of rows and columns
int max = m.getRows();
// update y-vector
for (int k = 0; k < (max-1); k++)
{
for (int i = k+1; i < max; i++)
{
CheckForOverFlow(m(p[i], k), y[p[k]]);
y[p[i]] -= m(p[i], k)*y[p[k]];
}
}
// start backward substitution
for (int i = max-1; i >= 0; i--)
{
// check for a singular matrix
if (fabs(m(p[i], i)) <= ep)
return(NOTOK);
// solve for x by substituting previous solutions
x[i] = y[p[i]];
for (int j = i+1; j < max; j++)
{
CheckForOverFlow(m(p[i], j), x[j]);
x[i] -= m(p[i], j)*x[j];
}
x[i] /= m(p[i], i);
}
// all done
return(OK);
}
int
determinantLUP(Matrix<T> &m, T &d)
{
// must be a square matrix
MustBeTrue(m.getRows() == m.getCols() && m.getRows() > 0);
// get number of rows and columns
int max = m.getRows();
// get determinant
for (int i = 0; i < max; i++)
{
CheckForOverFlow(d, m(i, i));
d = d*m(i, i);
}
// all done
return(OK);
}
/**
* Copy a matrix class, the matrix must be square ( checked
* by assertions
*
* @param im The input matrix to adapt to a SMatrix
*/
SMatrix::SMatrix( const Matrix &im ):
Matrix( im )
{
assert ( im.getCols() == im.getRows() && "Input matrix is not square");
}
int
gaussianLUP(Matrix<T> &m, Vector<int> &p, T ep, T &sign)
{
// must be a square matrix
MustBeTrue(m.getRows() == m.getCols() && m.getRows() > 0);
sign = -1;
// check epsilon, set if invalid
T minep = calcEpsilon(T(0));
if ((ep = fabs(ep)) < minep)
ep = minep;
// get number of rows and columns
int max = m.getRows();
// generate scaling information for each row. initialize
// permutation array, p.
int i;
Vector<T> s(max);
for (i = 0; i < max; i++)
{
p[i] = i;
if (1 < max)
s[i] = fabs(m(i, 1));
else
s[i] = fabs(m(i, 0));
for (int j = 2; j < max; j++)
{
T tmp = fabs(m(i, j));
if (tmp > s[i])
s[i] = tmp;
}
}
// start gaussian elimination process
for (int k = 0; k < (max-1); k++)
{
// find pivot row
int pivot = k;
T tmpf = fabs(m(p[pivot], k))/s[p[pivot]];
for (i = k+1; i < max; i++)
{
T tmpf2 = fabs(m(p[i], k))/s[p[i]];
if (tmpf2 > tmpf)
{
pivot = i;
tmpf = tmpf2;
}
}
if (pivot != k)
{
int tmpp = p[k];
p[k] = p[pivot];
p[pivot] = tmpp;
sign = -sign;
}
// check for division by zero
if (fabs(m(p[k], k)) <= ep)
return(NOTOK);
// calculate L and U matrices
for (i = k+1; i < max; i++)
{
// multiplier for column
T d = m(p[i], k)/m(p[k], k);
// save multiplier since it is L.
m(p[i], k) = d;
// reduce original matrix to get U.
for (int j = k+1; j < max; j++)
{
CheckForOverFlow(d, m(p[k], j));
m(p[i], j) -= d*m(p[k], j);
}
}
}
// all done
return(OK);
}
Quaternion::Quaternion(Matrix R)
{
if(R.getCols()==2 && R.getRows()==2)
{
double trace = R.trace() + 1.0;
_x = 0.0;
_y = 0.0;
if(trace>1e-6)
{
obfloat s = 0.5 / sqrt(trace+1.0);
_w = 0.25 / s;
_z = (R(1,0) - R(0,1)) * s;
}
else
{
obfloat s = 2.0 * sqrt(2.0 - R(0,0) - R(1,1));
_w = (R(1,0)-R(0,1)) / s;
_z = 0.25 * s;
}
}
else if(R.getCols()==3 && R.getRows()==3)
{
double trace = R.trace();
if(trace>1e-6)
{
obfloat s = 0.5 / sqrt(trace+1.0);
_w = 0.25 / s;
_x = (R(2,1) - R(1,2)) * s;
_y = (R(0,2) - R(2,0)) * s;
_z = (R(1,0) - R(0,1)) * s;
}
else
{
if(R(0,0)>R(1,1) && R(0,0)>R(2,2))
{
obfloat s = 2.0 * sqrt(1.0 + R(0,0) - R(1,1) - R(2,2));
_w = (R(2,1) - R(1,2)) / s;
_x = 0.25 * s;
_y = (R(0,1)+R(1,0)) / s;
_z = (R(0,2)+R(2,0)) / s;
}
else if(R(1,1)>R(2,2))
{
obfloat s = 2.0 * sqrt(1.0 + R(1,1) - R(0,0) - R(2,2));
_w = (R(0,2)-R(2,0)) / s;
_x = (R(0,1)+R(1,0)) / s;
_y = 0.25 * s;
_z = (R(1,2)+R(2,1)) / s;
}
else
{
obfloat s = 2.0 * sqrt(1.0 + R(2,2) - R(0,0) - R(1,1));
_w = (R(1,0)-R(0,1)) / s;
_x = (R(0,2)+R(2,0)) / s;
_y = (R(1,2)+R(2,1)) / s;
_z = 0.25 * s;
}
}
}
else
{
LOGMSG(DBG_ERROR, "No square matrix passed.");
}
}
// main for the feature transformation tool: "paramX"
int main(int argc, char *argv[]) {
try {
// (1) define command line parameters
CommandLineManager commandLineManager("paramx",SYSTEM_VERSION,SYSTEM_AUTHOR,SYSTEM_DATE);
commandLineManager.defineParameter("-cfg","feature configuration",PARAMETER_TYPE_FILE,false);
commandLineManager.defineParameter("-tra","feature transformation",PARAMETER_TYPE_FILE,false);
commandLineManager.defineParameter("-bat","batch file containing pairs (feaIn feaOut)",
PARAMETER_TYPE_FILE,true);
commandLineManager.defineParameter("-in","input feature vectors",PARAMETER_TYPE_FILE,true);
commandLineManager.defineParameter("-out","output feature vectors",PARAMETER_TYPE_FILE,true);
// parse the parameters
if (commandLineManager.parseParameters(argc,argv) == false) {
return -1;
}
// load the parameters
const char *strFileConfiguration = commandLineManager.getParameterValue("-cfg");
const char *strFileTransform = commandLineManager.getParameterValue("-tra");
const char *strFileBatch = commandLineManager.getParameterValue("-bat");
const char *strFileInput = commandLineManager.getParameterValue("-in");
const char *strFileOutput = commandLineManager.getParameterValue("-out");
// load the feature configuration
ConfigurationFeatures configurationFeatures(strFileConfiguration);
configurationFeatures.load();
// load the transformation
Transform transform;
transform.load(strFileTransform);
//transform->print(true);
// single feature file
if (strFileBatch == NULL) {
// load the features
int iDimensionality = configurationFeatures.getDimensionality();
FeatureFile featureFile(strFileInput,MODE_READ,FORMAT_FEATURES_FILE_DEFAULT,iDimensionality);
featureFile.load();
Matrix<float> *mFeatures = featureFile.getFeatureVectors();
Matrix<float> *mFeaturesX = transform.apply(*mFeatures);
// create the transformed feature file
FeatureFile featureFileX(strFileOutput,MODE_WRITE,FORMAT_FEATURES_FILE_DEFAULT,mFeaturesX->getCols());
featureFileX.store(*mFeaturesX);
delete mFeatures;
delete mFeaturesX;
}
// batch mode
else {
BatchFile batchFile(strFileBatch,"featuresIn|featuresOut");
batchFile.load();
for(unsigned int i=0 ; i < batchFile.size() ; ++i) {
// load the features
int iDimensionality = configurationFeatures.getDimensionality();
FeatureFile featureFile(batchFile.getField(i,"featuresIn"),MODE_READ,
FORMAT_FEATURES_FILE_DEFAULT,iDimensionality);
featureFile.load();
Matrix<float> *mFeatures = featureFile.getFeatureVectors();
Matrix<float> *mFeaturesX = transform.apply(*mFeatures);
// create the transformed feature file
FeatureFile featureFileX(batchFile.getField(i,"featuresOut"),MODE_WRITE,
FORMAT_FEATURES_FILE_DEFAULT,mFeaturesX->getCols());
featureFileX.store(*mFeaturesX);
delete mFeatures;
delete mFeaturesX;
}
}
} catch (std::runtime_error &e) {
std::cerr << e.what() << std::endl;
return -1;
}
return 0;
}
