diff --git a/CMakeLists.txt b/CMakeLists.txt
index 56d5da37458fab855c559abc3b42092d4b8ba595..2424b023433994127582a23f8a4cefb17d65ccff 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -56,7 +56,6 @@ SET(libs
task-dyn-joint-limits
solver-op-space
solver-dyn-reduced
- solver-dyn-red2
solver-kine
task-joint-limits
@@ -85,12 +84,13 @@ SET(headers
task-dyn-joint-limits.h
solver-op-space.h
solver-dyn-reduced.h
- solver-dyn-red2.h
task-joint-limits.h
task-inequality.h
solver-kine.h
feature-projected-line.h
+
+ col-piv-qr-solve-in-place.h
)
# Add subdirectories.
diff --git a/src/col-piv-qr-solve-in-place.h b/src/col-piv-qr-solve-in-place.h
new file mode 100644
index 0000000000000000000000000000000000000000..38e411110765a8c1892a2c096855ae207f8fb398
--- /dev/null
+++ b/src/col-piv-qr-solve-in-place.h
@@ -0,0 +1,118 @@
+/*
+ * 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/>.
+ */
+
+#ifndef __sot_dyninv_ColPivQRSolveInPlace_H__
+#define __sot_dyninv_ColPivQRSolveInPlace_H__
+
+#include <Eigen/QR>
+
+namespace Eigen
+{
+
+ /* This class add two methods to the classical ColPiv: solveInPlace for full
+ * col rank matrices (and the direct consequence of pseudoInverse for
+ * f-c-r). And solveTranposeInPlace for full row rank matrices (and the
+ * direct pseudoInverseTranspose).
+ */
+ class ColPivQRSolveInPlace
+ : public ColPivHouseholderQR< MatrixXd >
+ {
+ public:
+
+ /* Find a solution x to the problem G x = b. The input vector is b,
+ * completed by the 0 tail to obtain the x size. The solver only works for
+ * full-col rank matrices, ie matrices G = Q [ R; 0 ], with R full-rank
+ * (full diag) upper triangular. */
+ void solveInPlace( MatrixXd& Gp )
+ {
+ const int r = rank(), m=cols(), n=rows();
+ assert( r==m );
+ assert( Gp.rows() == m ); // TODO: if not proper size, resize.
+
+ VectorXd workspace( n );
+ /* P2*P1*P0 ... */
+ for (int k = r-1; k >= 0 ; --k)
+ {
+ int remainingSize = n-k;
+ Gp.rightCols( Gp.cols()-k )
+ .applyHouseholderOnTheRight(matrixQR().col(k).tail(remainingSize-1),
+ hCoeffs().coeff(k), &workspace.coeffRef(0));
+ }
+
+ matrixQR()
+ .topLeftCorner(r,r).triangularView<Upper>()
+ .solveInPlace(Gp);
+
+ Gp.applyOnTheLeft( colsPermutation() );
+ }
+
+ MatrixXd pseudoInverse( void )
+ {
+ MatrixXd res = MatrixXd::Identity( cols(),rows() );
+ solveInPlace( res );
+ return res;
+ }
+
+
+
+ /* Find a solution x to the problem G'x = b. The input vector is b,
+ * completed by the 0 tail to obtain the x size. The solver only works for
+ * full-row rank matrices, ie matrices G' = [ L 0 ] Q, with L full-rank
+ * (full diag) lower triangular. */
+ void solveTransposeInPlace( MatrixXd& Gtp )
+ {
+ /* r is the rank, nxm the size of the original matrix (ie whose transpose
+ * has been decomposed. n is the number of cols of the original matrix,
+ * thus number of rows of the transpose we want to inverse. */
+ const int r = rank(), n=cols(), m=rows();
+ assert( r==n );
+ assert( Gtp.rows() == m ); // TODO: if not proper size, resize.
+
+ /* G E = Q R :: E' G' = L Q', with L=R'
+ * => G^+T = Q R^+T E' */
+
+ /* Compute E'*X. */
+ Gtp.applyOnTheLeft( colsPermutation().transpose() );
+
+ /* Compute R^+T E'*X. */
+ matrixQR()
+ .topLeftCorner(r,r).transpose().triangularView<Lower>()
+ .solveInPlace(Gtp.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;
+ Gtp.bottomRows( Gtp.rows()-k )
+ .applyHouseholderOnTheLeft(matrixQR().col(k).tail(remainingSize-1),
+ hCoeffs().coeff(k), &workspace.coeffRef(0));
+ }
+ }
+
+ MatrixXd pseudoInverseTranspose( void )
+ {
+ MatrixXd Gtp = MatrixXd::Identity( rows(),cols() );
+ solveTransposeInPlace( Gtp );
+ return Gtp;
+ }
+
+ };
+}
+
+
+#endif // #ifndef __sot_dyninv_ColPivQRSolveInPlace_H__
diff --git a/unitTesting/qrt.cpp b/unitTesting/qrt.cpp
index 7792fec6b690390d0fd7e2beee599a5c6175c73e..30237ecc31ff79abe1f69e92fd7d43f7bb48e4ec 100644
--- a/unitTesting/qrt.cpp
+++ b/unitTesting/qrt.cpp
@@ -14,108 +14,15 @@
* with sot-dyninvc. If not, see <http://www.gnu.org/licenses/>.
*/
-#include <Eigen/QR>
+#include <sot-dyninv/col-piv-qr-solve-in-place.h>
#include <soth/Algebra.hpp>
#include <iostream>
std::ostream& p( const char * n ) { return std::cout << n << " = "; }
-namespace Eigen
-{
-
- /* This class add two methods to the classical ColPiv: solveInPlace for full
- * col rank matrices (and the direct consequence of pseudoInverse for
- * f-c-r). And solveTranposeInPlace for full row rank matrices (and the
- * direct pseudoInverseTranspose).
- */
- class ColPivQRSolveInPlace
- : public ColPivHouseholderQR< MatrixXd >
- {
- public:
-
- /* Find a solution x to the problem G x = b. The input vector is b,
- * completed by the 0 tail to obtain the x size. The solver only works for
- * full-col rank matrices, ie matrices G = Q [ R; 0 ], with R full-rank
- * (full diag) upper triangular. */
- void solveInPlace( MatrixXd& Gp )
- {
- const int r = rank(), m=cols(), n=rows();
- assert( r==m );
- assert( Gp.rows() == m ); // TODO: if not proper size, resize.
-
- VectorXd workspace( n );
- /* P2*P1*P0 ... */
- for (int k = r-1; k >= 0 ; --k)
- {
- int remainingSize = n-k;
- Gp.rightCols( Gp.cols()-k )
- .applyHouseholderOnTheRight(matrixQR().col(k).tail(remainingSize-1),
- hCoeffs().coeff(k), &workspace.coeffRef(0));
- }
-
- matrixQR()
- .topLeftCorner(r,r).triangularView<Upper>()
- .solveInPlace(Gp);
-
- Gp.applyOnTheLeft( colsPermutation() );
- }
-
- MatrixXd pseudoInverse( void )
- {
- MatrixXd res = MatrixXd::Identity( cols(),rows() );
- solveInPlace( res );
- return res;
- }
-
- /* Find a solution x to the problem G'x = b. The input vector is b,
- * completed by the 0 tail to obtain the x size. The solver only works for
- * full-row rank matrices, ie matrices G' = [ L 0 ] Q, with L full-rank
- * (full diag) lower triangular. */
- void solveTransposeInPlace( MatrixXd& Gtp )
- {
- /* r is the rank, nxm the size of the original matrix (ie whose transpose
- * has been decomposed. n is the number of cols of the original matrix,
- * thus number of rows of the transpose we want to inverse. */
- const int r = rank(), n=cols(), m=rows();
- assert( r==n );
- assert( Gtp.rows() == m ); // TODO: if not proper size, resize.
-
- /* G E = Q R :: E' G' = L Q', with L=R'
- * => G^+T = Q R^+T E' */
-
- /* Compute E'*X. */
- Gtp.applyOnTheLeft( colsPermutation().transpose() );
-
- /* Compute R^+T E'*X. */
- matrixQR()
- .topLeftCorner(r,r).transpose().triangularView<Lower>()
- .solveInPlace(Gtp.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;
- Gtp.bottomRows( Gtp.rows()-k )
- .applyHouseholderOnTheLeft(matrixQR().col(k).tail(remainingSize-1),
- hCoeffs().coeff(k), &workspace.coeffRef(0));
- }
- }
-
- MatrixXd pseudoInverseTranspose( void )
- {
- MatrixXd Gtp = MatrixXd::Identity( rows(),cols() );
- solveTransposeInPlace( Gtp );
- return Gtp;
- }
-
- };
-}
-
int main (void)
{
using namespace Eigen;