// ********************************************************************************************
// Singular value decomposition.
// Return othogonal matrices U and V and diagonal matrix with diagonal w such that
// (this) = U * Diag(w) * V^T (V^T is V-transpose.)
// Diagonal entries have all non-zero entries before all zero entries, but are not
// necessarily sorted. (Someday, I will write ComputedSortedSVD that handles
// sorting the eigenvalues by magnitude.)
// ********************************************************************************************
void MatrixRmn::ComputeSVD( MatrixRmn& U, VectorRn& w, MatrixRmn& V ) const
{
assert ( U.NumRows==NumRows && V.NumCols==NumCols
&& U.NumRows==U.NumCols && V.NumRows==V.NumCols
&& w.GetLength()==Min(NumRows,NumCols) );
// double temp=0.0;
VectorRn& superDiag = VectorRn::GetWorkVector( w.GetLength()-1 ); // Some extra work space. Will get passed around.
// Choose larger of U, V to hold intermediate results
// If U is larger than V, use U to store intermediate results
// Otherwise use V. In the latter case, we form the SVD of A transpose,
// (which is essentially identical to the SVD of A).
MatrixRmn* leftMatrix;
MatrixRmn* rightMatrix;
if ( NumRows >= NumCols ) {
U.LoadAsSubmatrix( *this ); // Copy A into U
leftMatrix = &U;
rightMatrix = &V;
}
else {
V.LoadAsSubmatrixTranspose( *this ); // Copy A-transpose into V
leftMatrix = &V;
rightMatrix = &U;
}
// Do the actual work to calculate the SVD
// Now matrix has at least as many rows as columns
CalcBidiagonal( *leftMatrix, *rightMatrix, w, superDiag );
ConvertBidiagToDiagonal( *leftMatrix, *rightMatrix, w, superDiag );
}
// Multiply transpose of this matrix by column vector v.
// Result is column vector "result"
// Equivalent to mult by row vector on left
void MatrixRmn::MultiplyTranspose(const VectorRn &v, VectorRn &result) const
{
assert(v.GetLength() == NumRows && result.GetLength() == NumCols);
double *out = result.GetPtr(); // Points to entry in result vector
const double *colPtr = x; // Points to beginning of next column in matrix
for (long i = NumCols; i > 0; i--) {
const double *in = v.GetPtr();
*out = 0.0f;
for (long j = NumRows; j > 0; j--) {
*out += (*(in++)) * (*(colPtr++));
}
out++;
}
}
// Solves the equation (*this)*xVec = b;
// Uses row operations. Assumes *this is square and invertible.
// No error checking for divide by zero or instability (except with asserts)
void MatrixRmn::Solve(const VectorRn &b, VectorRn *xVec) const
{
assert(NumRows == NumCols && NumCols == xVec->GetLength() && NumRows == b.GetLength());
// Copy this matrix and b into an Augmented Matrix
MatrixRmn &AugMat = GetWorkMatrix(NumRows, NumCols + 1);
AugMat.LoadAsSubmatrix(*this);
AugMat.SetColumn(NumRows, b);
// Put into row echelon form with row operations
AugMat.ConvertToRefNoFree();
// Solve for x vector values using back substitution
double *xLast = xVec->x + NumRows - 1; // Last entry in xVec
double *endRow = AugMat.x + NumRows * NumCols - 1; // Last entry in the current row of the coefficient part of Augmented Matrix
double *bPtr = endRow + NumRows; // Last entry in augmented matrix (end of last column, in augmented part)
for (long i = NumRows; i > 0; i--) {
double accum = *(bPtr--);
// Next loop computes back substitution terms
double *rowPtr = endRow; // Points to entries of the current row for back substitution.
double *xPtr = xLast; // Points to entries in the x vector (also for back substitution)
for (long j = NumRows - i; j > 0; j--) {
accum -= (*rowPtr) * (*(xPtr--));
rowPtr -= NumCols; // Previous entry in the row
}
assert(*rowPtr != 0.0); // Are not supposed to be any free variables in this matrix
*xPtr = accum / (*rowPtr);
endRow--;
}
}
// Multiply this matrix by column vector v.
// Result is column vector "result"
void MatrixRmn::Multiply(const VectorRn &v, VectorRn &result) const
{
assert(v.GetLength() == NumCols && result.GetLength() == NumRows);
double *out = result.GetPtr(); // Points to entry in result vector
const double *rowPtr = x; // Points to beginning of next row in matrix
for (long j = NumRows; j > 0; j--) {
const double *in = v.GetPtr();
const double *m = rowPtr++;
*out = 0.0f;
for (long i = NumCols; i > 0; i--) {
*out += (*(in++)) * (*m);
m += NumRows;
}
out++;
}
}
// Set the i-th column equal to d.
void MatrixRmn::SetColumn(long i, const VectorRn &d)
{
assert(NumRows == d.GetLength());
double *to = x + i * NumRows;
const double *from = d.x;
for (i = NumRows; i > 0; i--) {
*(to++) = *(from++);
}
}
// Set the i-th column equal to d.
void MatrixRmn::SetRow(long i, const VectorRn &d)
{
assert(NumCols == d.GetLength());
double *to = x + i;
const double *from = d.x;
for (i = NumRows; i > 0; i--) {
*to = *(from++);
to += NumRows;
}
}
// Form the dot product of a vector v with the i-th column of the array
double MatrixRmn::DotProductColumn(const VectorRn &v, long colNum) const
{
assert(v.GetLength() == NumRows);
double *ptrC = x + colNum * NumRows;
double *ptrV = v.x;
double ret = 0.0;
for (long i = NumRows; i > 0; i--) {
ret += (*(ptrC++)) * (*(ptrV++));
}
return ret;
}
void Jacobian::CalcDeltaThetasPseudoinverse()
{
MatrixRmn &J = const_cast<MatrixRmn &>(Jend);
// costruisco matrice J1
MatrixRmn J1;
J1.SetSize(2, J.getNumColumns());
for (int i = 0; i < 2; i++)
for (int j = 0; j < J.getNumColumns(); j++)
J1.Set(i, j, J.Get(i, j));
// COSTRUISCO VETTORI ds1 e ds2
VectorRn dS1(2);
for (int i = 0; i < 2; i++)
dS1.Set(i, dS.Get(i));
// calcolo dtheta1
MatrixRmn U, V;
VectorRn w;
U.SetSize(J1.getNumRows(), J1.getNumRows());
w.SetLength(min(J1.getNumRows(), J1.getNumColumns()));
V.SetSize(J1.getNumColumns(), J1.getNumColumns());
J1.ComputeSVD(U, w, V);
// Next line for debugging only
assert(J1.DebugCheckSVD(U, w, V));
// Calculate response vector dTheta that is the DLS solution.
// Delta target values are the dS values
// We multiply by Moore-Penrose pseudo-inverse of the J matrix
double pseudoInverseThreshold = PseudoInverseThresholdFactor * w.MaxAbs();
long diagLength = w.GetLength();
double *wPtr = w.GetPtr();
dTheta.SetZero();
for (long i = 0; i < diagLength; i++) {
double dotProdCol = U.DotProductColumn(dS1, i); // Dot product with i-th column of U
double alpha = *(wPtr++);
if (fabs(alpha) > pseudoInverseThreshold) {
alpha = 1.0 / alpha;
MatrixRmn::AddArrayScale(V.getNumRows(), V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, dotProdCol * alpha);
}
}
MatrixRmn JcurrentPinv(V.getNumRows(), J1.getNumRows()); // pseudoinversa di J1
MatrixRmn JProjPre(JcurrentPinv.getNumRows(), J1.getNumColumns()); // Proiezione di J1
if (skeleton->getNumEffector() > 1) {
// calcolo la pseudoinversa di J1
MatrixRmn VD(V.getNumRows(), J1.getNumRows()); // matrice del prodotto V*w
double *wPtr = w.GetPtr();
pseudoInverseThreshold = PseudoInverseThresholdFactor * w.MaxAbs();
for (int j = 0; j < VD.getNumColumns(); j++) {
double *VPtr = V.GetColumnPtr(j);
double alpha = *(wPtr++); // elemento matrice diagonale
for (int i = 0; i < V.getNumRows(); i++) {
if (fabs(alpha) > pseudoInverseThreshold) {
double entry = *(VPtr++);
VD.Set(i, j, entry * (1.0 / alpha));
}
}
}
MatrixRmn::MultiplyTranspose(VD, U, JcurrentPinv);
// calcolo la proiezione J1
MatrixRmn::Multiply(JcurrentPinv, J1, JProjPre);
for (int j = 0; j < JProjPre.getNumColumns(); j++)
for (int i = 0; i < JProjPre.getNumRows(); i++) {
double temp = JProjPre.Get(i, j);
JProjPre.Set(i, j, -1.0 * temp);
}
JProjPre.AddToDiagonal(diagMatIdentity);
}
//task priority strategy
for (int i = 1; i < skeleton->getNumEffector(); i++) {
// costruisco matrice Jcurrent (Ji)
MatrixRmn Jcurrent(2, J.getNumColumns());
for (int j = 0; j < J.getNumColumns(); j++)
for (int k = 0; k < 2; k++)
Jcurrent.Set(k, j, J.Get(k + 2 * i, j));
// costruisco il vettore dScurrent
VectorRn dScurrent(2);
for (int k = 0; k < 2; k++)
dScurrent.Set(k, dS.Get(k + 2 * i));
// Moltiplico Jcurrent per la proiezione di J(i-1)
MatrixRmn Jdst(Jcurrent.getNumRows(), JProjPre.getNumColumns());
MatrixRmn::Multiply(Jcurrent, JProjPre, Jdst);
// Calcolo la pseudoinversa di Jdst
MatrixRmn UU(Jdst.getNumRows(), Jdst.getNumRows()), VV(Jdst.getNumColumns(), Jdst.getNumColumns());
VectorRn ww(min(Jdst.getNumRows(), Jdst.getNumColumns()));
Jdst.ComputeSVD(UU, ww, VV);
assert(Jdst.DebugCheckSVD(UU, ww, VV));
MatrixRmn VVD(VV.getNumRows(), J1.getNumRows()); // matrice V*w
VVD.SetZero();
pseudoInverseThreshold = PseudoInverseThresholdFactor * ww.MaxAbs();
double *wwPtr = ww.GetPtr();
for (int j = 0; j < VVD.getNumColumns(); j++) {
double *VVPtr = VV.GetColumnPtr(j);
double alpha = 50 * (*(wwPtr++)); // elemento matrice diagonale
for (int i = 0; i < VV.getNumRows(); i++) {
if (fabs(alpha) > 100 * pseudoInverseThreshold) {
double entry = *(VVPtr++);
VVD.Set(i, j, entry * (1.0 / alpha));
}
}
}
MatrixRmn JdstPinv(VV.getNumRows(), J1.getNumRows());
MatrixRmn::MultiplyTranspose(VVD, UU, JdstPinv);
VectorRn dTemp(J1.getNumRows());
Jcurrent.Multiply(dTheta, dTemp);
VectorRn dTemp2(dScurrent.GetLength());
for (int k = 0; k < dScurrent.GetLength(); k++)
dTemp2[k] = dScurrent[k] - dTemp[k];
// Moltiplico JdstPinv per dTemp2
VectorRn dThetaCurrent(JdstPinv.getNumRows());
JdstPinv.Multiply(dTemp2, dThetaCurrent);
for (int k = 0; k < dTheta.GetLength(); k++)
dTheta[k] += dThetaCurrent[k];
// Infine mi calcolo la pseudoinversa di Jcurrent e quindi la sua proiezione che servirà al passo successivo
// calcolo la pseudoinversa di Jcurrent
Jcurrent.ComputeSVD(U, w, V);
assert(Jcurrent.DebugCheckSVD(U, w, V));
MatrixRmn VD(V.getNumRows(), Jcurrent.getNumRows()); // matrice del prodotto V*w
double *wPtr = w.GetPtr();
pseudoInverseThreshold = PseudoInverseThresholdFactor * w.MaxAbs();
for (int j = 0; j < VVD.getNumColumns(); j++) {
double *VPtr = V.GetColumnPtr(j);
double alpha = *(wPtr++); // elemento matrice diagonale
for (int i = 0; i < V.getNumRows(); i++) {
if (fabs(alpha) > pseudoInverseThreshold) {
double entry = *(VPtr++);
VD.Set(i, j, entry * (1.0 / alpha));
}
}
}
MatrixRmn::MultiplyTranspose(VD, U, JcurrentPinv);
// calcolo la proiezione Jcurrent
MatrixRmn::Multiply(JcurrentPinv, Jcurrent, JProjPre);
for (int j = 0; j < JProjPre.getNumColumns(); j++)
for (int k = 0; k < JProjPre.getNumRows(); k++) {
double temp = JProjPre.Get(k, j);
JProjPre.Set(k, j, -1.0 * temp);
}
JProjPre.AddToDiagonal(diagMatIdentity);
}
//sw.stop();
//std::ofstream os("C:\\buttami.txt", std::ios::app);
//sw.print(os);
//os.close();
// Scale back to not exceed maximum angle changes
double maxChange = 10 * dTheta.MaxAbs();
if (maxChange > MaxAnglePseudoinverse) {
dTheta *= MaxAnglePseudoinverse / maxChange;
}
}