Esempio n. 1
0
// ********************************************************************************************
// 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 );

}
Esempio n. 2
0
// 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++;
	}
}
Esempio n. 3
0
// 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--;
	}
}
Esempio n. 4
0
// 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++;
	}
}
Esempio n. 5
0
// 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++);
	}
}
Esempio n. 6
0
// 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;
	}
}
Esempio n. 7
0
// 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;
}
Esempio n. 8
0
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;
	}
}