IVIGIT.

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)

unitTesting/qrt.cpp

+/*
+ * 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;
+}