From 2ae4f93ef84530a7f862a45ac57a5120865073ed Mon Sep 17 00:00:00 2001
From: Mansard <nmansard@laas.fr>
Date: Tue, 23 Aug 2011 09:54:58 +0200
Subject: [PATCH] IVIGIT.
---
unitTesting/CMakeLists.txt | 18 +---
unitTesting/qrt.cpp | 186 +++++++++++++++++++++++++++++++++++++
2 files changed, 191 insertions(+), 13 deletions(-)
create mode 100644 unitTesting/qrt.cpp
diff --git a/unitTesting/CMakeLists.txt b/unitTesting/CMakeLists.txt
index dec17e2..8516db1 100644
--- a/unitTesting/CMakeLists.txt
+++ b/unitTesting/CMakeLists.txt
@@ -16,16 +16,17 @@
ADD_DEFINITIONS(-DDEBUG=2)
SET(tests
- dummy)
+ dummy
+ qrt)
FOREACH(test ${tests})
SET(EXECUTABLE_NAME "${test}_exe")
ADD_EXECUTABLE(${EXECUTABLE_NAME}
${test}.cpp)
- TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME}
- controller-pd
- )
+ #TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME}
+ #controller-pd
+ # )
PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} sot-core)
PKG_CONFIG_USE_DEPENDENCY(${EXECUTABLE_NAME} soth)
@@ -35,13 +36,4 @@ FOREACH(test ${tests})
TARGET_LINK_LIBRARIES(${EXECUTABLE_NAME} "${${test}_plugins_dependencies}")
ENDIF(${test}_plugins_dependencies)
-
- ADD_TEST(${test} ${EXECUTABLE_NAME}
- ${samplemodelpath} sample.wrl ${samplespec} ${sampleljr} )
-
- IF (UNIX)
- SET_PROPERTY(TEST ${test} PROPERTY
- ENVIRONMENT "LD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/lib:${CMAKE_BINARY_DIR}/src:${BOOST_ROOT}/lib")
- ENDIF(UNIX)
-
ENDFOREACH(test)
diff --git a/unitTesting/qrt.cpp b/unitTesting/qrt.cpp
new file mode 100644
index 0000000..136864d
--- /dev/null
+++ b/unitTesting/qrt.cpp
@@ -0,0 +1,186 @@
+/*
+ * Copyright 2011, Nicolas Mansard, LAAS-CNRS
+ *
+ * This file is part of sot-dyninv.
+ * sot-dyninv is free software: you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public License
+ * as published by the Free Software Foundation, either version 3 of
+ * the License, or (at your option) any later version.
+ * sot-dyninv is distributed in the hope that it will be
+ * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU Lesser General Public License for more details. You should
+ * have received a copy of the GNU Lesser General Public License along
+ * with sot-dyninvc. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <Eigen/QR>
+#include <soth/Algebra.hpp>
+#include <iostream>
+
+std::ostream& p( const char * n ) { return std::cout << n << " = "; }
+
+
+namespace Eigen
+{
+ class FullRowRankColPivQR
+ : private ColPivHouseholderQR< MatrixXd >
+ {
+ public:
+
+ void compute(const MatrixType& matrix)
+ {
+ assert( matrix.rows()<=matrix.cols() );
+ ColPivHouseholderQR< MatrixXd >::compute( matrix.transpose() );
+ assert( rank() == matrix.rows() );
+ }
+
+ void solveInPlace( MatrixXd& Gp )
+ {
+ /* r is the rank, nxm the size of the original matrix (ie whose transpose has
+ * been decomposed. */
+ const int r = rank(), n=cols(), m=rows();
+ assert( r==n );
+ assert( Gp.rows() == m );
+
+ /* G'E = Q R
+ * => G^+ = Q R^+T E' */
+
+ /* Compute E'*X. */
+ Gp.applyOnTheLeft( colsPermutation().transpose() );
+
+ /* Compute R^+T E'*X. */
+ matrixQR()
+ .topLeftCorner(r,r).transpose().triangularView<Lower>()
+ .solveInPlace(Gp.topRows(r));
+
+ /* Compute Q R^+T E'*X. */
+ /* Q = P1*P2* ... *Pn */
+ VectorXd workspace( n );
+ for (int k = r-1; k >= 0 ; --k)
+ {
+ int remainingSize = m-k;
+ Gp.bottomRows( Gp.rows()-k )
+ .applyHouseholderOnTheLeft(matrixQR().col(k).tail(remainingSize-1),
+ hCoeffs().coeff(k), &workspace.coeffRef(0));
+ }
+ }
+ };
+}
+
+int main (void)
+{
+ using namespace Eigen;
+ using std::endl; using std::cout; using soth::MATLAB;
+
+ { /* Compute the pseudo inverse of a full-col rank matrix: G = V [ R; 0 ]. */
+
+ const unsigned int n=6, m=3, r=3;
+
+ MatrixXd G = MatrixXd::Random(n,m);
+ p("G") << (MATLAB)G << endl;
+
+ Eigen::ColPivHouseholderQR<Eigen::MatrixXd> G_qr;
+ G_qr.compute(G);
+ p("QR") << (MATLAB)G_qr.matrixQR() << endl;
+
+ {
+ /* Q1 = ... */
+ MatrixXd Q1 = MatrixXd::Identity( n,r );
+ VectorXd workspace( n );
+ /* P0*P1*P2 ... */
+ for (int k = r-1; k >= 0 ; --k)
+ {
+ int remainingSize = n-k;
+ Q1.bottomRows( Q1.rows()-k )
+ .applyHouseholderOnTheLeft(G_qr.matrixQR().col(k).tail(remainingSize-1),
+ G_qr.hCoeffs().coeff(k), &workspace.coeffRef(0));
+ }
+ p("Q1") << (MATLAB)Q1 << endl;
+ }
+
+ {
+ /* Q1' = ... */
+ MatrixXd Q1t = MatrixXd::Identity( r,n );
+ VectorXd workspace( n );
+ /* P2*P1*P0 ... */
+ for (int k = r-1; k >= 0 ; --k)
+ {
+ int remainingSize = n-k;
+ Q1t.rightCols( Q1t.cols()-k )
+ .applyHouseholderOnTheRight(G_qr.matrixQR().col(k).tail(remainingSize-1),
+ G_qr.hCoeffs().coeff(k), &workspace.coeffRef(0));
+ }
+ p("Q1t") << (MATLAB)Q1t << endl;
+ }
+
+ {
+ MatrixXd Gp = MatrixXd::Identity( r,n );
+ VectorXd workspace( n );
+ /* P2*P1*P0 ... */
+ for (int k = r-1; k >= 0 ; --k)
+ {
+ int remainingSize = n-k;
+ Gp.rightCols( Gp.cols()-k )
+ .applyHouseholderOnTheRight(G_qr.matrixQR().col(k).tail(remainingSize-1),
+ G_qr.hCoeffs().coeff(k), &workspace.coeffRef(0));
+ }
+
+ G_qr.matrixQR()
+ .topLeftCorner(r,r).triangularView<Upper>()
+ .solveInPlace(Gp);
+ p("EtGp") << (MATLAB)Gp << endl;
+
+ Gp.applyOnTheLeft( G_qr.colsPermutation() );
+ p("Gp") << (MATLAB)Gp << endl;
+ }
+ }
+
+ { /* Compute the pseudo inverse of a full-row rank matrix: G = [ L; 0 ] V'. */
+ const unsigned int n=3, m=6, r=3;
+
+ MatrixXd G = MatrixXd::Random(n,m);
+ p("G") << (MATLAB)G << endl;
+
+ Eigen::ColPivHouseholderQR< Eigen::MatrixXd > Gt_qr;
+ Gt_qr.compute(G.transpose());
+ p("QR") << (MATLAB)Gt_qr.matrixQR() << endl;
+ p("Q") << (MATLAB)(MatrixXd)Gt_qr.householderQ() << endl;
+ p("R") << (MATLAB)(MatrixXd)Gt_qr.matrixQR().triangularView<Upper>() << endl;
+ p("E") << (MATLAB)Gt_qr.colsPermutation().toDenseMatrix() << endl;
+
+ {
+ MatrixXd Gp = MatrixXd::Identity( m,r );
+ Gp.applyOnTheLeft( Gt_qr.colsPermutation().transpose() );
+ p("Et") << (MATLAB)Gp << endl;
+
+ Gt_qr.matrixQR()
+ .topLeftCorner(r,r).transpose().triangularView<Lower>()
+ .solveInPlace(Gp.topRows(r));
+ p("RptEt") << (MATLAB)Gp << endl;
+
+ /* Q = P1*P2* ... *Pn */
+ VectorXd workspace( n );
+ for (int k = r-1; k >= 0 ; --k)
+ {
+ cout << k << endl;
+ int remainingSize = m-k;
+ Gp.bottomRows( Gp.rows()-k )
+ .applyHouseholderOnTheLeft(Gt_qr.matrixQR().col(k).tail(remainingSize-1),
+ Gt_qr.hCoeffs().coeff(k), &workspace.coeffRef(0));
+ }
+ p("Gp") << (MATLAB)Gp << endl;
+
+ }
+
+ {
+ FullRowRankColPivQR G_lq; G_lq.compute(G);
+ MatrixXd Gp = MatrixXd::Identity( m,r );
+ G_lq.solveInPlace(Gp);
+ p("Gp") << (MATLAB)Gp << endl;
+ }
+
+ }
+
+ return 0;
+}
--
GitLab