Commit b14c0c52 authored by Joseph Mirabel's avatar Joseph Mirabel

Put SVD related functions in namespace dynamicgraph

parent fbd82900
......@@ -14,25 +14,26 @@
#include <Eigen/SVD>
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
namespace Eigen {
namespace dynamicgraph {
void pseudoInverse(dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
typedef Eigen::JacobiSVD<Matrix> SVD_t;
void pseudoInverse(Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold = 1e-6);
void dampedInverse(const JacobiSVD<dg::Matrix> &svd, dg::Matrix &_inverseMatrix,
void dampedInverse(const SVD_t &svd, Matrix &_inverseMatrix,
const double threshold = 1e-6);
void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
dg::Matrix &Uref, dg::Vector &Sref, dg::Matrix &Vref,
void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
Matrix &Uref, Vector &Sref, Matrix &Vref,
const double threshold = 1e-6);
void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold = 1e-6);
} // namespace Eigen
} // namespace dynamicgraph
#endif /* #ifndef __SOT_MATRIX_SVD_H__ */
......@@ -12,6 +12,7 @@
#include "sot/core/api.hh"
#include <sot/core/task-abstract.hh>
#include <sot/core/matrix-svd.hh>
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
......
......@@ -4,14 +4,17 @@
#include <sot/core/debug.hh>
#include <sot/core/matrix-svd.hh>
namespace Eigen {
namespace dynamicgraph {
using Eigen::ComputeThinU;
using Eigen::ComputeThinV;
using Eigen::ComputeFullV;
void pseudoInverse(dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
void pseudoInverse(Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold) {
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
JacobiSVD<dg::Matrix>::SingularValuesType m_singularValues =
SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV);
SVD_t::SingularValuesType m_singularValues =
svd.singularValues();
JacobiSVD<dg::Matrix>::SingularValuesType singularValues_inv;
SVD_t::SingularValuesType singularValues_inv;
singularValues_inv.resizeLike(m_singularValues);
for (long i = 0; i < m_singularValues.size(); ++i) {
if (m_singularValues(i) > threshold)
......@@ -23,43 +26,34 @@ void pseudoInverse(dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
svd.matrixU().transpose());
}
void dampedInverse(const JacobiSVD<dg::Matrix> &svd, dg::Matrix &_inverseMatrix,
void dampedInverse(const SVD_t &svd, Matrix &_inverseMatrix,
const double threshold) {
typedef JacobiSVD<dg::Matrix>::SingularValuesType SV_t;
ArrayWrapper<const SV_t> sigmas(svd.singularValues());
typedef SVD_t::SingularValuesType SV_t;
Eigen::ArrayWrapper<const SV_t> sigmas(svd.singularValues());
SV_t sv_inv(sigmas / (sigmas.cwiseAbs2() + threshold * threshold));
const dg::Matrix::Index m = sv_inv.size();
const Matrix::Index m = sv_inv.size();
_inverseMatrix.noalias() = (svd.matrixV().leftCols(m) * sv_inv.asDiagonal() *
svd.matrixU().leftCols(m).transpose());
}
void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
dg::Matrix &Uref, dg::Vector &Sref, dg::Matrix &Vref,
void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
Matrix &Uref, Vector &Sref, Matrix &Vref,
const double threshold) {
sotDEBUGIN(15);
sotDEBUG(5) << "Input Matrix: " << _inputMatrix << std::endl;
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeThinV);
SVD_t svd(_inputMatrix, ComputeThinU | ComputeThinV);
dampedInverse(svd, _inverseMatrix, threshold);
Uref = svd.matrixU();
Vref = svd.matrixV();
Sref = svd.singularValues();
sotDEBUGOUT(15);
}
void dampedInverse(const dg::Matrix &_inputMatrix, dg::Matrix &_inverseMatrix,
void dampedInverse(const Matrix &_inputMatrix, Matrix &_inverseMatrix,
const double threshold) {
sotDEBUGIN(15);
sotDEBUG(5) << "Input Matrix: " << _inputMatrix << std::endl;
JacobiSVD<dg::Matrix> svd(_inputMatrix, ComputeThinU | ComputeFullV);
SVD_t svd(_inputMatrix, ComputeThinU | ComputeFullV);
dampedInverse(svd, _inverseMatrix, threshold);
sotDEBUGOUT(15);
}
} // namespace Eigen
......@@ -165,13 +165,13 @@ int main(int argc, char **argv) {
START_CHRONO(inv);
for (int i = 0; i < BENCH; ++i)
Eigen::pseudoInverse(M, Minv);
dynamicgraph::pseudoInverse(M, Minv);
STOP_CHRONO(inv, "init");
sotDEBUG(15) << "Minv = " << Minv << endl;
START_CHRONO(inv);
for (int i = 0; i < BENCH; ++i)
Eigen::pseudoInverse(M, Minv);
dynamicgraph::pseudoInverse(M, Minv);
STOP_CHRONO(inv, "M+standard");
cout << dt_inv << endl;
......@@ -232,7 +232,7 @@ int main(int argc, char **argv) {
// dynamicgraph::Matrix Mcreuseinv( Mcreuse.nbCols(),r );
Mcreuseinv.resize(Mcreuse.cols(), r);
Eigen::pseudoInverse(Mcreuse, Mcreuseinv);
dynamicgraph::pseudoInverse(Mcreuse, Mcreuseinv);
parc = 0;
Minv.fill(0.);
for (std::list<unsigned int>::iterator iter = nonzeros.begin();
......@@ -281,7 +281,7 @@ int main(int argc, char **argv) {
dynamicgraph::Matrix Mcreuseinv(Mcreuse.cols(), r);
START_CHRONO(inv);
for (int ib = 0; ib < BENCH; ++ib) {
Eigen::pseudoInverse(Mcreuse, Mcreuseinv);
dynamicgraph::pseudoInverse(Mcreuse, Mcreuseinv);
}
STOP_CHRONO(inv, "M+creuseseule");
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment